From c560d05d99f16559489fb393f42e53ed469f3870 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Mon, 25 Jun 2018 16:07:32 +0200 Subject: [PATCH] Improve internal naming of local SLAM data. (#908) Prevents confusion with TrajectoryState and GetTrajectoryStates() of the pose graph interface. The affected data is only local. --- .../cartographer_ros/map_builder_bridge.cc | 26 +++++----- .../cartographer_ros/map_builder_bridge.h | 11 +++-- cartographer_ros/cartographer_ros/node.cc | 48 +++++++++---------- cartographer_ros/cartographer_ros/node.h | 12 ++--- 4 files changed, 49 insertions(+), 48 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index df3d0c3..ad57927 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -221,25 +221,25 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() { return submap_list; } -std::unordered_map -MapBuilderBridge::GetTrajectoryStates() { - std::unordered_map trajectory_states; +std::unordered_map +MapBuilderBridge::GetLocalTrajectoryData() { + std::unordered_map local_trajectory_data; for (const auto& entry : sensor_bridges_) { const int trajectory_id = entry.first; const SensorBridge& sensor_bridge = *entry.second; - std::shared_ptr local_slam_data; + std::shared_ptr local_slam_data; { cartographer::common::MutexLocker lock(&mutex_); - if (trajectory_state_data_.count(trajectory_id) == 0) { + if (local_slam_data_.count(trajectory_id) == 0) { continue; } - local_slam_data = trajectory_state_data_.at(trajectory_id); + local_slam_data = local_slam_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] = { + local_trajectory_data[trajectory_id] = { local_slam_data, map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id), sensor_bridge.tf_bridge().LookupToTracking( @@ -247,7 +247,7 @@ MapBuilderBridge::GetTrajectoryStates() { trajectory_options_[trajectory_id].published_frame), trajectory_options_[trajectory_id]}; } - return trajectory_states; + return local_trajectory_data; } visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() { @@ -497,12 +497,12 @@ void MapBuilderBridge::OnLocalSlamResult( ::cartographer::sensor::RangeData range_data_in_local, const std::unique_ptr) { - std::shared_ptr local_slam_data = - std::make_shared( - TrajectoryState::LocalSlamData{time, local_pose, - std::move(range_data_in_local)}); + std::shared_ptr local_slam_data = + std::make_shared( + LocalTrajectoryData::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); + local_slam_data_[trajectory_id] = std::move(local_slam_data); } } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 8682ed1..2daaa45 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -40,8 +40,8 @@ namespace cartographer_ros { class MapBuilderBridge { public: - struct TrajectoryState { - // Contains the trajectory state data received from local SLAM, after + struct LocalTrajectoryData { + // Contains the trajectory data received from local SLAM, after // it had processed accumulated 'range_data_in_local' and estimated // current 'local_pose' at 'time'. struct LocalSlamData { @@ -79,7 +79,7 @@ class MapBuilderBridge { std::set GetFrozenTrajectoryIds(); cartographer_ros_msgs::SubmapList GetSubmapList(); - std::unordered_map GetTrajectoryStates() + std::unordered_map GetLocalTrajectoryData() EXCLUDES(mutex_); visualization_msgs::MarkerArray GetTrajectoryNodeList(); visualization_msgs::MarkerArray GetLandmarkPosesList(); @@ -98,8 +98,9 @@ class MapBuilderBridge { cartographer::common::Mutex mutex_; const NodeOptions node_options_; - std::unordered_map> - trajectory_state_data_ GUARDED_BY(mutex_); + std::unordered_map> + local_slam_data_ GUARDED_BY(mutex_); std::unique_ptr 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 cf18a71..25ce75c 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -117,9 +117,9 @@ Node::Node( wall_timers_.push_back(node_handle_.createWallTimer( ::ros::WallDuration(node_options_.submap_publish_period_sec), &Node::PublishSubmapList, this)); - publish_trajectory_states_timer_ = node_handle_.createTimer( + publish_local_trajectory_data_timer_ = node_handle_.createTimer( ::ros::Duration(node_options_.pose_publish_period_sec), - &Node::PublishTrajectoryStates, this); + &Node::PublishLocalTrajectoryData, this); wall_timers_.push_back(node_handle_.createWallTimer( ::ros::WallDuration(node_options_.trajectory_publish_period_sec), &Node::PublishTrajectoryNodeList, this)); @@ -176,36 +176,36 @@ void Node::AddSensorSamplers(const int trajectory_id, options.landmarks_sampling_ratio)); } -void Node::PublishTrajectoryStates(const ::ros::TimerEvent& timer_event) { +void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { carto::common::MutexLocker lock(&mutex_); - for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { - const auto& trajectory_state = entry.second; + for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) { + const auto& trajectory_data = entry.second; 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.local_slam_data->time != + if (trajectory_data.local_slam_data->time != extrapolator.GetLastPoseTime()) { if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) { // TODO(gaschler): Consider using other message without time // information. carto::sensor::TimedPointCloud point_cloud; - point_cloud.reserve(trajectory_state.local_slam_data - ->range_data_in_local.returns.size()); + point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local + .returns.size()); for (const Eigen::Vector3f point : - trajectory_state.local_slam_data->range_data_in_local.returns) { + trajectory_data.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.local_slam_data->time), + carto::common::ToUniversal(trajectory_data.local_slam_data->time), node_options_.map_frame, carto::sensor::TransformTimedPointCloud( - point_cloud, trajectory_state.local_to_map.cast()))); + point_cloud, trajectory_data.local_to_map.cast()))); } - extrapolator.AddPose(trajectory_state.local_slam_data->time, - trajectory_state.local_slam_data->local_pose); + extrapolator.AddPose(trajectory_data.local_slam_data->time, + trajectory_data.local_slam_data->local_pose); } geometry_msgs::TransformStamped stamped_transform; @@ -218,7 +218,7 @@ void Node::PublishTrajectoryStates(const ::ros::TimerEvent& timer_event) { stamped_transform.header.stamp = ToRos(now); const Rigid3d tracking_to_local = [&] { - if (trajectory_state.trajectory_options.publish_frame_projected_to_2d) { + if (trajectory_data.trajectory_options.publish_frame_projected_to_2d) { return carto::transform::Embed3D( carto::transform::Project2D(extrapolator.ExtrapolatePose(now))); } @@ -226,34 +226,34 @@ void Node::PublishTrajectoryStates(const ::ros::TimerEvent& timer_event) { }(); const Rigid3d tracking_to_map = - trajectory_state.local_to_map * tracking_to_local; + trajectory_data.local_to_map * tracking_to_local; - if (trajectory_state.published_to_tracking != nullptr) { - if (trajectory_state.trajectory_options.provide_odom_frame) { + if (trajectory_data.published_to_tracking != nullptr) { + if (trajectory_data.trajectory_options.provide_odom_frame) { std::vector stamped_transforms; stamped_transform.header.frame_id = node_options_.map_frame; stamped_transform.child_frame_id = - trajectory_state.trajectory_options.odom_frame; + trajectory_data.trajectory_options.odom_frame; stamped_transform.transform = - ToGeometryMsgTransform(trajectory_state.local_to_map); + ToGeometryMsgTransform(trajectory_data.local_to_map); stamped_transforms.push_back(stamped_transform); stamped_transform.header.frame_id = - trajectory_state.trajectory_options.odom_frame; + trajectory_data.trajectory_options.odom_frame; stamped_transform.child_frame_id = - trajectory_state.trajectory_options.published_frame; + trajectory_data.trajectory_options.published_frame; stamped_transform.transform = ToGeometryMsgTransform( - tracking_to_local * (*trajectory_state.published_to_tracking)); + tracking_to_local * (*trajectory_data.published_to_tracking)); stamped_transforms.push_back(stamped_transform); tf_broadcaster_.sendTransform(stamped_transforms); } else { stamped_transform.header.frame_id = node_options_.map_frame; stamped_transform.child_frame_id = - trajectory_state.trajectory_options.published_frame; + trajectory_data.trajectory_options.published_frame; stamped_transform.transform = ToGeometryMsgTransform( - tracking_to_map * (*trajectory_state.published_to_tracking)); + tracking_to_map * (*trajectory_data.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 9a41a22..9e855a8 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -152,7 +152,7 @@ class Node { void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options); void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options); - void PublishTrajectoryStates(const ::ros::TimerEvent& timer_event); + void PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event); void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event); void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event); void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); @@ -208,11 +208,11 @@ class Node { // We have to keep the timer handles of ::ros::WallTimers around, otherwise // they do not fire. std::vector<::ros::WallTimer> wall_timers_; - // The timer for publishing trajectory states (i.e. pose transforms) is a - // regular timer which is not triggered when simulation time is standing - // still. This prevents overflowing the transform listener buffer by - // publishing the same transforms over and over again. - ::ros::Timer publish_trajectory_states_timer_; + // The timer for publishing local trajectory data (i.e. pose transforms and + // range data point clouds) is a regular timer which is not triggered when + // simulation time is standing still. This prevents overflowing the transform + // listener buffer by publishing the same transforms over and over again. + ::ros::Timer publish_local_trajectory_data_timer_; }; } // namespace cartographer_ros