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();