Optionally read /tf from bag in place of a URDF. (#213)

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -10,6 +10,15 @@ google_library(assets_writer
     occupancy_grid
 )
 
+google_library(bag_reader
+  USES_CARTOGRAPHER
+  USES_GLOG
+  SRCS
+    bag_reader.cc
+  HDRS
+    bag_reader.h
+)
+
 google_library(map_builder_bridge
   USES_CARTOGRAPHER
   SRCS
@@ -158,6 +167,7 @@ google_binary(cartographer_assets_writer
   SRCS
     assets_writer_main.cc
   DEPENDS
+    bag_reader
     msg_conversion
     time_conversion
     urdf_reader
@@ -188,6 +198,7 @@ google_binary(cartographer_offline_node
   SRCS
     offline_node_main.cc
   DEPENDS
+    bag_reader
     node
     ros_log_sink
     urdf_reader

cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -24,6 +24,7 @@
 #include "cartographer/io/points_processor.h"
 #include "cartographer/io/points_processor_pipeline_builder.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"
@@ -34,6 +35,7 @@
 #include "rosbag/bag.h"
 #include "rosbag/view.h"
 #include "tf2_eigen/tf2_eigen.h"
+#include "tf2_msgs/TFMessage.h"
 #include "tf2_ros/buffer.h"
 #include "urdf/model.h"
 
@@ -57,20 +59,43 @@ namespace {
 
 namespace carto = ::cartographer;
 
+void HandlePointCloud2Message(
+    const sensor_msgs::PointCloud2::ConstPtr& msg,
+    const string& tracking_frame,
+    const tf2_ros::Buffer& tf_buffer,
+    const carto::transform::TransformInterpolationBuffer&
+        transform_interpolation_buffer,
+    const std::vector<std::unique_ptr<carto::io::PointsProcessor>>& pipeline) {
+  const carto::common::Time time = FromRos(msg->header.stamp);
+  if (!transform_interpolation_buffer.Has(time)) {
+    return;
+  }
+
+  const carto::transform::Rigid3d tracking_to_map =
+      transform_interpolation_buffer.Lookup(time);
+  const carto::transform::Rigid3d sensor_to_tracking =
+      ToRigid3d(tf_buffer.lookupTransform(tracking_frame, msg->header.frame_id,
+                                          msg->header.stamp));
+  const carto::transform::Rigid3f sensor_to_map =
+      (tracking_to_map * sensor_to_tracking).cast<float>();
+
+  auto batch = carto::common::make_unique<carto::io::PointsBatch>();
+  batch->time = time;
+  batch->origin = sensor_to_map * Eigen::Vector3f::Zero();
+  batch->frame_id = msg->header.frame_id;
+
+  pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
+  pcl::fromROSMsg(*msg, pcl_point_cloud);
+  for (const auto& point : pcl_point_cloud) {
+    batch->points.push_back(sensor_to_map *
+                            Eigen::Vector3f(point.x, point.y, point.z));
+  }
+  pipeline.back()->Process(std::move(batch));
+}
+
 void Run(const string& trajectory_filename, const string& bag_filename,
          const string& configuration_directory,
          const string& configuration_basename, const string& urdf_filename) {
-  std::ifstream stream(trajectory_filename.c_str());
-  carto::mapping::proto::Trajectory trajectory_proto;
-  CHECK(trajectory_proto.ParseFromIstream(&stream));
-
-  auto transform_interpolation_buffer =
-      carto::transform::TransformInterpolationBuffer::FromTrajectory(
-          trajectory_proto);
-
-  carto::io::PointsProcessorPipelineBuilder builder;
-  carto::io::RegisterBuiltInPointsProcessors(trajectory_proto, &builder);
-
   auto file_resolver =
       carto::common::make_unique<carto::common::ConfigurationFileResolver>(
           std::vector<string>{configuration_directory});
@@ -78,50 +103,31 @@ void Run(const string& trajectory_filename, const string& bag_filename,
       file_resolver->GetFileContentOrDie(configuration_basename);
   carto::common::LuaParameterDictionary lua_parameter_dictionary(
       code, std::move(file_resolver));
-  const string tracking_frame =
-      lua_parameter_dictionary.GetString("tracking_frame");
 
+  std::ifstream stream(trajectory_filename.c_str());
+  carto::mapping::proto::Trajectory trajectory_proto;
+  CHECK(trajectory_proto.ParseFromIstream(&stream));
+
+  carto::io::PointsProcessorPipelineBuilder builder;
+  carto::io::RegisterBuiltInPointsProcessors(trajectory_proto, &builder);
   std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
       builder.CreatePipeline(
           lua_parameter_dictionary.GetDictionary("pipeline").get());
 
-  tf2_ros::Buffer buffer;
+  auto tf_buffer = ::cartographer::common::make_unique<tf2_ros::Buffer>();
-  // TODO(hrapp): Also parse tf messages and keep the 'buffer' updated as to
-  // support non-rigid sensor configurations.
   if (!urdf_filename.empty()) {
-    ReadStaticTransformsFromUrdf(urdf_filename, &buffer);
+    ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer.get());
+  } else {
+    LOG(INFO) << "Pre-loading transforms from bag...";
+    tf_buffer = ReadTransformsFromBag(bag_filename);
   }
 
-  auto handle_point_cloud_2_message = [&](
+  const string tracking_frame =
-      const sensor_msgs::PointCloud2::ConstPtr& msg) {
+      lua_parameter_dictionary.GetString("tracking_frame");
-    const carto::common::Time time = FromRos(msg->header.stamp);
+  const auto transform_interpolation_buffer =
-    if (!transform_interpolation_buffer->Has(time)) {
+      carto::transform::TransformInterpolationBuffer::FromTrajectory(
-      return;
+          trajectory_proto);
-    }
+  LOG(INFO) << "Processing pipeline...";
-    carto::transform::Rigid3d tracking_to_map =
-        transform_interpolation_buffer->Lookup(time);
-    pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
-    pcl::fromROSMsg(*msg, pcl_point_cloud);
-
-    const carto::transform::Rigid3d sensor_to_tracking = ToRigid3d(
-        buffer.lookupTransform(tracking_frame, msg->header.frame_id,
-                               msg->header.stamp));
-
-    const carto::transform::Rigid3f sensor_to_map =
-        (tracking_to_map * sensor_to_tracking).cast<float>();
-
-    auto batch = carto::common::make_unique<carto::io::PointsBatch>();
-    batch->time = time;
-    batch->origin = sensor_to_map * Eigen::Vector3f::Zero();
-    batch->frame_id = msg->header.frame_id;
-
-    for (const auto& point : pcl_point_cloud) {
-      batch->points.push_back(sensor_to_map *
-                              Eigen::Vector3f(point.x, point.y, point.z));
-    }
-    pipeline.back()->Process(std::move(batch));
-  };
-
   do {
     rosbag::Bag bag;
     bag.open(bag_filename, rosbag::bagmode::Read);
@@ -131,7 +137,9 @@ void Run(const string& trajectory_filename, const string& bag_filename,
 
     for (const rosbag::MessageInstance& msg : view) {
       if (msg.isType<sensor_msgs::PointCloud2>()) {
-        handle_point_cloud_2_message(msg.instantiate<sensor_msgs::PointCloud2>());
+        HandlePointCloud2Message(msg.instantiate<sensor_msgs::PointCloud2>(),
+                                 tracking_frame, *tf_buffer,
+                                 *transform_interpolation_buffer, pipeline);
       }
       LOG_EVERY_N(INFO, 100000)
           << "Processed " << (msg.getTime() - begin_time).toSec() << " of "
@@ -146,6 +154,7 @@ void Run(const string& trajectory_filename, const string& bag_filename,
 }  // namespace cartographer_ros
 
 int main(int argc, char** argv) {
+  FLAGS_alsologtostderr = true;
   google::InitGoogleLogging(argv[0]);
   google::ParseCommandLineFlags(&argc, &argv, true);
 
@@ -156,7 +165,6 @@ int main(int argc, char** argv) {
   CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
   CHECK(!FLAGS_trajectory_filename.empty())
       << "-trajectory_filename is missing.";
-  CHECK(!FLAGS_urdf_filename.empty()) << "-urdf_filename is missing.";
 
   ::cartographer_ros::Run(FLAGS_trajectory_filename, FLAGS_bag_filename,
                           FLAGS_configuration_directory,

cartographer_ros/cartographer_ros/bag_reader.cc

@@ -0,0 +1,64 @@
+/*
+ * 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";
+
+std::unique_ptr<tf2_ros::Buffer> ReadTransformsFromBag(
+    const string& bag_filename) {
+  rosbag::Bag bag;
+  bag.open(bag_filename, rosbag::bagmode::Read);
+  rosbag::View view(bag);
+
+  auto tf_buffer =
+      ::cartographer::common::make_unique<tf2_ros::Buffer>(::ros::DURATION_MAX);
+  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();
+  return tf_buffer;
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/bag_reader.h

@@ -0,0 +1,30 @@
+/*
+ * 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 {
+
+std::unique_ptr<tf2_ros::Buffer> ReadTransformsFromBag(
+    const string& bag_filename);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_BAG_READER_H_

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -22,6 +22,7 @@
 #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"
@@ -29,6 +30,7 @@
 #include "rosbag/bag.h"
 #include "rosbag/view.h"
 #include "rosgraph_msgs/Clock.h"
+#include "tf2_msgs/TFMessage.h"
 #include "urdf/model.h"
 
 DEFINE_string(configuration_directory, "",
@@ -48,6 +50,7 @@ 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);
@@ -68,11 +71,23 @@ void Run(std::vector<string> bag_filenames) {
   cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
       code, std::move(file_resolver));
 
-  tf2_ros::Buffer tf_buffer;
+  auto tf_buffer = ::cartographer::common::make_unique<tf2_ros::Buffer>();
-  tf_buffer.setUsingDedicatedThread(true);
+  if (!FLAGS_urdf_filename.empty()) {
-  ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, &tf_buffer);
+    ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, tf_buffer.get());
-  const auto options = CreateNodeOptions(&lua_parameter_dictionary);
+  } else {
-  Node node(options, &tf_buffer);
+    LOG(INFO) << "Pre-loading transforms from bag...";
+    // TODO(damonkohler): Support multi-trajectory.
+    CHECK_EQ(bag_filenames.size(), 1);
+    tf_buffer = ReadTransformsFromBag(bag_filenames.back());
+  }
+  tf_buffer->setUsingDedicatedThread(true);
+
+  auto options = CreateNodeOptions(&lua_parameter_dictionary);
+  // 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.Initialize();
 
   std::unordered_set<string> expected_sensor_ids;
@@ -138,6 +153,7 @@ void Run(std::vector<string> bag_filenames) {
         return;
       }
 
+      // TODO(damonkohler): Republish non-conflicting tf messages.
       const string topic = node.node_handle()->resolveName(msg.getTopic());
       if (expected_sensor_ids.count(topic) == 0) {
         continue;
@@ -147,21 +163,25 @@ void Run(std::vector<string> bag_filenames) {
             ->sensor_bridge(trajectory_id)
             ->HandleLaserScanMessage(topic,
                                      msg.instantiate<sensor_msgs::LaserScan>());
-      } else if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
+      }
+      if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
         node.map_builder_bridge()
             ->sensor_bridge(trajectory_id)
             ->HandleMultiEchoLaserScanMessage(
                 topic, msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
-      } else if (msg.isType<sensor_msgs::PointCloud2>()) {
+      }
+      if (msg.isType<sensor_msgs::PointCloud2>()) {
         node.map_builder_bridge()
             ->sensor_bridge(trajectory_id)
             ->HandlePointCloud2Message(
                 topic, msg.instantiate<sensor_msgs::PointCloud2>());
-      } else if (msg.isType<sensor_msgs::Imu>()) {
+      }
+      if (msg.isType<sensor_msgs::Imu>()) {
         node.map_builder_bridge()
             ->sensor_bridge(trajectory_id)
             ->HandleImuMessage(topic, msg.instantiate<sensor_msgs::Imu>());
-      } else if (msg.isType<nav_msgs::Odometry>()) {
+      }
+      if (msg.isType<nav_msgs::Odometry>()) {
         node.map_builder_bridge()
             ->sensor_bridge(trajectory_id)
             ->HandleOdometryMessage(topic,
@@ -190,6 +210,7 @@ void Run(std::vector<string> bag_filenames) {
 }  // namespace cartographer_ros
 
 int main(int argc, char** argv) {
+  FLAGS_alsologtostderr = true;
   google::InitGoogleLogging(argv[0]);
   google::ParseCommandLineFlags(&argc, &argv, true);
 
@@ -198,7 +219,6 @@ int main(int argc, char** argv) {
   CHECK(!FLAGS_configuration_basename.empty())
       << "-configuration_basename is missing.";
   CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing.";
-  CHECK(!FLAGS_urdf_filename.empty()) << "-urdf_filename is missing.";
 
   ::ros::init(argc, argv, "cartographer_offline_node");
   ::ros::start();