Fixes pose publishing to include/exclude odom frame. (#3)

Changes the pose publishing to always publish the odom to map transform.
Either expects or provides (configurable) the odom to tracking transform.
Also cleans up the RViz plugin code a bit.

cartographer_ros/launch/3d_mapping.launch

@@ -84,7 +84,9 @@
       type="cartographer_node" output="screen" >
     <rosparam subst_value="true">
       map_frame: "map"
+      odom_frame: "odom"
       tracking_frame: "base_link"
+      provide_odom: true
       configuration_files_directories: [
         "$(find cartographer_ros)/configuration_files"
       ]

cartographer_ros/launch/mapping_2d.launch

@@ -25,7 +25,9 @@
       type="cartographer_node" output="screen" >
     <rosparam subst_value="true">
       map_frame: "map"
+      odom_frame: "odom"
       tracking_frame: "base_link"
+      provide_odom: true
       configuration_files_directories: [
         "$(find cartographer_ros)/configuration_files"
       ]

cartographer_ros/launch/state.launch

@@ -1,73 +0,0 @@
-<!--
-  Copyright 2016 The Cartographer Authors
-
-  Licensed under the Apache License, Version 2.0 (the "License");
-  you may not use this file except in compliance with the License.
-  You may obtain a copy of the License at
-
-       http://www.apache.org/licenses/LICENSE-2.0
-
-  Unless required by applicable law or agreed to in writing, software
-  distributed under the License is distributed on an "AS IS" BASIS,
-  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-  See the License for the specific language governing permissions and
-  limitations under the License.
--->
-
-<launch>
-  <param name="robot_description"
-    textfile="$(find cartographer_ros)/urdf/backpack_3d.urdf" />
-
-  <node name="robot_state_publisher" pkg="robot_state_publisher"
-    type="robot_state_publisher" />
-
-  <node name="cartographer" pkg="cartographer_ros"
-      type="cartographer_node" output="screen" >
-    <rosparam subst_value="true">
-      # This node publishes the transformation between the tracking and map
-      # frames.
-      map_frame: "map"
-      tracking_frame: "base_link"
-
-      # Lua configuration files are always searched in the cartographer
-      # installation directory. We also ship configuration that work well for
-      # the platforms that we used to collect our example data. You probably
-      # want to add your own configuration overwrites in your own node
-      # directories - you should add the path here as first entry then.
-      configuration_files_directories: [
-        "$(find cartographer_ros)/configuration_files"
-      ]
-
-      # Configuration file for SLAM. The settings in here are tweaked to the
-      # collection platform you are using.
-      mapping_configuration_basename: "backpack_3d.lua"
-
-      imu_topic: "/imu"
-
-      # Laser min and max range. Everything not in this range will not be used for mapping.
-      laser_min_range_m: 0.
-      laser_max_range_m: 30.
-
-      # Missing laser echoes will be inserted as free space at this distance.
-      laser_missing_echo_ray_length_m: 5.
-
-      # Only choose one in the next parameter block
-      # laser_scan_2d_topic: "/horizontal_2d_laser"
-      # multi_echo_laser_scan_2d_topic: "/horizontal_multiecho_2d_laser"
-      laser_scan_3d_topics: ["/horizontal_3d_laser", "/vertical_3d_laser"]
-    </rosparam>
-  </node>
-
-  <!-- TODO(hrapp): still useful for occasional testing, but delete eventually.
-  <node pkg="hector_mapping" type="hector_mapping" name="hector_height_mapping"
-      output="screen">
-    <param name="scan_topic" value="horizontal_2d_laser" />
-    <param name="pub_map_odom_transform" value="true"/>
-    <param name="map_frame" value="map" />
-    <param name="base_frame" value="base_link" />
-    <param name="odom_frame" value="base_link" />
-    <param name="map_size" value="4096" />
-    <param name="map_resolution" value="0.1" />
-  </node>
-  -->
-</launch>

cartographer_ros/launch/turtlebot.launch

@@ -25,7 +25,9 @@
       type="cartographer_node" output="screen" >
     <rosparam subst_value="true">
       map_frame: "map"
+      odom_frame: "odom"
       tracking_frame: "base_link"
+      provide_odom: false
       configuration_files_directories: [
         "$(find cartographer_ros)/configuration_files"
       ]

cartographer_ros/src/cartographer_node_main.cc

@@ -186,7 +186,9 @@ class Node {
   ros::Subscriber laser_2d_subscriber_;
   std::vector<ros::Subscriber> laser_3d_subscribers_;
   string tracking_frame_;
+  string odom_frame_;
   string map_frame_;
+  bool provide_odom_;
   double laser_min_range_m_;
   double laser_max_range_m_;
   double laser_missing_echo_ray_length_m_;
@@ -249,7 +251,7 @@ void Node::AddImu(const int64 timestamp, const string& frame_id,
                   ::cartographer::transform::ToEigen(imu.linear_acceleration()),
         sensor_to_tracking.rotation() *
             ::cartographer::transform::ToEigen(imu.angular_velocity()));
-  } catch (tf2::TransformException& ex) {
+  } catch (const tf2::TransformException& ex) {
     LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
                  << ": " << ex.what();
   }
@@ -282,7 +284,7 @@ void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
         ::cartographer::sensor::ToLaserFan3D(laser_fan),
         sensor_to_tracking.cast<float>());
     trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d);
