From 960d1a487c19074705c98582d6303d18fb071b03 Mon Sep 17 00:00:00 2001 From: Susanne Pielawa <32822068+spielawa@users.noreply.github.com> Date: Thu, 11 Jan 2018 13:11:27 +0100 Subject: [PATCH] Wiring for sensor_msgs::NavSatFix (#659) PAIR=wohe [RFC=0007](https://github.com/googlecartographer/rfcs/blob/master/text/0007-nav-sat-support.md) --- cartographer_ros/cartographer_ros/node.cc | 33 ++++++++++++++++--- cartographer_ros/cartographer_ros/node.h | 17 ++++++++-- .../cartographer_ros/node_constants.h | 1 + .../cartographer_ros/offline_node.cc | 5 +++ .../cartographer_ros/sensor_bridge.cc | 5 +++ .../cartographer_ros/sensor_bridge.h | 3 ++ .../cartographer_ros/trajectory_options.cc | 8 +++++ .../cartographer_ros/trajectory_options.h | 2 ++ .../configuration_files/backpack_2d.lua | 2 ++ .../configuration_files/backpack_3d.lua | 2 ++ cartographer_ros/configuration_files/pr2.lua | 2 ++ .../configuration_files/revo_lds.lua | 2 ++ .../configuration_files/taurob_tracker.lua | 2 ++ cartographer_ros_msgs/msg/SensorTopics.msg | 1 + .../msg/TrajectoryOptions.msg | 2 ++ 15 files changed, 80 insertions(+), 7 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index bc13be9..babb311 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -52,6 +52,7 @@ cartographer_ros_msgs::SensorTopics DefaultSensorTopics() { topics.point_cloud2_topic = kPointCloud2Topic; topics.imu_topic = kImuTopic; topics.odometry_topic = kOdometryTopic; + topics.nav_sat_fix_topic = kNavSatFixTopic; return topics; } @@ -159,9 +160,9 @@ void Node::AddSensorSamplers(const int trajectory_id, CHECK(sensor_samplers_.count(trajectory_id) == 0); sensor_samplers_.emplace( std::piecewise_construct, std::forward_as_tuple(trajectory_id), - std::forward_as_tuple(options.rangefinder_sampling_ratio, - options.odometry_sampling_ratio, - options.imu_sampling_ratio)); + std::forward_as_tuple( + options.rangefinder_sampling_ratio, options.odometry_sampling_ratio, + options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio)); } void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { @@ -284,6 +285,11 @@ std::unordered_set Node::ComputeExpectedTopics( if (options.use_odometry) { expected_topics.insert(topics.odometry_topic); } + // NavSatFix is optional. + if (options.use_nav_sat) { + expected_topics.insert(topics.nav_sat_fix_topic); + } + return expected_topics; } @@ -353,6 +359,14 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, &node_handle_, this), topic}); } + if (options.use_nav_sat) { + std::string topic = topics.nav_sat_fix_topic; + subscribers_[trajectory_id].push_back( + {SubscribeWithHandler( + &Node::HandleNavSatFixMessage, trajectory_id, topic, &node_handle_, + this), + topic}); + } } bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) { @@ -502,6 +516,17 @@ void Node::HandleOdometryMessage(const int trajectory_id, sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg); } +void Node::HandleNavSatFixMessage(const int trajectory_id, + const std::string& sensor_id, + const sensor_msgs::NavSatFix::ConstPtr& msg) { + carto::common::MutexLocker lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) { + return; + } + map_builder_bridge_.sensor_bridge(trajectory_id) + ->HandleNavSatFixMessage(sensor_id, msg); +} + void Node::HandleImuMessage(const int trajectory_id, const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg) { @@ -529,7 +554,7 @@ void Node::HandleLaserScanMessage(const int trajectory_id, } void Node::HandleMultiEchoLaserScanMessage( - int trajectory_id, const std::string& sensor_id, + const int trajectory_id, const std::string& sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { carto::common::MutexLocker lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 5fbd2b2..4db9a28 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -39,7 +39,13 @@ #include "cartographer_ros_msgs/SubmapQuery.h" #include "cartographer_ros_msgs/TrajectoryOptions.h" #include "cartographer_ros_msgs/WriteState.h" +#include "nav_msgs/Odometry.h" #include "ros/ros.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/LaserScan.h" +#include "sensor_msgs/MultiEchoLaserScan.h" +#include "sensor_msgs/NavSatFix.h" +#include "sensor_msgs/PointCloud2.h" #include "tf2_ros/transform_broadcaster.h" namespace cartographer_ros { @@ -79,6 +85,8 @@ class Node { // The following functions handle adding sensor data to a trajectory. void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg); + void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id, + const sensor_msgs::NavSatFix::ConstPtr& msg); void HandleImuMessage(int trajectory_id, const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg); void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id, @@ -156,15 +164,18 @@ class Node { ::ros::Publisher scan_matched_point_cloud_publisher_; struct TrajectorySensorSamplers { - TrajectorySensorSamplers(double rangefinder_sampling_ratio, - double odometry_sampling_ratio, - double imu_sampling_ratio) + TrajectorySensorSamplers(const double rangefinder_sampling_ratio, + const double odometry_sampling_ratio, + const double fixed_frame_pose_sampling_ratio, + const double imu_sampling_ratio) : rangefinder_sampler(rangefinder_sampling_ratio), odometry_sampler(odometry_sampling_ratio), + fixed_frame_pose_sampler(fixed_frame_pose_sampling_ratio), imu_sampler(imu_sampling_ratio) {} ::cartographer::common::FixedRatioSampler rangefinder_sampler; ::cartographer::common::FixedRatioSampler odometry_sampler; + ::cartographer::common::FixedRatioSampler fixed_frame_pose_sampler; ::cartographer::common::FixedRatioSampler imu_sampler; }; diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/cartographer_ros/node_constants.h index 1aeb6a8..de6bf6c 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/cartographer_ros/cartographer_ros/node_constants.h @@ -28,6 +28,7 @@ constexpr char kMultiEchoLaserScanTopic[] = "echoes"; constexpr char kPointCloud2Topic[] = "points2"; constexpr char kImuTopic[] = "imu"; constexpr char kOdometryTopic[] = "odom"; +constexpr char kNavSatFixTopic[] = "fix"; constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; constexpr char kOccupancyGridTopic[] = "map"; constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index f2bce3e..b7ad4fa 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -177,6 +177,11 @@ void RunOfflineNode( trajectory_id, topic, delayed_msg.instantiate()); } + if (delayed_msg.isType()) { + node.HandleNavSatFixMessage( + trajectory_id, topic, + delayed_msg.instantiate()); + } clock.clock = delayed_msg.getTime(); clock_publisher.publish(clock); diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 7b5499b..55aa4f0 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -73,6 +73,11 @@ void SensorBridge::HandleOdometryMessage( } } +void SensorBridge::HandleNavSatFixMessage( + const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg) { + LOG(FATAL) << "TODO(spielawa / wohe): NavSatFix support not yet implemented."; +} + std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData( const sensor_msgs::Imu::ConstPtr& msg) { CHECK_NE(msg->linear_acceleration_covariance[0], -1) diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index 86d46c3..a6e2e25 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/cartographer_ros/cartographer_ros/sensor_bridge.h @@ -32,6 +32,7 @@ #include "sensor_msgs/Imu.h" #include "sensor_msgs/LaserScan.h" #include "sensor_msgs/MultiEchoLaserScan.h" +#include "sensor_msgs/NavSatFix.h" #include "sensor_msgs/PointCloud2.h" namespace cartographer_ros { @@ -51,6 +52,8 @@ class SensorBridge { const nav_msgs::Odometry::ConstPtr& msg); void HandleOdometryMessage(const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg); + void HandleNavSatFixMessage(const std::string& sensor_id, + const sensor_msgs::NavSatFix::ConstPtr& msg); std::unique_ptr<::cartographer::sensor::ImuData> ToImuData( const sensor_msgs::Imu::ConstPtr& msg); void HandleImuMessage(const std::string& sensor_id, diff --git a/cartographer_ros/cartographer_ros/trajectory_options.cc b/cartographer_ros/cartographer_ros/trajectory_options.cc index 603c756..023b6b3 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.cc +++ b/cartographer_ros/cartographer_ros/trajectory_options.cc @@ -53,6 +53,7 @@ TrajectoryOptions CreateTrajectoryOptions( options.provide_odom_frame = lua_parameter_dictionary->GetBool("provide_odom_frame"); options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry"); + options.use_nav_sat = lua_parameter_dictionary->GetBool("use_nav_sat"); options.num_laser_scans = lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans"); options.num_multi_echo_laser_scans = @@ -66,6 +67,8 @@ TrajectoryOptions CreateTrajectoryOptions( lua_parameter_dictionary->GetDouble("rangefinder_sampling_ratio"); options.odometry_sampling_ratio = lua_parameter_dictionary->GetDouble("odometry_sampling_ratio"); + options.fixed_frame_pose_sampling_ratio = + lua_parameter_dictionary->GetDouble("fixed_frame_pose_sampling_ratio"); options.imu_sampling_ratio = lua_parameter_dictionary->GetDouble("imu_sampling_ratio"); CheckTrajectoryOptions(options); @@ -104,6 +107,7 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, options->odom_frame = msg.odom_frame; options->provide_odom_frame = msg.provide_odom_frame; options->use_odometry = msg.use_odometry; + options->use_nav_sat = msg.use_nav_sat; options->num_laser_scans = msg.num_laser_scans; options->num_multi_echo_laser_scans = msg.num_multi_echo_laser_scans; options->num_subdivisions_per_laser_scan = @@ -111,6 +115,8 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, options->num_point_clouds = msg.num_point_clouds; options->rangefinder_sampling_ratio = msg.rangefinder_sampling_ratio; options->odometry_sampling_ratio = msg.odometry_sampling_ratio; + options->fixed_frame_pose_sampling_ratio = + msg.fixed_frame_pose_sampling_ratio; options->imu_sampling_ratio = msg.imu_sampling_ratio; if (!options->trajectory_builder_options.ParseFromString( msg.trajectory_builder_options_proto)) { @@ -129,12 +135,14 @@ cartographer_ros_msgs::TrajectoryOptions ToRosMessage( msg.odom_frame = options.odom_frame; msg.provide_odom_frame = options.provide_odom_frame; msg.use_odometry = options.use_odometry; + msg.use_nav_sat = options.use_nav_sat; msg.num_laser_scans = options.num_laser_scans; msg.num_multi_echo_laser_scans = options.num_multi_echo_laser_scans; msg.num_subdivisions_per_laser_scan = options.num_subdivisions_per_laser_scan; msg.num_point_clouds = options.num_point_clouds; msg.rangefinder_sampling_ratio = options.rangefinder_sampling_ratio; msg.odometry_sampling_ratio = options.odometry_sampling_ratio; + msg.fixed_frame_pose_sampling_ratio = options.fixed_frame_pose_sampling_ratio; msg.imu_sampling_ratio = options.imu_sampling_ratio; options.trajectory_builder_options.SerializeToString( &msg.trajectory_builder_options_proto); diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/cartographer_ros/trajectory_options.h index c7848a8..3702357 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.h +++ b/cartographer_ros/cartographer_ros/trajectory_options.h @@ -34,12 +34,14 @@ struct TrajectoryOptions { std::string odom_frame; bool provide_odom_frame; bool use_odometry; + bool use_nav_sat; int num_laser_scans; int num_multi_echo_laser_scans; int num_subdivisions_per_laser_scan; int num_point_clouds; double rangefinder_sampling_ratio; double odometry_sampling_ratio; + double fixed_frame_pose_sampling_ratio; double imu_sampling_ratio; }; diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index d6d89ee..0c6e59e 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = true, use_odometry = false, + use_nav_sat = false, num_laser_scans = 0, num_multi_echo_laser_scans = 1, num_subdivisions_per_laser_scan = 10, @@ -34,6 +35,7 @@ options = { trajectory_publish_period_sec = 30e-3, rangefinder_sampling_ratio = 1., odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., } diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index 123543f..1435d72 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = true, use_odometry = false, + use_nav_sat = false, num_laser_scans = 0, num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, @@ -34,6 +35,7 @@ options = { trajectory_publish_period_sec = 30e-3, rangefinder_sampling_ratio = 1., odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., } diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index 87ea711..ea0a735 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/cartographer_ros/configuration_files/pr2.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = true, use_odometry = false, + use_nav_sat = false, num_laser_scans = 1, num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, @@ -34,6 +35,7 @@ options = { trajectory_publish_period_sec = 30e-3, rangefinder_sampling_ratio = 1., odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., } diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index 3ec0c4f..7447760 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/cartographer_ros/configuration_files/revo_lds.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = true, use_odometry = false, + use_nav_sat = false, num_laser_scans = 1, num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, @@ -34,6 +35,7 @@ options = { trajectory_publish_period_sec = 30e-3, rangefinder_sampling_ratio = 1., odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., } diff --git a/cartographer_ros/configuration_files/taurob_tracker.lua b/cartographer_ros/configuration_files/taurob_tracker.lua index 7d555fd..c98da3a 100644 --- a/cartographer_ros/configuration_files/taurob_tracker.lua +++ b/cartographer_ros/configuration_files/taurob_tracker.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = false, use_odometry = true, + use_nav_sat = false, num_laser_scans = 1, num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, @@ -34,6 +35,7 @@ options = { trajectory_publish_period_sec = 30e-3, rangefinder_sampling_ratio = 1., odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., } diff --git a/cartographer_ros_msgs/msg/SensorTopics.msg b/cartographer_ros_msgs/msg/SensorTopics.msg index 6004b14..ab13fbb 100644 --- a/cartographer_ros_msgs/msg/SensorTopics.msg +++ b/cartographer_ros_msgs/msg/SensorTopics.msg @@ -17,3 +17,4 @@ string multi_echo_laser_scan_topic string point_cloud2_topic string imu_topic string odometry_topic +string nav_sat_fix_topic diff --git a/cartographer_ros_msgs/msg/TrajectoryOptions.msg b/cartographer_ros_msgs/msg/TrajectoryOptions.msg index 95e62fe..fc92cb9 100644 --- a/cartographer_ros_msgs/msg/TrajectoryOptions.msg +++ b/cartographer_ros_msgs/msg/TrajectoryOptions.msg @@ -17,12 +17,14 @@ string published_frame string odom_frame bool provide_odom_frame bool use_odometry +bool use_nav_sat int32 num_laser_scans int32 num_multi_echo_laser_scans int32 num_subdivisions_per_laser_scan int32 num_point_clouds float64 rangefinder_sampling_ratio float64 odometry_sampling_ratio +float64 fixed_frame_pose_sampling_ratio float64 imu_sampling_ratio # This is a binary-encoded