i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-09 11:17:03 +08:00
Code Issues Packages Projects Releases Wiki Activity

odom: fixed large delay, fixed map Tf frame published with wrong stamp.

Browse Source
This commit is contained in:
matlabbe
2022-10-01 11:22:26 -07:00
parent c0259f31a2
commit 4fb73b4279
28 changed files with 298 additions and 298 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/impl/CommonDataSubscriberRGBD6.cpp
+10 -10
View File
@@ -284,15 +284,15 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
for(int i=0; i<6; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(queueSize).reliability(qosImage_).get_rmw_qos_profile());
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
}
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -303,7 +303,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -314,7 +314,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -325,7 +325,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
}
else
@@ -338,7 +338,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -349,7 +349,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -360,7 +360,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -371,7 +371,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
}
else