From 7fc931688cc5db50e1c192bb7f31a55497cf835a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= <juraj.orsulic@fer.hr>
Date: Thu, 25 Jan 2018 16:13:38 +0100
Subject: [PATCH] Do not forget to finish trajectory if last message is not
 from a sensor topic (#681)

Bug introduced in #636.
---
 .../cartographer_ros/offline_node.cc          | 54 +++++++++----------
 1 file changed, 26 insertions(+), 28 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc
index 0361faf..1456564 100644
--- a/cartographer_ros/cartographer_ros/offline_node.cc
+++ b/cartographer_ros/cartographer_ros/offline_node.cc
@@ -274,34 +274,32 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
     }
     const std::string topic =
         node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
-    if (bag_sensor_topics.at(bag_index).count(topic) == 0) {
-      continue;
-    }
-
-    if (msg.isType<sensor_msgs::LaserScan>()) {
-      node.HandleLaserScanMessage(trajectory_id, topic,
-                                  msg.instantiate<sensor_msgs::LaserScan>());
-    }
-    if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
-      node.HandleMultiEchoLaserScanMessage(
-          trajectory_id, topic,
-          msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
-    }
-    if (msg.isType<sensor_msgs::PointCloud2>()) {
-      node.HandlePointCloud2Message(
-          trajectory_id, topic, msg.instantiate<sensor_msgs::PointCloud2>());
-    }
-    if (msg.isType<sensor_msgs::Imu>()) {
-      node.HandleImuMessage(trajectory_id, topic,
-                            msg.instantiate<sensor_msgs::Imu>());
-    }
-    if (msg.isType<nav_msgs::Odometry>()) {
-      node.HandleOdometryMessage(trajectory_id, topic,
-                                 msg.instantiate<nav_msgs::Odometry>());
-    }
-    if (msg.isType<sensor_msgs::NavSatFix>()) {
-      node.HandleNavSatFixMessage(trajectory_id, topic,
-                                  msg.instantiate<sensor_msgs::NavSatFix>());
+    if (bag_sensor_topics.at(bag_index).count(topic) > 0) {
+      if (msg.isType<sensor_msgs::LaserScan>()) {
+        node.HandleLaserScanMessage(trajectory_id, topic,
+                                    msg.instantiate<sensor_msgs::LaserScan>());
+      }
+      if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
+        node.HandleMultiEchoLaserScanMessage(
+            trajectory_id, topic,
+            msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
+      }
+      if (msg.isType<sensor_msgs::PointCloud2>()) {
+        node.HandlePointCloud2Message(
+            trajectory_id, topic, msg.instantiate<sensor_msgs::PointCloud2>());
+      }
+      if (msg.isType<sensor_msgs::Imu>()) {
+        node.HandleImuMessage(trajectory_id, topic,
+                              msg.instantiate<sensor_msgs::Imu>());
+      }
+      if (msg.isType<nav_msgs::Odometry>()) {
+        node.HandleOdometryMessage(trajectory_id, topic,
+                                   msg.instantiate<nav_msgs::Odometry>());
+      }
+      if (msg.isType<sensor_msgs::NavSatFix>()) {
+        node.HandleNavSatFixMessage(trajectory_id, topic,
+                                    msg.instantiate<sensor_msgs::NavSatFix>());
+      }
     }
     clock.clock = msg.getTime();
     clock_publisher.publish(clock);