diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 4546a5b..ebfef81 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -65,7 +65,26 @@ void PushAndResetLineMarker(visualization_msgs::Marker* marker, MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer) : node_options_(node_options), - map_builder_(node_options.map_builder_options, {}), + map_builder_( + node_options.map_builder_options, + cartographer::mapping::MapBuilder::LocalSlamResultCallback( + [this]( + const int trajectory_id, + const ::cartographer::common::Time time, + const ::cartographer::transform::Rigid3d local_pose, + ::cartographer::sensor::RangeData range_data_in_local, + const std::unique_ptr) + EXCLUDES(mutex_) { + std::shared_ptr + local_slam_data = + std::make_shared( + TrajectoryState::LocalSlamData{ + time, local_pose, + std::move(range_data_in_local)}); + cartographer::common::MutexLocker lock(&mutex_); + trajectory_state_data_[trajectory_id] = + std::move(local_slam_data); + })), tf_buffer_(tf_buffer) {} void MapBuilderBridge::LoadMap(const std::string& map_filename) { @@ -169,24 +188,22 @@ MapBuilderBridge::GetTrajectoryStates() { const int trajectory_id = entry.first; const SensorBridge& sensor_bridge = *entry.second; - const cartographer::mapping::TrajectoryBuilder* const trajectory_builder = - map_builder_.GetTrajectoryBuilder(trajectory_id); - if (trajectory_builder == nullptr) { - continue; - } - const cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate = - trajectory_builder->pose_estimate(); - if (cartographer::common::ToUniversal(pose_estimate.time) < 0) { - continue; + std::shared_ptr local_slam_data; + { + cartographer::common::MutexLocker lock(&mutex_); + if (trajectory_state_data_.count(trajectory_id) == 0) { + continue; + } + local_slam_data = trajectory_state_data_.at(trajectory_id); } // Make sure there is a trajectory with 'trajectory_id'. CHECK_EQ(trajectory_options_.count(trajectory_id), 1); trajectory_states[trajectory_id] = { - pose_estimate, + local_slam_data, map_builder_.pose_graph()->GetLocalToGlobalTransform(trajectory_id), sensor_bridge.tf_bridge().LookupToTracking( - pose_estimate.time, + local_slam_data->time, trajectory_options_[trajectory_id].published_frame), trajectory_options_[trajectory_id]}; } diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index f4d4feb..2f9947f 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -39,7 +39,15 @@ namespace cartographer_ros { class MapBuilderBridge { public: struct TrajectoryState { - cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate; + // Contains the trajectory state data received from local SLAM, after + // it had processed accumulated 'range_data_in_local' and estimated + // current 'local_pose' at 'time'. + struct LocalSlamData { + ::cartographer::common::Time time; + ::cartographer::transform::Rigid3d local_pose; + ::cartographer::sensor::RangeData range_data_in_local; + }; + std::shared_ptr local_slam_data; cartographer::transform::Rigid3d local_to_map; std::unique_ptr published_to_tracking; TrajectoryOptions trajectory_options; @@ -62,14 +70,18 @@ class MapBuilderBridge { cartographer_ros_msgs::SubmapQuery::Response& response); cartographer_ros_msgs::SubmapList GetSubmapList(); - std::unordered_map GetTrajectoryStates(); + std::unordered_map GetTrajectoryStates() + EXCLUDES(mutex_); visualization_msgs::MarkerArray GetTrajectoryNodeList(); visualization_msgs::MarkerArray GetConstraintList(); SensorBridge* sensor_bridge(int trajectory_id); private: + cartographer::common::Mutex mutex_; const NodeOptions node_options_; + std::unordered_map> + trajectory_state_data_ GUARDED_BY(mutex_); cartographer::mapping::MapBuilder map_builder_; tf2_ros::Buffer* const tf_buffer_; diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index b68e16b..3d4399b 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -169,23 +169,25 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { auto& extrapolator = extrapolators_.at(entry.first); // 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 != extrapolator.GetLastPoseTime()) { + if (trajectory_state.local_slam_data->time != + extrapolator.GetLastPoseTime()) { // TODO(gaschler): Consider using other message without time information. carto::sensor::TimedPointCloud point_cloud; - point_cloud.reserve(trajectory_state.pose_estimate.point_cloud.size()); + point_cloud.reserve( + trajectory_state.local_slam_data->range_data_in_local.returns.size()); for (const Eigen::Vector3f point : - trajectory_state.pose_estimate.point_cloud) { + trajectory_state.local_slam_data->range_data_in_local.returns) { Eigen::Vector4f point_time; point_time << point, 0.f; point_cloud.push_back(point_time); } scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message( - carto::common::ToUniversal(trajectory_state.pose_estimate.time), + carto::common::ToUniversal(trajectory_state.local_slam_data->time), node_options_.map_frame, carto::sensor::TransformTimedPointCloud( point_cloud, trajectory_state.local_to_map.cast()))); - extrapolator.AddPose(trajectory_state.pose_estimate.time, - trajectory_state.pose_estimate.pose); + extrapolator.AddPose(trajectory_state.local_slam_data->time, + trajectory_state.local_slam_data->local_pose); } geometry_msgs::TransformStamped stamped_transform;