Do not use tf_buffer for keeping all transforms in memory. (#342)

PAIR=wohe

cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -29,7 +29,6 @@
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/sensor/range_data.h"
 #include "cartographer/transform/transform_interpolation_buffer.h"
-#include "cartographer_ros/bag_reader.h"
 #include "cartographer_ros/msg_conversion.h"
 #include "cartographer_ros/time_conversion.h"
 #include "cartographer_ros/urdf_reader.h"
@@ -64,8 +63,41 @@ DEFINE_bool(use_bag_transforms, true,
 namespace cartographer_ros {
 namespace {
 
+constexpr char kTfStaticTopic[] = "/tf_static";
 namespace carto = ::cartographer;
 
+// TODO(hrapp): We discovered that using tf_buffer with a large CACHE
+// is very inefficient. Switch asset writer to use our own
+// TransformInterpolationBuffer.
+void ReadTransformsFromBag(const string& bag_filename,
+                           tf2_ros::Buffer* const tf_buffer) {
+  rosbag::Bag bag;
+  bag.open(bag_filename, rosbag::bagmode::Read);
+  rosbag::View view(bag);
+
+  const ::ros::Time begin_time = view.getBeginTime();
+  const double duration_in_seconds = (view.getEndTime() - begin_time).toSec();
+  for (const rosbag::MessageInstance& msg : view) {
+    if (msg.isType<tf2_msgs::TFMessage>()) {
+      const auto tf_msg = msg.instantiate<tf2_msgs::TFMessage>();
+      for (const auto& transform : tf_msg->transforms) {
+        try {
+          // TODO(damonkohler): Handle topic remapping.
+          tf_buffer->setTransform(transform, "unused_authority",
+                                  msg.getTopic() == kTfStaticTopic);
+        } catch (const tf2::TransformException& ex) {
+          LOG(WARNING) << ex.what();
+        }
+      }
+    }
+    LOG_EVERY_N(INFO, 100000)
+        << "Processed " << (msg.getTime() - begin_time).toSec() << " of "
+        << duration_in_seconds << " bag time seconds...";
+  }
+
+  bag.close();
+}
+
 template <typename T>
 void HandleMessage(
     const T& message, const string& tracking_frame,
@@ -128,16 +160,15 @@ void Run(const string& trajectory_filename, const string& bag_filename,
       builder.CreatePipeline(
           lua_parameter_dictionary.GetDictionary("pipeline").get());
 
-  auto tf_buffer =
+  tf2_ros::Buffer tf_buffer(::ros::DURATION_MAX);
-      ::cartographer::common::make_unique<tf2_ros::Buffer>(::ros::DURATION_MAX);
 
   if (FLAGS_use_bag_transforms) {
     LOG(INFO) << "Pre-loading transforms from bag...";
-    ReadTransformsFromBag(bag_filename, tf_buffer.get());
+    ReadTransformsFromBag(bag_filename, &tf_buffer);
   }
 
   if (!urdf_filename.empty()) {
-    ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer.get());
+    ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer);
   }
 
   const string tracking_frame =
@@ -156,17 +187,17 @@ void Run(const string& trajectory_filename, const string& bag_filename,
     for (const rosbag::MessageInstance& message : view) {
       if (message.isType<sensor_msgs::PointCloud2>()) {
         HandleMessage(*message.instantiate<sensor_msgs::PointCloud2>(),
-                      tracking_frame, *tf_buffer,
+                      tracking_frame, tf_buffer,
                       *transform_interpolation_buffer, pipeline);
       }
       if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
         HandleMessage(*message.instantiate<sensor_msgs::MultiEchoLaserScan>(),
-                      tracking_frame, *tf_buffer,
+                      tracking_frame, tf_buffer,
                       *transform_interpolation_buffer, pipeline);
       }
       if (message.isType<sensor_msgs::LaserScan>()) {
         HandleMessage(*message.instantiate<sensor_msgs::LaserScan>(),
-                      tracking_frame, *tf_buffer,
+                      tracking_frame, tf_buffer,
                       *transform_interpolation_buffer, pipeline);
       }
       LOG_EVERY_N(INFO, 100000)

cartographer_ros/cartographer_ros/bag_reader.cc

@@ -1,61 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "cartographer_ros/bag_reader.h"
-
-#include <string>
-#include <vector>
-
-#include "cartographer/common/make_unique.h"
-#include "glog/logging.h"
-#include "rosbag/bag.h"
-#include "rosbag/view.h"
-#include "tf2_msgs/TFMessage.h"
-
-namespace cartographer_ros {
-
-constexpr char kTfStaticTopic[] = "/tf_static";
-
-void ReadTransformsFromBag(const string& bag_filename,
-                           tf2_ros::Buffer* const tf_buffer) {
-  rosbag::Bag bag;
-  bag.open(bag_filename, rosbag::bagmode::Read);
-  rosbag::View view(bag);
-
-  const ::ros::Time begin_time = view.getBeginTime();
-  const double duration_in_seconds = (view.getEndTime() - begin_time).toSec();
-  for (const rosbag::MessageInstance& msg : view) {
-    if (msg.isType<tf2_msgs::TFMessage>()) {
-      const auto tf_msg = msg.instantiate<tf2_msgs::TFMessage>();
-      for (const auto& transform : tf_msg->transforms) {
-        try {
-          // TODO(damonkohler): Handle topic remapping.
-          tf_buffer->setTransform(transform, "unused_authority",
-                                  msg.getTopic() == kTfStaticTopic);
-        } catch (const tf2::TransformException& ex) {
-          LOG(WARNING) << ex.what();
-        }
-      }
-    }
-    LOG_EVERY_N(INFO, 100000)
-        << "Processed " << (msg.getTime() - begin_time).toSec() << " of "
-        << duration_in_seconds << " bag time seconds...";
-  }
-
-  bag.close();
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/bag_reader.h

@@ -1,30 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef CARTOGRAPHER_ROS_BAG_READER_H_
-#define CARTOGRAPHER_ROS_BAG_READER_H_
-
-#include "cartographer/common/port.h"
-#include "tf2_ros/buffer.h"
-
-namespace cartographer_ros {
-
-void ReadTransformsFromBag(const string& bag_filename,
-                           tf2_ros::Buffer* const tf_buffer);
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_BAG_READER_H_

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -23,7 +23,6 @@
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
-#include "cartographer_ros/bag_reader.h"
 #include "cartographer_ros/node.h"
 #include "cartographer_ros/ros_log_sink.h"
 #include "cartographer_ros/urdf_reader.h"
@@ -53,9 +52,10 @@ DEFINE_bool(use_bag_transforms, true,
 namespace cartographer_ros {
 namespace {
 
-constexpr int kLatestOnlyPublisherQueueSize = 1;
 constexpr char kClockTopic[] = "clock";
+constexpr char kTfStaticTopic[] = "/tf_static";
 constexpr char kTfTopic[] = "tf";
+constexpr int kLatestOnlyPublisherQueueSize = 1;
 
 volatile std::sig_atomic_t sigint_triggered = 0;
 
@@ -86,29 +86,21 @@ NodeOptions LoadOptions() {
 void Run(const std::vector<string>& bag_filenames) {
   auto options = LoadOptions();
 
-  auto tf_buffer =
+  tf2_ros::Buffer tf_buffer;
-      ::cartographer::common::make_unique<tf2_ros::Buffer>(::ros::DURATION_MAX);
-
-  if (FLAGS_use_bag_transforms) {
-    LOG(INFO) << "Pre-loading transforms from bag...";
-    // TODO(damonkohler): Support multi-trajectory.
-    CHECK_EQ(bag_filenames.size(), 1);
-    ReadTransformsFromBag(bag_filenames.back(), tf_buffer.get());
-  }
 
   std::vector<geometry_msgs::TransformStamped> urdf_transforms;
   if (!FLAGS_urdf_filename.empty()) {
     urdf_transforms =
-        ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, tf_buffer.get());
+        ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, &tf_buffer);
   }
 
-  tf_buffer->setUsingDedicatedThread(true);
+  tf_buffer.setUsingDedicatedThread(true);
 
   // Since we preload the transform buffer, we should never have to wait for a
   // transform. When we finish processing the bag, we will simply drop any
   // remaining sensor data that cannot be transformed due to missing transforms.
   options.lookup_transform_timeout_sec = 0.;
-  Node node(options, tf_buffer.get());
+  Node node(options, &tf_buffer);
   node.Initialize();
 
   std::unordered_set<string> expected_sensor_ids;
@@ -178,6 +170,10 @@ void Run(const std::vector<string>& bag_filenames) {
     const ::ros::Time begin_time = view.getBeginTime();
     const double duration_in_seconds = (view.getEndTime() - begin_time).toSec();
 
+    // We make sure that tf_messages are published before any data messages, so
+    // that tf lookups always work and that tf_buffer has a small cache size -
+    // because it gets very inefficient with a large one.
+    std::deque<rosbag::MessageInstance> delayed_messages;
     for (const rosbag::MessageInstance& msg : view) {
       if (sigint_triggered) {
         break;
@@ -186,6 +182,57 @@ void Run(const std::vector<string>& bag_filenames) {
       if (FLAGS_use_bag_transforms && msg.isType<tf2_msgs::TFMessage>()) {
         auto tf_message = msg.instantiate<tf2_msgs::TFMessage>();
         tf_publisher.publish(tf_message);
+
+        if (FLAGS_use_bag_transforms) {
+          for (const auto& transform : tf_message->transforms) {
+            try {
+              tf_buffer.setTransform(transform, "unused_authority",
+                                      msg.getTopic() == kTfStaticTopic);
+            } catch (const tf2::TransformException& ex) {
+              LOG(WARNING) << ex.what();
+            }
+          }
+        }
+      }
+
+      while (!delayed_messages.empty() &&
+             delayed_messages.front().getTime() <
+                 msg.getTime() + ::ros::Duration(1.)) {
+        const rosbag::MessageInstance& delayed_msg = delayed_messages.front();
+        const string topic = node.node_handle()->resolveName(
+            delayed_msg.getTopic(), false /* resolve */);
+        if (delayed_msg.isType<sensor_msgs::LaserScan>()) {
+          node.map_builder_bridge()
+              ->sensor_bridge(trajectory_id)
+              ->HandleLaserScanMessage(
+                  topic, delayed_msg.instantiate<sensor_msgs::LaserScan>());
+        }
+        if (delayed_msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
+          node.map_builder_bridge()
+              ->sensor_bridge(trajectory_id)
+              ->HandleMultiEchoLaserScanMessage(
+                  topic,
+                  delayed_msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
+        }
+        if (delayed_msg.isType<sensor_msgs::PointCloud2>()) {
+          node.map_builder_bridge()
+              ->sensor_bridge(trajectory_id)
+              ->HandlePointCloud2Message(
+                  topic, delayed_msg.instantiate<sensor_msgs::PointCloud2>());
+        }
+        if (delayed_msg.isType<sensor_msgs::Imu>()) {
+          node.map_builder_bridge()
+              ->sensor_bridge(trajectory_id)
+              ->HandleImuMessage(topic,
+                                 delayed_msg.instantiate<sensor_msgs::Imu>());
+        }
+        if (delayed_msg.isType<nav_msgs::Odometry>()) {
+          node.map_builder_bridge()
+              ->sensor_bridge(trajectory_id)
+              ->HandleOdometryMessage(
+                  topic, delayed_msg.instantiate<nav_msgs::Odometry>());
+        }
+        delayed_messages.pop_front();
       }
 
       const string topic =
@@ -193,35 +240,7 @@ void Run(const std::vector<string>& bag_filenames) {
       if (expected_sensor_ids.count(topic) == 0) {
         continue;
       }
-      if (msg.isType<sensor_msgs::LaserScan>()) {
+      delayed_messages.push_back(msg);
-        node.map_builder_bridge()
-            ->sensor_bridge(trajectory_id)
-            ->HandleLaserScanMessage(topic,
-                                     msg.instantiate<sensor_msgs::LaserScan>());
-      }
-      if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
-        node.map_builder_bridge()
-            ->sensor_bridge(trajectory_id)
-            ->HandleMultiEchoLaserScanMessage(
-                topic, msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
-      }
-      if (msg.isType<sensor_msgs::PointCloud2>()) {
-        node.map_builder_bridge()
-            ->sensor_bridge(trajectory_id)
-            ->HandlePointCloud2Message(
-                topic, msg.instantiate<sensor_msgs::PointCloud2>());
-      }
-      if (msg.isType<sensor_msgs::Imu>()) {
-        node.map_builder_bridge()
-            ->sensor_bridge(trajectory_id)
-            ->HandleImuMessage(topic, msg.instantiate<sensor_msgs::Imu>());
-      }
-      if (msg.isType<nav_msgs::Odometry>()) {
-        node.map_builder_bridge()
-            ->sensor_bridge(trajectory_id)
-            ->HandleOdometryMessage(topic,
-                                    msg.instantiate<nav_msgs::Odometry>());
-      }
 
       rosgraph_msgs::Clock clock;
       clock.clock = msg.getTime();