From 64a2656052ae4afd41acb05ab99cea0031d439e2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 21 Nov 2023 14:01:45 -0800 Subject: [PATCH] Odom: Added 5 and 6 cameras internal sync callbacks for convenience. --- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 169 ++++++++---- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 244 +++++++++++++++++- 2 files changed, 360 insertions(+), 53 deletions(-) diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 1cb190897..f1932c7dc 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -72,6 +72,8 @@ class RGBDOdometry : public OdometryROS exactSync4_(0), approxSync5_(0), exactSync5_(0), + approxSync6_(0), + exactSync6_(0), queueSize_(5), keepColor_(false) { @@ -81,46 +83,18 @@ class RGBDOdometry : public OdometryROS { rgbdSub_.shutdown(); rgbdxSub_.shutdown(); - if(approxSync_) - { - delete approxSync_; - } - if(exactSync_) - { - delete exactSync_; - } - if(approxSync2_) - { - delete approxSync2_; - } - if(exactSync2_) - { - delete exactSync2_; - } - if(approxSync3_) - { - delete approxSync3_; - } - if(exactSync3_) - { - delete exactSync3_; - } - if(approxSync4_) - { - delete approxSync4_; - } - if(exactSync4_) - { - delete exactSync4_; - } - if(approxSync5_) - { - delete approxSync5_; - } - if(exactSync5_) - { - delete exactSync5_; - } + delete approxSync_; + delete exactSync_; + delete approxSync2_; + delete exactSync2_; + delete approxSync3_; + delete exactSync3_; + delete approxSync4_; + delete exactSync4_; + delete approxSync5_; + delete exactSync5_; + delete approxSync6_; + delete exactSync6_; } private: @@ -147,10 +121,6 @@ class RGBDOdometry : public OdometryROS { rgbdCameras = 0; } - if(rgbdCameras > 5) - { - NODELET_FATAL("Only 5 cameras maximum supported yet. Set 0 to use rgbd_images input (for which rgbdx_sync node can sync up to 8 cameras)."); - } pnh.param("keep_color", keepColor_, keepColor_); NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); @@ -181,6 +151,10 @@ class RGBDOdometry : public OdometryROS { rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1); } + if(rgbdCameras >= 6) + { + rgbd_image6_sub_.subscribe(nh, "rgbd_image5", 1); + } if(rgbdCameras == 2) { @@ -308,6 +282,53 @@ class RGBDOdometry : public OdometryROS rgbd_image4_sub_.getTopic().c_str(), rgbd_image5_sub_.getTopic().c_str()); } + else if(rgbdCameras == 6) + { + if(approxSync) + { + approxSync6_ = new message_filters::Synchronizer( + MyApproxSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + if(approxSyncMaxInterval > 0.0) + approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + approxSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } + else + { + exactSync6_ = new message_filters::Synchronizer( + MyExactSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + exactSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", + getName().c_str(), + approxSync?"approx":"exact", + approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + rgbd_image1_sub_.getTopic().c_str(), + rgbd_image2_sub_.getTopic().c_str(), + rgbd_image3_sub_.getTopic().c_str(), + rgbd_image4_sub_.getTopic().c_str(), + rgbd_image5_sub_.getTopic().c_str(), + rgbd_image6_sub_.getTopic().c_str()); + } + else + { + NODELET_FATAL("%s doesn't support more than 6 cameras (rgbd_cameras=%d) with " + "internal synchronization interface, set rgbd_cameras=0 and use " + "rgbd_images input topic instead for more cameras (for which " + "rgbdx_sync node can sync up to 8 cameras).", + getName().c_str(), rgbdCameras); + } } else if(rgbdCameras == 0) @@ -700,6 +721,35 @@ class RGBDOdometry : public OdometryROS this->commonCallback(imageMsgs, depthMsgs, infoMsgs); } } + void callbackRGBD6( + const rtabmap_msgs::RGBDImageConstPtr& image, + const rtabmap_msgs::RGBDImageConstPtr& image2, + const rtabmap_msgs::RGBDImageConstPtr& image3, + const rtabmap_msgs::RGBDImageConstPtr& image4, + const rtabmap_msgs::RGBDImageConstPtr& image5, + const rtabmap_msgs::RGBDImageConstPtr& image6) + { + if(!this->isPaused()) + { + std::vector imageMsgs(6); + std::vector depthMsgs(6); + std::vector infoMsgs; + rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]); + rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]); + rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]); + rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]); + rtabmap_conversions::toCvShare(image5, imageMsgs[4], depthMsgs[4]); + rtabmap_conversions::toCvShare(image6, imageMsgs[5], depthMsgs[5]); + infoMsgs.push_back(image->rgb_camera_info); + infoMsgs.push_back(image2->rgb_camera_info); + infoMsgs.push_back(image3->rgb_camera_info); + infoMsgs.push_back(image4->rgb_camera_info); + infoMsgs.push_back(image5->rgb_camera_info); + infoMsgs.push_back(image6->rgb_camera_info); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + } + } protected: virtual void flushCallbacks() @@ -801,6 +851,32 @@ class RGBDOdometry : public OdometryROS rgbd_image5_sub_); exactSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5)); } + if(approxSync6_) + { + delete approxSync6_; + approxSync6_ = new message_filters::Synchronizer( + MyApproxSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + approxSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } + if(exactSync6_) + { + delete exactSync6_; + exactSync6_ = new message_filters::Synchronizer( + MyExactSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + exactSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } } private: @@ -815,6 +891,7 @@ class RGBDOdometry : public OdometryROS message_filters::Subscriber rgbd_image3_sub_; message_filters::Subscriber rgbd_image4_sub_; message_filters::Subscriber rgbd_image5_sub_; + message_filters::Subscriber rgbd_image6_sub_; typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; message_filters::Synchronizer * approxSync_; @@ -836,6 +913,10 @@ class RGBDOdometry : public OdometryROS message_filters::Synchronizer * approxSync5_; typedef message_filters::sync_policies::ExactTime MyExactSync5Policy; message_filters::Synchronizer * exactSync5_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSync6Policy; + message_filters::Synchronizer * approxSync6_; + typedef message_filters::sync_policies::ExactTime MyExactSync6Policy; + message_filters::Synchronizer * exactSync6_; int queueSize_; bool keepColor_; }; diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index 6d65f2f91..889de3656 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -70,6 +70,10 @@ class StereoOdometry : public OdometryROS exactSync3_(0), approxSync4_(0), exactSync4_(0), + approxSync5_(0), + exactSync5_(0), + approxSync6_(0), + exactSync6_(0), queueSize_(5), keepColor_(false) { @@ -77,14 +81,20 @@ class StereoOdometry : public OdometryROS virtual ~StereoOdometry() { - if(approxSync_) - { - delete approxSync_; - } - if(exactSync_) - { - delete exactSync_; - } + rgbdSub_.shutdown(); + rgbdxSub_.shutdown(); + delete approxSync_; + delete exactSync_; + delete approxSync2_; + delete exactSync2_; + delete approxSync3_; + delete exactSync3_; + delete approxSync4_; + delete exactSync4_; + delete approxSync5_; + delete exactSync5_; + delete approxSync6_; + delete exactSync6_; } private: @@ -127,6 +137,14 @@ class StereoOdometry : public OdometryROS { rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1); } + if(rgbdCameras >= 5) + { + rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1); + } + if(rgbdCameras >= 6) + { + rgbd_image6_sub_.subscribe(nh, "rgbd_image5", 1); + } if(rgbdCameras == 2) { @@ -218,9 +236,88 @@ class StereoOdometry : public OdometryROS rgbd_image3_sub_.getTopic().c_str(), rgbd_image4_sub_.getTopic().c_str()); } + else if(rgbdCameras == 5) + { + if(approxSync) + { + approxSync5_ = new message_filters::Synchronizer( + MyApproxSync5Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_); + if(approxSyncMaxInterval > 0.0) + approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + approxSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5)); + } + else + { + exactSync5_ = new message_filters::Synchronizer( + MyExactSync5Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_); + exactSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", + getName().c_str(), + approxSync?"approx":"exact", + approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + rgbd_image1_sub_.getTopic().c_str(), + rgbd_image2_sub_.getTopic().c_str(), + rgbd_image3_sub_.getTopic().c_str(), + rgbd_image4_sub_.getTopic().c_str(), + rgbd_image5_sub_.getTopic().c_str()); + } + else if(rgbdCameras == 6) + { + if(approxSync) + { + approxSync6_ = new message_filters::Synchronizer( + MyApproxSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + if(approxSyncMaxInterval > 0.0) + approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + approxSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } + else + { + exactSync6_ = new message_filters::Synchronizer( + MyExactSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + exactSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", + getName().c_str(), + approxSync?"approx":"exact", + approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + rgbd_image1_sub_.getTopic().c_str(), + rgbd_image2_sub_.getTopic().c_str(), + rgbd_image3_sub_.getTopic().c_str(), + rgbd_image4_sub_.getTopic().c_str(), + rgbd_image5_sub_.getTopic().c_str(), + rgbd_image6_sub_.getTopic().c_str()); + } else { - ROS_FATAL("%s doesn't support more than 4 cameras (rgbd_cameras=%d) with internal synchronization interface, set rgbd_cameras=0 and use rgbd_images input topic instead for more cameras.", getName().c_str(), rgbdCameras); + NODELET_FATAL("%s doesn't support more than 6 cameras (rgbd_cameras=%d) with " + "internal synchronization interface, set rgbd_cameras=0 and use " + "rgbd_images input topic instead for more cameras (for which " + "rgbdx_sync node can sync up to 8 cameras).", + getName().c_str(), rgbdCameras); } } @@ -723,6 +820,76 @@ class StereoOdometry : public OdometryROS } } + void callbackRGBD5( + const rtabmap_msgs::RGBDImageConstPtr& image, + const rtabmap_msgs::RGBDImageConstPtr& image2, + const rtabmap_msgs::RGBDImageConstPtr& image3, + const rtabmap_msgs::RGBDImageConstPtr& image4, + const rtabmap_msgs::RGBDImageConstPtr& image5) + { + if(!this->isPaused()) + { + std::vector leftMsgs(5); + std::vector rightMsgs(5); + std::vector leftInfoMsgs; + std::vector rightInfoMsgs; + rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]); + rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]); + rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]); + rtabmap_conversions::toCvShare(image4, leftMsgs[3], rightMsgs[3]); + rtabmap_conversions::toCvShare(image5, leftMsgs[4], rightMsgs[4]); + leftInfoMsgs.push_back(image->rgb_camera_info); + leftInfoMsgs.push_back(image2->rgb_camera_info); + leftInfoMsgs.push_back(image3->rgb_camera_info); + leftInfoMsgs.push_back(image4->rgb_camera_info); + leftInfoMsgs.push_back(image5->rgb_camera_info); + rightInfoMsgs.push_back(image->depth_camera_info); + rightInfoMsgs.push_back(image2->depth_camera_info); + rightInfoMsgs.push_back(image3->depth_camera_info); + rightInfoMsgs.push_back(image4->depth_camera_info); + rightInfoMsgs.push_back(image5->depth_camera_info); + + this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs); + } + } + + void callbackRGBD6( + const rtabmap_msgs::RGBDImageConstPtr& image, + const rtabmap_msgs::RGBDImageConstPtr& image2, + const rtabmap_msgs::RGBDImageConstPtr& image3, + const rtabmap_msgs::RGBDImageConstPtr& image4, + const rtabmap_msgs::RGBDImageConstPtr& image5, + const rtabmap_msgs::RGBDImageConstPtr& image6) + { + if(!this->isPaused()) + { + std::vector leftMsgs(6); + std::vector rightMsgs(6); + std::vector leftInfoMsgs; + std::vector rightInfoMsgs; + rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]); + rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]); + rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]); + rtabmap_conversions::toCvShare(image4, leftMsgs[3], rightMsgs[3]); + rtabmap_conversions::toCvShare(image5, leftMsgs[4], rightMsgs[4]); + rtabmap_conversions::toCvShare(image6, leftMsgs[5], rightMsgs[5]); + leftInfoMsgs.push_back(image->rgb_camera_info); + leftInfoMsgs.push_back(image2->rgb_camera_info); + leftInfoMsgs.push_back(image3->rgb_camera_info); + leftInfoMsgs.push_back(image4->rgb_camera_info); + leftInfoMsgs.push_back(image5->rgb_camera_info); + leftInfoMsgs.push_back(image6->rgb_camera_info); + rightInfoMsgs.push_back(image->depth_camera_info); + rightInfoMsgs.push_back(image2->depth_camera_info); + rightInfoMsgs.push_back(image3->depth_camera_info); + rightInfoMsgs.push_back(image4->depth_camera_info); + rightInfoMsgs.push_back(image5->depth_camera_info); + rightInfoMsgs.push_back(image6->depth_camera_info); + + this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs); + } + } + protected: virtual void flushCallbacks() { @@ -799,6 +966,56 @@ class StereoOdometry : public OdometryROS rgbd_image4_sub_); exactSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } + if(approxSync5_) + { + delete approxSync5_; + approxSync5_ = new message_filters::Synchronizer( + MyApproxSync5Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_); + approxSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5)); + } + if(exactSync5_) + { + delete exactSync5_; + exactSync5_ = new message_filters::Synchronizer( + MyExactSync5Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_); + exactSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5)); + } + if(approxSync6_) + { + delete approxSync6_; + approxSync6_ = new message_filters::Synchronizer( + MyApproxSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + approxSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } + if(exactSync6_) + { + delete exactSync6_; + exactSync6_ = new message_filters::Synchronizer( + MyExactSync6Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_, + rgbd_image6_sub_); + exactSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); + } } private: @@ -814,6 +1031,7 @@ class StereoOdometry : public OdometryROS message_filters::Subscriber rgbd_image3_sub_; message_filters::Subscriber rgbd_image4_sub_; message_filters::Subscriber rgbd_image5_sub_; + message_filters::Subscriber rgbd_image6_sub_; typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; message_filters::Synchronizer * approxSync_; @@ -831,6 +1049,14 @@ class StereoOdometry : public OdometryROS message_filters::Synchronizer * approxSync4_; typedef message_filters::sync_policies::ExactTime MyExactSync4Policy; message_filters::Synchronizer * exactSync4_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSync5Policy; + message_filters::Synchronizer * approxSync5_; + typedef message_filters::sync_policies::ExactTime MyExactSync5Policy; + message_filters::Synchronizer * exactSync5_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSync6Policy; + message_filters::Synchronizer * approxSync6_; + typedef message_filters::sync_policies::ExactTime MyExactSync6Policy; + message_filters::Synchronizer * exactSync6_; int queueSize_; bool keepColor_;