i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-12 14:20:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixing topic sync lagging and delay issues, more examples (depthai, zed) (#1206)

Browse Source 
* Fixed odometry latency

* Fixed node ID=0 issues as msgs may not have seq. (#1202)

(cherry picked from commit 097cab0667)

* Odom: removed long processing from ros callbacks (decreasing delay, also making delay independent of the message filters topic_queue_size and sync_queue_size parameters)

* Changed a log from info->debug

* Changed a log from info->debug

* Updated default topic and sync queue_size inside nodes. Exposing topic and sync queue size params in warning when cannot synchronize. Added zed and depthai examples. Odom: Fixed imu callback group, added multi-threaded executors for all odometry nodes.

* fixed merge

* Include everything needed in example launch files for simple launch. Small fixes.

---------

Co-authored-by: Borong Yuan <yuanborong@hotmail.com>
This commit is contained in:
matlabbe
2024-09-04 00:46:44 -07:00
committed by GitHub
co-authored by Borong Yuan
parent 268609b0b7
commit c956e3780f
27 changed files with 465 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_sync/src/CommonDataSubscriber.cpp
+7 -3
View File
@@ -31,8 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync {
CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
topicQueueSize_(1),
syncQueueSize_(10),
topicQueueSize_(10),
syncQueueSize_(2),
approxSync_(true),
subscribedToDepth_(!gui),
subscribedToStereo_(false),
@@ -699,8 +699,12 @@ void CommonDataSubscriber::setupCallbacks(
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
"header are set. If topics are coming from different computers, make sure "
"the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
"the clocks of the computers are synchronized (\"ntpdate\"). Ajusting "
"topic_queue_size (%d) and sync_queue_size (%d) can also help for better "
"synchronization if framerates and/or delays are different. %s%s",
name_.c_str(),
topicQueueSize_,
syncQueueSize_,
approxSync_?
uFormat("If topics are not published at the same rate, you could increase \"sync_queue_size\" and/or \"topic_queue_size\" parameters (current=%d and %d respectively).", syncQueueSize_, topicQueueSize_).c_str():
"Parameter \"approx_sync\" is false, which means that input topics should have all the exact timestamp for the callback to be called.",
rtabmap_sync/src/nodelets/rgb_sync.cpp
+6 -3
View File
@@ -51,8 +51,8 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
approxSync_(0),
exactSync_(0)
{
int topicQueueSize = 1;
int syncQueueSize = 10;
int topicQueueSize = 10;
int syncQueueSize = 2;
bool approxSync = true;
int qos = 0;
double approxSyncMaxInterval = 0.0;
@@ -115,8 +115,11 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
syncDiagnostic_->init(imageSub_.getSubscriber().getTopic(),
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
"header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) "
"can also help for better synchronization if framerates and/or delays are different. %s%s",
this->get_name(),
topicQueueSize,
syncQueueSize,
approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg.c_str()));
rtabmap_sync/src/nodelets/rgbd_sync.cpp
+6 -3
View File
@@ -53,8 +53,8 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
approxSyncDepth_(0),
exactSyncDepth_(0)
{
int topicQueueSize = 1;
int syncQueueSize = 10;
int topicQueueSize = 10;
int syncQueueSize = 2;
bool approxSync = true;
double approxSyncMaxInterval = 0.0;
int qos = 0;
@@ -128,8 +128,11 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
syncDiagnostic_->init(imageSub_.getSubscriber().getTopic(),
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
"header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) "
"can also help for better synchronization if framerates and/or delays are different. %s%s",
get_name(),
topicQueueSize,
syncQueueSize,
approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg.c_str()));
rtabmap_sync/src/nodelets/rgbdx_sync.cpp
+6 -3
View File
@@ -42,8 +42,8 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
SYNC_INIT(rgbd7),
SYNC_INIT(rgbd8)
{
int topicQueueSize = 1;
int syncQueueSize = 10;
int topicQueueSize = 10;
int syncQueueSize = 2;
bool approxSync = true;
int rgbdCameras = 2;
double approxSyncMaxInterval = 0.0;
@@ -151,8 +151,11 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
syncDiagnostic_->init("",
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
"header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) "
"can also help for better synchronization if framerates and/or delays are different.%s%s",
get_name(),
topicQueueSize,
syncQueueSize,
approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg.c_str()));
rtabmap_sync/src/nodelets/stereo_sync.cpp
+6 -3
View File
@@ -50,8 +50,8 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
approxSync_(0),
exactSync_(0)
{
int topicQueueSize = 1;
int syncQueueSize = 10;
int topicQueueSize = 10;
int syncQueueSize = 2;
bool approxSync = false;
double approxSyncMaxInterval = 0.0;
int qos = 0;
@@ -117,8 +117,11 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
syncDiagnostic_->init(imageLeftSub_.getSubscriber().getTopic(),
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
"header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) "
"can also help for better synchronization if framerates and/or delays are different.%s%s",
get_name(),
topicQueueSize,
syncQueueSize,
approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg.c_str()));