-  } catch (tf2::TransformException& ex) {
+  } catch (const tf2::TransformException& ex) {
     LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
                  << ": " << ex.what();
   }
@@ -322,7 +324,7 @@ void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id,
         time, ::cartographer::sensor::TransformLaserFan3D(
                   ::cartographer::sensor::FromProto(laser_fan_3d),
                   sensor_to_tracking.cast<float>()));
-  } catch (tf2::TransformException& ex) {
+  } catch (const tf2::TransformException& ex) {
     LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
                  << ": " << ex.what();
   }
@@ -339,7 +341,9 @@ const T Node::GetParamOrDie(const string& name) {
 
 void Node::Initialize() {
   tracking_frame_ = GetParamOrDie<string>("tracking_frame");
+  odom_frame_ = GetParamOrDie<string>("odom_frame");
   map_frame_ = GetParamOrDie<string>("map_frame");
+  provide_odom_ = GetParamOrDie<bool>("provide_odom");
   laser_min_range_m_ = GetParamOrDie<double>("laser_min_range_m");
   laser_max_range_m_ = GetParamOrDie<double>("laser_max_range_m");
   laser_missing_echo_ray_length_m_ =
@@ -515,24 +519,41 @@ void Node::PublishSubmapList(int64 timestamp) {
 }
 
 void Node::PublishPose(int64 timestamp) {
-  ::cartographer::common::MutexLocker lock(&mutex_);
-  const ::cartographer::mapping::Submaps* submaps =
-      trajectory_builder_->submaps();
-  const ::cartographer::transform::Rigid3d odometry_to_map =
-      sparse_pose_graph_->GetOdometryToMapTransform(*submaps);
-  const auto& pose_estimate = trajectory_builder_->pose_estimate();
-
-  const ::cartographer::transform::Rigid3d pose =
-      odometry_to_map * pose_estimate.pose;
   const ::cartographer::common::Time time =
       ::cartographer::common::FromUniversal(timestamp);
-
+  const Rigid3d tracking_to_map = trajectory_builder_->pose_estimate().pose;
   geometry_msgs::TransformStamped stamped_transform;
   stamped_transform.header.stamp = ToRos(time);
   stamped_transform.header.frame_id = map_frame_;
-  stamped_transform.child_frame_id = tracking_frame_;
-  stamped_transform.transform = ToGeometryMsgTransform(pose);
-  tf_broadcaster_.sendTransform(stamped_transform);
+  stamped_transform.child_frame_id = odom_frame_;
+
+  if (provide_odom_) {
+    ::cartographer::common::MutexLocker lock(&mutex_);
+    const ::cartographer::mapping::Submaps* submaps =
+        trajectory_builder_->submaps();
+    const ::cartographer::transform::Rigid3d odom_to_map =
+        sparse_pose_graph_->GetOdometryToMapTransform(*submaps);
+    stamped_transform.transform = ToGeometryMsgTransform(odom_to_map);
+    tf_broadcaster_.sendTransform(stamped_transform);
+
+    stamped_transform.header.frame_id = odom_frame_;
+    stamped_transform.child_frame_id = tracking_frame_;
+    const ::cartographer::transform::Rigid3d tracking_to_odom =
+        odom_to_map.inverse() * tracking_to_map;
+    stamped_transform.transform = ToGeometryMsgTransform(tracking_to_odom);
+    tf_broadcaster_.sendTransform(stamped_transform);
+  } else {
+    try {
+      const Rigid3d tracking_to_odom =
+          LookupToTrackingTransformOrThrow(time, odom_frame_).inverse();
+      const Rigid3d odom_to_map = tracking_to_map * tracking_to_odom.inverse();
+      stamped_transform.transform = ToGeometryMsgTransform(odom_to_map);
+      tf_broadcaster_.sendTransform(stamped_transform);
+    } catch (const tf2::TransformException& ex) {
+      LOG(WARNING) << "Cannot transform " << tracking_frame_ << " -> "
+                   << odom_frame_ << ": " << ex.what();
+    }
+  }
   last_pose_publish_timestamp_ = timestamp;
 }
 

cartographer_ros/src/submaps_display.cc

@@ -79,12 +79,12 @@ SubmapsDisplay::SubmapsDisplay()
       QString::fromStdString(
           ros::message_traits::datatype<::cartographer_ros_msgs::SubmapList>()),
       "cartographer_ros_msgs::SubmapList topic to subscribe to.", this,
-      SLOT(UpdateTopic()));
+      SLOT(UpdateSubmapTopicOrService()));
   submap_query_service_property_ = new ::rviz::StringProperty(
       "Submap query service",
       QString("/cartographer/") + kSubmapQueryServiceName,
       "Submap query service to connect to.", this,
-      SLOT(UpdateSubmapQueryServiceName()));
+      SLOT(UpdateSubmapTopicOrService()));
   map_frame_property_ = new ::rviz::StringProperty(
       "Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.",
       this);
