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.
master
Juraj Oršulić 2017-06-19 10:17:40 +02:00 committed by Holger Rapp
parent bf16ec6458
commit 23869164a8
1 changed files with 10 additions and 9 deletions

View File

@ -237,6 +237,16 @@ void Run(const std::vector<string>& bag_filenames) {
->HandleOdometryMessage( ->HandleOdometryMessage(
topic, delayed_msg.instantiate<nav_msgs::Odometry>()); topic, delayed_msg.instantiate<nav_msgs::Odometry>());
} }
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(); delayed_messages.pop_front();
} }
@ -247,15 +257,6 @@ void Run(const std::vector<string>& bag_filenames) {
} }
delayed_messages.push_back(msg); 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(); bag.close();