Make the published frame configurable. (#61)

This fixes #59.

cartographer_ros/configuration_files/backpack_2d.lua

@@ -18,6 +18,7 @@ options = {
   map_builder = MAP_BUILDER,
   map_frame = "map",
   tracking_frame = "base_link",
+  published_frame = "base_link",
   odom_frame = "odom",
   provide_odom_frame = true,
   use_odometry_data = false,

cartographer_ros/configuration_files/backpack_3d.lua

@@ -18,6 +18,7 @@ options = {
   map_builder = MAP_BUILDER,
   map_frame = "map",
   tracking_frame = "base_link",
+  published_frame = "base_link",
   odom_frame = "odom",
   provide_odom_frame = true,
   use_odometry_data = false,

cartographer_ros/src/cartographer_node_main.cc

@@ -104,6 +104,7 @@ struct NodeOptions {
   carto::mapping::proto::MapBuilderOptions map_builder_options;
   string map_frame;
   string tracking_frame;
+  string published_frame;
   string odom_frame;
   bool publish_occupancy_grid;
   bool provide_odom_frame;
@@ -130,6 +131,8 @@ NodeOptions CreateNodeOptions(
   options.map_frame = lua_parameter_dictionary->GetString("map_frame");
   options.tracking_frame =
       lua_parameter_dictionary->GetString("tracking_frame");
+  options.published_frame =
+      lua_parameter_dictionary->GetString("published_frame");
   options.odom_frame = lua_parameter_dictionary->GetString("odom_frame");
   options.publish_occupancy_grid =
       lua_parameter_dictionary->GetBool("publish_occupancy_grid");
@@ -140,9 +143,11 @@ NodeOptions CreateNodeOptions(
   options.use_constant_odometry_variance =
       lua_parameter_dictionary->GetBool("use_constant_odometry_variance");
   options.constant_odometry_translational_variance =
-      lua_parameter_dictionary->GetDouble("constant_odometry_translational_variance");
+      lua_parameter_dictionary->GetDouble(
+          "constant_odometry_translational_variance");
   options.constant_odometry_rotational_variance =
-      lua_parameter_dictionary->GetDouble("constant_odometry_rotational_variance");
+      lua_parameter_dictionary->GetDouble(
+          "constant_odometry_rotational_variance");
   options.use_horizontal_laser =
       lua_parameter_dictionary->GetBool("use_horizontal_laser");
   options.use_horizontal_multi_echo_laser =
@@ -293,9 +298,11 @@ void Node::AddOdometry(int64 timestamp, const string& frame_id,
     const Eigen::Matrix3d rotational =
         Eigen::Matrix3d::Identity() *
         options_.constant_odometry_rotational_variance;
-    applied_covariance <<  //
-        translational, Eigen::Matrix3d::Zero(),  //
+    // clang-format off
+    applied_covariance <<
+        translational, Eigen::Matrix3d::Zero(),
         Eigen::Matrix3d::Zero(), rotational;
+    // clang-format on
   }
   map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
       ->AddOdometerPose(time, pose, applied_covariance);
@@ -556,35 +563,36 @@ void Node::PublishPoseAndScanMatchedPointCloud(
 
   geometry_msgs::TransformStamped stamped_transform;
   stamped_transform.header.stamp = ToRos(last_pose_estimate.time);
+
+  try {
+    const Rigid3d published_to_tracking = LookupToTrackingTransformOrThrow(
+        last_pose_estimate.time, options_.published_frame);
+    if (options_.provide_odom_frame) {
       stamped_transform.header.frame_id = options_.map_frame;
       stamped_transform.child_frame_id = options_.odom_frame;
-
-  if (options_.provide_odom_frame) {
       stamped_transform.transform = ToGeometryMsgTransform(local_to_map);
       tf_broadcaster_.sendTransform(stamped_transform);
 
       stamped_transform.header.frame_id = options_.odom_frame;
-    stamped_transform.child_frame_id = options_.tracking_frame;
-    stamped_transform.transform = ToGeometryMsgTransform(tracking_to_local);
+      stamped_transform.child_frame_id = options_.published_frame;
+      stamped_transform.transform =
+          ToGeometryMsgTransform(tracking_to_local * published_to_tracking);
       tf_broadcaster_.sendTransform(stamped_transform);
     } else {
-    try {
-      const Rigid3d tracking_to_odom =
-          LookupToTrackingTransformOrThrow(last_pose_estimate.time,
-                                           options_.odom_frame)
-              .inverse();
-      const Rigid3d odom_to_map = tracking_to_map * tracking_to_odom.inverse();
-      stamped_transform.transform = ToGeometryMsgTransform(odom_to_map);
+      stamped_transform.header.frame_id = options_.map_frame;
+      stamped_transform.child_frame_id = options_.published_frame;
+      stamped_transform.transform =
+          ToGeometryMsgTransform(tracking_to_map * published_to_tracking);
       tf_broadcaster_.sendTransform(stamped_transform);
-    } catch (const tf2::TransformException& ex) {
-      LOG(WARNING) << "Cannot transform " << options_.tracking_frame << " -> "
-                   << options_.odom_frame << ": " << ex.what();
     }
+  } catch (const tf2::TransformException& ex) {
+    LOG(WARNING) << "Cannot transform " << options_.published_frame << " -> "
+                 << options_.tracking_frame << ": " << ex.what();
   }
 
   scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
-      carto::common::ToUniversal(last_pose_estimate.time), options_.tracking_frame,
-      carto::sensor::TransformPointCloud(
+      carto::common::ToUniversal(last_pose_estimate.time),
+      options_.tracking_frame, carto::sensor::TransformPointCloud(
                                    last_pose_estimate.point_cloud,
                                    tracking_to_local.inverse().cast<float>())));
 }
@@ -792,5 +800,4 @@ int main(int argc, char** argv) {
   ::cartographer_ros::ScopedRosLogSink ros_log_sink;
   ::cartographer_ros::Run();
   ::ros::shutdown();
-  return 0;
 }

cartographer_ros/src/submaps_display.cc

@@ -47,8 +47,7 @@ constexpr char kDefaultTrackingFrame[] = "base_link";
 
 SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) {
   submap_query_service_property_ = new ::rviz::StringProperty(
-      "Submap query service",
-      QString("/") + kSubmapQueryServiceName,
+      "Submap query service", QString("/") + kSubmapQueryServiceName,
       "Submap query service to connect to.", this, SLOT(Reset()));
   map_frame_property_ = new ::rviz::StringProperty(
       "Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.",