mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-06 17:57:45 +08:00
Fixing topic sync lagging and delay issues, more examples (depthai, zed) (#1206)
* 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 <[email protected]>
This commit is contained in:
matlabbe
@@ -97,8 +97,11 @@ void ICPOdometry::onOdomInit()
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing = %s", deskewing_?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp = %s", deskewingSlerp_?"true":"false");
|
||||
|
||||
scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1));
|
||||
cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1));
|
||||
rclcpp::SubscriptionOptions options;
|
||||
options.callback_group = dataCallbackGroup_;
|
||||
|
||||
scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options);
|
||||
cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options);
|
||||
|
||||
filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()));
|
||||
|
||||
|
||||
@@ -63,8 +63,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
|
||||
exactSync5_(0),
|
||||
approxSync6_(0),
|
||||
exactSync6_(0),
|
||||
topicQueueSize_(1),
|
||||
syncQueueSize_(5),
|
||||
topicQueueSize_(10),
|
||||
syncQueueSize_(2),
|
||||
keepColor_(false)
|
||||
{
|
||||
OdometryROS::init(false, true, false);
|
||||
@@ -125,29 +125,32 @@ void RGBDOdometry::onOdomInit()
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color = %s", keepColor_?"true":"false");
|
||||
|
||||
rclcpp::SubscriptionOptions options;
|
||||
options.callback_group = dataCallbackGroup_;
|
||||
|
||||
std::string subscribedTopic;
|
||||
std::string subscribedTopicsMsg;
|
||||
if(subscribeRGBD)
|
||||
{
|
||||
if(rgbdCameras >= 2)
|
||||
{
|
||||
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
if(rgbdCameras >= 3)
|
||||
{
|
||||
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 4)
|
||||
{
|
||||
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 5)
|
||||
{
|
||||
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 6)
|
||||
{
|
||||
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
|
||||
if(rgbdCameras == 2)
|
||||
@@ -327,7 +330,7 @@ void RGBDOdometry::onOdomInit()
|
||||
}
|
||||
else if(rgbdCameras == 0)
|
||||
{
|
||||
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1));
|
||||
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1), options);
|
||||
|
||||
subscribedTopic = rgbdxSub_->get_topic_name();
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s",
|
||||
@@ -336,7 +339,7 @@ void RGBDOdometry::onOdomInit()
|
||||
}
|
||||
else
|
||||
{
|
||||
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
|
||||
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1), options);
|
||||
|
||||
subscribedTopic = rgbdSub_->get_topic_name();
|
||||
subscribedTopicsMsg =
|
||||
@@ -348,9 +351,9 @@ void RGBDOdometry::onOdomInit()
|
||||
else
|
||||
{
|
||||
image_transport::TransportHints hints(this);
|
||||
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options);
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
@@ -366,10 +369,12 @@ void RGBDOdometry::onOdomInit()
|
||||
}
|
||||
|
||||
subscribedTopic = image_mono_sub_.getSubscriber().getTopic();
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s, topic_queue_size=%d, sync_queue_size=%d):\n %s,\n %s,\n %s",
|
||||
get_name(),
|
||||
approxSync?"approx":"exact",
|
||||
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
topicQueueSize_,
|
||||
syncQueueSize_,
|
||||
image_mono_sub_.getSubscriber().getTopic().c_str(),
|
||||
image_depth_sub_.getSubscriber().getTopic().c_str(),
|
||||
info_sub_.getSubscriber()->get_topic_name());
|
||||
|
||||
@@ -63,8 +63,8 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
|
||||
exactSync5_(0),
|
||||
approxSync6_(0),
|
||||
exactSync6_(0),
|
||||
topicQueueSize_(1),
|
||||
syncQueueSize_(5),
|
||||
topicQueueSize_(10),
|
||||
syncQueueSize_(2),
|
||||
keepColor_(false)
|
||||
{
|
||||
OdometryROS::init(true, true, false);
|
||||
@@ -120,29 +120,32 @@ void StereoOdometry::onOdomInit()
|
||||
RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "StereoOdometry: keep_color = %s", keepColor_?"true":"false");
|
||||
|
||||
rclcpp::SubscriptionOptions options;
|
||||
options.callback_group = dataCallbackGroup_;
|
||||
|
||||
std::string subscribedTopic;
|
||||
std::string subscribedTopicsMsg;
|
||||
if(subscribeRGBD)
|
||||
{
|
||||
if(rgbdCameras >= 2)
|
||||
{
|
||||
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
if(rgbdCameras >= 3)
|
||||
{
|
||||
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 4)
|
||||
{
|
||||
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 5)
|
||||
{
|
||||
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 6)
|
||||
{
|
||||
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
|
||||
if(rgbdCameras == 2)
|
||||
@@ -323,7 +326,7 @@ void StereoOdometry::onOdomInit()
|
||||
}
|
||||
else if(rgbdCameras == 0)
|
||||
{
|
||||
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1));
|
||||
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1), options);
|
||||
|
||||
subscribedTopic = rgbdxSub_->get_topic_name();
|
||||
subscribedTopicsMsg =
|
||||
@@ -333,7 +336,7 @@ void StereoOdometry::onOdomInit()
|
||||
}
|
||||
else
|
||||
{
|
||||
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
|
||||
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1), options);
|
||||
|
||||
subscribedTopic = rgbdSub_->get_topic_name();
|
||||
subscribedTopicsMsg =
|
||||
@@ -345,10 +348,10 @@ void StereoOdometry::onOdomInit()
|
||||
else
|
||||
{
|
||||
image_transport::TransportHints hints(this);
|
||||
imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options);
|
||||
cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options);
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
@@ -364,10 +367,12 @@ void StereoOdometry::onOdomInit()
|
||||
}
|
||||
|
||||
subscribedTopic = imageRectLeft_.getTopic();
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s, topic_queue_size=%d, sync_queue_size=%d):\n %s \\\n %s \\\n %s \\\n %s",
|
||||
get_name(),
|
||||
approxSync?"approx":"exact",
|
||||
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
topicQueueSize_,
|
||||
syncQueueSize_,
|
||||
imageRectLeft_.getTopic().c_str(),
|
||||
imageRectRight_.getTopic().c_str(),
|
||||
cameraInfoLeft_.getSubscriber()->get_topic_name(),
|
||||
|
||||
Reference in New Issue
Block a user