diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index a5d0fe2..83d7d2f 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -94,6 +94,8 @@ add_executable(cartographer_node src/sensor_bridge.cc src/sensor_bridge.h src/sensor_data.h + src/tf_bridge.cc + src/tf_bridge.h src/time_conversion.cc src/time_conversion.h ) diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 241e3d9..89bcf00 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -16,20 +16,21 @@ include "map_builder.lua" options = { map_builder = MAP_BUILDER, + sensor_bridge = { + horizontal_laser_min_range = 0., + horizontal_laser_max_range = 30., + horizontal_laser_missing_echo_ray_length = 5., + constant_odometry_translational_variance = 0., + constant_odometry_rotational_variance = 0., + }, map_frame = "map", tracking_frame = "base_link", published_frame = "base_link", odom_frame = "odom", provide_odom_frame = true, use_odometry_data = false, - use_constant_odometry_variance = false, - constant_odometry_translational_variance = 0., - constant_odometry_rotational_variance = 0., use_horizontal_laser = false, use_horizontal_multi_echo_laser = true, - horizontal_laser_min_range = 0., - horizontal_laser_max_range = 30., - horizontal_laser_missing_echo_ray_length = 5., num_lasers_3d = 0, lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index e25a0af..5d5282b 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -16,20 +16,21 @@ include "map_builder.lua" options = { map_builder = MAP_BUILDER, + sensor_bridge = { + horizontal_laser_min_range = 0., + horizontal_laser_max_range = 30., + horizontal_laser_missing_echo_ray_length = 5., + constant_odometry_translational_variance = 0., + constant_odometry_rotational_variance = 0., + }, map_frame = "map", tracking_frame = "base_link", published_frame = "base_link", odom_frame = "odom", provide_odom_frame = true, use_odometry_data = false, - use_constant_odometry_variance = false, - constant_odometry_translational_variance = 0., - constant_odometry_rotational_variance = 0., use_horizontal_laser = false, use_horizontal_multi_echo_laser = false, - horizontal_laser_min_range = 0., - horizontal_laser_max_range = 30., - horizontal_laser_missing_echo_ray_length = 5., num_lasers_3d = 2, lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index 341de00..a604af2 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/cartographer_ros/configuration_files/pr2.lua @@ -16,20 +16,21 @@ include "map_builder.lua" options = { map_builder = MAP_BUILDER, + sensor_bridge = { + horizontal_laser_min_range = 0., + horizontal_laser_max_range = 30., + horizontal_laser_missing_echo_ray_length = 5., + constant_odometry_translational_variance = 0., + constant_odometry_rotational_variance = 0., + }, map_frame = "map", tracking_frame = "base_footprint", published_frame = "base_footprint", odom_frame = "odom", provide_odom_frame = true, use_odometry_data = false, - use_constant_odometry_variance = false, - constant_odometry_translational_variance = 0., - constant_odometry_rotational_variance = 0., use_horizontal_laser = true, use_horizontal_multi_echo_laser = false, - horizontal_laser_min_range = 0., - horizontal_laser_max_range = 30., - horizontal_laser_missing_echo_ray_length = 5., num_lasers_3d = 0, lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index 0eeb65b..f198fc2 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/cartographer_ros/configuration_files/revo_lds.lua @@ -16,20 +16,21 @@ include "map_builder.lua" options = { map_builder = MAP_BUILDER, + sensor_bridge = { + horizontal_laser_min_range = 0.3, + horizontal_laser_max_range = 8., + horizontal_laser_missing_echo_ray_length = 1., + constant_odometry_translational_variance = 0., + constant_odometry_rotational_variance = 0., + }, map_frame = "map", tracking_frame = "horizontal_laser_link", published_frame = "horizontal_laser_link", odom_frame = "odom", provide_odom_frame = true, use_odometry_data = false, - use_constant_odometry_variance = false, - constant_odometry_translational_variance = 0., - constant_odometry_rotational_variance = 0., use_horizontal_laser = true, use_horizontal_multi_echo_laser = false, - horizontal_laser_min_range = 0.3, - horizontal_laser_max_range = 8., - horizontal_laser_missing_echo_ray_length = 1., num_lasers_3d = 0, lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 66fc05f..3d98a2e 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -69,6 +69,7 @@ #include "ros_log_sink.h" #include "sensor_bridge.h" #include "sensor_data.h" +#include "tf_bridge.h" #include "time_conversion.h" DEFINE_string(configuration_directory, "", @@ -122,20 +123,6 @@ class Node { private: void HandleSensorData(int64 timestamp, std::unique_ptr sensor_data); - - void AddOdometry(int64 timestamp, const string& frame_id, - const SensorData::Odometry& odometry); - void AddImu(int64 timestamp, const string& frame_id, const SensorData::Imu& imu); - void AddHorizontalLaserFan(int64 timestamp, const string& frame_id, - const proto::LaserScan& laser_scan); - void AddLaserFan3D(int64 timestamp, const string& frame_id, - const proto::LaserFan3D& laser_fan_3d); - - // Returns a transform for 'frame_id' to 'options_.tracking_frame' if it - // exists at 'time' or throws tf2::TransformException if it does not exist. - Rigid3d LookupToTrackingTransformOrThrow(carto::common::Time time, - const string& frame_id); - bool HandleSubmapQuery( ::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Response& response); @@ -150,6 +137,11 @@ class Node { const NodeOptions options_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_; + tf2_ros::TransformBroadcaster tf_broadcaster_; + TfBridge tf_bridge_; + carto::common::Mutex mutex_; std::deque constant_data_ GUARDED_BY(mutex_); @@ -170,10 +162,6 @@ class Node { carto::common::Time::min(); ::ros::ServiceServer finish_trajectory_server_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_; - tf2_ros::TransformBroadcaster tf_broadcaster_; - ::ros::Publisher occupancy_grid_publisher_; std::thread occupancy_grid_thread_; bool terminating_ = false GUARDED_BY(mutex_); @@ -189,10 +177,13 @@ class Node { Node::Node(const NodeOptions& options) : options_(options), - map_builder_(options.map_builder_options, &constant_data_), - sensor_bridge_(kTrajectoryBuilderId, &sensor_collator_), tf_buffer_(::ros::Duration(1000)), - tf_(tf_buffer_) {} + tf_(tf_buffer_), + tf_bridge_(options_.tracking_frame, options_.lookup_transform_timeout_sec, + &tf_buffer_), + map_builder_(options.map_builder_options, &constant_data_), + sensor_bridge_(options.sensor_bridge_options, &tf_bridge_, + kTrajectoryBuilderId, &sensor_collator_) {} Node::~Node() { { @@ -204,107 +195,6 @@ Node::~Node() { } } -Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time, - const string& frame_id) { - ::ros::Duration timeout(options_.lookup_transform_timeout_sec); - const ::ros::Time latest_tf_time = - tf_buffer_ - .lookupTransform(options_.tracking_frame, frame_id, ::ros::Time(0.), - timeout) - .header.stamp; - const ::ros::Time requested_time = ToRos(time); - if (latest_tf_time >= requested_time) { - // We already have newer data, so we do not wait. Otherwise, we would wait - // for the full 'timeout' even if we ask for data that is too old. - timeout = ::ros::Duration(0.); - } - return ToRigid3d(tf_buffer_.lookupTransform(options_.tracking_frame, frame_id, - requested_time, timeout)); -} - -void Node::AddOdometry(const int64 timestamp, const string& frame_id, - const SensorData::Odometry& odometry) { - const carto::common::Time time = carto::common::FromUniversal(timestamp); - PoseCovariance applied_covariance = odometry.covariance; - if (options_.use_constant_odometry_variance) { - const Eigen::Matrix3d translational = - Eigen::Matrix3d::Identity() * - options_.constant_odometry_translational_variance; - const Eigen::Matrix3d rotational = - Eigen::Matrix3d::Identity() * - options_.constant_odometry_rotational_variance; - // clang-format off - applied_covariance << - translational, Eigen::Matrix3d::Zero(), - Eigen::Matrix3d::Zero(), rotational; - // clang-format on - } - try { - const Rigid3d sensor_to_tracking = - LookupToTrackingTransformOrThrow(time, frame_id); - map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) - ->AddOdometerPose(time, odometry.pose * sensor_to_tracking.inverse(), - applied_covariance); - } catch (const tf2::TransformException& ex) { - LOG(WARNING) << ex.what(); - } -} - -void Node::AddImu(const int64 timestamp, const string& frame_id, - const SensorData::Imu& imu) { - const carto::common::Time time = carto::common::FromUniversal(timestamp); - try { - const Rigid3d sensor_to_tracking = - LookupToTrackingTransformOrThrow(time, frame_id); - 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."; - map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) - ->AddImuData(time, - sensor_to_tracking.rotation() * imu.linear_acceleration, - sensor_to_tracking.rotation() * imu.angular_velocity); - } catch (const tf2::TransformException& ex) { - LOG(WARNING) << ex.what(); - } -} - -void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id, - const proto::LaserScan& laser_scan) { - const carto::common::Time time = carto::common::FromUniversal(timestamp); - try { - const Rigid3d sensor_to_tracking = - LookupToTrackingTransformOrThrow(time, frame_id); - const carto::sensor::LaserFan laser_fan = carto::sensor::ToLaserFan( - laser_scan, options_.horizontal_laser_min_range, - options_.horizontal_laser_max_range, - options_.horizontal_laser_missing_echo_ray_length); - - const auto laser_fan_3d = carto::sensor::TransformLaserFan3D( - carto::sensor::ToLaserFan3D(laser_fan), - sensor_to_tracking.cast()); - map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) - ->AddHorizontalLaserFan(time, laser_fan_3d); - } catch (const tf2::TransformException& ex) { - LOG(WARNING) << ex.what(); - } -} - -void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id, - const proto::LaserFan3D& laser_fan_3d) { - const carto::common::Time time = carto::common::FromUniversal(timestamp); - try { - const Rigid3d sensor_to_tracking = - LookupToTrackingTransformOrThrow(time, frame_id); - map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) - ->AddLaserFan3D(time, carto::sensor::TransformLaserFan3D( - carto::sensor::FromProto(laser_fan_3d), - sensor_to_tracking.cast())); - } catch (const tf2::TransformException& ex) { - LOG(WARNING) << ex.what(); - } -} - void Node::Initialize() { // Set of all topics we subscribe to. We use the non-remapped default names // which are unique. @@ -316,7 +206,7 @@ void Node::Initialize() { kLaserScanTopic, kInfiniteSubscriberQueueSize, boost::function( [this](const sensor_msgs::LaserScan::ConstPtr& msg) { - sensor_bridge_.AddLaserScanMessage(kLaserScanTopic, msg); + sensor_bridge_.HandleLaserScanMessage(kLaserScanTopic, msg); })); expected_sensor_identifiers.insert(kLaserScanTopic); } @@ -325,7 +215,7 @@ void Node::Initialize() { kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize, boost::function( [this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { - sensor_bridge_.AddMultiEchoLaserScanMessage( + sensor_bridge_.HandleMultiEchoLaserScanMessage( kMultiEchoLaserScanTopic, msg); })); expected_sensor_identifiers.insert(kMultiEchoLaserScanTopic); @@ -342,7 +232,7 @@ void Node::Initialize() { topic, kInfiniteSubscriberQueueSize, boost::function( [this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { - sensor_bridge_.AddPointCloud2Message(topic, msg); + sensor_bridge_.HandlePointCloud2Message(topic, msg); }))); expected_sensor_identifiers.insert(topic); } @@ -358,7 +248,7 @@ void Node::Initialize() { kImuTopic, kInfiniteSubscriberQueueSize, boost::function( [this](const sensor_msgs::Imu::ConstPtr& msg) { - sensor_bridge_.AddImuMessage(kImuTopic, msg); + sensor_bridge_.HandleImuMessage(kImuTopic, msg); })); expected_sensor_identifiers.insert(kImuTopic); } @@ -368,7 +258,7 @@ void Node::Initialize() { kOdometryTopic, kInfiniteSubscriberQueueSize, boost::function( [this](const nav_msgs::Odometry::ConstPtr& msg) { - sensor_bridge_.AddOdometryMessage(kOdometryTopic, msg); + sensor_bridge_.HandleOdometryMessage(kOdometryTopic, msg); })); expected_sensor_identifiers.insert(kOdometryTopic); } @@ -533,9 +423,9 @@ void Node::PublishPoseAndScanMatchedPointCloud( geometry_msgs::TransformStamped stamped_transform; stamped_transform.header.stamp = ToRos(last_pose_estimate.time); - try { - const Rigid3d published_to_tracking = LookupToTrackingTransformOrThrow( - last_pose_estimate.time, options_.published_frame); + const auto published_to_tracking = tf_bridge_.LookupToTracking( + last_pose_estimate.time, options_.published_frame); + if (published_to_tracking != nullptr) { if (options_.provide_odom_frame) { stamped_transform.header.frame_id = options_.map_frame; stamped_transform.child_frame_id = options_.odom_frame; @@ -545,17 +435,15 @@ void Node::PublishPoseAndScanMatchedPointCloud( 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); + ToGeometryMsgTransform(tracking_to_local * (*published_to_tracking)); tf_broadcaster_.sendTransform(stamped_transform); } 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); + ToGeometryMsgTransform(tracking_to_map * (*published_to_tracking)); tf_broadcaster_.sendTransform(stamped_transform); } - } catch (const tf2::TransformException& ex) { - LOG(WARNING) << ex.what(); } // We only publish a point cloud if it has changed. It is not needed at high @@ -615,23 +503,28 @@ void Node::HandleSensorData(const int64 timestamp, last_sensor_data_rates_logging_time_ = std::chrono::steady_clock::now(); } + const auto time = carto::common::FromUniversal(timestamp); + auto* const trajectory_builder = + map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId); switch (sensor_data->type) { case SensorType::kImu: - AddImu(timestamp, sensor_data->frame_id, sensor_data->imu); - return; - - case SensorType::kLaserScan: - AddHorizontalLaserFan(timestamp, sensor_data->frame_id, - sensor_data->laser_scan); + trajectory_builder->AddImuData(time, sensor_data->imu.linear_acceleration, + sensor_data->imu.angular_velocity); return; case SensorType::kLaserFan3D: - AddLaserFan3D(timestamp, sensor_data->frame_id, - sensor_data->laser_fan_3d); + if (options_.map_builder_options.use_trajectory_builder_2d()) { + trajectory_builder->AddHorizontalLaserFan(time, + sensor_data->laser_fan_3d); + } else { + CHECK(options_.map_builder_options.use_trajectory_builder_3d()); + trajectory_builder->AddLaserFan3D(time, sensor_data->laser_fan_3d); + } return; case SensorType::kOdometry: - AddOdometry(timestamp, sensor_data->frame_id, sensor_data->odometry); + trajectory_builder->AddOdometerPose(time, sensor_data->odometry.pose, + sensor_data->odometry.covariance); return; } LOG(FATAL); diff --git a/cartographer_ros/src/msg_conversion.h b/cartographer_ros/src/msg_conversion.h index a14b18e..2d3d329 100644 --- a/cartographer_ros/src/msg_conversion.h +++ b/cartographer_ros/src/msg_conversion.h @@ -73,6 +73,7 @@ geometry_msgs::Pose ToGeometryMsgPose( ::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::Pose& pose); Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3); + Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion); ::cartographer::kalman_filter::PoseCovariance ToPoseCovariance( diff --git a/cartographer_ros/src/node_options.cc b/cartographer_ros/src/node_options.cc index fc2a3ac..73dcec0 100644 --- a/cartographer_ros/src/node_options.cc +++ b/cartographer_ros/src/node_options.cc @@ -27,6 +27,8 @@ NodeOptions CreateNodeOptions( options.map_builder_options = ::cartographer::mapping::CreateMapBuilderOptions( lua_parameter_dictionary->GetDictionary("map_builder").get()); + options.sensor_bridge_options = CreateSensorBridgeOptions( + lua_parameter_dictionary->GetDictionary("sensor_bridge").get()); options.map_frame = lua_parameter_dictionary->GetString("map_frame"); options.tracking_frame = lua_parameter_dictionary->GetString("tracking_frame"); @@ -37,25 +39,10 @@ NodeOptions CreateNodeOptions( lua_parameter_dictionary->GetBool("provide_odom_frame"); options.use_odometry_data = lua_parameter_dictionary->GetBool("use_odometry_data"); - options.use_constant_odometry_variance = - lua_parameter_dictionary->GetBool("use_constant_odometry_variance"); - options.constant_odometry_translational_variance = - lua_parameter_dictionary->GetDouble( - "constant_odometry_translational_variance"); - options.constant_odometry_rotational_variance = - lua_parameter_dictionary->GetDouble( - "constant_odometry_rotational_variance"); options.use_horizontal_laser = lua_parameter_dictionary->GetBool("use_horizontal_laser"); options.use_horizontal_multi_echo_laser = lua_parameter_dictionary->GetBool("use_horizontal_multi_echo_laser"); - options.horizontal_laser_min_range = - lua_parameter_dictionary->GetDouble("horizontal_laser_min_range"); - options.horizontal_laser_max_range = - lua_parameter_dictionary->GetDouble("horizontal_laser_max_range"); - options.horizontal_laser_missing_echo_ray_length = - lua_parameter_dictionary->GetDouble( - "horizontal_laser_missing_echo_ray_length"); options.num_lasers_3d = lua_parameter_dictionary->GetNonNegativeInt("num_lasers_3d"); options.lookup_transform_timeout_sec = diff --git a/cartographer_ros/src/node_options.h b/cartographer_ros/src/node_options.h index bc8bc08..78e87b7 100644 --- a/cartographer_ros/src/node_options.h +++ b/cartographer_ros/src/node_options.h @@ -23,25 +23,22 @@ #include "cartographer/common/port.h" #include "cartographer/mapping/map_builder.h" +#include "sensor_bridge.h" + namespace cartographer_ros { // Top-level options of Cartographer's ROS integration. struct NodeOptions { ::cartographer::mapping::proto::MapBuilderOptions map_builder_options; + SensorBridgeOptions sensor_bridge_options; string map_frame; string tracking_frame; string published_frame; string odom_frame; bool provide_odom_frame; bool use_odometry_data; - bool use_constant_odometry_variance; - double constant_odometry_translational_variance; - double constant_odometry_rotational_variance; bool use_horizontal_laser; bool use_horizontal_multi_echo_laser; - double horizontal_laser_min_range; - double horizontal_laser_max_range; - double horizontal_laser_missing_echo_ray_length; int num_lasers_3d; double lookup_transform_timeout_sec; double submap_publish_period_sec; diff --git a/cartographer_ros/src/sensor_bridge.cc b/cartographer_ros/src/sensor_bridge.cc index e9c7b87..4ded8c6 100644 --- a/cartographer_ros/src/sensor_bridge.cc +++ b/cartographer_ros/src/sensor_bridge.cc @@ -16,75 +16,144 @@ #include "sensor_bridge.h" +#include "cartographer/kalman_filter/pose_tracker.h" + #include "msg_conversion.h" #include "time_conversion.h" namespace cartographer_ros { +namespace carto = ::cartographer; + +using carto::transform::Rigid3d; + +SensorBridgeOptions CreateSensorBridgeOptions( + carto::common::LuaParameterDictionary* const lua_parameter_dictionary) { + SensorBridgeOptions options; + options.horizontal_laser_min_range = + lua_parameter_dictionary->GetDouble("horizontal_laser_min_range"); + options.horizontal_laser_max_range = + lua_parameter_dictionary->GetDouble("horizontal_laser_max_range"); + options.horizontal_laser_missing_echo_ray_length = + lua_parameter_dictionary->GetDouble( + "horizontal_laser_missing_echo_ray_length"); + options.constant_odometry_translational_variance = + lua_parameter_dictionary->GetDouble( + "constant_odometry_translational_variance"); + options.constant_odometry_rotational_variance = + lua_parameter_dictionary->GetDouble( + "constant_odometry_rotational_variance"); + return options; +} + SensorBridge::SensorBridge( + const SensorBridgeOptions& options, const TfBridge* const tf_bridge, const int trajectory_id, - ::cartographer::mapping::SensorCollator* sensor_collator) - : trajectory_id_(trajectory_id), sensor_collator_(sensor_collator) {} + carto::mapping::SensorCollator* const sensor_collator) + : options_(options), + tf_bridge_(tf_bridge), + trajectory_id_(trajectory_id), + sensor_collator_(sensor_collator) {} -void SensorBridge::AddOdometryMessage(const string& topic, - const nav_msgs::Odometry::ConstPtr& msg) { - auto sensor_data = ::cartographer::common::make_unique( - msg->child_frame_id, - SensorData::Odometry{ToRigid3d(msg->pose.pose), - ToPoseCovariance(msg->pose.covariance)}); - sensor_collator_->AddSensorData( - trajectory_id_, - ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, - std::move(sensor_data)); +void SensorBridge::HandleOdometryMessage( + const string& topic, const nav_msgs::Odometry::ConstPtr& msg) { + const carto::common::Time time = FromRos(msg->header.stamp); + const Eigen::Matrix3d translational = + Eigen::Matrix3d::Identity() * + options_.constant_odometry_translational_variance; + const Eigen::Matrix3d rotational = + Eigen::Matrix3d::Identity() * + options_.constant_odometry_rotational_variance; + // clang-format off + carto::kalman_filter::PoseCovariance covariance; + covariance << + translational, Eigen::Matrix3d::Zero(), + Eigen::Matrix3d::Zero(), rotational; + // clang-format on + const auto sensor_to_tracking = + tf_bridge_->LookupToTracking(time, msg->child_frame_id); + if (sensor_to_tracking != nullptr) { + sensor_collator_->AddSensorData( + trajectory_id_, carto::common::ToUniversal(time), topic, + carto::common::make_unique( + msg->child_frame_id, + SensorData::Odometry{ + ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(), + covariance})); + } } -void SensorBridge::AddImuMessage(const string& topic, - const sensor_msgs::Imu::ConstPtr& msg) { - CHECK_NE(msg->angular_velocity_covariance[0], -1); +void SensorBridge::HandleImuMessage(const string& topic, + const sensor_msgs::Imu::ConstPtr& msg) { CHECK_NE(msg->linear_acceleration_covariance[0], -1); - auto sensor_data = ::cartographer::common::make_unique( - msg->header.frame_id, - SensorData::Imu{{msg->angular_velocity.x, msg->angular_velocity.y, - msg->angular_velocity.z}, - {msg->linear_acceleration.x, msg->linear_acceleration.y, - msg->linear_acceleration.z}}); - sensor_collator_->AddSensorData( - trajectory_id_, - ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, - std::move(sensor_data)); + CHECK_NE(msg->angular_velocity_covariance[0], -1); + const carto::common::Time time = FromRos(msg->header.stamp); + const auto sensor_to_tracking = + tf_bridge_->LookupToTracking(time, 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."; + sensor_collator_->AddSensorData( + trajectory_id_, carto::common::ToUniversal(time), topic, + carto::common::make_unique( + msg->header.frame_id, + SensorData::Imu{ + sensor_to_tracking->rotation() * + ToEigen(msg->linear_acceleration), + sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity), + })); + } } -void SensorBridge::AddLaserScanMessage( +void SensorBridge::HandleLaserScanMessage( const string& topic, const sensor_msgs::LaserScan::ConstPtr& msg) { - auto sensor_data = ::cartographer::common::make_unique( - msg->header.frame_id, ToCartographer(*msg)); - sensor_collator_->AddSensorData( - trajectory_id_, - ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, - std::move(sensor_data)); + HandleLaserScanProto(topic, FromRos(msg->header.stamp), msg->header.frame_id, + ToCartographer(*msg)); } -void SensorBridge::AddMultiEchoLaserScanMessage( +void SensorBridge::HandleMultiEchoLaserScanMessage( const string& topic, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { - auto sensor_data = ::cartographer::common::make_unique( - msg->header.frame_id, ToCartographer(*msg)); - sensor_collator_->AddSensorData( - trajectory_id_, - ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, - std::move(sensor_data)); + HandleLaserScanProto(topic, FromRos(msg->header.stamp), msg->header.frame_id, + ToCartographer(*msg)); } -void SensorBridge::AddPointCloud2Message( +void SensorBridge::HandlePointCloud2Message( const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg) { - pcl::PointCloud pcl_points; - pcl::fromROSMsg(*msg, pcl_points); + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(*msg, pcl_point_cloud); + const carto::common::Time time = FromRos(msg->header.stamp); + const auto sensor_to_tracking = + tf_bridge_->LookupToTracking(time, msg->header.frame_id); + if (sensor_to_tracking != nullptr) { + sensor_collator_->AddSensorData( + trajectory_id_, carto::common::ToUniversal(time), topic, + carto::common::make_unique( + msg->header.frame_id, + carto::sensor::TransformLaserFan3D( + carto::sensor::FromProto(ToCartographer(pcl_point_cloud)), + sensor_to_tracking->cast()))); + } +} - auto sensor_data = ::cartographer::common::make_unique( - msg->header.frame_id, ToCartographer(pcl_points)); - sensor_collator_->AddSensorData( - trajectory_id_, - ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic, - std::move(sensor_data)); +void SensorBridge::HandleLaserScanProto( + const string& topic, const ::cartographer::common::Time time, + const string& frame_id, + const ::cartographer::sensor::proto::LaserScan& laser_scan) { + const auto laser_fan_2d = carto::sensor::ToLaserFan( + laser_scan, options_.horizontal_laser_min_range, + options_.horizontal_laser_max_range, + options_.horizontal_laser_missing_echo_ray_length); + const auto sensor_to_tracking = tf_bridge_->LookupToTracking(time, frame_id); + if (sensor_to_tracking != nullptr) { + sensor_collator_->AddSensorData( + trajectory_id_, carto::common::ToUniversal(time), topic, + carto::common::make_unique( + frame_id, carto::sensor::TransformLaserFan3D( + carto::sensor::ToLaserFan3D(laser_fan_2d), + sensor_to_tracking->cast()))); + } } } // namespace cartographer_ros diff --git a/cartographer_ros/src/sensor_bridge.h b/cartographer_ros/src/sensor_bridge.h index 3f13f6c..4cfae39 100644 --- a/cartographer_ros/src/sensor_bridge.h +++ b/cartographer_ros/src/sensor_bridge.h @@ -18,6 +18,8 @@ #define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_ #include "cartographer/mapping/sensor_collator.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" #include "geometry_msgs/Transform.h" #include "geometry_msgs/TransformStamped.h" #include "nav_msgs/OccupancyGrid.h" @@ -28,34 +30,54 @@ #include "sensor_msgs/MultiEchoLaserScan.h" #include "sensor_msgs/PointCloud2.h" +#include "tf_bridge.h" + namespace cartographer_ros { -// A wrapper around SensorCollator that converts ROS messages into our internal -// representation and passes them on to the 'sensor_collator'. +struct SensorBridgeOptions { + double horizontal_laser_min_range; + double horizontal_laser_max_range; + double horizontal_laser_missing_echo_ray_length; + double constant_odometry_translational_variance; + double constant_odometry_rotational_variance; +}; + +SensorBridgeOptions CreateSensorBridgeOptions( + ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); + +// Converts ROS messages into SensorData in tracking frame for the MapBuilder. class SensorBridge { public: explicit SensorBridge( + const SensorBridgeOptions& options, const TfBridge* tf_bridge, int trajectory_id, ::cartographer::mapping::SensorCollator* sensor_collator); SensorBridge(const SensorBridge&) = delete; SensorBridge& operator=(const SensorBridge&) = delete; - void AddOdometryMessage(const string& topic, - const nav_msgs::Odometry::ConstPtr& msg); - void AddImuMessage(const string& topic, - const sensor_msgs::Imu::ConstPtr& msg); - void AddLaserScanMessage(const string& topic, - const sensor_msgs::LaserScan::ConstPtr& msg); - void AddMultiEchoLaserScanMessage( + void HandleOdometryMessage(const string& topic, + const nav_msgs::Odometry::ConstPtr& msg); + void HandleImuMessage(const string& topic, + const sensor_msgs::Imu::ConstPtr& msg); + void HandleLaserScanMessage(const string& topic, + const sensor_msgs::LaserScan::ConstPtr& msg); + void HandleMultiEchoLaserScanMessage( const string& topic, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); - void AddPointCloud2Message(const string& topic, - const sensor_msgs::PointCloud2::ConstPtr& msg); + void HandlePointCloud2Message(const string& topic, + const sensor_msgs::PointCloud2::ConstPtr& msg); private: + void HandleLaserScanProto( + const string& topic, const ::cartographer::common::Time time, + const string& frame_id, + const ::cartographer::sensor::proto::LaserScan& laser_scan); + + const SensorBridgeOptions options_; + const TfBridge* const tf_bridge_; const int trajectory_id_; - ::cartographer::mapping::SensorCollator* sensor_collator_; + ::cartographer::mapping::SensorCollator* const sensor_collator_; }; } // namespace cartographer_ros diff --git a/cartographer_ros/src/sensor_data.h b/cartographer_ros/src/sensor_data.h index ec89a2a..e4d6ab3 100644 --- a/cartographer_ros/src/sensor_data.h +++ b/cartographer_ros/src/sensor_data.h @@ -20,16 +20,15 @@ #include #include "cartographer/kalman_filter/pose_tracker.h" -#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/sensor/laser.h" #include "cartographer/transform/rigid_transform.h" -#include "glog/logging.h" namespace cartographer_ros { // This type is a logical union, i.e. only one type of sensor data is actually // filled in. It is only used for time ordering sensor data before passing it // on. -enum class SensorType { kImu, kLaserScan, kLaserFan3D, kOdometry }; +enum class SensorType { kImu, kLaserFan3D, kOdometry }; struct SensorData { struct Odometry { ::cartographer::transform::Rigid3d pose; @@ -37,8 +36,8 @@ struct SensorData { }; struct Imu { - Eigen::Vector3d angular_velocity; Eigen::Vector3d linear_acceleration; + Eigen::Vector3d angular_velocity; }; SensorData(const string& frame_id, const Imu& imu) @@ -46,33 +45,30 @@ struct SensorData { frame_id(CheckNoLeadingSlash(frame_id)), imu(imu) {} - SensorData(const string& frame_id, const ::cartographer::sensor::proto::LaserScan& laser_scan) - : type(SensorType::kLaserScan), - frame_id(CheckNoLeadingSlash(frame_id)), - laser_scan(laser_scan) {} - - SensorData(const string& frame_id, const ::cartographer::sensor::proto::LaserFan3D& laser_fan_3d) + SensorData(const string& frame_id, + const ::cartographer::sensor::LaserFan3D& laser_fan_3d) : type(SensorType::kLaserFan3D), frame_id(CheckNoLeadingSlash(frame_id)), laser_fan_3d(laser_fan_3d) {} SensorData(const string& frame_id, const Odometry& odometry) - : type(SensorType::kOdometry), frame_id(frame_id), odometry(odometry) {} + : type(SensorType::kOdometry), + frame_id(CheckNoLeadingSlash(frame_id)), + odometry(odometry) {} SensorType type; string frame_id; Imu imu; - ::cartographer::sensor::proto::LaserScan laser_scan; - ::cartographer::sensor::proto::LaserFan3D laser_fan_3d; + ::cartographer::sensor::LaserFan3D laser_fan_3d; Odometry odometry; - private: - static const string& CheckNoLeadingSlash(const string& frame_id) { - if (frame_id.size() > 0) { - CHECK_NE(frame_id[0], '/'); - } - return frame_id; + private: + static const string& CheckNoLeadingSlash(const string& frame_id) { + if (frame_id.size() > 0) { + CHECK_NE(frame_id[0], '/'); } + return frame_id; + } }; } // namespace cartographer_ros diff --git a/cartographer_ros/src/tf_bridge.cc b/cartographer_ros/src/tf_bridge.cc new file mode 100644 index 0000000..f5bbcd6 --- /dev/null +++ b/cartographer_ros/src/tf_bridge.cc @@ -0,0 +1,56 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/common/make_unique.h" + +#include "msg_conversion.h" +#include "tf_bridge.h" + +namespace cartographer_ros { + +TfBridge::TfBridge(const string& tracking_frame, + const double lookup_transform_timeout_sec, + const tf2_ros::Buffer* buffer) + : tracking_frame_(tracking_frame), + lookup_transform_timeout_sec_(lookup_transform_timeout_sec), + buffer_(buffer) {} + +std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking( + const ::cartographer::common::Time time, const string& frame_id) const { + ::ros::Duration timeout(lookup_transform_timeout_sec_); + std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking; + try { + const ::ros::Time latest_tf_time = + buffer_ + ->lookupTransform(tracking_frame_, frame_id, ::ros::Time(0.), + timeout) + .header.stamp; + const ::ros::Time requested_time = ToRos(time); + if (latest_tf_time >= requested_time) { + // We already have newer data, so we do not wait. Otherwise, we would wait + // for the full 'timeout' even if we ask for data that is too old. + timeout = ::ros::Duration(0.); + } + return ::cartographer::common::make_unique< + ::cartographer::transform::Rigid3d>(ToRigid3d(buffer_->lookupTransform( + tracking_frame_, frame_id, requested_time, timeout))); + } catch (const tf2::TransformException& ex) { + LOG(WARNING) << ex.what(); + } + return nullptr; +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/src/tf_bridge.h b/cartographer_ros/src/tf_bridge.h new file mode 100644 index 0000000..1e05af6 --- /dev/null +++ b/cartographer_ros/src/tf_bridge.h @@ -0,0 +1,51 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_TF_BRIDGE_H_ +#define CARTOGRAPHER_ROS_TF_BRIDGE_H_ + +#include + +#include "cartographer/transform/rigid_transform.h" +#include "tf2_ros/buffer.h" + +#include "time_conversion.h" + +namespace cartographer_ros { + +class TfBridge { + public: + TfBridge(const string& tracking_frame, double lookup_transform_timeout_sec, + const tf2_ros::Buffer* buffer); + ~TfBridge() {} + + TfBridge(const TfBridge&) = delete; + TfBridge& operator=(const TfBridge&) = delete; + + // Returns the transform for 'frame_id' to 'tracking_frame_' if it exists at + // 'time'. + std::unique_ptr<::cartographer::transform::Rigid3d> LookupToTracking( + ::cartographer::common::Time time, const string& frame_id) const; + + private: + const string tracking_frame_; + const double lookup_transform_timeout_sec_; + const tf2_ros::Buffer* const buffer_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_TF_BRIDGE_H_ diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 3cc1de4..6c23667 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -55,17 +55,11 @@ use_odometry_data If enabled, subscribes to `nav_msgs/Odometry`_ on the topic "odom". Odometry must be provided in this case, and the information will be included in SLAM. -use_constant_odometry_variance - If enabled, the configured variances will be used instead of the ones provided - in the Odometry messages. +sensor_bridge.constant_odometry_translational_variance + The variance to use for the translational component of odometry. -constant_odometry_translational_variance - The variance to use for the translational component of odometry to use if - *use_constant_odometry_variance* is enabled. - -constant_odometry_rotational_variance - The variance to use for the rotational component of odometry to use if - *use_constant_odometry_variance* is enabled. +sensor_bridge.constant_odometry_rotational_variance + The variance to use for the rotational component of odometry. use_horizontal_laser If enabled, the node subscribes to `sensor_msgs/LaserScan`_ on the "scan" @@ -77,13 +71,13 @@ use_horizontal_multi_echo_laser "echoes" topic. If 2D SLAM is used, either this or *use_horizontal_laser* must be enabled. -horizontal_laser_min_range +sensor_bridge.horizontal_laser_min_range Range in meters below which laser returns are ignored for the purpose of SLAM. -horizontal_laser_max_range +sensor_bridge.horizontal_laser_max_range Range in meters above which laser returns are ignored for the purpose of SLAM. -horizontal_laser_missing_echo_ray_length +sensor_bridge.horizontal_laser_missing_echo_ray_length Range in meters to use for inserting free space when no laser return was detected.