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-01-03 18:11:16 -08:00
parent 2f365ab7e1
commit 80ae448d1b
14 changed files with 493 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp
+13
View File
@@ -95,6 +95,14 @@ private:
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5);
void callbackRGBD6(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6);
protected:
virtual void flushCallbacks();
@@ -110,6 +118,7 @@ private:
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image3_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image4_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image5_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image6_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
@@ -131,6 +140,10 @@ private:
message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
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> MyExactSync5Policy;
message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync6Policy;
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_;
bool keepColor_;
};
rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp
+23
View File
@@ -84,6 +84,19 @@ private:
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4);
void callbackRGBD5(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5);
void callbackRGBD6(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6);
protected:
@@ -101,6 +114,8 @@ private:
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image2_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image3_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image4_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image5_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image6_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
@@ -118,6 +133,14 @@ private:
message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync4Policy;
message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync5Policy;
message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
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> MyExactSync5Policy;
message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync6Policy;
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_;
bool keepColor_;
rtabmap_odom/src/nodelets/icp_odometry.cpp
+1
View File
@@ -317,6 +317,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
*scanMsg,
scanOut,
this->tfBuffer(),
-1.0f,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull())
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+112 -4
View File
@@ -57,6 +57,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
exactSync4_(0),
approxSync5_(0),
exactSync5_(0),
approxSync6_(0),
exactSync6_(0),
queueSize_(5),
keepColor_(false)
{
@@ -75,6 +77,8 @@ RGBDOdometry::~RGBDOdometry()
delete exactSync4_;
delete approxSync5_;
delete exactSync5_;
delete approxSync6_;
delete exactSync6_;
}
void RGBDOdometry::onOdomInit()
@@ -93,10 +97,6 @@ void RGBDOdometry::onOdomInit()
{
rgbdCameras = 1;
}
if(rgbdCameras > 5)
{
RCLCPP_FATAL(this->get_logger(), "Only 5 cameras maximum supported yet. Set 0 to use rgbd_images input (for which rgbdx_sync node can sync up to 8 cameras).");
}
keepColor_ = this->declare_parameter("keep_color", keepColor_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
@@ -129,6 +129,10 @@ void RGBDOdometry::onOdomInit()
{
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).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());
}
if(rgbdCameras == 2)
{
@@ -256,6 +260,54 @@ void RGBDOdometry::onOdomInit()
rgbd_image4_sub_.getSubscriber()->get_topic_name(),
rgbd_image5_sub_.getSubscriber()->get_topic_name());
}
else if(rgbdCameras == 6)
{
if(approxSync)
{
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
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(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
else
{
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
exactSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
get_name(),
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
{
RCLCPP_FATAL(this->get_logger(),
"%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).",
get_name(), rgbdCameras);
}
}
else if(rgbdCameras == 0)
{
@@ -649,6 +701,36 @@ void RGBDOdometry::callbackRGBD5(
}
}
void RGBDOdometry::callbackRGBD6(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
{
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(6);
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(6);
std::vector<sensor_msgs::msg::CameraInfo> 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);
}
}
void RGBDOdometry::flushCallbacks()
{
// flush callbacks
@@ -748,6 +830,32 @@ void RGBDOdometry::flushCallbacks()
rgbd_image5_sub_);
exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
if(approxSync6_)
{
delete approxSync6_;
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
approxSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
if(exactSync6_)
{
delete exactSync6_;
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
exactSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
}
}
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+223 -1
View File
@@ -55,6 +55,10 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
exactSync3_(0),
approxSync4_(0),
exactSync4_(0),
approxSync5_(0),
exactSync5_(0),
approxSync6_(0),
exactSync6_(0),
queueSize_(5),
keepColor_(false)
{
@@ -65,6 +69,16 @@ StereoOdometry::~StereoOdometry()
{
delete approxSync_;
delete exactSync_;
delete approxSync2_;
delete exactSync2_;
delete approxSync3_;
delete exactSync3_;
delete approxSync4_;
delete exactSync4_;
delete approxSync5_;
delete exactSync5_;
delete approxSync6_;
delete exactSync6_;
}
void StereoOdometry::onOdomInit()
@@ -106,6 +120,14 @@ void StereoOdometry::onOdomInit()
{
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).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());
}
if(rgbdCameras >= 6)
{
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras == 2)
{
@@ -197,9 +219,89 @@ void StereoOdometry::onOdomInit()
rgbd_image3_sub_.getTopic().c_str(),
rgbd_image4_sub_.getTopic().c_str());
}
else if(rgbdCameras == 5)
{
if(approxSync)
{
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync5_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync5_->registerCallback(std::bind(&StereoOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
else
{
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
exactSync5_->registerCallback(std::bind(&StereoOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
get_name(),
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>(
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(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync6_->registerCallback(std::bind(&StereoOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
else
{
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
exactSync6_->registerCallback(std::bind(&StereoOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
get_name(),
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
{
RCLCPP_FATAL(this->get_logger(), "%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.", get_name(), rgbdCameras);
RCLCPP_FATAL(this->get_logger(),
"%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).",
get_name(), rgbdCameras);
}
}
@@ -735,6 +837,76 @@ void StereoOdometry::callbackRGBD4(
}
}
void StereoOdometry::callbackRGBD5(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
{
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(5);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(5);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> 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 StereoOdometry::callbackRGBD6(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
{
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(6);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(6);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> 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);
}
}
void StereoOdometry::flushCallbacks()
{
//flush callbacks
@@ -810,6 +982,56 @@ void StereoOdometry::flushCallbacks()
rgbd_image4_sub_);
exactSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
if(approxSync5_)
{
delete approxSync5_;
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
approxSync5_->registerCallback(std::bind(&StereoOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
if(exactSync5_)
{
delete exactSync5_;
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
exactSync5_->registerCallback(std::bind(&StereoOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
if(approxSync6_)
{
delete approxSync6_;
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
approxSync6_->registerCallback(std::bind(&StereoOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
if(exactSync6_)
{
delete exactSync6_;
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
exactSync6_->registerCallback(std::bind(&StereoOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
}
}