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

@@ -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

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

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

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

cartographer_ros/launch/backpack_2d.launch

@@ -22,22 +22,11 @@
     type="robot_state_publisher" />
 
   <node name="cartographer" pkg="cartographer_ros"
-      type="cartographer_node" output="screen" >
-    <rosparam subst_value="true">
-      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.
-    </rosparam>
+      type="cartographer_node" args="
+          -configuration_directory $(find cartographer_ros)/configuration_files
+          -configuration_basename backpack_2d.lua"
+      output="screen" >
+    <remap from="echoes" to="horizontal_laser_2d" />
   </node>
 
 </launch>

cartographer_ros/launch/backpack_3d.launch

@@ -22,22 +22,12 @@
     type="robot_state_publisher" />
 
   <node name="cartographer" pkg="cartographer_ros"
-      type="cartographer_node" output="screen" >
-    <rosparam subst_value="true">
-      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.
-    </rosparam>
+      type="cartographer_node" args="
+          -configuration_directory $(find cartographer_ros)/configuration_files
+          -configuration_basename backpack_3d.lua"
+      output="screen" >
+    <remap from="points2_1" to="horizontal_laser_3d" />
+    <remap from="points2_2" to="vertical_laser_3d" />
   </node>
 
 </launch>

cartographer_ros/launch/turtlebot.launch

@@ -22,22 +22,11 @@
     type="robot_state_publisher" />
 
   <node name="cartographer" pkg="cartographer_ros"
-      type="cartographer_node" output="screen" >
-    <rosparam subst_value="true">
-      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.
-    </rosparam>
+      type="cartographer_node" args="
+          -configuration_directory $(find cartographer_ros)/configuration_files
+          -configuration_basename turtlebot.lua"
+      output="screen" >
+    <remap from="scan" to="horizontal_laser_2d" />
   </node>
 
 </launch>

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<SensorData> 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 <typename T>
-  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<SensorData> sensor_collator_;
   ros::NodeHandle node_handle_;
   ros::Subscriber imu_subscriber_;
-  ros::Subscriber laser_2d_subscriber_;
-  std::vector<ros::Subscriber> laser_3d_subscribers_;
+  ros::Subscriber laser_subscriber_2d_;
+  std::vector<ros::Subscriber> 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<SensorData>(
       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 <typename T>
-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<string>("tracking_frame");
-  odom_frame_ = GetParamOrDie<string>("odom_frame");
-  map_frame_ = GetParamOrDie<string>("map_frame");
-  provide_odom_ = GetParamOrDie<bool>("provide_odom");
-  laser_min_range_m_ = GetParamOrDie<double>("laser_min_range_m");
-  laser_max_range_m_ = GetParamOrDie<double>("laser_max_range_m");
-  laser_missing_echo_ray_length_m_ =
-      GetParamOrDie<double>("laser_missing_echo_ray_length_m");
+  auto file_resolver = ::cartographer::common::make_unique<
+      ::cartographer::common::ConfigurationFileResolver>(
+      std::vector<string>{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<string> 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<string>("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<string>("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<std::vector<string>>("configuration_files_directories"));
-  const string code = file_resolver->GetFileContentOrDie(
-      GetParamOrDie<string>("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<std::vector<string>>("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<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
               [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<string>("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();