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

Added multi-cameras demo: demo_two_kinects.launch

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-31 01:28:54 -04:00
parent fcd343cdd9
commit 96d3ad35e2
8 changed files with 1254 additions and 365 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.h
+40 -18
View File
@@ -83,7 +83,13 @@ public:
virtual ~CoreWrapper();
private:
void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeStereo, int queueSize, bool stereoApproxSync);
void setupCallbacks(
bool subscribeDepth,
bool subscribeLaserScan,
bool subscribeStereo,
int queueSize,
bool stereoApproxSync,
int depthCameras);
void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
bool commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
@@ -93,8 +99,14 @@ private:
void commonDepthCallback(
const std::string & odomFrameId,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg);
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);
void commonStereoCallback(
const std::string & odomFrameId,
@@ -129,6 +141,14 @@ private:
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& 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(
@@ -158,22 +178,12 @@ private:
void updateGoal(const ros::Time & stamp);
void process(
int id,
const ros::Time & stamp,
const cv::Mat & image,
const rtabmap::SensorData & data,
const rtabmap::Transform & odom = rtabmap::Transform(),
const std::string & odomFrameId = "",
double odomRotationalVariance = 1.0,
double odomTransitionalVariance = 1.0,
const cv::Mat & depthOrRightImage = cv::Mat(),
double fx = 0.0,
double fy = 0.0,
double cx = 0.0,
double cy = 0.0,
double baseline = 0.0,
const rtabmap::Transform & localTransform = rtabmap::Transform(),
const cv::Mat & scan = cv::Mat(),
int scanMaxPts = 0);
double odomTransitionalVariance = 1.0);
bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -225,6 +235,8 @@ private:
std::string databasePath_;
bool waitForTransform_;
bool useActionForGoal_;
bool genScan_;
double genScanMaxDepth_;
rtabmap::Transform mapToOdom_;
boost::mutex mapToOdomMutex_;
@@ -247,9 +259,9 @@ private:
image_transport::Subscriber defaultSub_;
//for depth callback
image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
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_;
@@ -300,6 +312,16 @@ private:
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,