From b6b1c572f6dbc7958b32e59b9d3bf012f49c6981 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 28 Jul 2017 17:52:39 +0200 Subject: [PATCH] Also extrapolate based on IMU if available. (#458) --- cartographer_ros/cartographer_ros/node.cc | 32 ++++++++++------- cartographer_ros/cartographer_ros/node.h | 1 + .../cartographer_ros/sensor_bridge.cc | 35 +++++++++++++------ .../cartographer_ros/sensor_bridge.h | 5 +++ 4 files changed, 50 insertions(+), 23 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 74aa1b6..0b00239 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -106,21 +106,24 @@ 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) { + 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); +} + void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { carto::common::MutexLocker lock(&mutex_); for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { const auto& trajectory_state = entry.second; - // TODO(whess): Use improved models, e.g. using odometry and IMU when - // available. - constexpr double extrapolation_estimation_time_sec = 0.001; // 1 ms - if (extrapolators_.count(entry.first) == 0) { - extrapolators_.emplace( - std::piecewise_construct, std::forward_as_tuple(entry.first), - std::forward_as_tuple(::cartographer::common::FromSeconds( - extrapolation_estimation_time_sec))); - } - auto& extrapolator = extrapolators_.at(entry.first); + auto& extrapolator = *GetExtrapolator(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()) { @@ -292,8 +295,13 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, boost::function( [this, trajectory_id, topic](const sensor_msgs::Imu::ConstPtr& msg) { - map_builder_bridge_.sensor_bridge(trajectory_id) - ->HandleImuMessage(topic, msg); + 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); + } }))); } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 38395dc..c9ad1b5 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -85,6 +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 PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event); void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event); void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 965db39..e183358 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -16,6 +16,7 @@ #include "cartographer_ros/sensor_bridge.h" +#include "cartographer/common/make_unique.h" #include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/time_conversion.h" @@ -58,8 +59,8 @@ void SensorBridge::HandleOdometryMessage( } } -void SensorBridge::HandleImuMessage(const string& sensor_id, - const sensor_msgs::Imu::ConstPtr& msg) { +std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData( + const sensor_msgs::Imu::ConstPtr& msg) { CHECK_NE(msg->linear_acceleration_covariance[0], -1) << "Your IMU data claims to not contain linear acceleration measurements " "by setting linear_acceleration_covariance[0] to -1. Cartographer " @@ -74,15 +75,27 @@ void SensorBridge::HandleImuMessage(const string& sensor_id, const carto::common::Time time = FromRos(msg->header.stamp); const auto sensor_to_tracking = tf_bridge_.LookupToTracking( time, CheckNoLeadingSlash(msg->header.frame_id)); - if (sensor_to_tracking != nullptr) { - CHECK(sensor_to_tracking->translation().norm() < 1e-5) - << "The IMU frame must be colocated with the tracking frame. " - "Transforming linear acceleration into the tracking frame will " - "otherwise be imprecise."; - trajectory_builder_->AddImuData( - sensor_id, time, - sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration), - sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)); + if (sensor_to_tracking == nullptr) { + return nullptr; + } + CHECK(sensor_to_tracking->translation().norm() < 1e-5) + << "The IMU frame must be colocated with the tracking frame. " + "Transforming linear acceleration into the tracking frame will " + "otherwise be imprecise."; + return ::cartographer::common::make_unique<::cartographer::sensor::ImuData>( + ::cartographer::sensor::ImuData{ + time, + sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration), + sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)}); +} + +void SensorBridge::HandleImuMessage(const string& sensor_id, + const sensor_msgs::Imu::ConstPtr& msg) { + std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg); + if (imu_data != nullptr) { + trajectory_builder_->AddImuData(sensor_id, imu_data->time, + imu_data->linear_acceleration, + imu_data->angular_velocity); } } diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index 6e728c3..ec10af4 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/cartographer_ros/cartographer_ros/sensor_bridge.h @@ -17,7 +17,10 @@ #ifndef CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_ #define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_ +#include + #include "cartographer/mapping/trajectory_builder.h" +#include "cartographer/sensor/imu_data.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/tf_bridge.h" @@ -45,6 +48,8 @@ class SensorBridge { void HandleOdometryMessage(const string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg); + std::unique_ptr<::cartographer::sensor::ImuData> ToImuData( + const sensor_msgs::Imu::ConstPtr& msg); void HandleImuMessage(const string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg); void HandleLaserScanMessage(const string& sensor_id,