From 4cdd88e968fa1fbc8f6afb1a098648b1fb2c6500 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20C=2E=20Monteiro?= Date: Thu, 19 Nov 2020 11:46:42 +0100 Subject: [PATCH] add the publishers' queue length as a parameter when replaying offline Having a small queue can lead to message drops if the algorithm is not able to process the messages as fast as the publisher publishes them --- src/offline_replay.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/offline_replay.cpp b/src/offline_replay.cpp index 3b1bfd1..6437ea9 100644 --- a/src/offline_replay.cpp +++ b/src/offline_replay.cpp @@ -47,6 +47,8 @@ void lama::ReplayRosbag(ros::NodeHandle& pnh, const std::string& rosbag_filename { std::string scan_topic; pnh.param("scan_topic", scan_topic, std::string("/scan")); + int queue_length; + pnh.param("queue_length", queue_length, 100); ROS_INFO("Scan topic: %s", scan_topic.c_str()); @@ -59,10 +61,10 @@ void lama::ReplayRosbag(ros::NodeHandle& pnh, const std::string& rosbag_filename ROS_FATAL("Unable to open rosbag [%s]: %s", rosbag_filename.c_str(), ex.what()); } - auto pub_scan = pnh.advertise(scan_topic,10); - auto pub_tf = pnh.advertise("/tf",10); - auto pub_tf_static = pnh.advertise("/tf_static",10, true); - auto pub_clock = pnh.advertise("/clock",10); + auto pub_scan = pnh.advertise(scan_topic, queue_length); + auto pub_tf = pnh.advertise("/tf", queue_length); + auto pub_tf_static = pnh.advertise("/tf_static", queue_length, true); + auto pub_clock = pnh.advertise("/clock", queue_length); ROS_INFO("Allow time for the subscribers to connect"); ros::WallDuration wait(2); wait.sleep();