From 794ca96b92898738c7e3e63c0ff1954a9b8c2161 Mon Sep 17 00:00:00 2001
From: Damon Kohler <damonkohler@google.com>
Date: Thu, 8 Dec 2016 11:15:02 +0100
Subject: [PATCH] Fix offline node. (#219)

Flips topic remapping to match online node and checks that topics are
wired to unique sensor inputs.
Fixes broken launch files.
---
 .../cartographer_ros/offline_node_main.cc     | 23 +++++++++----------
 .../launch/offline_backpack_2d.launch         |  4 ++--
 .../launch/offline_backpack_3d.launch         |  6 ++---
 3 files changed, 16 insertions(+), 17 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc
index 0a710a1..335f665 100644
--- a/cartographer_ros/cartographer_ros/offline_node_main.cc
+++ b/cartographer_ros/cartographer_ros/offline_node_main.cc
@@ -51,7 +51,6 @@ namespace {
 
 constexpr int kLatestOnlyPublisherQueueSize = 1;
 constexpr char kClockTopic[] = "clock";
-constexpr char kTfStaticTopic[] = "/tf_static";
 
 std::vector<string> SplitString(const string& input, const char delimiter) {
   std::stringstream stream(input);
@@ -92,15 +91,17 @@ void Run(std::vector<string> bag_filenames) {
   node.Initialize();
 
   std::unordered_set<string> expected_sensor_ids;
+  const auto check_insert = [&expected_sensor_ids, &node](const string& topic) {
+    CHECK(expected_sensor_ids.insert(node.node_handle()->resolveName(topic))
+              .second);
+  };
 
   // For 2D SLAM, subscribe to exactly one horizontal laser.
   if (options.use_laser_scan) {
-    expected_sensor_ids.insert(
-        node.node_handle()->resolveName(kLaserScanTopic, false /* remap */));
+    check_insert(kLaserScanTopic);
   }
   if (options.use_multi_echo_laser_scan) {
-    expected_sensor_ids.insert(node.node_handle()->resolveName(
-        kMultiEchoLaserScanTopic, false /* remap */));
+    check_insert(kMultiEchoLaserScanTopic);
   }
 
   // For 3D SLAM, subscribe to all point clouds topics.
@@ -110,8 +111,7 @@ void Run(std::vector<string> bag_filenames) {
       if (options.num_point_clouds > 1) {
         topic += "_" + std::to_string(i + 1);
       }
-      expected_sensor_ids.insert(
-          node.node_handle()->resolveName(topic, false /* remap */));
+      check_insert(topic);
     }
   }
 
@@ -121,14 +121,12 @@ void Run(std::vector<string> bag_filenames) {
       (options.map_builder_options.use_trajectory_builder_2d() &&
        options.map_builder_options.trajectory_builder_2d_options()
            .use_imu_data())) {
-    expected_sensor_ids.insert(
-        node.node_handle()->resolveName(kImuTopic, false /* remap */));
+    check_insert(kImuTopic);
   }
 
   // For both 2D and 3D SLAM, odometry is optional.
   if (options.use_odometry) {
-    expected_sensor_ids.insert(
-        node.node_handle()->resolveName(kOdometryTopic, false /* remap */));
+    check_insert(kOdometryTopic);
   }
 
   ::ros::Publisher clock_publisher =
@@ -155,7 +153,8 @@ void Run(std::vector<string> bag_filenames) {
       }
 
       // TODO(damonkohler): Republish non-conflicting tf messages.
-      const string topic = node.node_handle()->resolveName(msg.getTopic());
+      const string topic =
+          node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
       if (expected_sensor_ids.count(topic) == 0) {
         continue;
       }
diff --git a/cartographer_ros/launch/offline_backpack_2d.launch b/cartographer_ros/launch/offline_backpack_2d.launch
index 5388b61..3a738b2 100644
--- a/cartographer_ros/launch/offline_backpack_2d.launch
+++ b/cartographer_ros/launch/offline_backpack_2d.launch
@@ -23,10 +23,10 @@
   <node name="cartographer_offline_node" pkg="cartographer_ros"
       type="cartographer_offline_node" args="
           -configuration_directory $(find cartographer_ros)/configuration_files
-          -configuration_basename backpack_2d.lua"
+          -configuration_basename backpack_2d.lua
           -urdf_filename $(find cartographer_ros)/urdf/backpack_2d.urdf
           -bag_filenames $(arg bag_filenames)"
       output="screen">
-    <remap from="horizontal_laser_2d" to="echoes" />
+    <remap from="echoes" to="horizontal_laser_2d" />
   </node>
 </launch>
diff --git a/cartographer_ros/launch/offline_backpack_3d.launch b/cartographer_ros/launch/offline_backpack_3d.launch
index bbd10cf..2979ec7 100644
--- a/cartographer_ros/launch/offline_backpack_3d.launch
+++ b/cartographer_ros/launch/offline_backpack_3d.launch
@@ -23,11 +23,11 @@
   <node name="cartographer_offline_node" pkg="cartographer_ros"
       type="cartographer_offline_node" args="
           -configuration_directory $(find cartographer_ros)/configuration_files
-          -configuration_basename backpack_3d.lua"
+          -configuration_basename backpack_3d.lua
           -urdf_filename $(find cartographer_ros)/urdf/backpack_3d.urdf
           -bag_filenames $(arg bag_filenames)"
       output="screen">
-    <remap from="horizontal_laser_3d" to="points2_1" />
-    <remap from="vertical_laser_3d" to="points2_2" />
+    <remap from="points2_1" to="horizontal_laser_3d" />
+    <remap from="points2_2" to="vertical_laser_3d" />
   </node>
 </launch>