Offline node improvements (#266)

* Unify signatures of ReadTransformsFromBag and ReadStaticTransformsFromUrdf. Both functions append data into the buffer, so they can be combined.
* Modifies offline_node and assets_writer to optionally use both the .urdf file and bag transform data, if available (use case: fixed joint descriptions of the robot are in the .urdf, while the odometry-base link transforms are read from the bag); the static transforms are added to the buffer after it has been filled with transforms from the bag (if any).
* offline_node optionally publishes all transforms from the bag (checking if any are in conflict with Cartographer is still TODO; however, this should be the responsibility of the bag provider, just like when using the online node, so it should be OK to leave this unimplemented) and all static transforms from the URDF.

cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -58,6 +58,9 @@ DEFINE_string(bag_filename, "", "Bag to process.");
 DEFINE_string(
     trajectory_filename, "",
     "Proto containing the trajectory written by /finish_trajectory service.");
+DEFINE_bool(
+    use_bag_transforms, true,
+    "Whether to read and use the transforms from the bag.");
 
 namespace cartographer_ros {
 namespace {
@@ -150,12 +153,16 @@ void Run(const string& trajectory_filename, const string& bag_filename,
       builder.CreatePipeline(
           lua_parameter_dictionary.GetDictionary("pipeline").get());
 
-  auto tf_buffer = ::cartographer::common::make_unique<tf2_ros::Buffer>();
+  auto tf_buffer =
+      ::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());
+  }
+
   if (!urdf_filename.empty()) {
     ReadStaticTransformsFromUrdf(urdf_filename, tf_buffer.get());
-  } else {
-    LOG(INFO) << "Pre-loading transforms from bag...";
-    tf_buffer = ReadTransformsFromBag(bag_filename);
   }
 
   const string tracking_frame =

cartographer_ros/cartographer_ros/bag_reader.cc

@@ -29,14 +29,12 @@ namespace cartographer_ros {
 
 constexpr char kTfStaticTopic[] = "/tf_static";
 
-std::unique_ptr<tf2_ros::Buffer> ReadTransformsFromBag(
-    const string& bag_filename) {
+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);
 
-  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) {
@@ -58,7 +56,6 @@ std::unique_ptr<tf2_ros::Buffer> ReadTransformsFromBag(
   }
 
   bag.close();
-  return tf_buffer;
 }
 
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/bag_reader.h

@@ -22,8 +22,8 @@
 
 namespace cartographer_ros {
 
-std::unique_ptr<tf2_ros::Buffer> ReadTransformsFromBag(
-    const string& bag_filename);
+void ReadTransformsFromBag(
+    const string& bag_filename, tf2_ros::Buffer* const tf_buffer);
 
 }  // namespace cartographer_ros
 

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -33,6 +33,7 @@
 #include "rosbag/view.h"
 #include "rosgraph_msgs/Clock.h"
 #include "tf2_msgs/TFMessage.h"
+#include "tf2_ros/static_transform_broadcaster.h"
 #include "urdf/model.h"
 
 DEFINE_string(configuration_directory, "",
@@ -46,12 +47,16 @@ DEFINE_string(bag_filenames, "", "Comma-separated list of bags to process.");
 DEFINE_string(
     urdf_filename, "",
     "URDF file that contains static links for your sensor configuration.");
+DEFINE_bool(
+    use_bag_transforms, true,
+    "Whether to read, use and republish the transforms from the bag.");
 
 namespace cartographer_ros {
 namespace {
 
 constexpr int kLatestOnlyPublisherQueueSize = 1;
 constexpr char kClockTopic[] = "clock";
+constexpr char kTfTopic [] = "tf";
 
 volatile std::sig_atomic_t sigint_triggered = 0;
 
@@ -82,15 +87,22 @@ NodeOptions LoadOptions() {
 void Run(const std::vector<string>& bag_filenames) {
   auto options = LoadOptions();
 
-  auto tf_buffer = ::cartographer::common::make_unique<tf2_ros::Buffer>();
-  if (!FLAGS_urdf_filename.empty()) {
-    ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, tf_buffer.get());
-  } else {
+  auto 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);
-    tf_buffer = ReadTransformsFromBag(bag_filenames.back());
+    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());
+  }
+
   tf_buffer->setUsingDedicatedThread(true);
 
   // Since we preload the transform buffer, we should never have to wait for a
@@ -139,10 +151,20 @@ void Run(const std::vector<string>& bag_filenames) {
     check_insert(kOdometryTopic);
   }
 
+  ::ros::Publisher tf_publisher =
+      node.node_handle()->advertise<tf2_msgs::TFMessage>(
+          kTfTopic, kLatestOnlyPublisherQueueSize);
+
+  ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster;
+
   ::ros::Publisher clock_publisher =
       node.node_handle()->advertise<rosgraph_msgs::Clock>(
           kClockTopic, kLatestOnlyPublisherQueueSize);
 
+  if (urdf_transforms.size() > 0) {
+    static_tf_broadcaster.sendTransform(urdf_transforms);
+  }
+
   for (const string& bag_filename : bag_filenames) {
     if (sigint_triggered) {
       break;
@@ -162,7 +184,11 @@ void Run(const std::vector<string>& bag_filenames) {
         break;
       }
 
-      // TODO(damonkohler): Republish non-conflicting tf messages.
+      if (FLAGS_use_bag_transforms && msg.isType<tf2_msgs::TFMessage>()) {
+        auto tf_message = msg.instantiate<tf2_msgs::TFMessage>();
+        tf_publisher.publish(tf_message);
+      }
+
       const string topic =
           node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
       if (expected_sensor_ids.count(topic) == 0) {

cartographer_ros/cartographer_ros/urdf_reader.cc

@@ -24,8 +24,8 @@
 
 namespace cartographer_ros {
 
-void ReadStaticTransformsFromUrdf(const string& urdf_filename,
-                                  tf2_ros::Buffer* const buffer) {
+std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
+    const string& urdf_filename, tf2_ros::Buffer* const tf_buffer) {
   urdf::Model model;
   CHECK(model.initFile(urdf_filename));
 #if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS
@@ -34,6 +34,7 @@ void ReadStaticTransformsFromUrdf(const string& urdf_filename,
   std::vector<boost::shared_ptr<urdf::Link> > links;
 #endif
   model.getLinks(links);
+  std::vector<geometry_msgs::TransformStamped> transforms;
   for (const auto& link : links) {
     if (!link->getParent() || link->parent_joint->type != urdf::Joint::FIXED) {
       continue;
@@ -49,8 +50,10 @@ void ReadStaticTransformsFromUrdf(const string& urdf_filename,
                                pose.rotation.y, pose.rotation.z)));
     transform.child_frame_id = link->name;
     transform.header.frame_id = link->getParent()->name;
-    buffer->setTransform(transform, "urdf", true /* is_static */);
+    tf_buffer->setTransform(transform, "urdf", true /* is_static */);
+    transforms.push_back(transform);
   }
+  return transforms;
 }
 
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/urdf_reader.h

@@ -19,11 +19,12 @@
 
 #include "cartographer/common/port.h"
 #include "tf2_ros/buffer.h"
+#include <vector>
 
 namespace cartographer_ros {
 
-void ReadStaticTransformsFromUrdf(const string& urdf_filename,
-                                  tf2_ros::Buffer* buffer);
+std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
+  const string& urdf_filename, tf2_ros::Buffer* tf_buffer);
 
 }  // namespace cartographer_ros