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
parent
bf16ec6458
commit
23869164a8
|
@ -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();
|
||||||
|
|
Loading…
Reference in New Issue