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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2024-06-12 22:51:30 -07:00
parent 911e4e8941 ae44e1a215
commit d3d874c862
35 changed files with 857 additions and 771 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp
+2 -1
View File
@@ -148,7 +148,8 @@ private:
message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync6Policy;
message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
int queueSize_;
int topicQueueSize_;
int syncQueueSize_;
bool keepColor_;
};
rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp
+2 -1
View File
@@ -146,7 +146,8 @@ private:
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync6Policy;
message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
int queueSize_;
int topicQueueSize_;
int syncQueueSize_;
bool keepColor_;
};
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+51 -39
View File
@@ -63,7 +63,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
exactSync5_(0),
approxSync6_(0),
exactSync6_(0),
queueSize_(5),
topicQueueSize_(1),
syncQueueSize_(5),
keepColor_(false)
{
OdometryROS::init(false, true, false);
@@ -93,7 +94,17 @@ void RGBDOdometry::onOdomInit()
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize_ = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize_);
}
syncQueueSize_ = this->declare_parameter("sync_queue_size", syncQueueSize_);
int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
@@ -106,8 +117,9 @@ void RGBDOdometry::onOdomInit()
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: queue_size = %d", queueSize_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos());
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: topic_queue_size = %d", topicQueueSize_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: sync_queue_size = %d", syncQueueSize_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos());
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos_camera_info = %d", qosCamInfo);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
@@ -119,23 +131,23 @@ void RGBDOdometry::onOdomInit()
{
if(rgbdCameras >= 2)
{
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).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());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
if(rgbdCameras >= 3)
{
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).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());
}
if(rgbdCameras >= 4)
{
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).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());
}
if(rgbdCameras >= 5)
{
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).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());
}
if(rgbdCameras >= 6)
{
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(1).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());
}
if(rgbdCameras == 2)
@@ -143,7 +155,7 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0)
@@ -153,7 +165,7 @@ void RGBDOdometry::onOdomInit()
else
{
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
MyExactSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -170,7 +182,7 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
MyApproxSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -181,7 +193,7 @@ void RGBDOdometry::onOdomInit()
else
{
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
MyExactSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -200,7 +212,7 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_),
MyApproxSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -212,7 +224,7 @@ void RGBDOdometry::onOdomInit()
else
{
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_),
MyExactSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -233,7 +245,7 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
MyApproxSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -246,7 +258,7 @@ void RGBDOdometry::onOdomInit()
else
{
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
MyExactSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -269,7 +281,7 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
MyApproxSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -283,7 +295,7 @@ void RGBDOdometry::onOdomInit()
else
{
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
MyExactSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -315,7 +327,7 @@ void RGBDOdometry::onOdomInit()
}
else if(rgbdCameras == 0)
{
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).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));
subscribedTopic = rgbdxSub_->get_topic_name();
subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s",
@@ -324,7 +336,7 @@ void RGBDOdometry::onOdomInit()
}
else
{
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).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));
subscribedTopic = rgbdSub_->get_topic_name();
subscribedTopicsMsg =
@@ -336,20 +348,20 @@ void RGBDOdometry::onOdomInit()
else
{
image_transport::TransportHints hints(this);
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).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());
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());
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
@@ -741,20 +753,20 @@ void RGBDOdometry::flushCallbacks()
if(approxSync_)
{
delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
if(exactSync_)
{
delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
if(approxSync2_)
{
delete approxSync2_;
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -763,7 +775,7 @@ void RGBDOdometry::flushCallbacks()
{
delete exactSync2_;
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
MyExactSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -772,7 +784,7 @@ void RGBDOdometry::flushCallbacks()
{
delete approxSync3_;
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
MyApproxSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -782,7 +794,7 @@ void RGBDOdometry::flushCallbacks()
{
delete exactSync3_;
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
MyExactSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -792,7 +804,7 @@ void RGBDOdometry::flushCallbacks()
{
delete approxSync4_;
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_),
MyApproxSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -803,7 +815,7 @@ void RGBDOdometry::flushCallbacks()
{
delete exactSync4_;
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_),
MyExactSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -814,7 +826,7 @@ void RGBDOdometry::flushCallbacks()
{
delete approxSync5_;
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
MyApproxSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -826,7 +838,7 @@ void RGBDOdometry::flushCallbacks()
{
delete exactSync5_;
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
MyExactSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -838,7 +850,7 @@ void RGBDOdometry::flushCallbacks()
{
delete approxSync6_;
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
MyApproxSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -851,7 +863,7 @@ void RGBDOdometry::flushCallbacks()
{
delete exactSync6_;
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
MyExactSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+53 -41
View File
@@ -63,7 +63,8 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
exactSync5_(0),
approxSync6_(0),
exactSync6_(0),
queueSize_(5),
topicQueueSize_(1),
syncQueueSize_(5),
keepColor_(false)
{
OdometryROS::init(true, true, false);
@@ -93,7 +94,17 @@ void StereoOdometry::onOdomInit()
int rgbdCameras = 1;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize_ = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize_);
}
syncQueueSize_ = this->declare_parameter("sync_queue_size", syncQueueSize_);
int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
@@ -102,9 +113,10 @@ void StereoOdometry::onOdomInit()
RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: queue_size = %d", queueSize_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos());
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos_camera_info = %d", qosCamInfo);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: topic_queue_size = %d", topicQueueSize_);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: sync_queue_size = %d", syncQueueSize_);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: qos = %d", (int)qos());
RCLCPP_INFO(this->get_logger(), "StereoOdometry: qos_camera_info = %d", qosCamInfo);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
RCLCPP_INFO(this->get_logger(), "StereoOdometry: keep_color = %s", keepColor_?"true":"false");
@@ -114,23 +126,23 @@ void StereoOdometry::onOdomInit()
{
if(rgbdCameras >= 2)
{
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).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());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
if(rgbdCameras >= 3)
{
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).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());
}
if(rgbdCameras >= 4)
{
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).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());
}
if(rgbdCameras >= 5)
{
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).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());
}
if(rgbdCameras >= 6)
{
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(1).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());
}
if(rgbdCameras == 2)
@@ -138,7 +150,7 @@ void StereoOdometry::onOdomInit()
if(approxSync)
{
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0)
@@ -148,7 +160,7 @@ void StereoOdometry::onOdomInit()
else
{
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
MyExactSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -165,7 +177,7 @@ void StereoOdometry::onOdomInit()
if(approxSync)
{
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
MyApproxSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -176,7 +188,7 @@ void StereoOdometry::onOdomInit()
else
{
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
MyExactSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -195,7 +207,7 @@ void StereoOdometry::onOdomInit()
if(approxSync)
{
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_),
MyApproxSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -207,7 +219,7 @@ void StereoOdometry::onOdomInit()
else
{
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_),
MyExactSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -228,7 +240,7 @@ void StereoOdometry::onOdomInit()
if(approxSync)
{
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
MyApproxSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -241,7 +253,7 @@ void StereoOdometry::onOdomInit()
else
{
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
MyExactSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -264,7 +276,7 @@ void StereoOdometry::onOdomInit()
if(approxSync)
{
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
MyApproxSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -278,7 +290,7 @@ void StereoOdometry::onOdomInit()
else
{
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
MyExactSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -311,7 +323,7 @@ void StereoOdometry::onOdomInit()
}
else if(rgbdCameras == 0)
{
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).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));
subscribedTopic = rgbdxSub_->get_topic_name();
subscribedTopicsMsg =
@@ -321,7 +333,7 @@ void StereoOdometry::onOdomInit()
}
else
{
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).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));
subscribedTopic = rgbdSub_->get_topic_name();
subscribedTopicsMsg =
@@ -333,21 +345,21 @@ void StereoOdometry::onOdomInit()
else
{
image_transport::TransportHints hints(this);
imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(1).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());
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());
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
@@ -917,20 +929,20 @@ void StereoOdometry::flushCallbacks()
if(approxSync_)
{
delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
if(exactSync_)
{
delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
if(approxSync2_)
{
delete approxSync2_;
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
approxSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -939,7 +951,7 @@ void StereoOdometry::flushCallbacks()
{
delete exactSync2_;
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
MyExactSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -948,7 +960,7 @@ void StereoOdometry::flushCallbacks()
{
delete approxSync3_;
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
MyApproxSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -958,7 +970,7 @@ void StereoOdometry::flushCallbacks()
{
delete exactSync3_;
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
MyExactSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -968,7 +980,7 @@ void StereoOdometry::flushCallbacks()
{
delete approxSync4_;
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_),
MyApproxSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -979,7 +991,7 @@ void StereoOdometry::flushCallbacks()
{
delete exactSync4_;
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_),
MyExactSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -990,7 +1002,7 @@ void StereoOdometry::flushCallbacks()
{
delete approxSync5_;
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
MyApproxSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -1002,7 +1014,7 @@ void StereoOdometry::flushCallbacks()
{
delete exactSync5_;
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
MyExactSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -1014,7 +1026,7 @@ void StereoOdometry::flushCallbacks()
{
delete approxSync6_;
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
MyApproxSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -1027,7 +1039,7 @@ void StereoOdometry::flushCallbacks()
{
delete exactSync6_;
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
MyExactSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,