diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 63c9971..359aa34 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -127,12 +127,33 @@ MapBuilderBridge::BuildOccupancyGrid() { return occupancy_grid; } +std::unordered_map +MapBuilderBridge::GetTrajectoryStates() { + std::unordered_map trajectory_states; + for (const auto& entry : sensor_bridges_) { + const int trajectory_id = entry.first; + const SensorBridge& sensor_bridge = *entry.second; + + const cartographer::mapping::TrajectoryBuilder* const trajectory_builder = + map_builder_.GetTrajectoryBuilder(trajectory_id); + const cartographer::mapping::TrajectoryBuilder::PoseEstimate + pose_estimate = trajectory_builder->pose_estimate(); + if (cartographer::common::ToUniversal(pose_estimate.time) < 0) { + continue; + } + + trajectory_states[trajectory_id] = { + pose_estimate, + map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform( + *trajectory_builder->submaps()), + sensor_bridge.tf_bridge().LookupToTracking(pose_estimate.time, + options_.published_frame)}; + } + return trajectory_states; +} + SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) { return sensor_bridges_.at(trajectory_id).get(); } -cartographer::mapping::MapBuilder* MapBuilderBridge::map_builder() { - return &map_builder_; -} - } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 31ee7ba..56ee1ef 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -35,6 +35,12 @@ namespace cartographer_ros { class MapBuilderBridge { public: + struct TrajectoryState { + cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate; + cartographer::transform::Rigid3d local_to_map; + std::unique_ptr published_to_tracking; + }; + MapBuilderBridge(const NodeOptions& options, tf2_ros::Buffer* tf_buffer); MapBuilderBridge(const MapBuilderBridge&) = delete; @@ -51,9 +57,9 @@ class MapBuilderBridge { cartographer_ros_msgs::SubmapList GetSubmapList(); std::unique_ptr BuildOccupancyGrid(); + std::unordered_map GetTrajectoryStates(); SensorBridge* sensor_bridge(int trajectory_id); - cartographer::mapping::MapBuilder* map_builder(); private: const NodeOptions options_; diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index dc94b81..7ce2f81 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -179,7 +179,7 @@ void Node::Initialize() { &Node::PublishSubmapList, this)); wall_timers_.push_back(node_handle_.createWallTimer( ::ros::WallDuration(options_.pose_publish_period_sec), - &Node::PublishPoseAndScanMatchedPointCloud, this)); + &Node::PublishTrajectoryStates, this)); } bool Node::HandleSubmapQuery( @@ -206,69 +206,59 @@ void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); } -void Node::PublishPoseAndScanMatchedPointCloud( - const ::ros::WallTimerEvent& timer_event) { +void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { carto::common::MutexLocker lock(&mutex_); - const carto::mapping::TrajectoryBuilder* trajectory_builder = - map_builder_bridge_.map_builder()->GetTrajectoryBuilder(trajectory_id_); - const carto::mapping::TrajectoryBuilder::PoseEstimate last_pose_estimate = - trajectory_builder->pose_estimate(); - if (carto::common::ToUniversal(last_pose_estimate.time) < 0) { - return; - } + for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { + const auto& trajectory_state = entry.second; - const Rigid3d tracking_to_local = last_pose_estimate.pose; - const Rigid3d local_to_map = - map_builder_bridge_.map_builder() - ->sparse_pose_graph() - ->GetLocalToGlobalTransform(*trajectory_builder->submaps()); - const Rigid3d tracking_to_map = local_to_map * tracking_to_local; + geometry_msgs::TransformStamped stamped_transform; + stamped_transform.header.stamp = ToRos(trajectory_state.pose_estimate.time); - geometry_msgs::TransformStamped stamped_transform; - stamped_transform.header.stamp = ToRos(last_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 (last_pose_estimate.time != last_scan_matched_point_cloud_time_) { - scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( - carto::common::ToUniversal(last_pose_estimate.time), - options_.tracking_frame, - carto::sensor::TransformPointCloud( - last_pose_estimate.point_cloud, - tracking_to_local.inverse().cast()))); - last_scan_matched_point_cloud_time_ = last_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(); - } - - const auto published_to_tracking = - map_builder_bridge_.sensor_bridge(trajectory_id_) - ->tf_bridge() - .LookupToTracking(last_pose_estimate.time, options_.published_frame); - if (published_to_tracking != nullptr) { - if (options_.provide_odom_frame) { - std::vector stamped_transforms; - - stamped_transform.header.frame_id = options_.map_frame; - stamped_transform.child_frame_id = options_.odom_frame; - stamped_transform.transform = ToGeometryMsgTransform(local_to_map); - stamped_transforms.push_back(stamped_transform); - - stamped_transform.header.frame_id = options_.odom_frame; - stamped_transform.child_frame_id = options_.published_frame; - stamped_transform.transform = - ToGeometryMsgTransform(tracking_to_local * (*published_to_tracking)); - stamped_transforms.push_back(stamped_transform); - - tf_broadcaster_.sendTransform(stamped_transforms); + // 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), + options_.tracking_frame, + carto::sensor::TransformPointCloud( + trajectory_state.pose_estimate.point_cloud, + tracking_to_local.inverse().cast()))); + last_scan_matched_point_cloud_time_ = trajectory_state.pose_estimate.time; } else { - 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); + // 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(); + } + + if (trajectory_state.published_to_tracking != nullptr) { + if (options_.provide_odom_frame) { + std::vector stamped_transforms; + + stamped_transform.header.frame_id = options_.map_frame; + // TODO(damonkohler): 'odom_frame' and 'published_frame' must be + // per-trajectory to fully support the multi-robot use case. + stamped_transform.child_frame_id = options_.odom_frame; + stamped_transform.transform = ToGeometryMsgTransform(trajectory_state.local_to_map); + stamped_transforms.push_back(stamped_transform); + + stamped_transform.header.frame_id = options_.odom_frame; + stamped_transform.child_frame_id = options_.published_frame; + stamped_transform.transform = ToGeometryMsgTransform( + tracking_to_local * (*trajectory_state.published_to_tracking)); + stamped_transforms.push_back(stamped_transform); + + tf_broadcaster_.sendTransform(stamped_transforms); + } else { + stamped_transform.header.frame_id = options_.map_frame; + stamped_transform.child_frame_id = options_.published_frame; + stamped_transform.transform = ToGeometryMsgTransform( + tracking_to_map * (*trajectory_state.published_to_tracking)); + tf_broadcaster_.sendTransform(stamped_transform); + } } } } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index ac464b1..1e1ba19 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -55,8 +55,7 @@ class Node { cartographer_ros_msgs::FinishTrajectory::Response& response); void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); - void PublishPoseAndScanMatchedPointCloud( - const ::ros::WallTimerEvent& timer_event); + void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event); void SpinOccupancyGridThreadForever(); const NodeOptions options_;