@@ -104,11 +104,7 @@ SubmapsDisplay::SubmapsDisplay()
   Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
 }
 
-SubmapsDisplay::~SubmapsDisplay() {
-  Unsubscribe();
-  client_.shutdown();
-  Clear();
-}
+SubmapsDisplay::~SubmapsDisplay() { UnsubscribeAndClear(); }
 
 void SubmapsDisplay::onInitialize() {
   submaps_scene_manager_ =
@@ -138,44 +134,31 @@ void SubmapsDisplay::onInitialize() {
       kSubmapTexturesGroup);
 
   scene_manager_->addListener(&scene_manager_listener_);
-  // TODO(whess): Combine UpdateTopic()/UpdateSubmapQueryServiceName().
-  UpdateSubmapQueryServiceName();
+  UpdateSubmapTopicOrService();
 }
 
-void SubmapsDisplay::UpdateTopic() {
-  Unsubscribe();
-  Clear();
-  Subscribe();
-}
-
-void SubmapsDisplay::UpdateSubmapQueryServiceName() {
-  Unsubscribe();
-  Clear();
-  client_.shutdown();
-  client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
-      submap_query_service_property_->getStdString());
+void SubmapsDisplay::UpdateSubmapTopicOrService() {
+  UnsubscribeAndClear();
   Subscribe();
 }
 
 void SubmapsDisplay::reset() {
   Display::reset();
-
-  Clear();
-  UpdateTopic();
+  UpdateSubmapTopicOrService();
 }
 
 void SubmapsDisplay::onEnable() { Subscribe(); }
 
-void SubmapsDisplay::onDisable() {
-  Unsubscribe();
-  Clear();
-}
+void SubmapsDisplay::onDisable() { UnsubscribeAndClear(); }
 
 void SubmapsDisplay::Subscribe() {
   if (!isEnabled()) {
     return;
   }
 
+  client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
+      submap_query_service_property_->getStdString());
+
   if (!topic_property_->getTopic().isEmpty()) {
     try {
       submap_list_subscriber_ =
@@ -190,15 +173,15 @@ void SubmapsDisplay::Subscribe() {
   }
 }
 
-void SubmapsDisplay::Unsubscribe() { submap_list_subscriber_.shutdown(); }
-
 void SubmapsDisplay::IncomingSubmapList(
     const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
   submap_list_ = *msg;
   Q_EMIT SubmapListUpdated();
 }
 
-void SubmapsDisplay::Clear() {
+void SubmapsDisplay::UnsubscribeAndClear() {
+  submap_list_subscriber_.shutdown();
+  client_.shutdown();
   ::cartographer::common::MutexLocker locker(&mutex_);
   submaps_scene_manager_->clearScene();
   if (!rttTexture_.isNull()) {

cartographer_ros/src/submaps_display.h

@@ -72,8 +72,7 @@ class SubmapsDisplay : public ::rviz::Display {
 
  private Q_SLOTS:
   void RequestNewSubmaps();
-  void UpdateTopic();
-  void UpdateSubmapQueryServiceName();
+  void UpdateSubmapTopicOrService();
 
  private:
   class SceneManagerListener : public Ogre::SceneManager::Listener {
@@ -91,13 +90,10 @@ class SubmapsDisplay : public ::rviz::Display {
   void onEnable() override;
   void onDisable() override;
   void Subscribe();
-  void Unsubscribe();
+  void UnsubscribeAndClear();
   void UpdateMapTexture();
   void IncomingSubmapList(
       const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg);
-  // Clears the current map.
-  void Clear();
-  void UpdateCurrentTrackingZ(const tf::tfMessage::ConstPtr& msg);
 
   int rtt_count_;
   SceneManagerListener scene_manager_listener_;