12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Keep offline node running, republish the clock. (#468)

master
Juraj Oršulić 2017-08-04 17:46:16 +02:00 committed by Wolfgang Hess
parent 4034f4f801
commit b99c8b4cda
1 changed files with 25 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
cartographer_ros/cartographer_ros/offline_node_main.cc
View File

@ -51,6 +51,9 @@ DEFINE_bool(use_bag_transforms, true,
"Whether to read, use and republish the transforms from the bag."); "Whether to read, use and republish the transforms from the bag.");
DEFINE_string(pbstream_filename, "", DEFINE_string(pbstream_filename, "",
"If non-empty, filename of a pbstream to load."); "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 cartographer_ros {
namespace { namespace {
@ -58,6 +61,8 @@ namespace {
constexpr char kClockTopic[] = "clock"; constexpr char kClockTopic[] = "clock";
constexpr char kTfStaticTopic[] = "/tf_static"; constexpr char kTfStaticTopic[] = "/tf_static";
constexpr char kTfTopic[] = "tf"; constexpr char kTfTopic[] = "tf";
constexpr double kClockPublishFrequencySec = 1. / 30.;
constexpr int kSingleThreaded = 1;
void Run(const std::vector<string>& bag_filenames) { void Run(const std::vector<string>& bag_filenames) {
const std::chrono::time_point<std::chrono::steady_clock> start_time = const std::chrono::time_point<std::chrono::steady_clock> start_time =
@ -108,6 +113,16 @@ void Run(const std::vector<string>& bag_filenames) {
static_tf_broadcaster.sendTransform(urdf_transforms); 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) { for (const string& bag_filename : bag_filenames) {
if (!::ros::ok()) { if (!::ros::ok()) {
break; break;
@ -175,12 +190,9 @@ void Run(const std::vector<string>& bag_filenames) {
trajectory_id, topic, trajectory_id, topic,
delayed_msg.instantiate<nav_msgs::Odometry>()); delayed_msg.instantiate<nav_msgs::Odometry>());
} }
rosgraph_msgs::Clock clock;
clock.clock = delayed_msg.getTime(); clock.clock = delayed_msg.getTime();
clock_publisher.publish(clock); clock_publisher.publish(clock);
::ros::spinOnce();
LOG_EVERY_N(INFO, 100000) LOG_EVERY_N(INFO, 100000)
<< "Processed " << (delayed_msg.getTime() - begin_time).toSec() << "Processed " << (delayed_msg.getTime() - begin_time).toSec()
<< " of " << duration_in_seconds << " bag time seconds..."; << " of " << duration_in_seconds << " bag time seconds...";
@ -197,9 +209,16 @@ void Run(const std::vector<string>& bag_filenames) {
} }
bag.close(); bag.close();
// Ensure the clock is republished also during trajectory finalization,
// which might take a while.
clock_republish_timer.start();
node.FinishTrajectory(trajectory_id); 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<std::chrono::steady_clock> end_time = const std::chrono::time_point<std::chrono::steady_clock> end_time =
std::chrono::steady_clock::now(); std::chrono::steady_clock::now();
const double wall_clock_seconds = const double wall_clock_seconds =
@ -216,6 +235,9 @@ void Run(const std::vector<string>& bag_filenames) {
#endif #endif
node.SerializeState(bag_filenames.front() + ".pbstream"); node.SerializeState(bag_filenames.front() + ".pbstream");
if (FLAGS_keep_running) {
::ros::waitForShutdown();
}
} }
} // namespace } // namespace