mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-06 17:57:45 +08:00
Updated for rtabmap 0.10.0 (multi-cameras)
This commit is contained in:
Mathieu Labbe
+124
-22
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "rtabmap_ros/MapData.h"
|
||||
#include "rtabmap_ros/OdomInfo.h"
|
||||
#include "rtabmap/utilite/UEventsHandler.h"
|
||||
#include "rtabmap/core/Transform.h"
|
||||
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
@@ -73,33 +74,54 @@ private:
|
||||
void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);
|
||||
|
||||
void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeOdomInfo, bool subscribeStereo, int queueSize);
|
||||
void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); // odom
|
||||
void depthCallback(const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageDepthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
|
||||
void depthOdomInfoCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
|
||||
void commonDepthCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
|
||||
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 rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
|
||||
|
||||
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 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 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 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 stereoScanCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
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 stereoOdomInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
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,
|
||||
@@ -111,7 +133,41 @@ private:
|
||||
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 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 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);
|
||||
rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
|
||||
|
||||
private:
|
||||
QApplication * app_;
|
||||
@@ -121,6 +177,7 @@ private:
|
||||
|
||||
// odometry subscription stuffs
|
||||
std::string frameId_;
|
||||
std::string odomFrameId_;
|
||||
bool waitForTransform_;
|
||||
tf::TransformListener tfListener_;
|
||||
|
||||
@@ -145,25 +202,26 @@ private:
|
||||
rtabmap_ros::MapData> MyInfoMapSyncPolicy;
|
||||
message_filters::Synchronizer<MyInfoMapSyncPolicy> * infoMapSync_;
|
||||
|
||||
// with odom msg
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::LaserScan,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthScanSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::Image,
|
||||
nav_msgs::Odometry,
|
||||
rtabmap_ros::OdomInfo,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo> MyDepthOdomInfoSyncPolicy;
|
||||
message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy> * depthOdomInfoSync_;
|
||||
@@ -177,8 +235,8 @@ private:
|
||||
message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::LaserScan,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
@@ -186,13 +244,57 @@ private:
|
||||
message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
nav_msgs::Odometry,
|
||||
rtabmap_ros::OdomInfo,
|
||||
nav_msgs::Odometry,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoOdomInfoSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy> * stereoOdomInfoSync_;
|
||||
|
||||
// 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::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<
|
||||
rtabmap_ros::OdomInfo,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::Image,
|
||||
sensor_msgs::CameraInfo,
|
||||
sensor_msgs::CameraInfo> MyStereoOdomInfoTFSyncPolicy;
|
||||
message_filters::Synchronizer<MyStereoOdomInfoTFSyncPolicy> * stereoOdomInfoTFSync_;
|
||||
};
|
||||
|
||||
#endif /* GUIWRAPPER_H_ */
|
||||
|
||||
Reference in New Issue
Block a user