From e49fecbf112e4fa346278b847be8faf2139e921f Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 25 Jul 2017 15:22:12 +0200 Subject: [PATCH] Publish the scan matched point cloud in the map frame. (#442) This is to make sure that the scan match result is visualized with correct pose in RViz independent of tf timing. --- cartographer_ros/cartographer_ros/node.cc | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index c010d6a..ea9d1f6 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -111,30 +111,27 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { const auto& trajectory_state = entry.second; - geometry_msgs::TransformStamped stamped_transform; - stamped_transform.header.stamp = ToRos(trajectory_state.pose_estimate.time); - - const auto& tracking_to_local = trajectory_state.pose_estimate.pose; - const Rigid3d tracking_to_map = - trajectory_state.local_to_map * tracking_to_local; - // We only publish a point cloud if it has changed. It is not needed at high // frequency, and republishing it would be computationally wasteful. if (trajectory_state.pose_estimate.time != last_scan_matched_point_cloud_time_) { scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( carto::common::ToUniversal(trajectory_state.pose_estimate.time), - trajectory_state.trajectory_options.tracking_frame, + node_options_.map_frame, carto::sensor::TransformPointCloud( trajectory_state.pose_estimate.point_cloud, - tracking_to_local.inverse().cast()))); + trajectory_state.local_to_map.cast()))); last_scan_matched_point_cloud_time_ = trajectory_state.pose_estimate.time; - } else { - // If we do not publish a new point cloud, we still allow time of the - // published poses to advance. - stamped_transform.header.stamp = ros::Time::now(); } + geometry_msgs::TransformStamped stamped_transform; + // If we do not publish a new point cloud, we still allow time of the + // published poses to advance. + stamped_transform.header.stamp = ros::Time::now(); + const auto& tracking_to_local = trajectory_state.pose_estimate.pose; + const Rigid3d tracking_to_map = + trajectory_state.local_to_map * tracking_to_local; + if (trajectory_state.published_to_tracking != nullptr) { if (trajectory_state.trajectory_options.provide_odom_frame) { std::vector stamped_transforms;