diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index fea3a08..5db3523 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -51,6 +51,9 @@ DEFINE_bool(use_bag_transforms, true, "Whether to read, use and republish the transforms from the bag."); DEFINE_string(pbstream_filename, "", "If non-empty, filename of a pbstream to load."); +DEFINE_bool(keep_running, false, + "Keep running the offline node after all messages from the bag " + "have been processed."); namespace cartographer_ros { namespace { @@ -58,6 +61,8 @@ namespace { constexpr char kClockTopic[] = "clock"; constexpr char kTfStaticTopic[] = "/tf_static"; constexpr char kTfTopic[] = "tf"; +constexpr double kClockPublishFrequencySec = 1. / 30.; +constexpr int kSingleThreaded = 1; void Run(const std::vector& bag_filenames) { const std::chrono::time_point start_time = @@ -108,6 +113,16 @@ void Run(const std::vector& bag_filenames) { static_tf_broadcaster.sendTransform(urdf_transforms); } + ros::AsyncSpinner async_spinner(kSingleThreaded); + async_spinner.start(); + rosgraph_msgs::Clock clock; + auto clock_republish_timer = node.node_handle()->createWallTimer( + ::ros::WallDuration(kClockPublishFrequencySec), + [&clock_publisher, &clock](const ::ros::WallTimerEvent&) { + clock_publisher.publish(clock); + }, + false /* oneshot */, false /* autostart */); + for (const string& bag_filename : bag_filenames) { if (!::ros::ok()) { break; @@ -175,12 +190,9 @@ void Run(const std::vector& bag_filenames) { trajectory_id, topic, delayed_msg.instantiate()); } - rosgraph_msgs::Clock clock; clock.clock = delayed_msg.getTime(); clock_publisher.publish(clock); - ::ros::spinOnce(); - LOG_EVERY_N(INFO, 100000) << "Processed " << (delayed_msg.getTime() - begin_time).toSec() << " of " << duration_in_seconds << " bag time seconds..."; @@ -197,9 +209,16 @@ void Run(const std::vector& bag_filenames) { } bag.close(); + // Ensure the clock is republished also during trajectory finalization, + // which might take a while. + clock_republish_timer.start(); node.FinishTrajectory(trajectory_id); + clock_republish_timer.stop(); } + // Republish the clock after bag processing has been completed. + clock_republish_timer.start(); + const std::chrono::time_point end_time = std::chrono::steady_clock::now(); const double wall_clock_seconds = @@ -216,6 +235,9 @@ void Run(const std::vector& bag_filenames) { #endif node.SerializeState(bag_filenames.front() + ".pbstream"); + if (FLAGS_keep_running) { + ::ros::waitForShutdown(); + } } } // namespace