From 23869164a829e8488335f60d9215584ffcb09017 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Mon, 19 Jun 2017 10:17:40 +0200 Subject: [PATCH] Publish more accurate clock in offline node (#380) Clock of the message in the msg variable is one second later than the message in delayed_message. It should be more correct to publish the clock of the message which really has been processed in SLAM and which affects the position and the map, rather than the one that has only been pushed into the delayed_msgs queue. Also change the status message printing not to include the one second buffer into the amount of bag processed seconds. Spinning the ROS event queue has also been moved into the inner loop. --- .../cartographer_ros/offline_node_main.cc | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index bfd32dd..8d86601 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -237,6 +237,16 @@ void Run(const std::vector& bag_filenames) { ->HandleOdometryMessage( 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..."; + delayed_messages.pop_front(); } @@ -247,15 +257,6 @@ void Run(const std::vector& bag_filenames) { } delayed_messages.push_back(msg); - rosgraph_msgs::Clock clock; - clock.clock = msg.getTime(); - clock_publisher.publish(clock); - - ::ros::spinOnce(); - - LOG_EVERY_N(INFO, 100000) - << "Processed " << (msg.getTime() - begin_time).toSec() << " of " - << duration_in_seconds << " bag time seconds..."; } bag.close();