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 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).

Browse Source
This commit is contained in:
matlabbe
2016-09-26 16:37:31 -04:00
parent 3b6daf7189
commit babb8c8509
24 changed files with 2942 additions and 2920 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CommonDataSubscriber.cpp
+365
View File
@@ -0,0 +1,365 @@
/*
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.
*/
#include <rtabmap_ros/CommonDataSubscriber.h>
namespace rtabmap_ros {
CommonDataSubscriber::CommonDataSubscriber() :
queueSize_(10),
subscribedToDepth_(true),
subscribedToStereo_(false),
subscribedToRGBD_(false),
subscribedToScan2d_(false),
subscribedToScan3d_(false),
subscribedToOdomInfo_(false),
// RGB + Depth
SYNC_INIT(depth),
SYNC_INIT(depthScan2d),
SYNC_INIT(depthScan3d),
SYNC_INIT(depthInfo),
// RGB + Depth + Odom
SYNC_INIT(depthOdom),
SYNC_INIT(depthOdomScan2d),
SYNC_INIT(depthOdomScan3d),
SYNC_INIT(depthOdomInfo),
// RGB + Depth + User Data
SYNC_INIT(depthData),
SYNC_INIT(depthDataScan2d),
SYNC_INIT(depthDataScan3d),
SYNC_INIT(depthDataInfo),
// RGB + Depth + Odom + User Data
SYNC_INIT(depthOdomData),
SYNC_INIT(depthOdomDataScan2d),
SYNC_INIT(depthOdomDataScan3d),
SYNC_INIT(depthOdomDataInfo),
// Stereo
SYNC_INIT(stereo),
SYNC_INIT(stereoInfo),
// Stereo + Odom
SYNC_INIT(stereoOdom),
SYNC_INIT(stereoOdomInfo),
// 1 RGBD
SYNC_INIT(rgbdScan2d),
SYNC_INIT(rgbdScan3d),
SYNC_INIT(rgbdInfo),
// 1 RGBD + Odom
SYNC_INIT(rgbdOdom),
SYNC_INIT(rgbdOdomScan2d),
SYNC_INIT(rgbdOdomScan3d),
SYNC_INIT(rgbdOdomInfo),
// 1 RGBD + User Data
SYNC_INIT(rgbdData),
SYNC_INIT(rgbdDataScan2d),
SYNC_INIT(rgbdDataScan3d),
SYNC_INIT(rgbdDataInfo),
// 1 RGBD + Odom + User Data
SYNC_INIT(rgbdOdomData),
SYNC_INIT(rgbdOdomDataScan2d),
SYNC_INIT(rgbdOdomDataScan3d),
SYNC_INIT(rgbdOdomDataInfo),
// 2 RGBD
SYNC_INIT(rgbd2),
SYNC_INIT(rgbd2Scan2d),
SYNC_INIT(rgbd2Scan3d),
SYNC_INIT(rgbd2Info),
// 2 RGBD + Odom
SYNC_INIT(rgbd2Odom),
SYNC_INIT(rgbd2OdomScan2d),
SYNC_INIT(rgbd2OdomScan3d),
SYNC_INIT(rgbd2OdomInfo),
// 2 RGBD + User Data
SYNC_INIT(rgbd2Data),
SYNC_INIT(rgbd2DataScan2d),
SYNC_INIT(rgbd2DataScan3d),
SYNC_INIT(rgbd2DataInfo),
// 2 RGBD + Odom + User Data
SYNC_INIT(rgbd2OdomData),
SYNC_INIT(rgbd2OdomDataScan2d),
SYNC_INIT(rgbd2OdomDataScan3d),
SYNC_INIT(rgbd2OdomDataInfo)
{
ros::NodeHandle pnh("~");
bool subscribeScan2d = false;
bool subscribeScan3d = false;
bool subscribeOdomInfo = false;
bool subscribeUserData = false;
int rgbdCameras = 1;
bool approxSync = true;
// ROS related parameters (private)
pnh.param("subscribe_depth", subscribedToDepth_, subscribedToDepth_);
if(pnh.getParam("subscribe_laserScan", subscribeScan2d) && subscribeScan2d)
{
ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
}
pnh.param("subscribe_scan", subscribeScan2d, subscribeScan2d);
pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
pnh.param("subscribe_stereo", subscribedToStereo_, subscribedToStereo_);
pnh.param("subscribe_rgbd", subscribedToRGBD_, subscribedToRGBD_);
pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData);
if(subscribedToDepth_ && subscribedToStereo_)
{
ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
subscribedToDepth_ = false;
}
if(subscribedToDepth_ && subscribedToRGBD_)
{
ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.");
subscribedToDepth_ = false;
}
if(subscribedToStereo_ && subscribedToRGBD_)
{
ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false.");
subscribedToStereo_ = false;
}
if(subscribeScan2d && subscribeScan3d)
{
ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
subscribeScan3d = false;
}
if(subscribeScan2d || subscribeScan3d)
{
if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_)
{
ROS_WARN("When subscribing to laser scan, you should subscribe to depth, stereo or rgbd too. Subscribing to depth by default...");
subscribedToDepth_ = true;
}
}
if(subscribedToStereo_)
{
approxSync = false; // default for stereo: exact sync
}
std::string odomFrameId;
pnh.getParam("odom_frame_id", odomFrameId);
pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
if(pnh.hasParam("depth_cameras"))
{
ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" used when \"subscribe_rgbd\" is true.");
}
pnh.param("queue_size", queueSize_, queueSize_);
if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
{
ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
"to \"approx_sync\"! Your value is still copied to "
"corresponding parameter.");
pnh.param("stereo_approx_sync", approxSync, approxSync);
}
else
{
pnh.param("approx_sync", approxSync, approxSync);
}
if(rgbdCameras <= 0 && subscribedToRGBD_)
{
rgbdCameras = 1;
}
ROS_INFO("%s: queue_size = %d", ros::this_node::getName().c_str(), queueSize_);
ROS_INFO("%s: rgbd_cameras = %d", ros::this_node::getName().c_str(), rgbdCameras);
ROS_INFO("%s: approx_sync = %s", ros::this_node::getName().c_str(), approxSync?"true":"false");
bool subscribeOdom = odomFrameId.empty();
if(subscribedToDepth_)
{
setupDepthCallbacks(
subscribeOdom,
subscribeUserData,
subscribeScan2d,
subscribeScan3d,
subscribeOdomInfo,
queueSize_,
approxSync);
}
else if(subscribedToStereo_)
{
setupStereoCallbacks(
subscribeOdom,
subscribeOdomInfo,
queueSize_,
approxSync);
}
else if(subscribedToRGBD_)
{
if(rgbdCameras == 2)
{
setupRGBD2Callbacks(
subscribeOdom,
subscribeUserData,
subscribeScan2d,
subscribeScan3d,
subscribeOdomInfo,
queueSize_,
approxSync);
}
else
{
setupRGBDCallbacks(
subscribeOdom,
subscribeUserData,
subscribeScan2d,
subscribeScan3d,
subscribeOdomInfo,
queueSize_,
approxSync);
}
}
}
CommonDataSubscriber::~CommonDataSubscriber()
{
// RGB + Depth
SYNC_DEL(depth);
SYNC_DEL(depthScan2d);
SYNC_DEL(depthScan3d);
SYNC_DEL(depthInfo);
// RGB + Depth + Odom
SYNC_DEL(depthOdom);
SYNC_DEL(depthOdomScan2d);
SYNC_DEL(depthOdomScan3d);
SYNC_DEL(depthOdomInfo);
// RGB + Depth + User Data
SYNC_DEL(depthData);
SYNC_DEL(depthDataScan2d);
SYNC_DEL(depthDataScan3d);
SYNC_DEL(depthDataInfo);
// RGB + Depth + Odom + User Data
SYNC_DEL(depthOdomData);
SYNC_DEL(depthOdomDataScan2d);
SYNC_DEL(depthOdomDataScan3d);
SYNC_DEL(depthOdomDataInfo);
// Stereo
SYNC_DEL(stereo);
SYNC_DEL(stereoInfo);
// Stereo + Odom
SYNC_DEL(stereoOdom);
SYNC_DEL(stereoOdomInfo);
// 1 RGBD
SYNC_DEL(rgbdScan2d);
SYNC_DEL(rgbdScan3d);
SYNC_DEL(rgbdInfo);
// 1 RGBD + Odom
SYNC_DEL(rgbdOdom);
SYNC_DEL(rgbdOdomScan2d);
SYNC_DEL(rgbdOdomScan3d);
SYNC_DEL(rgbdOdomInfo);
// 1 RGBD + User Data
SYNC_DEL(rgbdData);
SYNC_DEL(rgbdDataScan2d);
SYNC_DEL(rgbdDataScan3d);
SYNC_DEL(rgbdDataInfo);
// 1 RGBD + Odom + User Data
SYNC_DEL(rgbdOdomData);
SYNC_DEL(rgbdOdomDataScan2d);
SYNC_DEL(rgbdOdomDataScan3d);
SYNC_DEL(rgbdOdomDataInfo);
// 2 RGBD
SYNC_DEL(rgbd2);
SYNC_DEL(rgbd2Scan2d);
SYNC_DEL(rgbd2Scan3d);
SYNC_DEL(rgbd2Info);
// 2 RGBD + Odom
SYNC_DEL(rgbd2Odom);
SYNC_DEL(rgbd2OdomScan2d);
SYNC_DEL(rgbd2OdomScan3d);
SYNC_DEL(rgbd2OdomInfo);
// 2 RGBD + User Data
SYNC_DEL(rgbd2Data);
SYNC_DEL(rgbd2DataScan2d);
SYNC_DEL(rgbd2DataScan3d);
SYNC_DEL(rgbd2DataInfo);
// 2 RGBD + Odom + User Data
SYNC_DEL(rgbd2OdomData);
SYNC_DEL(rgbd2OdomDataScan2d);
SYNC_DEL(rgbd2OdomDataScan3d);
SYNC_DEL(rgbd2OdomDataInfo);
for(unsigned int i=0; i<rgbdSubs_.size(); ++i)
{
delete rgbdSubs_[i];
}
rgbdSubs_.clear();
}
void CommonDataSubscriber::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)
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
if(imageMsg.get())
{
imageMsgs.push_back(imageMsg);
}
if(depthMsg.get())
{
depthMsgs.push_back(depthMsg);
}
cameraInfoMsgs.push_back(cameraInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
} /* namespace rtabmap_ros */
src/CoreNode.cpp
+1 -1
View File
@@ -89,7 +89,7 @@ int main(int argc, char** argv)
rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv);
CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart, parameters);
rtabmap_ros::CoreWrapper * rtabmap = new rtabmap_ros::CoreWrapper(deleteDbOnStart, parameters);
ROS_INFO("rtabmap %s started...", RTABMAP_VERSION);
ros::spin();
src/CoreWrapper.cpp
+138 -852
View File
File diff suppressed because it is too large Load Diff
src/GuiNode.cpp
+1 -1
View File
@@ -54,7 +54,7 @@ int main(int argc, char** argv)
app = new QApplication(argc, argv);
app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) );
GuiWrapper * gui = new GuiWrapper(argc, argv);
rtabmap_ros::GuiWrapper * gui = new rtabmap_ros::GuiWrapper(argc, argv);
// Catch ctrl-c to close the gui
// (Place this after QApplication's constructor)
src/GuiWrapper.cpp
+29 -1089
View File
File diff suppressed because it is too large Load Diff
src/MsgConversion.cpp
+155 -26
View File
@@ -40,7 +40,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_geometry/pinhole_camera_model.h>
#include <image_geometry/stereo_camera_model.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <laser_geometry/laser_geometry.h>
#include <rtabmap/core/util3d_surface.h>
@@ -119,6 +118,78 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg)
return rtabmap::Transform::fromEigen3d(tfPose);
}
void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth)
{
if(!image.rgb.data.empty())
{
rgb = cv_bridge::toCvCopy(image.rgb);
}
else if(!image.rgbCompressed.data.empty())
{
rgb = cv_bridge::toCvCopy(image.rgbCompressed);
}
else
{
// empty
rgb = boost::make_shared<cv_bridge::CvImage>();
}
if(!image.depth.data.empty())
{
depth = cv_bridge::toCvCopy(image.depth);
}
else if(!image.depthCompressed.data.empty())
{
cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
ptr->header = image.depthCompressed.header;
ptr->image = rtabmap::uncompressImage(image.depthCompressed.data);
ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
depth = ptr;
}
else
{
// empty
depth = boost::make_shared<cv_bridge::CvImage>();
}
}
void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth)
{
if(!image->rgb.data.empty())
{
rgb = cv_bridge::toCvShare(image->rgb, image);
}
else if(!image->rgbCompressed.data.empty())
{
rgb = cv_bridge::toCvCopy(image->rgbCompressed);
}
else
{
// empty
rgb = boost::make_shared<cv_bridge::CvImage>();
}
if(!image->depth.data.empty())
{
depth = cv_bridge::toCvShare(image->depth, image);
}
else if(!image->depthCompressed.data.empty())
{
cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
ptr->header = image->depthCompressed.header;
ptr->image = rtabmap::uncompressImage(image->depthCompressed.data);
ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
depth = ptr;
}
else
{
// empty
depth = boost::make_shared<cv_bridge::CvImage>();
}
}
void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes)
{
UASSERT(compressed.empty() || compressed.type() == CV_8UC1);
@@ -566,9 +637,11 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
{
// stereo model
if(msg.fx.size() == 1 &&
msg.fy.size() == 1,
msg.cx.size() == 1,
msg.cy.size() == 1,
msg.fy.size() == 1 &&
msg.cx.size() == 1 &&
msg.cy.size() == 1 &&
msg.width.size() == 1 &&
msg.height.size() == 1 &&
msg.localTransform.size() == 1)
{
stereoModel = rtabmap::StereoCameraModel(
@@ -577,7 +650,8 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
msg.cx[0],
msg.cy[0],
msg.baseline,
transformFromGeometryMsg(msg.localTransform[0]));
transformFromGeometryMsg(msg.localTransform[0]),
cv::Size(msg.width[0], msg.height[0]));
}
}
else
@@ -596,7 +670,9 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
msg.fy[i],
msg.cx[i],
msg.cy[i],
transformFromGeometryMsg(msg.localTransform[i])));
transformFromGeometryMsg(msg.localTransform[i]),
0.0,
cv::Size(msg.width[i], msg.height[i])));
}
}
}
@@ -672,6 +748,8 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
msg.fy.resize(signature.sensorData().cameraModels().size());
msg.cx.resize(signature.sensorData().cameraModels().size());
msg.cy.resize(signature.sensorData().cameraModels().size());
msg.width.resize(signature.sensorData().cameraModels().size());
msg.height.resize(signature.sensorData().cameraModels().size());
msg.localTransform.resize(signature.sensorData().cameraModels().size());
for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
{
@@ -679,6 +757,8 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
msg.fy[i] = signature.sensorData().cameraModels()[i].fy();
msg.cx[i] = signature.sensorData().cameraModels()[i].cx();
msg.cy[i] = signature.sensorData().cameraModels()[i].cy();
msg.width[i] = signature.sensorData().cameraModels()[i].imageWidth();
msg.height[i] = signature.sensorData().cameraModels()[i].imageHeight();
transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.localTransform[i]);
}
}
@@ -688,6 +768,8 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
msg.fy.push_back(signature.sensorData().stereoCameraModel().left().fy());
msg.cx.push_back(signature.sensorData().stereoCameraModel().left().cx());
msg.cy.push_back(signature.sensorData().stereoCameraModel().left().cy());
msg.width.push_back(signature.sensorData().stereoCameraModel().left().imageWidth());
msg.height.push_back(signature.sensorData().stereoCameraModel().left().imageHeight());
msg.baseline = signature.sensorData().stereoCameraModel().baseline();
msg.localTransform.resize(1);
transformToGeometryMsg(signature.sensorData().stereoCameraModel().left().localTransform(), msg.localTransform[0]);
@@ -868,6 +950,52 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.localScanMap = rtabmap::compressData(info.localScanMap);
}
cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg)
{
cv::Mat data;
if(!dataMsg.data.empty())
{
if(dataMsg.cols > 0 && dataMsg.rows > 0 && dataMsg.type >= 0)
{
data = cv::Mat(dataMsg.rows, dataMsg.cols, dataMsg.type, (void*)dataMsg.data.data()).clone();
}
else
{
if(dataMsg.cols != (int)dataMsg.data.size() || dataMsg.rows != 1 || dataMsg.type != CV_8UC1)
{
ROS_ERROR("cols, rows and type fields of the UserData msg "
"are not correctly set (cols=%d, rows=%d, type=%d)! We assume that the data "
"is compressed (cols=%d, rows=1, type=%d(CV_8UC1)).",
dataMsg.cols, dataMsg.rows, dataMsg.type, (int)dataMsg.data.size(), CV_8UC1);
}
data = cv::Mat(1, dataMsg.data.size(), CV_8UC1, (void*)dataMsg.data.data()).clone();
}
}
return data;
}
void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress)
{
if(!data.empty())
{
if(compress)
{
dataMsg.data = rtabmap::compressData(data);
dataMsg.rows = 1;
dataMsg.cols = dataMsg.data.size();
dataMsg.type = CV_8UC1;
}
else
{
dataMsg.data.resize(data.step[0] * data.rows); // use step for non-contiguous matrices
memcpy(dataMsg.data.data(), data.data, dataMsg.data.size());
dataMsg.rows = data.rows;
dataMsg.cols = data.cols;
dataMsg.type = data.type();
}
}
}
rtabmap::Transform getTransform(
const std::string & fromFrameId,
const std::string & toFrameId,
@@ -940,9 +1068,9 @@ rtabmap::Transform getTransform(
}
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,
@@ -956,8 +1084,8 @@ bool convertRGBDMsgs(
imageMsgs.size() == depthMsgs.size() &&
imageMsgs.size() == cameraInfoMsgs.size());
int imageWidth = imageMsgs[0]->width;
int imageHeight = imageMsgs[0]->height;
int imageWidth = imageMsgs[0]->image.cols;
int imageHeight = imageMsgs[0]->image.rows;
int cameraCount = imageMsgs.size();
for(unsigned int i=0; i<imageMsgs.size(); ++i)
{
@@ -974,18 +1102,18 @@ bool convertRGBDMsgs(
return false;
}
UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight,
UASSERT_MSG(imageMsgs[i]->image.cols == imageWidth && imageMsgs[i]->image.rows == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
imageMsgs[i]->width,
imageMsgs[i]->image.cols,
imageHeight,
imageMsgs[i]->height).c_str());
UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight,
imageMsgs[i]->image.rows).c_str());
UASSERT_MSG(depthMsgs[i]->image.cols == imageWidth && depthMsgs[i]->image.rows == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
depthMsgs[i]->width,
depthMsgs[i]->image.cols,
imageHeight,
depthMsgs[i]->height).c_str());
depthMsgs[i]->image.rows).c_str());
rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp, listener, waitForTransform);
if(localTransform.isNull())
@@ -1015,21 +1143,22 @@ bool convertRGBDMsgs(
}
}
cv_bridge::CvImageConstPtr ptrImage;
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
cv_bridge::CvImageConstPtr ptrImage = imageMsgs[i];
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
// do nothing
}
else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
ptrImage = cv_bridge::cvtColor(imageMsgs[i], "mono8");
}
else
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8");
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i];
cv::Mat subDepth = ptrDepth->image;
// initialize
@@ -1062,7 +1191,7 @@ bool convertRGBDMsgs(
return false;
}
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform));
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform));
}
return true;
}
src/impl/CommonDataSubscriberDepth.cpp
+369
View File
@@ -0,0 +1,369 @@
/*
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.
*/
#include <rtabmap_ros/CommonDataSubscriber.h>
namespace rtabmap_ros {
// RGB + Depth
void CommonDataSubscriber::depthCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScan2dCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScan3dCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthInfoCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
// RGB + Depth + Odom
void CommonDataSubscriber::depthOdomCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScan2dCallback(
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)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
// RGB + Depth + User Data
void CommonDataSubscriber::depthDataCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
{
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScan2dCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
nav_msgs::OdometryConstPtr odomMsg; // null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScan3dCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
{
nav_msgs::OdometryConstPtr odomMsg; // null
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataInfoCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
nav_msgs::OdometryConstPtr odomMsg; // null
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
// RGB + Depth + Odom + User Data
void CommonDataSubscriber::depthOdomDataCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
{
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
{
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::setupDepthCallbacks(
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
{
ROS_INFO("Setup depth callback");
ros::NodeHandle nh; // public
ros::NodeHandle pnh("~"); // private
std::string rgbPrefix = "rgb";
std::string depthPrefix = "depth";
ros::NodeHandle rgb_nh(nh, rgbPrefix);
ros::NodeHandle depth_nh(nh, depthPrefix);
ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
ros::NodeHandle depth_pnh(pnh, depthPrefix);
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(nh, "odom", 1);
userDataSub_.subscribe(nh, "user_data", 1);
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL6(depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
else if(subscribeOdom)
{
odomSub_.subscribe(nh, "odom", 1);
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL5(depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
else if(subscribeUserData)
{
userDataSub_.subscribe(nh, "user_data", 1);
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL5(depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
else
{
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL4(depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
}
} /* namespace rtabmap_ros */
src/impl/CommonDataSubscriberRGBD.cpp
+386
View File
@@ -0,0 +1,386 @@
/*
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.
*/
#include <rtabmap_ros/CommonDataSubscriber.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap_ros/MsgConversion.h>
#include <cv_bridge/cv_bridge.h>
namespace rtabmap_ros {
// 1 RGBD camera
void CommonDataSubscriber::rgbdCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbdScan2dCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbdScan3dCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbdInfoCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
// 1 RGBD camera + Odom
void CommonDataSubscriber::rgbdOdomCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbdOdomScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbdOdomScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbdOdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
// 1 RGBD camera + User Data
void CommonDataSubscriber::rgbdDataCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbdDataScan2dCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbdDataScan3dCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbdDataInfoCallback(
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
// 1 RGBD camera + Odom + User Data
void CommonDataSubscriber::rgbdOdomDataCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
sensor_msgs::LaserScanConstPtr scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbdOdomDataInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
cv_bridge::CvImageConstPtr rgb, depth;
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::setupRGBDCallbacks(
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
{
ROS_INFO("Setup rgbd callback");
ros::NodeHandle nh; // public
ros::NodeHandle pnh("~"); // private
if(subscribeOdom || subscribeUserData || subscribeScan2d || subscribeScan3d || subscribeOdomInfo)
{
rgbdSubs_.resize(1);
rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
rgbdSubs_[0]->subscribe(nh, "rgbd_image", 1);
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(nh, "odom", 1);
userDataSub_.subscribe(nh, "user_data", 1);
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL4(rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL3(rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
}
}
else if(subscribeOdom)
{
odomSub_.subscribe(nh, "odom", 1);
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL3(rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL2(rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
}
}
else if(subscribeUserData)
{
userDataSub_.subscribe(nh, "user_data", 1);
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL3(rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL2(rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
}
}
else
{
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL2(rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
ROS_FATAL("Not supposed to be here!");
}
}
}
else
{
rgbdSub_ = nh.subscribe("rgbd_image", 1, &CommonDataSubscriber::rgbdCallback, this);
ROS_INFO("\n%s subscribed to:\n %s",
ros::this_node::getName().c_str(),
rgbdSub_.getTopic().c_str());
}
}
} /* namespace rtabmap_ros */
src/impl/CommonDataSubscriberRGBD2.cpp
+385
View File
@@ -0,0 +1,385 @@
/*
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.
*/
#include <rtabmap_ros/CommonDataSubscriber.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap_ros/MsgConversion.h>
#include <cv_bridge/cv_bridge.h>
namespace rtabmap_ros {
#define IMAGE_CONVERSION() \
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2); \
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2); \
rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
cameraInfoMsgs.push_back(image1Msg->cameraInfo); \
cameraInfoMsgs.push_back(image2Msg->cameraInfo);
// 2 RGBD
void CommonDataSubscriber::rgbd2Callback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd2Scan2dCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd2Scan3dCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd2InfoCallback(
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
// 2 RGBD + Odom
void CommonDataSubscriber::rgbd2OdomCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg)
{
IMAGE_CONVERSION();
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd2OdomScan2dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
IMAGE_CONVERSION();
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd2OdomScan3dCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
{
IMAGE_CONVERSION();
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd2OdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
IMAGE_CONVERSION();
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
// 2 RGBD + User Data
void CommonDataSubscriber::rgbd2DataCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd2DataScan2dCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd2DataScan3dCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd2DataInfoCallback(
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
IMAGE_CONVERSION();
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
// 2 RGBD + Odom + User Data
void CommonDataSubscriber::rgbd2OdomDataCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg)
{
IMAGE_CONVERSION();
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
IMAGE_CONVERSION();
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
{
IMAGE_CONVERSION();
sensor_msgs::LaserScanConstPtr scanMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
const nav_msgs::OdometryConstPtr& odomMsg,
const rtabmap_ros::UserDataConstPtr& userDataMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
IMAGE_CONVERSION();
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::setupRGBD2Callbacks(
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
{
ROS_INFO("Setup rgbd2 callback");
ros::NodeHandle nh; // public
ros::NodeHandle pnh("~"); // private
rgbdSubs_.resize(2);
for(int i=0; i<2; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1);
}
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(nh, "odom", 1);
userDataSub_.subscribe(nh, "user_data", 1);
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL5(rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL4(rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
else if(subscribeOdom)
{
odomSub_.subscribe(nh, "odom", 1);
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL4(rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL3(rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
else if(subscribeUserData)
{
userDataSub_.subscribe(nh, "user_data", 1);
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL4(rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL3(rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
else
{
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL3(rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL2(rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
}
} /* namespace rtabmap_ros */
src/impl/CommonDataSubscriberStereo.cpp
+138
View File
@@ -0,0 +1,138 @@
/*
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.
*/
#include <rtabmap_ros/CommonDataSubscriber.h>
namespace rtabmap_ros {
// Stereo
void CommonDataSubscriber::stereoCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
{
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::stereoInfoCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scan2dMsg; // null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
// Stereo + Odom
void CommonDataSubscriber::stereoOdomCallback(
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)
{
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::stereoOdomInfoCallback(
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 rtabmap_ros::OdomInfoConstPtr & odomInfoMsg)
{
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::setupStereoCallbacks(
bool subscribeOdom,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
{
ROS_INFO("Setup stereo callback");
ros::NodeHandle nh; // public
ros::NodeHandle pnh("~"); // private
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
ros::NodeHandle left_pnh(pnh, "left");
ros::NodeHandle right_pnh(pnh, "right");
image_transport::ImageTransport left_it(left_nh);
image_transport::ImageTransport right_it(right_nh);
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
if(subscribeOdom)
{
odomSub_.subscribe(nh, "odom", 1);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL6(stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
}
else
{
SYNC_DECL5(stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
}
}
else
{
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL5(stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
}
else
{
SYNC_DECL4(stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
}
}
}
} /* namespace rtabmap_ros */
src/nodelets/rgbd_odometry.cpp
+193 -230
View File
@@ -63,7 +63,8 @@ public:
OdometryROS(false, true, false),
approxSync_(0),
exactSync_(0),
sync2_(0),
approxSync2_(0),
exactSync2_(0),
queueSize_(5)
{
}
@@ -78,9 +79,13 @@ public:
{
delete exactSync_;
}
if(sync2_)
if(approxSync2_)
{
delete sync2_;
delete approxSync2_;
}
if(exactSync2_)
{
delete exactSync2_;
}
}
@@ -91,65 +96,50 @@ private:
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int depthCameras = 1;
int rgbdCameras = 1;
bool approxSync = true;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize_, queueSize_);
pnh.param("depth_cameras", depthCameras, depthCameras);
if(depthCameras <= 0)
if(pnh.hasParam("depth_cameras"))
{
depthCameras = 1;
ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" with the \"rgbd_image\" input topics.");
}
if(depthCameras > 2)
pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
if(rgbdCameras <= 0)
{
rgbdCameras = 1;
}
if(rgbdCameras > 2)
{
NODELET_FATAL("Only 2 cameras maximum supported yet.");
}
if(depthCameras == 2)
if(rgbdCameras == 2)
{
ros::NodeHandle rgb0_nh(nh, "rgb0");
ros::NodeHandle depth0_nh(nh, "depth0");
ros::NodeHandle rgb0_pnh(pnh, "rgb0");
ros::NodeHandle depth0_pnh(pnh, "depth0");
image_transport::ImageTransport rgb0_it(rgb0_nh);
image_transport::ImageTransport depth0_it(depth0_nh);
image_transport::TransportHints hintsRgb0("raw", ros::TransportHints(), rgb0_pnh);
image_transport::TransportHints hintsDepth0("raw", ros::TransportHints(), depth0_pnh);
rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1);
rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1);
image_mono_sub_.subscribe(rgb0_it, rgb0_nh.resolveName("image"), 1, hintsRgb0);
image_depth_sub_.subscribe(depth0_it, depth0_nh.resolveName("image"), 1, hintsDepth0);
info_sub_.subscribe(rgb0_nh, "camera_info", 1);
ros::NodeHandle rgb1_nh(nh, "rgb1");
ros::NodeHandle depth1_nh(nh, "depth1");
ros::NodeHandle rgb1_pnh(pnh, "rgb1");
ros::NodeHandle depth1_pnh(pnh, "depth1");
image_transport::ImageTransport rgb1_it(rgb1_nh);
image_transport::ImageTransport depth1_it(depth1_nh);
image_transport::TransportHints hintsRgb1("raw", ros::TransportHints(), rgb1_pnh);
image_transport::TransportHints hintsDepth1("raw", ros::TransportHints(), depth1_pnh);
image_mono2_sub_.subscribe(rgb1_it, rgb1_nh.resolveName("image"), 1, hintsRgb1);
image_depth2_sub_.subscribe(depth1_it, depth1_nh.resolveName("image"), 1, hintsDepth1);
info2_sub_.subscribe(rgb1_nh, "camera_info", 1);
sync2_ = new message_filters::Synchronizer<MySync2Policy>(
MySync2Policy(queueSize_),
image_mono_sub_,
image_depth_sub_,
info_sub_,
image_mono2_sub_,
image_depth2_sub_,
info2_sub_);
sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
NODELET_INFO("\n%s subscribed to (approx sync):\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
if(approxSync)
{
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2));
}
else
{
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2));
}
NODELET_INFO("\n%s subscribed to (%s sync):\n %s,\n %s",
ros::this_node::getName().c_str(),
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str(),
image_mono2_sub_.getTopic().c_str(),
image_depth2_sub_.getTopic().c_str(),
info2_sub_.getTopic().c_str());
approxSync?"approx":"exact",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str());
}
else
{
@@ -197,6 +187,117 @@ private:
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
}
void commonCallback(
const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
const std::vector<sensor_msgs::CameraInfo>& cameraInfos)
{
ROS_ASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
ros::Time higherStamp;
int imageWidth = rgbImages[0]->image.cols;
int imageHeight = rgbImages[0]->image.rows;
int cameraCount = rgbImages.size();
cv::Mat rgb;
cv::Mat depth;
pcl::PointCloud<pcl::PointXYZ> scanCloud;
std::vector<CameraModel> cameraModels;
for(unsigned int i=0; i<rgbImages.size(); ++i)
{
if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
rgbImages[i]->image.cols,
imageHeight,
rgbImages[i]->image.rows).c_str());
UASSERT_MSG(depthImages[i]->image.cols == imageWidth && depthImages[i]->image.rows == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
depthImages[i]->image.cols,
imageHeight,
depthImages[i]->image.rows).c_str());
ros::Time stamp = rgbImages[i]->header.stamp>depthImages[i]->header.stamp?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
if(i == 0)
{
higherStamp = stamp;
}
else if(stamp > higherStamp)
{
higherStamp = stamp;
}
Transform localTransform = getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp);
if(localTransform.isNull())
{
return;
}
cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
}
cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
cv::Mat subDepth = ptrDepth->image;
// initialize
if(rgb.empty())
{
rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
}
if(depth.empty())
{
depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
}
if(ptrImage->image.type() == rgb.type())
{
ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
NODELET_ERROR("Some RGB images are not the same type!");
return;
}
if(subDepth.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
NODELET_ERROR("Some Depth images are not the same type!");
return;
}
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfos[i], localTransform));
}
rtabmap::SensorData data(
rgb,
depth,
cameraModels,
0,
rtabmap_ros::timestampFromROS(higherStamp));
this->processData(data, higherStamp);
}
void callback(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
@@ -204,178 +305,32 @@ private:
{
if(!this->isPaused())
{
if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
{
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 "
"recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s",
image->encoding.c_str(), depth->encoding.c_str());
return;
}
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
std::vector<sensor_msgs::CameraInfo> infoMsgs;
imageMsgs[0] = cv_bridge::toCvShare(image);
depthMsgs[0] = cv_bridge::toCvShare(depth);
infoMsgs.push_back(*cameraInfo);
// use the highest stamp to make sure that there will be no future interpolation required when synchronized with another node
ros::Time stamp = image->header.stamp > depth->header.stamp? image->header.stamp : depth->header.stamp;
Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);
if(localTransform.isNull())
{
return;
}
if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
{
rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
rtabmap::SensorData data(
ptrImage->image,
ptrDepth->image,
rtabmapModel,
0,
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, stamp);
}
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
}
}
void callback2(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo,
const sensor_msgs::ImageConstPtr& image2,
const sensor_msgs::ImageConstPtr& depth2,
const sensor_msgs::CameraInfoConstPtr& cameraInfo2)
const rtabmap_ros::RGBDImageConstPtr& image,
const rtabmap_ros::RGBDImageConstPtr& image2)
{
if(!this->isPaused())
{
std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfoConstPtr> infoMsgs;
imageMsgs.push_back(image);
imageMsgs.push_back(image2);
depthMsgs.push_back(depth);
depthMsgs.push_back(depth2);
infoMsgs.push_back(cameraInfo);
infoMsgs.push_back(cameraInfo2);
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2);
std::vector<sensor_msgs::CameraInfo> infoMsgs;
rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
infoMsgs.push_back(image->cameraInfo);
infoMsgs.push_back(image2->cameraInfo);
ros::Time higherStamp;
int imageWidth = imageMsgs[0]->width;
int imageHeight = imageMsgs[0]->height;
int cameraCount = imageMsgs.size();
cv::Mat rgb;
cv::Mat depth;
pcl::PointCloud<pcl::PointXYZ> scanCloud;
std::vector<CameraModel> cameraModels;
for(unsigned int i=0; i<imageMsgs.size(); ++i)
{
if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
imageMsgs[i]->width,
imageHeight,
imageMsgs[i]->height).c_str());
UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
depthMsgs[i]->width,
imageHeight,
depthMsgs[i]->height).c_str());
ros::Time stamp = imageMsgs[i]->header.stamp>depthMsgs[i]->header.stamp?imageMsgs[i]->header.stamp:depthMsgs[i]->header.stamp;
if(i == 0)
{
higherStamp = stamp;
}
else if(stamp > higherStamp)
{
higherStamp = stamp;
}
Transform localTransform = getTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp);
if(localTransform.isNull())
{
return;
}
cv_bridge::CvImageConstPtr ptrImage;
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
}
else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
}
else
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
cv::Mat subDepth = ptrDepth->image;
// initialize
if(rgb.empty())
{
rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
}
if(depth.empty())
{
depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
}
if(ptrImage->image.type() == rgb.type())
{
ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
NODELET_ERROR("Some RGB images are not the same type!");
return;
}
if(subDepth.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
NODELET_ERROR("Some Depth images are not the same type!");
return;
}
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform));
}
rtabmap::SensorData data(
rgb,
depth,
cameraModels,
0,
rtabmap_ros::timestampFromROS(higherStamp));
this->processData(data, higherStamp);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
}
}
@@ -395,18 +350,23 @@ protected:
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
if(sync2_)
if(approxSync2_)
{
delete sync2_;
sync2_ = new message_filters::Synchronizer<MySync2Policy>(
MySync2Policy(queueSize_),
image_mono_sub_,
image_depth_sub_,
info_sub_,
image_mono2_sub_,
image_depth2_sub_,
info2_sub_);
sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
delete approxSync2_;
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2));
}
if(exactSync2_)
{
delete exactSync2_;
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2));
}
}
@@ -414,15 +374,18 @@ private:
image_transport::SubscriberFilter image_mono_sub_;
image_transport::SubscriberFilter image_depth_sub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
image_transport::SubscriberFilter image_mono2_sub_;
image_transport::SubscriberFilter image_depth2_sub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> info2_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image1_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image2_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySync2Policy;
message_filters::Synchronizer<MySync2Policy> * sync2_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync2Policy;
message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync2Policy;
message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
int queueSize_;
};
src/nodelets/rgbd_sync.cpp
+165
View File
@@ -0,0 +1,165 @@
/*
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.
*/
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/subscriber.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap_ros/RGBDImage.h"
#include "rtabmap/core/Compression.h"
namespace rtabmap_ros
{
class RGBDSync : public nodelet::Nodelet
{
public:
RGBDSync() :
approxSyncDepth_(0),
exactSyncDepth_(0)
{}
virtual ~RGBDSync()
{
if(approxSyncDepth_)
delete approxSyncDepth_;
if(exactSyncDepth_)
delete exactSyncDepth_;
}
private:
virtual void onInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
bool approxSync = true;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
}
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
}
void callback(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
{
rtabmap_ros::RGBDImage msg;
msg.header.frame_id = cameraInfo->header.frame_id;
msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
msg.cameraInfo = *cameraInfo;
if(rgbdImageCompressedPub_.getNumSubscribers())
{
rtabmap_ros::RGBDImage msgCompressed = msg;
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
ROS_ASSERT(imageDepthPtr->image.type() == CV_32FC1 || imageDepthPtr->image.type() == CV_16UC1);
msgCompressed.depthCompressed.header = imageDepthPtr->header;
msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
msgCompressed.depthCompressed.format = "png";
rgbdImageCompressedPub_.publish(msgCompressed);
}
if(rgbdImagePub_.getNumSubscribers())
{
msg.rgb = *image;
msg.depth = *depth;
rgbdImagePub_.publish(msg);
}
}
}
private:
ros::Publisher rgbdImagePub_;
ros::Publisher rgbdImageCompressedPub_;
image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDSync, nodelet::Nodelet);
}