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

Added OdomInfo + scan2d/scan3d synchronization interface

Browse Source
This commit is contained in:
matlabbe
2017-04-02 16:55:38 -04:00
parent 89bc0c29ca
commit ed3508fe4b
6 changed files with 652 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/CommonDataSubscriber.h
+24
View File
@@ -182,24 +182,32 @@ private:
DATA_SYNCS4(depthScan2d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
DATA_SYNCS4(depthScan3d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
DATA_SYNCS4(depthInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
DATA_SYNCS5(depthScan2dInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS5(depthScan3dInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
// RGB + Depth + Odom
DATA_SYNCS4(depthOdom, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
DATA_SYNCS5(depthOdomScan2d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
DATA_SYNCS5(depthOdomScan3d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
DATA_SYNCS5(depthOdomInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
DATA_SYNCS6(depthOdomScan2dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS6(depthOdomScan3dInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
// RGB + Depth + User Data
DATA_SYNCS4(depthData, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
DATA_SYNCS5(depthDataScan2d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
DATA_SYNCS5(depthDataScan3d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
DATA_SYNCS5(depthDataInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
DATA_SYNCS6(depthDataScan2dInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS6(depthDataScan3dInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
// RGB + Depth + Odom + User Data
DATA_SYNCS5(depthOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
DATA_SYNCS6(depthOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
DATA_SYNCS6(depthOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
DATA_SYNCS6(depthOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
DATA_SYNCS7(depthOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS7(depthOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
// Stereo
DATA_SYNCS4(stereo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo);
@@ -214,48 +222,64 @@ private:
DATA_SYNCS2(rgbdScan2d, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
DATA_SYNCS2(rgbdScan3d, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
DATA_SYNCS2(rgbdInfo, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
DATA_SYNCS3(rgbdScan2dInfo, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS3(rgbdScan3dInfo, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
// 1 RGBD + Odom
DATA_SYNCS2(rgbdOdom, nav_msgs::Odometry, rtabmap_ros::RGBDImage);
DATA_SYNCS3(rgbdOdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
DATA_SYNCS3(rgbdOdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
DATA_SYNCS3(rgbdOdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
DATA_SYNCS4(rgbdOdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS4(rgbdOdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
// 1 RGBD + User Data
DATA_SYNCS2(rgbdData, rtabmap_ros::UserData, rtabmap_ros::RGBDImage);
DATA_SYNCS3(rgbdDataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
DATA_SYNCS3(rgbdDataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
DATA_SYNCS3(rgbdDataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
DATA_SYNCS4(rgbdDataScan2dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS4(rgbdDataScan3dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
// 1 RGBD + Odom + User Data
DATA_SYNCS3(rgbdOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage);
DATA_SYNCS4(rgbdOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
DATA_SYNCS4(rgbdOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
DATA_SYNCS5(rgbdOdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS5(rgbdOdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
// 2 RGBD
DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS3(rgbd2Scan2d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
DATA_SYNCS3(rgbd2Scan3d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
DATA_SYNCS3(rgbd2Info, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
DATA_SYNCS4(rgbd2Scan2dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS4(rgbd2Scan3dInfo, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
// 2 RGBD + Odom
DATA_SYNCS3(rgbd2Odom, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS4(rgbd2OdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
DATA_SYNCS4(rgbd2OdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
DATA_SYNCS4(rgbd2OdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
DATA_SYNCS5(rgbd2OdomScan2dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS5(rgbd2OdomScan3dInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
// 2 RGBD + User Data
DATA_SYNCS3(rgbd2Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS4(rgbd2DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
DATA_SYNCS4(rgbd2DataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
DATA_SYNCS4(rgbd2DataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
DATA_SYNCS5(rgbd2DataScan2dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS5(rgbd2DataScan3dInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
// 2 RGBD + Odom + User Data
DATA_SYNCS4(rgbd2OdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
DATA_SYNCS5(rgbd2OdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
DATA_SYNCS5(rgbd2OdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
DATA_SYNCS5(rgbd2OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
DATA_SYNCS6(rgbd2OdomDataScan2dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan, rtabmap_ros::OdomInfo);
DATA_SYNCS6(rgbd2OdomDataScan3dInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2, rtabmap_ros::OdomInfo);
};
include/rtabmap_ros/CommonDataSubscriberDefines.h
+34
View File
@@ -75,6 +75,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
DATA_SYNC6(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&, const MSG4##ConstPtr&, const MSG5##ConstPtr&); \
#define DATA_SYNC7(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6) \
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6> PREFIX##SYNC_NAME##SyncPolicy; \
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
#define DATA_SYNCS7(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6) \
DATA_SYNC7(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6) \
DATA_SYNC7(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5, MSG6) \
void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&, const MSG4##ConstPtr&, const MSG5##ConstPtr&, const MSG6##ConstPtr&); \
// Constructor
#define SYNC_INIT(PREFIX) \
PREFIX##ApproximateSync_(0), \
@@ -191,5 +201,29 @@ 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) \
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)); \
} \
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)); \
} \
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(), \
APPROX?"approx":"exact", \
SUB0.getTopic().c_str(), \
SUB1.getTopic().c_str(), \
SUB2.getTopic().c_str(), \
SUB3.getTopic().c_str(), \
SUB4.getTopic().c_str(), \
SUB5.getTopic().c_str(), \
SUB6.getTopic().c_str());
#endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_ */