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

Merge pull request #644 from ruipimentelfigueiredo/master

Browse Source 
Added support for 5 cameras to rgbd_odometry
This commit is contained in:
matlabbe
2021-09-20 16:20:06 -04:00
committed by GitHub
parent a80158642c 1e4ce5820e
commit f02851ae65
1 changed files with 99 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/rgbd_odometry.cpp
+99 -2
View File
@@ -69,6 +69,8 @@ public:
exactSync3_(0),
approxSync4_(0),
exactSync4_(0),
approxSync5_(0),
exactSync5_(0),
queueSize_(5),
keepColor_(false)
{
@@ -133,9 +135,9 @@ private:
{
rgbdCameras = 1;
}
if(rgbdCameras > 4)
if(rgbdCameras > 5)
{
NODELET_FATAL("Only 4 cameras maximum supported yet.");
NODELET_FATAL("Only 5 cameras maximum supported yet.");
}
pnh.param("keep_color", keepColor_, keepColor_);
@@ -160,6 +162,10 @@ private:
{
rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1);
}
if(rgbdCameras >= 5)
{
rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1);
}
if(rgbdCameras == 2)
{
@@ -242,6 +248,40 @@ private:
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_);
approxSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _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(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
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
{
@@ -553,6 +593,34 @@ private:
}
}
void callbackRGBD5(
const rtabmap_ros::RGBDImageConstPtr& image,
const rtabmap_ros::RGBDImageConstPtr& image2,
const rtabmap_ros::RGBDImageConstPtr& image3,
const rtabmap_ros::RGBDImageConstPtr& image4,
const rtabmap_ros::RGBDImageConstPtr& image5)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(5);
std::vector<sensor_msgs::CameraInfo> infoMsgs;
rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
rtabmap_ros::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
rtabmap_ros::toCvShare(image5, imageMsgs[4], depthMsgs[4]);
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);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
}
}
protected:
virtual void flushCallbacks()
{
@@ -629,6 +697,30 @@ protected:
rgbd_image4_sub_);
exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _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(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _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(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5));
}
}
private:
@@ -641,6 +733,7 @@ private:
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image2_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image3_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image4_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image5_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
@@ -658,6 +751,10 @@ private:
message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync4Policy;
message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync5Policy;
message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync5Policy;
message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
int queueSize_;
bool keepColor_;
};