mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 00:37:46 +08:00
Added rtabmap_ros/RGBDImage and rtabmap_ros/UserData topics. Refactored input messages synchronization of rtabmap and rtabmapviz nodes. "depth_cameras" parameter is replaced with "rgbd_cameras" parameter. Multiple camera is handled through rtabmap_ros/RGBDImage input topics (same for rgbd_odometry node).
This commit is contained in:
matlabbe
@@ -0,0 +1,244 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_
|
||||
#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <image_transport/subscriber_filter.h>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
|
||||
#include <nav_msgs/Odometry.h>
|
||||
|
||||
#include <rtabmap_ros/RGBDImage.h>
|
||||
#include <rtabmap_ros/UserData.h>
|
||||
#include <rtabmap_ros/OdomInfo.h>
|
||||
#include <rtabmap_ros/CommonDataSubscriberDefines.h>
|
||||
|
||||
namespace rtabmap_ros {
|
||||
|
||||
class CommonDataSubscriber {
|
||||
public:
|
||||
CommonDataSubscriber();
|
||||
virtual ~CommonDataSubscriber();
|
||||
|
||||
bool isSubscribedToDepth() const {return subscribedToDepth_;}
|
||||
bool isSubscribedToStereo() const {return subscribedToStereo_;}
|
||||
bool isSubscribedToRGBD() const {return subscribedToRGBD_;}
|
||||
bool isSubscribedToScan2d() const {return subscribedToScan2d_;}
|
||||
bool isSubscribedToScan3d() const {return subscribedToScan3d_;}
|
||||
bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
|
||||
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD();}
|
||||
int getQueueSize() const {return queueSize_;}
|
||||
|
||||
protected:
|
||||
virtual void commonDepthCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
|
||||
const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) = 0;
|
||||
virtual void commonStereoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) = 0;
|
||||
|
||||
void commonSingleDepthCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const cv_bridge::CvImageConstPtr & imageMsg,
|
||||
const cv_bridge::CvImageConstPtr & depthMsg,
|
||||
const sensor_msgs::CameraInfo & cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
|
||||
|
||||
private:
|
||||
void setupDepthCallbacks(
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeOdomInfo,
|
||||
int queueSize,
|
||||
bool approxSync);
|
||||
void setupStereoCallbacks(
|
||||
bool subscribeOdom,
|
||||
bool subscribeOdomInfo,
|
||||
int queueSize,
|
||||
bool approxSync);
|
||||
void setupRGBDCallbacks(
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeOdomInfo,
|
||||
int queueSize,
|
||||
bool approxSync);
|
||||
void setupRGBD2Callbacks(
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeOdomInfo,
|
||||
int queueSize,
|
||||
bool approxSync);
|
||||
|
||||
private:
|
||||
int queueSize_;
|
||||
bool subscribedToDepth_;
|
||||
bool subscribedToStereo_;
|
||||
bool subscribedToRGBD_;
|
||||
bool subscribedToScan2d_;
|
||||
bool subscribedToScan3d_;
|
||||
bool subscribedToOdomInfo_;
|
||||
|
||||
//for depth callback
|
||||
image_transport::SubscriberFilter imageSub_;
|
||||
image_transport::SubscriberFilter imageDepthSub_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
|
||||
|
||||
//for rgbd callback
|
||||
ros::Subscriber rgbdSub_;
|
||||
std::vector<message_filters::Subscriber<rtabmap_ros::RGBDImage>*> rgbdSubs_;
|
||||
|
||||
//stereo callback
|
||||
image_transport::SubscriberFilter imageRectLeft_;
|
||||
image_transport::SubscriberFilter imageRectRight_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
|
||||
|
||||
message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
|
||||
message_filters::Subscriber<rtabmap_ros::UserData> userDataSub_;
|
||||
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
|
||||
message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
|
||||
message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
|
||||
|
||||
// RGB + Depth
|
||||
DATA_SYNCS3(depth, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
|
||||
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);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
|
||||
// Stereo
|
||||
DATA_SYNCS4(stereo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo);
|
||||
DATA_SYNCS5(stereoInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
|
||||
|
||||
// Stereo + Odom
|
||||
DATA_SYNCS5(stereoOdom, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo);
|
||||
DATA_SYNCS6(stereoOdomInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
|
||||
|
||||
// 1 RGBD
|
||||
void rgbdCallback(const rtabmap_ros::RGBDImageConstPtr&);
|
||||
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);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
|
||||
};
|
||||
|
||||
} /* namespace rtabmap_ros */
|
||||
|
||||
#endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_ */
|
||||
@@ -0,0 +1,194 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
|
||||
#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
|
||||
|
||||
|
||||
#define DATA_SYNC2(PREFIX, SYNC_NAME, MSG0, MSG1) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS2(PREFIX, MSG0, MSG1) \
|
||||
DATA_SYNC2(PREFIX, Approximate, MSG0, MSG1) \
|
||||
DATA_SYNC2(PREFIX, Exact, MSG0, MSG1) \
|
||||
void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&);
|
||||
|
||||
#define DATA_SYNC3(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS3(PREFIX, MSG0, MSG1, MSG2) \
|
||||
DATA_SYNC3(PREFIX, Approximate, MSG0, MSG1, MSG2) \
|
||||
DATA_SYNC3(PREFIX, Exact, MSG0, MSG1, MSG2) \
|
||||
void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&); \
|
||||
|
||||
#define DATA_SYNC4(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS4(PREFIX, MSG0, MSG1, MSG2, MSG3) \
|
||||
DATA_SYNC4(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3) \
|
||||
DATA_SYNC4(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3) \
|
||||
void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&); \
|
||||
|
||||
#define DATA_SYNC5(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS5(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4) \
|
||||
DATA_SYNC5(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4) \
|
||||
DATA_SYNC5(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4) \
|
||||
void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&, const MSG4##ConstPtr&); \
|
||||
|
||||
#define DATA_SYNC6(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
|
||||
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4, MSG5> PREFIX##SYNC_NAME##SyncPolicy; \
|
||||
message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
|
||||
|
||||
#define DATA_SYNCS6(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
|
||||
DATA_SYNC6(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
|
||||
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&); \
|
||||
|
||||
// Constructor
|
||||
#define SYNC_INIT(PREFIX) \
|
||||
PREFIX##ApproximateSync_(0), \
|
||||
PREFIX##ExactSync_(0)
|
||||
|
||||
// Destructor
|
||||
#define SYNC_DEL(PREFIX) \
|
||||
if(PREFIX##ApproximateSync_) delete PREFIX##ApproximateSync_; \
|
||||
if(PREFIX##ExactSync_) delete PREFIX##ExactSync_;
|
||||
|
||||
// Sync declarations
|
||||
#define SYNC_DECL2(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)); \
|
||||
} \
|
||||
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)); \
|
||||
} \
|
||||
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s", \
|
||||
ros::this_node::getName().c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
SUB0.getTopic().c_str(), \
|
||||
SUB1.getTopic().c_str());
|
||||
|
||||
#define SYNC_DECL3(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)); \
|
||||
} \
|
||||
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)); \
|
||||
} \
|
||||
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s", \
|
||||
ros::this_node::getName().c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
SUB0.getTopic().c_str(), \
|
||||
SUB1.getTopic().c_str(), \
|
||||
SUB2.getTopic().c_str());
|
||||
|
||||
#define SYNC_DECL4(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)); \
|
||||
} \
|
||||
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)); \
|
||||
} \
|
||||
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", \
|
||||
ros::this_node::getName().c_str(), \
|
||||
APPROX?"approx":"exact", \
|
||||
SUB0.getTopic().c_str(), \
|
||||
SUB1.getTopic().c_str(), \
|
||||
SUB2.getTopic().c_str(), \
|
||||
SUB3.getTopic().c_str());
|
||||
|
||||
#define SYNC_DECL5(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)); \
|
||||
} \
|
||||
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)); \
|
||||
} \
|
||||
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s", \
|
||||
ros::this_node::getName().c_str(), \
|
||||
approxSync?"approx":"exact", \
|
||||
SUB0.getTopic().c_str(), \
|
||||
SUB1.getTopic().c_str(), \
|
||||
SUB2.getTopic().c_str(), \
|
||||
SUB3.getTopic().c_str(), \
|
||||
SUB4.getTopic().c_str());
|
||||
|
||||
#define SYNC_DECL6(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)); \
|
||||
} \
|
||||
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)); \
|
||||
} \
|
||||
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", \
|
||||
ros::this_node::getName().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());
|
||||
|
||||
|
||||
#endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_ */
|
||||
@@ -38,11 +38,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
#include <std_msgs/Empty.h>
|
||||
#include <std_msgs/Int32.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <nav_msgs/GetMap.h>
|
||||
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
@@ -54,17 +49,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "rtabmap_ros/SetGoal.h"
|
||||
#include "rtabmap_ros/SetLabel.h"
|
||||
#include "rtabmap_ros/Goal.h"
|
||||
#include "rtabmap_ros/CommonDataSubscriber.h"
|
||||
|
||||
#include "MapsManager.h"
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <image_transport/subscriber_filter.h>
|
||||
|
||||
#ifdef WITH_OCTOMAP_ROS
|
||||
#include <octomap_msgs/GetOctomap.h>
|
||||
#endif
|
||||
@@ -81,128 +69,48 @@ namespace rtabmap {
|
||||
class StereoDense;
|
||||
}
|
||||
|
||||
class CoreWrapper
|
||||
namespace rtabmap_ros {
|
||||
|
||||
class CoreWrapper : public CommonDataSubscriber
|
||||
{
|
||||
public:
|
||||
CoreWrapper(bool deleteDbOnStart, const rtabmap::ParametersMap & parameters);
|
||||
virtual ~CoreWrapper();
|
||||
|
||||
private:
|
||||
void setupCallbacks(
|
||||
bool subscribeDepth,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScan3d,
|
||||
bool subscribeStereo,
|
||||
int queueSize,
|
||||
bool approxSync,
|
||||
int depthCameras);
|
||||
void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
|
||||
|
||||
bool commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
|
||||
bool commonOdomTFUpdate(const ros::Time & stamp); // TF odom
|
||||
bool odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
|
||||
bool odomTFUpdate(const ros::Time & stamp); // TF odom
|
||||
|
||||
void commonDepthCallback(
|
||||
const std::string & odomFrameId,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
virtual void commonDepthCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
|
||||
const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
|
||||
void commonDepthCallback(
|
||||
const std::string & odomFrameId,
|
||||
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
|
||||
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
|
||||
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
|
||||
void commonStereoCallback(
|
||||
const std::string & odomFrameId,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
|
||||
void commonDepthCallbackImpl(
|
||||
const std::string & odomFrameId,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
|
||||
const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
|
||||
const sensor_msgs::LaserScanConstPtr& scan2dMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
|
||||
virtual void commonStereoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
|
||||
|
||||
// with odom msg
|
||||
void depthCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
|
||||
void depthScanCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg);
|
||||
void depthScan3dCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg);
|
||||
void stereoCallback(
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg);
|
||||
void stereoScanCallback(
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg);
|
||||
void stereoScan3dCallback(
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg);
|
||||
void depth2Callback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& image1Msg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepth1Msg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfo1Msg,
|
||||
const sensor_msgs::ImageConstPtr& image2Msg,
|
||||
const sensor_msgs::ImageConstPtr& imageDept2hMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfo2Msg);
|
||||
|
||||
// without odom, when TF is used for odom
|
||||
void depthTFCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
|
||||
void depthScanTFCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg);
|
||||
void depthScan3dTFCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg);
|
||||
void stereoTFCallback(
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg);
|
||||
void stereoScanTFCallback(
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg);
|
||||
void stereoScan3dTFCallback(
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg);
|
||||
void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
|
||||
|
||||
void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
|
||||
void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
|
||||
@@ -269,8 +177,8 @@ private:
|
||||
rtabmap::ParametersMap parameters_;
|
||||
|
||||
std::string frameId_;
|
||||
std::string mapFrameId_;
|
||||
std::string odomFrameId_;
|
||||
std::string mapFrameId_;
|
||||
std::string groundTruthFrameId_;
|
||||
std::string groundTruthBaseFrameId_;
|
||||
std::string configPath_;
|
||||
@@ -302,179 +210,6 @@ private:
|
||||
ros::Publisher globalPathPub_;
|
||||
ros::Publisher localPathPub_;
|
||||
|
||||
// for loop closure detection only
|
||||
image_transport::Subscriber defaultSub_;
|
||||
|
||||
//for depth callback
|
||||
std::vector<image_transport::SubscriberFilter*> imageSubs_;
|
||||
std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
|
||||
std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>*> cameraInfoSubs_;
|
||||
|
||||
//stereo callback
|
||||
image_transport::SubscriberFilter imageRectLeft_;
|
||||
image_transport::SubscriberFilter imageRectRight_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
|
||||
|
||||
message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
|
||||
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
|
||||
message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
|
||||
typedef message_filters::sync_policies::ExactTime<
|
||||
sensor_msgs::Image,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::LaserScan> MyDepthScanExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScanExactSyncPolicy> * depthScanExactSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::PointCloud2> MyDepthScan3dSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScan3dSyncPolicy> * depthScan3dSync_;
|
||||
typedef message_filters::sync_policies::ExactTime<
|
||||
sensor_msgs::Image,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::PointCloud2> MyDepthScan3dExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScan3dExactSyncPolicy> * depthScan3dExactSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
|
||||
typedef message_filters::sync_policies::ExactTime<
|
||||
sensor_msgs::Image,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthExactSyncPolicy> * depthExactSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::LaserScan,
|
||||
nav_msgs::Odometry> MyStereoScanSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::PointCloud2,
|
||||
nav_msgs::Odometry> MyStereoScan3dSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoScan3dSyncPolicy> * stereoScan3dSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo,
|
||||
nav_msgs::Odometry> MyStereoApproxSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoApproxSyncPolicy> * stereoApproxSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo,
|
||||
nav_msgs::Odometry> MyStereoExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoExactSyncPolicy> * stereoExactSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepth2SyncPolicy;
|
||||
message_filters::Synchronizer<MyDepth2SyncPolicy> * depth2Sync_;
|
||||
|
||||
// without odom, when TF is used for odom
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::LaserScan> MyDepthScanTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
|
||||
typedef message_filters::sync_policies::ExactTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::LaserScan> MyDepthScanTFExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScanTFExactSyncPolicy> * depthScanTFExactSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::PointCloud2> MyDepthScan3dTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy> * depthScan3dTFSync_;
|
||||
typedef message_filters::sync_policies::ExactTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::PointCloud2> MyDepthScan3dTFExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScan3dTFExactSyncPolicy> * depthScan3dTFExactSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
|
||||
typedef message_filters::sync_policies::ExactTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthTFExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthTFExactSyncPolicy> * depthTFExactSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::LaserScan> MyStereoScanTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::PointCloud2> MyStereoScan3dTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy> * stereoScan3dTFSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoApproxTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoApproxTFSyncPolicy> * stereoApproxTFSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoExactTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoExactTFSyncPolicy> * stereoExactTFSync_;
|
||||
|
||||
tf2_ros::TransformBroadcaster tfBroadcaster_;
|
||||
tf::TransformListener tfListener_;
|
||||
|
||||
@@ -508,6 +243,9 @@ private:
|
||||
|
||||
boost::thread* transformThread_;
|
||||
|
||||
// for loop closure detection only
|
||||
image_transport::Subscriber defaultSub_;
|
||||
|
||||
bool stereoToDepth_;
|
||||
bool odomSensorSync_;
|
||||
float rate_;
|
||||
@@ -516,5 +254,7 @@ private:
|
||||
ros::Time previousStamp_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* COREWRAPPER_H_ */
|
||||
|
||||
|
||||
@@ -39,21 +39,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <nav_msgs/Path.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <image_transport/subscriber_filter.h>
|
||||
#include <rtabmap_ros/CommonDataSubscriber.h>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
@@ -62,7 +51,9 @@ namespace rtabmap
|
||||
|
||||
class QApplication;
|
||||
|
||||
class GuiWrapper : public UEventsHandler
|
||||
namespace rtabmap_ros {
|
||||
|
||||
class GuiWrapper : public UEventsHandler, public CommonDataSubscriber
|
||||
{
|
||||
public:
|
||||
GuiWrapper(int & argc, char** argv);
|
||||
@@ -76,32 +67,16 @@ private:
|
||||
void goalPathCallback(const rtabmap_ros::GoalConstPtr & goalMsg, const nav_msgs::PathConstPtr & pathMsg);
|
||||
void goalReachedCallback(const std_msgs::BoolConstPtr & value);
|
||||
|
||||
void setupCallbacks(
|
||||
bool subscribeDepth,
|
||||
bool subscribeLaserScan2d,
|
||||
bool subscribeLaserScan3d,
|
||||
bool subscribeOdomInfo,
|
||||
bool subscribeStereo,
|
||||
int queueSize,
|
||||
int depthCameras);
|
||||
|
||||
void commonDepthCallback(
|
||||
virtual void commonDepthCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
|
||||
const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
|
||||
const sensor_msgs::LaserScanConstPtr& scan2dMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
|
||||
void commonDepthCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
|
||||
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
|
||||
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
|
||||
const sensor_msgs::LaserScanConstPtr& scan2dMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
|
||||
void commonStereoCallback(
|
||||
virtual void commonStereoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
@@ -113,150 +88,6 @@ private:
|
||||
|
||||
void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg);
|
||||
|
||||
// With odom msg
|
||||
void depthCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
|
||||
void depth2Callback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& image1Msg,
|
||||
const sensor_msgs::ImageConstPtr& depth1Msg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
|
||||
const sensor_msgs::ImageConstPtr& image2Msg,
|
||||
const sensor_msgs::ImageConstPtr& depth2Msg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg);
|
||||
void depthOdomInfoCallback(
|
||||
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
|
||||
void depthOdomInfo2Callback(
|
||||
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& image1Msg,
|
||||
const sensor_msgs::ImageConstPtr& depth1Msg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
|
||||
const sensor_msgs::ImageConstPtr& image2Msg,
|
||||
const sensor_msgs::ImageConstPtr& depth2Msg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg);
|
||||
void depthScanCallback(
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
|
||||
void depthScanOdomInfoCallback(
|
||||
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
|
||||
void depthScan3dCallback(
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
|
||||
void depthScan3dOdomInfoCallback(
|
||||
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
|
||||
|
||||
void stereoScanCallback(
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
|
||||
void stereoScanOdomInfoCallback(
|
||||
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
|
||||
void stereoScan3dCallback(
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
|
||||
void stereoScan3dOdomInfoCallback(
|
||||
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
|
||||
void stereoOdomInfoCallback(
|
||||
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
|
||||
void stereoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
|
||||
|
||||
// with TF
|
||||
void depthTFCallback(const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
|
||||
void depthOdomInfoTFCallback(
|
||||
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
|
||||
void depthScanTFCallback(
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs:: CameraInfoConstPtr& camInfoMsg);
|
||||
void depthScan3dTFCallback(
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs:: CameraInfoConstPtr& camInfoMsg);
|
||||
|
||||
void stereoScanTFCallback(
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
|
||||
void stereoScan3dTFCallback(
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
|
||||
void stereoOdomInfoTFCallback(
|
||||
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
|
||||
void stereoTFCallback(
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
|
||||
|
||||
void processRequestedMap(const rtabmap_ros::MapData & map);
|
||||
|
||||
private:
|
||||
@@ -280,18 +111,6 @@ private:
|
||||
ros::Subscriber goalReachedTopic_;
|
||||
|
||||
ros::Subscriber defaultSub_; // odometry only
|
||||
std::vector<image_transport::SubscriberFilter*> imageSubs_;
|
||||
std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
|
||||
std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>* > cameraInfoSubs_;
|
||||
message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
|
||||
message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
|
||||
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
|
||||
message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
|
||||
|
||||
image_transport::SubscriberFilter imageRectLeft_;
|
||||
image_transport::SubscriberFilter imageRectRight_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<
|
||||
rtabmap_ros::Info,
|
||||
@@ -302,191 +121,8 @@ private:
|
||||
rtabmap_ros::Goal,
|
||||
nav_msgs::Path> MyGoalPathSyncPolicy;
|
||||
message_filters::Synchronizer<MyGoalPathSyncPolicy> * goalPathSync_;
|
||||
|
||||
// with odom msg
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::LaserScan,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthScanSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
rtabmap_ros::OdomInfo,
|
||||
sensor_msgs::LaserScan,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthScanOdomInfoSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScanOdomInfoSyncPolicy> * depthScanOdomInfoSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::PointCloud2,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthScan3dSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScan3dSyncPolicy> * depthScan3dSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
rtabmap_ros::OdomInfo,
|
||||
sensor_msgs::PointCloud2,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthScan3dOdomInfoSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScan3dOdomInfoSyncPolicy> * depthScan3dOdomInfoSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
rtabmap_ros::OdomInfo,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthOdomInfoSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy> * depthOdomInfoSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::LaserScan,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoScanSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
rtabmap_ros::OdomInfo,
|
||||
sensor_msgs::LaserScan,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoScanOdomInfoSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoScanOdomInfoSyncPolicy> * stereoScanOdomInfoSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::PointCloud2,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoScan3dSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoScan3dSyncPolicy> * stereoScan3dSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
rtabmap_ros::OdomInfo,
|
||||
sensor_msgs::PointCloud2,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoScan3dOdomInfoSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoScan3dOdomInfoSyncPolicy> * stereoScan3dOdomInfoSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
rtabmap_ros::OdomInfo,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoOdomInfoSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy> * stereoOdomInfoSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepth2SyncPolicy;
|
||||
message_filters::Synchronizer<MyDepth2SyncPolicy> * depth2Sync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
rtabmap_ros::OdomInfo,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthOdomInfo2SyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthOdomInfo2SyncPolicy> * depthOdomInfo2Sync_;
|
||||
|
||||
// with odom TF
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::LaserScan,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthScanTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::PointCloud2,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthScan3dTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy> * depthScan3dTFSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
rtabmap_ros::OdomInfo,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthOdomInfoTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthOdomInfoTFSyncPolicy> * depthOdomInfoTFSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoTFSyncPolicy> * stereoTFSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::LaserScan,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoScanTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::PointCloud2,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoScan3dTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy> * stereoScan3dTFSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
rtabmap_ros::OdomInfo,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoOdomInfoTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoOdomInfoTFSyncPolicy> * stereoOdomInfoTFSync_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* GUIWRAPPER_H_ */
|
||||
|
||||
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/features2d/features2d.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <rtabmap/core/Link.h>
|
||||
@@ -55,6 +56,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap_ros/NodeData.h>
|
||||
#include <rtabmap_ros/OdomInfo.h>
|
||||
#include <rtabmap_ros/Info.h>
|
||||
#include <rtabmap_ros/RGBDImage.h>
|
||||
#include <rtabmap_ros/UserData.h>
|
||||
|
||||
namespace rtabmap_ros {
|
||||
|
||||
@@ -67,6 +70,9 @@ rtabmap::Transform transformFromGeometryMsg(const geometry_msgs::Transform & msg
|
||||
void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::Pose & msg);
|
||||
rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg);
|
||||
|
||||
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);
|
||||
|
||||
// copy data
|
||||
void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes);
|
||||
cv::Mat compressedMatFromBytes(const std::vector<unsigned char> & bytes, bool copy = true);
|
||||
@@ -140,6 +146,9 @@ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
|
||||
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg);
|
||||
void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);
|
||||
|
||||
cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg);
|
||||
void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress);
|
||||
|
||||
inline double timestampFromROS(const ros::Time & stamp) {return double(stamp.sec) + double(stamp.nsec)/1000000000.0;}
|
||||
|
||||
// common stuff
|
||||
@@ -162,9 +171,9 @@ rtabmap::Transform getTransform(
|
||||
double waitForTransform);
|
||||
|
||||
bool convertRGBDMsgs(
|
||||
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
|
||||
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
|
||||
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
|
||||
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
|
||||
const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
|
||||
const std::string & frameId,
|
||||
const std::string & odomFrameId,
|
||||
const ros::Time & odomStamp,
|
||||
|
||||
Reference in New Issue
Block a user