diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index 27ea478..a40935f 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/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 diff --git a/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc b/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc new file mode 100644 index 0000000..e069482 --- /dev/null +++ b/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 +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 message, const ros::Publisher& publisher, + ros::Duration bag_to_current) { + for (const auto& transform : message->transforms) { + ros::Time& stamp = const_cast(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 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()) { + PublishWithModifiedTimestamp( + message.instantiate(), publisher, + bag_to_current); + } else if (message.isType()) { + PublishWithModifiedTimestamp( + message.instantiate(), publisher, + bag_to_current); + } else if (message.isType()) { + PublishWithModifiedTimestamp( + message.instantiate(), publisher, + bag_to_current); + } else if (message.isType()) { + PublishWithModifiedTimestamp(message.instantiate(), + publisher, bag_to_current); + } else if (message.isType()) { + PublishWithModifiedTimestamp(message.instantiate(), + publisher, bag_to_current); + } else if (message.isType()) { + PublishWithModifiedTimestamp(message.instantiate(), + 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(); +}