From cbc26545ad46a3e1fd87a724dc61a63402184208 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 1 Aug 2017 11:28:03 +0200 Subject: [PATCH] Follow googlecartographer/cartographer#436. (#459) --- cartographer_ros/cartographer_ros/node.cc | 36 +++++++++++++++-------- cartographer_ros/cartographer_ros/node.h | 2 +- 2 files changed, 25 insertions(+), 13 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 0b00239..c14ba87 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -106,16 +106,21 @@ void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); } -::cartographer::mapping::PoseExtrapolator* Node::GetExtrapolator( - int trajectory_id) { +void Node::AddExtrapolator(const int trajectory_id, + const TrajectoryOptions& options) { constexpr double kExtrapolationEstimationTimeSec = 0.001; // 1 ms - if (extrapolators_.count(trajectory_id) == 0) { - extrapolators_.emplace( - std::piecewise_construct, std::forward_as_tuple(trajectory_id), - std::forward_as_tuple(::cartographer::common::FromSeconds( - kExtrapolationEstimationTimeSec))); - } - return &extrapolators_.at(trajectory_id); + CHECK(extrapolators_.count(trajectory_id) == 0); + const double gravity_time_constant = + node_options_.map_builder_options.use_trajectory_builder_3d() + ? options.trajectory_builder_options.trajectory_builder_3d_options() + .imu_gravity_time_constant() + : options.trajectory_builder_options.trajectory_builder_2d_options() + .imu_gravity_time_constant(); + extrapolators_.emplace( + std::piecewise_construct, std::forward_as_tuple(trajectory_id), + std::forward_as_tuple( + ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), + gravity_time_constant)); } void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { @@ -123,7 +128,7 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { const auto& trajectory_state = entry.second; - auto& extrapolator = *GetExtrapolator(entry.first); + 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()) { @@ -236,6 +241,7 @@ int Node::AddTrajectory(const TrajectoryOptions& options, ComputeExpectedTopics(options, topics); const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); + AddExtrapolator(trajectory_id, options); LaunchSubscribers(options, topics, trajectory_id); subscribed_topics_.insert(expected_sensor_ids.begin(), expected_sensor_ids.end()); @@ -253,6 +259,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, boost::function( [this, trajectory_id, topic](const sensor_msgs::LaserScan::ConstPtr& msg) { + carto::common::MutexLocker lock(&mutex_); map_builder_bridge_.sensor_bridge(trajectory_id) ->HandleLaserScanMessage(topic, msg); }))); @@ -266,6 +273,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, boost::function( [this, trajectory_id, topic](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { + carto::common::MutexLocker lock(&mutex_); map_builder_bridge_.sensor_bridge(trajectory_id) ->HandleMultiEchoLaserScanMessage(topic, msg); }))); @@ -277,6 +285,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, boost::function( [this, trajectory_id, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { + carto::common::MutexLocker lock(&mutex_); map_builder_bridge_.sensor_bridge(trajectory_id) ->HandlePointCloud2Message(topic, msg); }))); @@ -295,13 +304,14 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, boost::function( [this, trajectory_id, topic](const sensor_msgs::Imu::ConstPtr& msg) { + carto::common::MutexLocker lock(&mutex_); auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id); - sensor_bridge_ptr->HandleImuMessage(topic, msg); auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg); if (imu_data_ptr != nullptr) { - GetExtrapolator(trajectory_id)->AddImuData(*imu_data_ptr); + extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr); } + sensor_bridge_ptr->HandleImuMessage(topic, msg); }))); } @@ -313,6 +323,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, boost::function( [this, trajectory_id, topic](const nav_msgs::Odometry::ConstPtr& msg) { + carto::common::MutexLocker lock(&mutex_); map_builder_bridge_.sensor_bridge(trajectory_id) ->HandleOdometryMessage(topic, msg); }))); @@ -431,6 +442,7 @@ void Node::FinishAllTrajectories() { } void Node::LoadMap(const std::string& map_filename) { + carto::common::MutexLocker lock(&mutex_); map_builder_bridge_.LoadMap(map_filename); } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index c9ad1b5..8f440fa 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -85,7 +85,7 @@ class Node { const cartographer_ros_msgs::SensorTopics& topics, int trajectory_id); void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); - ::cartographer::mapping::PoseExtrapolator* GetExtrapolator(int trajectory_id); + void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options); void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event); void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event); void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);