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

Added RGBDImages msg. Added rgbdx_sync to sync up to 8 cameras. For rtabmap node, rgbd_cameras=0 means subscribing to a rgbd_images topic (containing N cameras).

Browse Source
This commit is contained in:
matlabbe
2021-08-02 17:47:30 -04:00
parent d2465851de
commit 523b001ca3
22 changed files with 1265 additions and 230 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/CommonDataSubscriber.h
+44
View File
@@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <nav_msgs/Odometry.h>
#include <rtabmap_ros/RGBDImage.h>
#include <rtabmap_ros/RGBDImages.h>
#include <rtabmap_ros/UserData.h>
#include <rtabmap_ros/OdomInfo.h>
#include <rtabmap_ros/ScanDescriptor.h>
@@ -176,6 +177,17 @@ private:
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
void setupRGBDXCallbacks(
ros::NodeHandle & nh,
ros::NodeHandle & pnh,
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
#ifdef RTABMAP_SYNC_MULTI_RGBD
void setupRGBD2Callbacks(
ros::NodeHandle & nh,
@@ -278,6 +290,8 @@ private:
//for rgbd callback
ros::Subscriber rgbdSub_;
std::vector<message_filters::Subscriber<rtabmap_ros::RGBDImage>*> rgbdSubs_;
ros::Subscriber rgbdXSubOnly_;
message_filters::Subscriber<rtabmap_ros::RGBDImages> rgbdXSub_;
//stereo callback
image_transport::SubscriberFilter imageRectLeft_;
@@ -419,6 +433,36 @@ private:
DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
#endif
// X RGBD
void rgbdXCallback(const rtabmap_ros::RGBDImagesConstPtr&);
DATA_SYNCS2(rgbdXScan2d, rtabmap_ros::RGBDImages, sensor_msgs::LaserScan);
DATA_SYNCS2(rgbdXScan3d, rtabmap_ros::RGBDImages, sensor_msgs::PointCloud2)
DATA_SYNCS2(rgbdXScanDesc, rtabmap_ros::RGBDImages, rtabmap_ros::ScanDescriptor);
DATA_SYNCS2(rgbdXInfo, rtabmap_ros::RGBDImages, rtabmap_ros::OdomInfo);
// X RGBD + Odom
DATA_SYNCS2(rgbdXOdom, nav_msgs::Odometry, rtabmap_ros::RGBDImages);
DATA_SYNCS3(rgbdXOdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImages, sensor_msgs::LaserScan);
DATA_SYNCS3(rgbdXOdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImages, sensor_msgs::PointCloud2);
DATA_SYNCS3(rgbdXOdomScanDesc, nav_msgs::Odometry, rtabmap_ros::RGBDImages, rtabmap_ros::ScanDescriptor);
DATA_SYNCS3(rgbdXOdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImages, rtabmap_ros::OdomInfo);
#ifdef RTABMAP_SYNC_USER_DATA
// X RGBD + User Data
DATA_SYNCS2(rgbdXData, rtabmap_ros::UserData, rtabmap_ros::RGBDImages);
DATA_SYNCS3(rgbdXDataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, sensor_msgs::LaserScan);
DATA_SYNCS3(rgbdXDataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, sensor_msgs::PointCloud2);
DATA_SYNCS3(rgbdXDataScanDesc, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, rtabmap_ros::ScanDescriptor);
DATA_SYNCS3(rgbdXDataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, rtabmap_ros::OdomInfo);
// X RGBD + Odom + User Data
DATA_SYNCS3(rgbdXOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages);
DATA_SYNCS4(rgbdXOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, sensor_msgs::LaserScan);
DATA_SYNCS4(rgbdXOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, sensor_msgs::PointCloud2);
DATA_SYNCS4(rgbdXOdomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, rtabmap_ros::ScanDescriptor);
DATA_SYNCS4(rgbdXOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, rtabmap_ros::OdomInfo);
#endif
#ifdef RTABMAP_SYNC_MULTI_RGBD
// 2 RGBD
DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
include/rtabmap_ros/CommonDataSubscriberDefines.h
+21 -21
View File
@@ -105,18 +105,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
if(PREFIX##ExactSync_) delete PREFIX##ExactSync_;
// Sync declarations
#define SYNC_DECL2(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1) \
#define SYNC_DECL2(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s", \
name_.c_str(), \
@@ -124,18 +124,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
SUB0.getTopic().c_str(), \
SUB1.getTopic().c_str());
#define SYNC_DECL3(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2) \
#define SYNC_DECL3(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s", \
name_.c_str(), \
@@ -144,18 +144,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
SUB1.getTopic().c_str(), \
SUB2.getTopic().c_str());
#define SYNC_DECL4(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3) \
#define SYNC_DECL4(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s", \
name_.c_str(), \
@@ -165,18 +165,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
SUB2.getTopic().c_str(), \
SUB3.getTopic().c_str());
#define SYNC_DECL5(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4) \
#define SYNC_DECL5(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
name_.c_str(), \
@@ -187,18 +187,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
SUB3.getTopic().c_str(), \
SUB4.getTopic().c_str());
#define SYNC_DECL6(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5) \
#define SYNC_DECL6(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
name_.c_str(), \
@@ -210,18 +210,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
SUB4.getTopic().c_str(), \
SUB5.getTopic().c_str());
#define SYNC_DECL7(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6) \
#define SYNC_DECL7(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
name_.c_str(), \
@@ -234,18 +234,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
SUB5.getTopic().c_str(), \
SUB6.getTopic().c_str());
#define SYNC_DECL8(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7) \
#define SYNC_DECL8(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7) \
if(APPROX) \
{ \
PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); \
PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); \
} \
else \
{ \
PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); \
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
name_.c_str(), \
include/rtabmap_ros/MsgConversion.h
+1
View File
@@ -74,6 +74,7 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg, bool ig
void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth);
void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
void toCvShare(const rtabmap_ros::RGBDImage & image, const boost::shared_ptr<void const>& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_ros::RGBDImage & msg, const std::string & sensorFrameId);
rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & image);