Add cartographer_dev_rosbag_publisher (#854)

This adds a tool to publish a bag file without publishing a simulated clock, modifying header timestamps.

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -102,6 +102,17 @@ install(TARGETS cartographer_pbstream_map_publisher
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+google_binary(cartographer_dev_rosbag_publisher
+  SRCS
+  dev/rosbag_publisher_main.cc
+)
+
+install(TARGETS cartographer_dev_rosbag_publisher
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 google_binary(cartographer_dev_trajectory_comparison
   SRCS
   dev/trajectory_comparison_main.cc

cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc

@@ -0,0 +1,148 @@
+/*
+ * Copyright 2018 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/common/time.h"
+#include "cartographer_ros/ros_log_sink.h"
+#include "cartographer_ros/time_conversion.h"
+#include "gflags/gflags.h"
+#include "nav_msgs/Odometry.h"
+#include "ros/ros.h"
+#include "ros/time.h"
+#include "rosbag/bag.h"
+#include "rosbag/view.h"
+#include "sensor_msgs/Imu.h"
+#include "sensor_msgs/LaserScan.h"
+#include "sensor_msgs/MultiEchoLaserScan.h"
+#include "sensor_msgs/PointCloud2.h"
+#include "tf2_msgs/TFMessage.h"
+
+DEFINE_string(bag_filename, "", "Bag to publish.");
+
+const int kQueueSize = 1;
+
+template <typename MessagePtrType>
+void PublishWithModifiedTimestamp(MessagePtrType message,
+                                  const ros::Publisher& publisher,
+                                  ros::Duration bag_to_current) {
+  ros::Time& stamp = message->header.stamp;
+  stamp += bag_to_current;
+  publisher.publish(message);
+}
+
+template <>
+void PublishWithModifiedTimestamp<tf2_msgs::TFMessage::Ptr>(
+    tf2_msgs::TFMessage::Ptr message, const ros::Publisher& publisher,
+    ros::Duration bag_to_current) {
+  for (const auto& transform : message->transforms) {
+    ros::Time& stamp = const_cast<ros::Time&>(transform.header.stamp);
+    stamp += bag_to_current;
+  }
+  publisher.publish(message);
+}
+
+int main(int argc, char** argv) {
+  google::InitGoogleLogging(argv[0]);
+  google::SetUsageMessage(
+      "\n\n"
+      "This replays and publishes messages from a given bag file, modifying "
+      "their header timestamps to match current ROS time.\n\n"
+      "Messages are published in the same sequence and with the same delay "
+      "they were recorded."
+      "Contrary to rosbag play, it does not publish a clock, so time is"
+      "hopefully smoother and it should be possible to reproduce timing"
+      "issues.\n"
+      "It only plays message types related to Cartographer.\n");
+  google::ParseCommandLineFlags(&argc, &argv, true);
+  CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
+
+  ros::init(argc, argv, "rosbag_publisher");
+  ros::start();
+
+  cartographer_ros::ScopedRosLogSink ros_log_sink;
+
+  rosbag::Bag bag;
+  bag.open(FLAGS_bag_filename, rosbag::bagmode::Read);
+  rosbag::View view(bag);
+  ros::NodeHandle node_handle;
+  bool use_sim_time;
+  node_handle.getParam("/use_sim_time", use_sim_time);
+  if (use_sim_time) {
+    LOG(ERROR) << "use_sim_time is true but not supported. Expect conflicting "
+                  "ros::Time and message header times or weird behavior.";
+  }
+  std::map<std::string, ros::Publisher> topic_to_publisher;
+  for (const rosbag::ConnectionInfo* c : view.getConnections()) {
+    const std::string& topic = c->topic;
+    if (topic_to_publisher.count(topic) == 0) {
+      ros::AdvertiseOptions options(c->topic, kQueueSize, c->md5sum,
+                                    c->datatype, c->msg_def);
+      topic_to_publisher[topic] = node_handle.advertise(options);
+    }
+  }
+  ros::Duration(1).sleep();
+  CHECK(ros::ok());
+
+  ros::Time current_start = ros::Time::now();
+  ros::Time bag_start = view.getBeginTime();
+  ros::Duration bag_to_current = current_start - bag_start;
+  for (const rosbag::MessageInstance& message : view) {
+    ros::Duration after_bag_start = message.getTime() - bag_start;
+    if (!::ros::ok()) {
+      break;
+    }
+    ros::Time planned_publish_time = current_start + after_bag_start;
+    ros::Time::sleepUntil(planned_publish_time);
+
+    ros::Publisher& publisher = topic_to_publisher.at(message.getTopic());
+    if (message.isType<sensor_msgs::PointCloud2>()) {
+      PublishWithModifiedTimestamp(
+          message.instantiate<sensor_msgs::PointCloud2>(), publisher,
+          bag_to_current);
+    } else if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
+      PublishWithModifiedTimestamp(
+          message.instantiate<sensor_msgs::MultiEchoLaserScan>(), publisher,
+          bag_to_current);
+    } else if (message.isType<sensor_msgs::LaserScan>()) {
+      PublishWithModifiedTimestamp(
+          message.instantiate<sensor_msgs::LaserScan>(), publisher,
+          bag_to_current);
+    } else if (message.isType<sensor_msgs::Imu>()) {
+      PublishWithModifiedTimestamp(message.instantiate<sensor_msgs::Imu>(),
+                                   publisher, bag_to_current);
+    } else if (message.isType<nav_msgs::Odometry>()) {
+      PublishWithModifiedTimestamp(message.instantiate<nav_msgs::Odometry>(),
+                                   publisher, bag_to_current);
+    } else if (message.isType<tf2_msgs::TFMessage>()) {
+      PublishWithModifiedTimestamp(message.instantiate<tf2_msgs::TFMessage>(),
+                                   publisher, bag_to_current);
+    } else {
+      LOG(WARNING) << "Skipping message with type " << message.getDataType();
+    }
+
+    ros::Time current_time = ros::Time::now();
+    double simulation_delay = cartographer::common::ToSeconds(
+        cartographer_ros::FromRos(current_time) -
+        cartographer_ros::FromRos(planned_publish_time));
+    if (std::abs(simulation_delay) > 0.001) {
+      LOG(WARNING) << "Playback delayed by " << simulation_delay
+                   << " s. planned_publish_time: " << planned_publish_time
+                   << " current_time: " << current_time;
+    }
+  }
+  bag.close();
+
+  ros::shutdown();
+}