From c5ac8e70b5ed46545717714382c75c2b0892f6ec Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 12 Aug 2016 17:14:08 +0200 Subject: [PATCH] Configure the Cartographer node without rosparam. (#21) Configuration of Cartographer's ROS integration is now entirely handled by Lua. The path and basename of the configuration are given by command line flags. The node is listening to default topic names which are expected to be remapped as needed. --- cartographer_ros/CMakeLists.txt | 1 + .../configuration_files/backpack_2d.lua | 8 + .../configuration_files/backpack_3d.lua | 8 + .../configuration_files/turtlebot.lua | 8 + cartographer_ros/launch/backpack_2d.launch | 21 +-- cartographer_ros/launch/backpack_3d.launch | 22 +-- cartographer_ros/launch/turtlebot.launch | 21 +-- .../src/cartographer_node_main.cc | 155 ++++++++++-------- 8 files changed, 126 insertions(+), 118 deletions(-) diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index f5a97ee..89ed279 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -92,6 +92,7 @@ target_link_libraries(cartographer_node ${CARTOGRAPHER_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} + gflags # TODO(whess): Use or remove gflags_catkin. ) add_library(cartographer_rviz_submaps_visualization diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 75b035f..21a5c44 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -18,6 +18,14 @@ include "sparse_pose_graph.lua" options = { sparse_pose_graph = SPARSE_POSE_GRAPH, trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + odom_frame = "odom", + tracking_frame = "base_link", + provide_odom = true, + laser_min_range = 0., + laser_max_range = 30., + laser_missing_echo_ray_length = 5., + use_multi_echo_laser_scan_2d = true } return options diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index ce2f912..5c17ee6 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -18,6 +18,14 @@ include "sparse_pose_graph.lua" options = { sparse_pose_graph = SPARSE_POSE_GRAPH, trajectory_builder = TRAJECTORY_BUILDER_3D, + map_frame = "map", + odom_frame = "odom", + tracking_frame = "base_link", + provide_odom = true, + laser_min_range = 0., + laser_max_range = 30., + laser_missing_echo_ray_length = 5., + num_lasers_3d = 2 } options.sparse_pose_graph.optimize_every_n_scans = 320 diff --git a/cartographer_ros/configuration_files/turtlebot.lua b/cartographer_ros/configuration_files/turtlebot.lua index 2f1ff65..2cdff44 100644 --- a/cartographer_ros/configuration_files/turtlebot.lua +++ b/cartographer_ros/configuration_files/turtlebot.lua @@ -18,6 +18,14 @@ include "sparse_pose_graph.lua" options = { sparse_pose_graph = SPARSE_POSE_GRAPH, trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", + odom_frame = "odom", + tracking_frame = "base_link", + provide_odom = false, + laser_min_range = 0., + laser_max_range = 30., + laser_missing_echo_ray_length = 5., + use_laser_scan_2d = true } options.trajectory_builder.expect_imu_data = false diff --git a/cartographer_ros/launch/backpack_2d.launch b/cartographer_ros/launch/backpack_2d.launch index e0a80a2..ab332cb 100644 --- a/cartographer_ros/launch/backpack_2d.launch +++ b/cartographer_ros/launch/backpack_2d.launch @@ -22,22 +22,11 @@ type="robot_state_publisher" /> - - map_frame: "map" - odom_frame: "odom" - tracking_frame: "base_link" - provide_odom: true - configuration_files_directories: [ - "$(find cartographer_ros)/configuration_files" - ] - mapping_configuration_basename: "backpack_2d.lua" - imu_topic: "/imu" - multi_echo_laser_scan_2d_topic: "/horizontal_2d_laser" - laser_min_range_m: 0. - laser_max_range_m: 30. - laser_missing_echo_ray_length_m: 5. - + type="cartographer_node" args=" + -configuration_directory $(find cartographer_ros)/configuration_files + -configuration_basename backpack_2d.lua" + output="screen" > + diff --git a/cartographer_ros/launch/backpack_3d.launch b/cartographer_ros/launch/backpack_3d.launch index b679ecb..d1c2fcc 100644 --- a/cartographer_ros/launch/backpack_3d.launch +++ b/cartographer_ros/launch/backpack_3d.launch @@ -22,22 +22,12 @@ type="robot_state_publisher" /> - - map_frame: "map" - odom_frame: "odom" - tracking_frame: "base_link" - provide_odom: true - configuration_files_directories: [ - "$(find cartographer_ros)/configuration_files" - ] - mapping_configuration_basename: "backpack_3d.lua" - imu_topic: "/imu" - laser_scan_3d_topics: ["/horizontal_3d_laser", "/vertical_3d_laser"] - laser_min_range_m: 0. - laser_max_range_m: 30. - laser_missing_echo_ray_length_m: 5. - + type="cartographer_node" args=" + -configuration_directory $(find cartographer_ros)/configuration_files + -configuration_basename backpack_3d.lua" + output="screen" > + + diff --git a/cartographer_ros/launch/turtlebot.launch b/cartographer_ros/launch/turtlebot.launch index 15b1a12..0174c7e 100644 --- a/cartographer_ros/launch/turtlebot.launch +++ b/cartographer_ros/launch/turtlebot.launch @@ -22,22 +22,11 @@ type="robot_state_publisher" /> - - map_frame: "map" - odom_frame: "odom" - tracking_frame: "base_link" - provide_odom: false - configuration_files_directories: [ - "$(find cartographer_ros)/configuration_files" - ] - mapping_configuration_basename: "turtlebot.lua" - imu_topic: "/imu" - laser_scan_2d_topic: "/horizontal_2d_laser" - laser_min_range_m: 0. - laser_max_range_m: 30. - laser_missing_echo_ray_length_m: 5. - + type="cartographer_node" args=" + -configuration_directory $(find cartographer_ros)/configuration_files + -configuration_basename turtlebot.lua" + output="screen" > + diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 2840a87..46b3c8a 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -48,6 +48,7 @@ #include "cartographer_ros_msgs/TrajectorySubmapList.h" #include "geometry_msgs/Transform.h" #include "geometry_msgs/TransformStamped.h" +#include "gflags/gflags.h" #include "glog/log_severity.h" #include "glog/logging.h" #include "pcl/point_cloud.h" @@ -67,6 +68,14 @@ #include "node_constants.h" #include "time_conversion.h" +DEFINE_string(configuration_directory, "", + "First directory in which configuration files are searched, " + "second is always the Cartographer installation to allow " + "including files from there."); +DEFINE_string(configuration_basename, "", + "Basename, i.e. not containing any directory prefix, of the " + "configuration file."); + namespace cartographer_ros { namespace { @@ -80,6 +89,12 @@ constexpr int kSubmapPublishPeriodInUts = 300 * 10000ll; // 300 milliseconds constexpr int kPosePublishPeriodInUts = 5 * 10000ll; // 5 milliseconds constexpr double kMaxTransformDelaySeconds = 0.01; +// Unique default topic names. Expected to be remapped as needed. +constexpr char kLaserScanTopic[] = "/scan"; +constexpr char kMultiEchoLaserScanTopic[] = "/echoes"; +constexpr char kPointCloud2Topic[] = "/points2"; +constexpr char kImuTopic[] = "/imu"; + Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) { return Rigid3d(Eigen::Vector3d(transform.transform.translation.x, transform.transform.translation.y, @@ -153,7 +168,7 @@ class Node { std::unique_ptr sensor_data); void ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg); void LaserScanMessageCallback(const sensor_msgs::LaserScan::ConstPtr& msg); - void MultiEchoLaserScanCallback( + void MultiEchoLaserScanMessageCallback( const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); void PointCloud2MessageCallback( const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg); @@ -163,9 +178,6 @@ class Node { void AddLaserFan3D(int64 timestamp, const string& frame_id, const proto::LaserFan3D& laser_fan_3d); - template - const T GetParamOrDie(const string& name); - // Returns a transform for 'frame_id' to 'tracking_frame_' if it exists at // 'time' or throws tf2::TransformException if it does not exist. Rigid3d LookupToTrackingTransformOrThrow(::cartographer::common::Time time, @@ -183,15 +195,15 @@ class Node { ::cartographer::mapping::SensorCollator sensor_collator_; ros::NodeHandle node_handle_; ros::Subscriber imu_subscriber_; - ros::Subscriber laser_2d_subscriber_; - std::vector laser_3d_subscribers_; + ros::Subscriber laser_subscriber_2d_; + std::vector laser_subscribers_3d_; string tracking_frame_; string odom_frame_; string map_frame_; bool provide_odom_; - double laser_min_range_m_; - double laser_max_range_m_; - double laser_missing_echo_ray_length_m_; + double laser_min_range_; + double laser_max_range_; + double laser_missing_echo_ray_length_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_; @@ -231,7 +243,7 @@ void Node::ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg) { sensor_collator_.AddSensorData( kTrajectoryId, ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), - imu_subscriber_.getTopic(), std::move(sensor_data)); + kImuTopic, std::move(sensor_data)); } void Node::AddImu(const int64 timestamp, const string& frame_id, @@ -242,10 +254,9 @@ void Node::AddImu(const int64 timestamp, const string& frame_id, const Rigid3d sensor_to_tracking = LookupToTrackingTransformOrThrow(time, frame_id); CHECK(sensor_to_tracking.translation().norm() < 1e-5) - << "The IMU is not colocated with the tracking frame. This makes it " - "hard " - "and inprecise to transform its linear accelaration into the " - "tracking_frame and will decrease the quality of the SLAM."; + << "The IMU frame must be colocated with the tracking frame. " + "Transforming linear accelaration into the tracking frame will " + "otherwise be imprecise."; trajectory_builder_->AddImuData( time, sensor_to_tracking.rotation() * ::cartographer::transform::ToEigen(imu.linear_acceleration()), @@ -264,7 +275,7 @@ void Node::LaserScanMessageCallback( sensor_collator_.AddSensorData( kTrajectoryId, ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), - laser_2d_subscriber_.getTopic(), std::move(sensor_data)); + kLaserScanTopic, std::move(sensor_data)); } void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id, @@ -274,11 +285,10 @@ void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id, try { const Rigid3d sensor_to_tracking = LookupToTrackingTransformOrThrow(time, frame_id); - // TODO(hrapp): Make things configurable? Through Lua? Through ROS params? const ::cartographer::sensor::LaserFan laser_fan = - ::cartographer::sensor::ToLaserFan(laser_scan, laser_min_range_m_, - laser_max_range_m_, - laser_missing_echo_ray_length_m_); + ::cartographer::sensor::ToLaserFan(laser_scan, laser_min_range_, + laser_max_range_, + laser_missing_echo_ray_length_); const auto laser_fan_3d = ::cartographer::sensor::TransformLaserFan3D( ::cartographer::sensor::ToLaserFan3D(laser_fan), @@ -290,14 +300,14 @@ void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id, } } -void Node::MultiEchoLaserScanCallback( +void Node::MultiEchoLaserScanMessageCallback( const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { auto sensor_data = ::cartographer::common::make_unique( msg->header.frame_id, ToCartographer(*msg)); sensor_collator_.AddSensorData( kTrajectoryId, ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), - laser_2d_subscriber_.getTopic(), std::move(sensor_data)); + kMultiEchoLaserScanTopic, std::move(sensor_data)); } void Node::PointCloud2MessageCallback( @@ -330,61 +340,59 @@ void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id, } } -template -const T Node::GetParamOrDie(const string& name) { - CHECK(node_handle_.hasParam(name)) << "Required parameter '" << name - << "' is unset."; - T value; - node_handle_.getParam(name, value); - return value; -} - void Node::Initialize() { - tracking_frame_ = GetParamOrDie("tracking_frame"); - odom_frame_ = GetParamOrDie("odom_frame"); - map_frame_ = GetParamOrDie("map_frame"); - provide_odom_ = GetParamOrDie("provide_odom"); - laser_min_range_m_ = GetParamOrDie("laser_min_range_m"); - laser_max_range_m_ = GetParamOrDie("laser_max_range_m"); - laser_missing_echo_ray_length_m_ = - GetParamOrDie("laser_missing_echo_ray_length_m"); + auto file_resolver = ::cartographer::common::make_unique< + ::cartographer::common::ConfigurationFileResolver>( + std::vector{FLAGS_configuration_directory}); + const string code = + file_resolver->GetFileContentOrDie(FLAGS_configuration_basename); + ::cartographer::common::LuaParameterDictionary lua_parameter_dictionary( + code, std::move(file_resolver), nullptr); + tracking_frame_ = lua_parameter_dictionary.GetString("tracking_frame"); + odom_frame_ = lua_parameter_dictionary.GetString("odom_frame"); + map_frame_ = lua_parameter_dictionary.GetString("map_frame"); + provide_odom_ = lua_parameter_dictionary.GetBool("provide_odom"); + laser_min_range_ = lua_parameter_dictionary.GetDouble("laser_min_range"); + laser_max_range_ = lua_parameter_dictionary.GetDouble("laser_max_range"); + laser_missing_echo_ray_length_ = + lua_parameter_dictionary.GetDouble("laser_missing_echo_ray_length"); + + // Set of all topics we subscribe to. We use the non-remapped default names + // which are unique. std::unordered_set expected_sensor_identifiers; // Subscribe to exactly one laser. - const bool has_laser_scan_2d = node_handle_.hasParam("laser_scan_2d_topic"); + const bool has_laser_scan_2d = + lua_parameter_dictionary.HasKey("use_laser_scan_2d") && + lua_parameter_dictionary.GetBool("use_laser_scan_2d"); const bool has_multi_echo_laser_scan_2d = - node_handle_.hasParam("multi_echo_laser_scan_2d_topic"); - const bool has_laser_scan_3d = node_handle_.hasParam("laser_scan_3d_topics"); + lua_parameter_dictionary.HasKey("use_multi_echo_laser_scan_2d") && + lua_parameter_dictionary.GetBool("use_multi_echo_laser_scan_2d"); + const int num_lasers_3d = + lua_parameter_dictionary.HasKey("num_lasers_3d") + ? lua_parameter_dictionary.GetNonNegativeInt("num_lasers_3d") + : 0; - CHECK(has_laser_scan_2d + has_multi_echo_laser_scan_2d + has_laser_scan_3d == + CHECK(has_laser_scan_2d + has_multi_echo_laser_scan_2d + + (num_lasers_3d > 0) == 1) - << "Parameters 'laser_scan_2d_topic', 'multi_echo_laser_scan_2d_topic' " - "and 'laser_scan_3d_topics' are mutually exclusive, but one is " - "required."; + << "Parameters 'use_laser_scan_2d', 'use_multi_echo_laser_scan_2d' and " + "'num_lasers_3d' are mutually exclusive, but one is required."; if (has_laser_scan_2d) { - const string topic = GetParamOrDie("laser_scan_2d_topic"); - laser_2d_subscriber_ = node_handle_.subscribe( - topic, kSubscriberQueueSize, &Node::LaserScanMessageCallback, this); - expected_sensor_identifiers.insert(topic); + laser_subscriber_2d_ = + node_handle_.subscribe(kLaserScanTopic, kSubscriberQueueSize, + &Node::LaserScanMessageCallback, this); + expected_sensor_identifiers.insert(kLaserScanTopic); } if (has_multi_echo_laser_scan_2d) { - const string topic = - GetParamOrDie("multi_echo_laser_scan_2d_topic"); - laser_2d_subscriber_ = node_handle_.subscribe( - topic, kSubscriberQueueSize, &Node::MultiEchoLaserScanCallback, this); - expected_sensor_identifiers.insert(topic); + laser_subscriber_2d_ = + node_handle_.subscribe(kMultiEchoLaserScanTopic, kSubscriberQueueSize, + &Node::MultiEchoLaserScanMessageCallback, this); + expected_sensor_identifiers.insert(kMultiEchoLaserScanTopic); } - auto file_resolver = ::cartographer::common::make_unique< - ::cartographer::common::ConfigurationFileResolver>( - GetParamOrDie>("configuration_files_directories")); - const string code = file_resolver->GetFileContentOrDie( - GetParamOrDie("mapping_configuration_basename")); - - ::cartographer::common::LuaParameterDictionary lua_parameter_dictionary( - code, std::move(file_resolver), nullptr); bool expect_imu_data = true; if (has_laser_scan_2d || has_multi_echo_laser_scan_2d) { auto sparse_pose_graph_2d = ::cartographer::common::make_unique< @@ -402,11 +410,13 @@ void Node::Initialize() { sparse_pose_graph_ = std::move(sparse_pose_graph_2d); } - if (has_laser_scan_3d) { - const auto topics = - GetParamOrDie>("laser_scan_3d_topics"); - for (const auto& topic : topics) { - laser_3d_subscribers_.push_back(node_handle_.subscribe( + if (num_lasers_3d > 0) { + for (int i = 0; i < num_lasers_3d; ++i) { + string topic = kPointCloud2Topic; + if (num_lasers_3d > 1) { + topic += "_" + std::to_string(i + 1); + } + laser_subscribers_3d_.push_back(node_handle_.subscribe( topic, kSubscriberQueueSize, boost::function( [this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { @@ -430,10 +440,9 @@ void Node::Initialize() { // Maybe subscribe to the IMU. if (expect_imu_data) { - const string imu_topic = GetParamOrDie("imu_topic"); - imu_subscriber_ = node_handle_.subscribe(imu_topic, kSubscriberQueueSize, + imu_subscriber_ = node_handle_.subscribe(kImuTopic, kSubscriberQueueSize, &Node::ImuMessageCallback, this); - expected_sensor_identifiers.insert(imu_topic); + expected_sensor_identifiers.insert(kImuTopic); } // TODO(hrapp): Add odometry subscribers here. @@ -656,6 +665,12 @@ class ScopedRosLogSink : public google::LogSink { int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); + google::ParseCommandLineFlags(&argc, &argv, true); + + CHECK(!FLAGS_configuration_directory.empty()) + << "-configuration_directory is missing."; + CHECK(!FLAGS_configuration_basename.empty()) + << "-configuration_basename is missing."; ros::init(argc, argv, "cartographer_node"); ros::start();