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

Added multi-cameras demo: demo_two_kinects.launch

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-31 01:28:54 -04:00
parent fcd343cdd9
commit 96d3ad35e2
8 changed files with 1254 additions and 365 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/demo/demo_two_kinects.launch
+139
View File
@@ -0,0 +1,139 @@
<launch>
<!-- Multi-cameras demo with 2 Kinects -->
<!-- Cameras -->
<include file="$(find freenect_launch)/launch/freenect.launch">
<arg name="depth_registration" value="True" />
<arg name="camera" value="camera1" />
<arg name="device_id" value="#1" />
</include>
<include file="$(find freenect_launch)/launch/freenect.launch">
<arg name="depth_registration" value="True" />
<arg name="camera" value="camera2" />
<arg name="device_id" value="#2" />
</include>
<!-- Frames: Kinects are placed at 90 degrees -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_link 100" />
<node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
args="-0.1325 -0.1975 0.0 -1.570796327 0.0 0.0 /base_link /camera2_link 100" />
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<!-- ODOMETRY MAIN ARGUMENTS:
-"strategy" : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-"feature" : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-"nn" : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-"max_depth" : Maximum features depth (m)
-"min_inliers" : Minimum visual correspondences to accept a transformation (m)
-"inlier_distance" : RANSAC maximum inliers distance (m)
-"local_map" : Local map size: number of unique features to keep track
-"odom_info_data" : Fill odometry info messages with inliers/outliers data.
-->
<arg name="strategy" default="0" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="max_depth" default="4.0" />
<arg name="min_inliers" default="20" />
<arg name="inlier_distance" default="0.02" />
<arg name="local_map" default="1000" />
<arg name="odom_info_data" default="true" />
<arg name="wait_for_transform" default="true" />
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="rgb0/image" to="/camera1/rgb/image_rect_color"/>
<remap from="depth0/image" to="/camera1/depth_registered/image_raw"/>
<remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
<remap from="rgb1/image" to="/camera2/rgb/image_rect_color"/>
<remap from="depth1/image" to="/camera2/depth_registered/image_raw"/>
<remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="depth_cameras" type="int" value="2"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="depth_cameras" type="int" value="2"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="gen_scan" type="bool" value="true"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<remap from="rgb0/image" to="/camera1/rgb/image_rect_color"/>
<remap from="depth0/image" to="/camera1/depth_registered/image_raw"/>
<remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
<remap from="rgb1/image" to="/camera2/rgb/image_rect_color"/>
<remap from="depth1/image" to="/camera2/depth_registered/image_raw"/>
<remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="$(arg odom_info_data)"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="depth_cameras" type="int" value="2"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<remap from="rgb0/image" to="/camera1/rgb/image_rect_color"/>
<remap from="depth0/image" to="/camera1/depth_registered/image_raw"/>
<remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
<remap from="rgb1/image" to="/camera2/rgb/image_rect_color"/>
<remap from="depth1/image" to="/camera2/depth_registered/image_raw"/>
<remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="camera1/rgb/image_rect_color"/>
<remap from="depth/image_in" to="camera1/depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="camera1/rgb/camera_info"/>
<remap from="odom_in" to="rtabmap/odom"/>
<remap from="rgb/image_out" to="data_odom_sync/image"/>
<remap from="depth/image_out" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
<remap from="odom_out" to="odom_sync"/>
</node>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="data_odom_sync/image"/>
<remap from="depth/image" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="voxel_size" type="double" value="0.01"/>
</node>
</launch>
src/CoreWrapper.cpp
+345 -175
View File
@@ -45,7 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/core/util3d_conversions.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/OdometryEvent.h>
@@ -83,12 +83,15 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
waitForTransform_(false),
useActionForGoal_(false),
genScan_(false),
genScanMaxDepth_(4.0),
mapToOdom_(rtabmap::Transform::getIdentity()),
depthSync_(0),
depthScanSync_(0),
stereoScanSync_(0),
stereoApproxSync_(0),
stereoExactSync_(0),
depth2Sync_(0),
depthTFSync_(0),
depthScanTFSync_(0),
stereoScanTFSync_(0),
@@ -105,6 +108,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
bool subscribeLaserScan = false;
bool subscribeDepth = true;
bool subscribeStereo = false;
int depthCameras = 1;
int queueSize = 10;
bool publishTf = true;
double tfDelay = 0.05; // 20 Hz
@@ -134,6 +138,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
pnh.param("frame_id", frameId_, frameId_);
pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
pnh.param("depth_cameras", depthCameras, depthCameras);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("stereo_approx_sync", stereoApproxSync, stereoApproxSync);
@@ -141,6 +146,13 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
pnh.param("tf_delay", tfDelay, tfDelay);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
pnh.param("gen_scan", genScan_, genScan_);
pnh.param("gen_scan_max_depth", genScanMaxDepth_, genScanMaxDepth_);
if(depthCameras <= 0 && subscribeDepth)
{
depthCameras = 1;
}
ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str());
if(!odomFrameId_.empty())
@@ -150,6 +162,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
ROS_INFO("rtabmap: queue_size = %d", queueSize);
ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
ROS_INFO("rtabmap: depth_cameras = %d", depthCameras);
infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
@@ -352,7 +365,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this);
#endif
setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync);
setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync, depthCameras);
int optimizeIterations = 0;
Parameters::parse(parameters_, Parameters::kRGBDOptimizeIterations(), optimizeIterations);
@@ -385,6 +398,8 @@ CoreWrapper::~CoreWrapper()
delete stereoApproxSync_;
if(stereoExactSync_)
delete stereoExactSync_;
if(depth2Sync_)
delete depth2Sync_;
if(depthTFSync_)
delete depthTFSync_;
if(depthScanTFSync_)
@@ -396,6 +411,22 @@ CoreWrapper::~CoreWrapper()
if(stereoExactTFSync_)
delete stereoExactTFSync_;
for(unsigned int i=0; i<imageSubs_.size(); ++i)
{
delete imageSubs_[i];
}
imageSubs_.clear();
for(unsigned int i=0; i<imageDepthSubs_.size(); ++i)
{
delete imageDepthSubs_[i];
}
imageDepthSubs_.clear();
for(unsigned int i=0; i<cameraInfoSubs_.size(); ++i)
{
delete cameraInfoSubs_[i];
}
cameraInfoSubs_.clear();
this->saveParameters(configPath_);
ros::NodeHandle nh;
@@ -667,17 +698,24 @@ void CoreWrapper::commonDepthCallback(
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
imageMsgs.push_back(imageMsg);
depthMsgs.push_back(depthMsg);
cameraInfoMsgs.push_back(cameraInfoMsg);
commonDepthCallback(odomFrameId, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg);
}
void CoreWrapper::commonDepthCallback(
const std::string & odomFrameId,
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
UASSERT(imageMsgs.size()>0 &&
imageMsgs.size() == depthMsgs.size() &&
imageMsgs.size() == cameraInfoMsgs.size());
//for sync transform
Transform odomT = getTransform(odomFrameId, frameId_, lastPoseStamp_);
@@ -687,17 +725,40 @@ void CoreWrapper::commonDepthCallback(
odomFrameId.c_str(), frameId_.c_str());
}
Transform localTransform = getTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp);
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::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))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight);
UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight);
Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp);
if(localTransform.isNull())
{
return;
}
// sync with odometry stamp
if(lastPoseStamp_ != depthMsg->header.stamp)
if(lastPoseStamp_ != depthMsgs[i]->header.stamp)
{
if(!odomT.isNull())
{
Transform sensorT = getTransform(odomFrameId, frameId_, depthMsg->header.stamp);
Transform sensorT = getTransform(odomFrameId, frameId_, depthMsgs[i]->header.stamp);
if(sensorT.isNull())
{
return;
@@ -706,6 +767,82 @@ void CoreWrapper::commonDepthCallback(
}
}
cv_bridge::CvImageConstPtr ptrImage;
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;
if(subDepth.type() == CV_32FC1)
{
subDepth = util3d::cvtDepthFromFloat(subDepth);
static bool shown = false;
if(!shown)
{
ROS_WARN("Use depth image with \"unsigned short\" type to "
"avoid conversion. This message is only printed once...");
shown = true;
}
}
// 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
{
ROS_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
{
ROS_ERROR("Some Depth images are not the same type!");
return;
}
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsgs[i]);
cameraModels.push_back(rtabmap::CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
localTransform));
if(scanMsg.get() == 0 && genScan_)
{
scanCloud += util3d::laserScanFromDepthImage(
subDepth,
model.fx(),
model.fy(),
model.cx(),
model.cy(),
genScanMaxDepth_,
localTransform);
}
}
cv::Mat scan;
if(scanMsg.get() != 0)
{
@@ -746,42 +883,25 @@ void CoreWrapper::commonDepthCallback(
}
scan = util3d::laserScanFromPointCloud(*pclScan);
}
cv_bridge::CvImageConstPtr ptrImage;
if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
else if(scanCloud.size())
{
ptrImage = cv_bridge::toCvShare(imageMsg, "mono8");
scan = util3d::laserScanFromPointCloud(scanCloud);
}
else
{
ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
double fx = model.fx();
double fy = model.fy();
double cx = model.cx();
double cy = model.cy();
ros::Time stamp = scanMsg.get() != 0?scanMsg->header.stamp:depthMsgs[0]->header.stamp;
process(ptrImage->header.seq,
scanMsg.get() != 0?scanMsg->header.stamp:ptrDepth->header.stamp,
ptrImage->image,
process(stamp,
SensorData(scan,
scanMsg.get() != 0?(int)scanMsg->ranges.size():0,
rgb,
depth,
cameraModels,
imageMsgs[0]->header.seq,
rtabmap_ros::timestampFromROS(stamp)),
lastPose_,
odomFrameId,
rotVariance_>0?rotVariance_:1.0,
transVariance_>0?transVariance_:1.0,
ptrDepth->image,
fx,
fy,
cx,
cy,
0,
localTransform,
scan,
scanMsg.get() != 0?(int)scanMsg->ranges.size():0);
transVariance_>0?transVariance_:1.0);
rotVariance_ = 0;
transVariance_ = 0;
}
@@ -891,29 +1011,28 @@ void CoreWrapper::commonStereoCallback(
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg);
rtabmap::StereoCameraModel stereoModel(
model.left().fx(),
model.left().fy(),
model.left().cx(),
model.left().cy(),
model.baseline(),
localTransform);
double fx = model.left().fx();
double fy = model.left().fy();
double cx = model.left().cx();
double cy = model.left().cy();
double baseline = model.baseline();
process(leftImageMsg->header.seq,
scanMsg.get() != 0?scanMsg->header.stamp:leftImageMsg->header.stamp,
ros::Time stamp = scanMsg.get() != 0?scanMsg->header.stamp:leftImageMsg->header.stamp;
process(stamp,
SensorData(scan,
scanMsg.get() != 0?(int)scanMsg->ranges.size():0,
ptrLeftImage->image,
ptrRightImage->image,
stereoModel,
leftImageMsg->header.seq,
rtabmap_ros::timestampFromROS(stamp)),
lastPose_,
odomFrameId,
rotVariance_>0?rotVariance_:1.0,
transVariance_>0?transVariance_:1.0,
ptrRightImage->image,
fx,
fy,
cx,
cy,
baseline,
localTransform,
scan,
scanMsg.get() != 0?(int)scanMsg->ranges.size():0);
transVariance_>0?transVariance_:1.0);
rotVariance_ = 0;
transVariance_ = 0;
}
@@ -975,6 +1094,34 @@ void CoreWrapper::stereoScanCallback(
commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg);
}
void CoreWrapper::depth2Callback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& image1Msg,
const sensor_msgs::ImageConstPtr& depth1Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
const sensor_msgs::ImageConstPtr& image2Msg,
const sensor_msgs::ImageConstPtr& depth2Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg)
{
if(!commonOdomUpdate(odomMsg))
{
return;
}
std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
imageMsgs.push_back(image1Msg);
imageMsgs.push_back(image2Msg);
depthMsgs.push_back(depth1Msg);
depthMsgs.push_back(depth2Msg);
cameraInfoMsgs.push_back(cameraInfo1Msg);
cameraInfoMsgs.push_back(cameraInfo2Msg);
sensor_msgs::LaserScanConstPtr scanMsg; // Null
commonDepthCallback(odomMsg->header.frame_id, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg);
}
void CoreWrapper::depthTFCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
@@ -1029,90 +1176,17 @@ void CoreWrapper::stereoScanTFCallback(
}
void CoreWrapper::process(
int id,
const ros::Time & stamp,
const cv::Mat & image,
const SensorData & data,
const Transform & odom,
const std::string & odomFrameId,
double odomRotationalVariance,
double odomTransitionalVariance,
const cv::Mat & depthOrRightImage,
double fx,
double fy,
double cx,
double cy,
double baseline,
const Transform & localTransform,
const cv::Mat & scan,
int scanMaxPts)
double odomTransitionalVariance)
{
UTimer timer;
if(rtabmap_.isIDsGenerated() || id > 0)
if(rtabmap_.isIDsGenerated() || data.id() > 0)
{
double timeRtabmap = 0.0;
cv::Mat imageB;
if(!depthOrRightImage.empty())
{
if(depthOrRightImage.type() == CV_8UC1)
{
//right image
imageB = depthOrRightImage.clone();
}
else if(depthOrRightImage.type() != CV_16UC1)
{
// depth float
if(depthOrRightImage.type() == CV_32FC1)
{
//convert to 16 bits
imageB = util3d::cvtDepthFromFloat(depthOrRightImage);
static bool shown = false;
if(!shown)
{
ROS_WARN("Use depth image with \"unsigned short\" type to "
"avoid conversion. This message is only printed once...");
shown = true;
}
}
else
{
ROS_ERROR("Depth image must be of type \"unsigned short\"!");
return;
}
}
else
{
// depth short
imageB = depthOrRightImage.clone();
}
}
SensorData data;
if(baseline > 0)
{
//stereo
data = SensorData(
scan,
scanMaxPts,
image.clone(),
imageB,
StereoCameraModel(fx, fy, cx, cy, baseline, localTransform),
id,
rtabmap_ros::timestampFromROS(stamp));
}
else
{
//depth
data = SensorData(
scan,
scanMaxPts,
image.clone(),
imageB,
CameraModel(fx, fy, cx, cy, localTransform),
id,
rtabmap_ros::timestampFromROS(stamp));
}
if(rtabmap_.process(data, odom, OdometryEvent::generateCovarianceMatrix(odomRotationalVariance, odomTransitionalVariance)))
{
timeRtabmap = timer.ticks();
@@ -1482,12 +1556,6 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
req.global);
}
if(poses.size() && poses.size() != signatures.size())
{
ROS_ERROR("poses and signatures are not the same size!? %d vs %d", (int)poses.size(), (int)signatures.size());
return false;
}
//RGB-D SLAM data
rtabmap_ros::mapDataToROS(poses,
constraints,
@@ -2109,25 +2177,45 @@ void CoreWrapper::setupCallbacks(
bool subscribeLaserScan,
bool subscribeStereo,
int queueSize,
bool stereoApproxSync)
bool stereoApproxSync,
int depthCameras)
{
ros::NodeHandle nh; // public
ros::NodeHandle pnh("~"); // private
if(subscribeDepth)
{
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
UASSERT(depthCameras >= 1 && depthCameras <= 2);
UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan || !odomFrameId_.empty()), "Not yet supported!");
imageSubs_.resize(depthCameras);
imageDepthSubs_.resize(depthCameras);
cameraInfoSubs_.resize(depthCameras);
for(int i=0; i<depthCameras; ++i)
{
std::string rgbPrefix = "rgb";
std::string depthPrefix = "depth";
if(depthCameras>1)
{
rgbPrefix += uNumber2Str(i);
depthPrefix += uNumber2Str(i);
}
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);
imageSubs_[i] = new image_transport::SubscriberFilter;
imageDepthSubs_[i] = new image_transport::SubscriberFilter;
cameraInfoSubs_[i] = new message_filters::Subscriber<sensor_msgs::CameraInfo>;
imageSubs_[i]->subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
imageDepthSubs_[i]->subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSubs_[i]->subscribe(rgb_nh, "camera_info", 1);
}
if(odomFrameId_.empty())
{
@@ -2136,57 +2224,104 @@ void CoreWrapper::setupCallbacks(
{
ROS_INFO("Registering Depth+LaserScan callback...");
scanSub_.subscribe(nh, "scan", 1);
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
MyDepthScanSyncPolicy(queueSize),
*imageSubs_[0],
odomSub_,
*imageDepthSubs_[0],
*cameraInfoSubs_[0],
scanSub_);
depthScanSync_->registerCallback(boost::bind(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
odomSub_.getTopic().c_str(),
scanSub_.getTopic().c_str());
}
else //!subscribeLaserScan
{
if(depthCameras > 1)
{
ROS_INFO("Registering Depth2 callback...");
depth2Sync_ = new message_filters::Synchronizer<MyDepth2SyncPolicy>(
MyDepth2SyncPolicy(queueSize),
odomSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0],
*imageSubs_[1],
*imageDepthSubs_[1],
*cameraInfoSubs_[1]);
depth2Sync_->registerCallback(boost::bind(&CoreWrapper::depth2Callback, this, _1, _2, _3, _4, _5, _6, _7));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
imageSubs_[1]->getTopic().c_str(),
imageDepthSubs_[1]->getTopic().c_str(),
cameraInfoSubs_[1]->getTopic().c_str(),
odomSub_.getTopic().c_str());
}
else
{
ROS_INFO("Registering Depth callback...");
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(
MyDepthSyncPolicy(queueSize),
*imageSubs_[0],
odomSub_,
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
odomSub_.getTopic().c_str());
}
}
}
else
{
// use odom from TF, so subscribe to sensors only
if(subscribeLaserScan)
{
scanSub_.subscribe(nh, "scan", 1);
depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(MyDepthScanTFSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(
MyDepthScanTFSyncPolicy(queueSize),
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0],
scanSub_);
depthScanTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScanTFCallback, this, _1, _2, _3, _4));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
scanSub_.getTopic().c_str());
}
else //!subscribeLaserScan
{
depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(MyDepthTFSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
MyDepthTFSyncPolicy(queueSize),
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str());
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str());
}
}
}
@@ -2212,7 +2347,14 @@ void CoreWrapper::setupCallbacks(
if(subscribeLaserScan)
{
scanSub_.subscribe(nh, "scan", 1);
stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(MyStereoScanSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, scanSub_, odomSub_);
stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(
MyStereoScanSyncPolicy(queueSize),
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_,
scanSub_,
odomSub_);
stereoScanSync_->registerCallback(boost::bind(&CoreWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
@@ -2229,13 +2371,25 @@ void CoreWrapper::setupCallbacks(
if(stereoApproxSync)
{
ROS_INFO("Registering Stereo Approx callback...");
stereoApproxSync_ = new message_filters::Synchronizer<MyStereoApproxSyncPolicy>(MyStereoApproxSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomSub_);
stereoApproxSync_ = new message_filters::Synchronizer<MyStereoApproxSyncPolicy>(
MyStereoApproxSyncPolicy(queueSize),
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_,
odomSub_);
stereoApproxSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
}
else
{
ROS_INFO("Registering Stereo Exact callback...");
stereoExactSync_ = new message_filters::Synchronizer<MyStereoExactSyncPolicy>(MyStereoExactSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomSub_);
stereoExactSync_ = new message_filters::Synchronizer<MyStereoExactSyncPolicy>(
MyStereoExactSyncPolicy(queueSize),
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_,
odomSub_);
stereoExactSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
}
@@ -2255,7 +2409,13 @@ void CoreWrapper::setupCallbacks(
{
ROS_INFO("Registering Stereo+LaserScan+OdomTF callback...");
scanSub_.subscribe(nh, "scan", 1);
stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(MyStereoScanTFSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, scanSub_);
stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
MyStereoScanTFSyncPolicy(queueSize),
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_,
scanSub_);
stereoScanTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoScanTFCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
@@ -2271,13 +2431,23 @@ void CoreWrapper::setupCallbacks(
if(stereoApproxSync)
{
ROS_INFO("Registering Stereo+OdomTF Approx callback...");
stereoApproxTFSync_ = new message_filters::Synchronizer<MyStereoApproxTFSyncPolicy>(MyStereoApproxTFSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
stereoApproxTFSync_ = new message_filters::Synchronizer<MyStereoApproxTFSyncPolicy>(
MyStereoApproxTFSyncPolicy(queueSize),
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoApproxTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4));
}
else
{
ROS_INFO("Registering Stereo+OdomTF Exact callback...");
stereoExactTFSync_ = new message_filters::Synchronizer<MyStereoExactTFSyncPolicy>(MyStereoExactTFSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
stereoExactTFSync_ = new message_filters::Synchronizer<MyStereoExactTFSyncPolicy>(
MyStereoExactTFSyncPolicy(queueSize),
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoExactTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4));
}
src/CoreWrapper.h
+40 -18
View File
@@ -83,7 +83,13 @@ public:
virtual ~CoreWrapper();
private:
void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeStereo, int queueSize, bool stereoApproxSync);
void setupCallbacks(
bool subscribeDepth,
bool subscribeLaserScan,
bool subscribeStereo,
int queueSize,
bool stereoApproxSync,
int depthCameras);
void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
bool commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
@@ -93,8 +99,14 @@ private:
void commonDepthCallback(
const std::string & odomFrameId,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg);
void commonDepthCallback(
const std::string & odomFrameId,
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
const sensor_msgs::LaserScanConstPtr& scanMsg);
void commonStereoCallback(
const std::string & odomFrameId,
@@ -129,6 +141,14 @@ private:
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg);
void depth2Callback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& image1Msg,
const sensor_msgs::ImageConstPtr& imageDepth1Msg,
const sensor_msgs::CameraInfoConstPtr& camInfo1Msg,
const sensor_msgs::ImageConstPtr& image2Msg,
const sensor_msgs::ImageConstPtr& imageDept2hMsg,
const sensor_msgs::CameraInfoConstPtr& camInfo2Msg);
// without odom, when TF is used for odom
void depthTFCallback(
@@ -158,22 +178,12 @@ private:
void updateGoal(const ros::Time & stamp);
void process(
int id,
const ros::Time & stamp,
const cv::Mat & image,
const rtabmap::SensorData & data,
const rtabmap::Transform & odom = rtabmap::Transform(),
const std::string & odomFrameId = "",
double odomRotationalVariance = 1.0,
double odomTransitionalVariance = 1.0,
const cv::Mat & depthOrRightImage = cv::Mat(),
double fx = 0.0,
double fy = 0.0,
double cx = 0.0,
double cy = 0.0,
double baseline = 0.0,
const rtabmap::Transform & localTransform = rtabmap::Transform(),
const cv::Mat & scan = cv::Mat(),
int scanMaxPts = 0);
double odomTransitionalVariance = 1.0);
bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -225,6 +235,8 @@ private:
std::string databasePath_;
bool waitForTransform_;
bool useActionForGoal_;
bool genScan_;
double genScanMaxDepth_;
rtabmap::Transform mapToOdom_;
boost::mutex mapToOdomMutex_;
@@ -247,9 +259,9 @@ private:
image_transport::Subscriber defaultSub_;
//for depth callback
image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
std::vector<image_transport::SubscriberFilter*> imageSubs_;
std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>*> cameraInfoSubs_;
//stereo callback
image_transport::SubscriberFilter imageRectLeft_;
@@ -300,6 +312,16 @@ private:
nav_msgs::Odometry> MyStereoExactSyncPolicy;
message_filters::Synchronizer<MyStereoExactSyncPolicy> * stereoExactSync_;
typedef message_filters::sync_policies::ApproximateTime<
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepth2SyncPolicy;
message_filters::Synchronizer<MyDepth2SyncPolicy> * depth2Sync_;
// without odom, when TF is used for odom
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
src/DbPlayerNode.cpp
+1 -1
View File
@@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <std_srvs/Empty.h>
#include <rtabmap_ros/MsgConversion.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/util3d_conversions.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/DBReader.h>
bool paused = false;
src/GuiWrapper.cpp
+393 -97
View File
@@ -47,7 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/ParamEvent.h>
#include <rtabmap/core/OdometryEvent.h>
#include <rtabmap/core/util3d_conversions.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/utilite/UTimer.h>
@@ -78,7 +78,9 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
depthOdomInfoSync_(0),
stereoSync_(0),
stereoScanSync_(0),
stereoOdomInfoSync_(0)
stereoOdomInfoSync_(0),
depth2Sync_(0),
depthOdomInfo2Sync_(0)
{
ros::NodeHandle nh;
app_ = new QApplication(argc, argv);
@@ -117,52 +119,72 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
bool subscribeOdomInfo = false;
bool subscribeStereo = false;
int queueSize = 10;
int depthCameras = 1;
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
pnh.param("depth_cameras", depthCameras, depthCameras);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap/camera when pausing the process
this->setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeOdomInfo, subscribeStereo, queueSize);
this->setupCallbacks(
subscribeDepth,
subscribeLaserScan,
subscribeOdomInfo,
subscribeStereo,
queueSize,
depthCameras);
UEventsManager::addHandler(this);
UEventsManager::addHandler(mainWindow_);
infoTopic_.subscribe(nh, "info", 1);
mapDataTopic_.subscribe(nh, "mapData", 1);
infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(MyInfoMapSyncPolicy(queueSize), infoTopic_, mapDataTopic_);
infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(
MyInfoMapSyncPolicy(queueSize),
infoTopic_,
mapDataTopic_);
infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, _1, _2));
}
GuiWrapper::~GuiWrapper()
{
if(depthSync_)
{
delete depthSync_;
}
if(depth2Sync_)
delete depth2Sync_;
if(depthScanSync_)
{
delete depthScanSync_;
}
if(depthOdomInfoSync_)
{
delete depthOdomInfoSync_;
}
if(depthOdomInfo2Sync_)
delete depthOdomInfo2Sync_;
if(stereoSync_)
{
delete stereoSync_;
}
if(stereoScanSync_)
{
delete stereoScanSync_;
}
if(stereoOdomInfoSync_)
{
delete stereoOdomInfoSync_;
for(unsigned int i=0; i<imageSubs_.size(); ++i)
{
delete imageSubs_[i];
}
imageSubs_.clear();
for(unsigned int i=0; i<imageDepthSubs_.size(); ++i)
{
delete imageDepthSubs_[i];
}
imageDepthSubs_.clear();
for(unsigned int i=0; i<cameraInfoSubs_.size(); ++i)
{
delete cameraInfoSubs_[i];
}
cameraInfoSubs_.clear();
delete infoMapSync_;
delete mainWindow_;
delete app_;
@@ -373,6 +395,23 @@ void GuiWrapper::commonDepthCallback(
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
imageMsgs.push_back(imageMsg);
depthMsgs.push_back(depthMsg);
cameraInfoMsgs.push_back(cameraInfoMsg);
commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, odomInfoMsg);
}
void GuiWrapper::commonDepthCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
!mainWindow_->isProcessingOdometry() &&
@@ -380,19 +419,9 @@ void GuiWrapper::commonDepthCallback(
{
lastOdomInfoUpdateTime_ = UTimer::now();
if(!(imageMsg.get() == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depthMsg.get() == 0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
UASSERT(imageMsgs.size()>0 &&
imageMsgs.size() == depthMsgs.size() &&
imageMsgs.size() == cameraInfoMsgs.size());
std_msgs::Header odomHeader;
if(odomMsg.get())
@@ -405,17 +434,17 @@ void GuiWrapper::commonDepthCallback(
{
odomHeader = scanMsg->header;
}
else if(cameraInfoMsg.get())
else if(cameraInfoMsgs.size() && cameraInfoMsgs[0].get())
{
odomHeader = cameraInfoMsg->header;
odomHeader = cameraInfoMsgs[0]->header;
}
else if(depthMsg.get())
else if(depthMsgs.size() && depthMsgs[0].get())
{
odomHeader = depthMsg->header;
odomHeader = depthMsgs[0]->header;
}
else if(imageMsg.get())
else if(imageMsgs.size() && imageMsgs[0].get())
{
odomHeader = imageMsg->header;
odomHeader = imageMsgs[0]->header;
}
odomHeader.frame_id = odomFrameId_;
}
@@ -447,51 +476,110 @@ void GuiWrapper::commonDepthCallback(
return;
}
CameraModel cameraModel;
if(cameraInfoMsg.get())
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)
{
Transform localTransform = getTransform(frameId_, cameraInfoMsg->header.frame_id, cameraInfoMsg->header.stamp);
if(!(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))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight);
UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight);
Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp);
if(localTransform.isNull())
{
return;
}
// sync with odometry stamp
if(odomHeader.stamp != cameraInfoMsg->header.stamp)
if(odomHeader.stamp != depthMsgs[i]->header.stamp)
{
Transform sensorT = getTransform(odomHeader.frame_id, frameId_, cameraInfoMsg->header.stamp);
if(!odomT.isNull())
{
Transform sensorT = getTransform(odomHeader.frame_id, frameId_, depthMsgs[i]->header.stamp);
if(sensorT.isNull())
{
return;
}
localTransform = odomT.inverse() * sensorT * localTransform;
}
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
if(!localTransform.isNull())
{
cameraModel = CameraModel(model.fx(), model.fy(), model.cx(), model.cy(), localTransform);
}
}
cv::Mat rgb;
if(imageMsg.get())
cv_bridge::CvImageConstPtr ptrImage;
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
rgb = cv_bridge::toCvCopy(imageMsg, "mono8")->image;
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
}
else
{
rgb = cv_bridge::toCvCopy(imageMsg, "bgr8")->image;
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
cv::Mat subDepth = ptrDepth->image;
if(subDepth.type() == CV_32FC1)
{
subDepth = util3d::cvtDepthFromFloat(subDepth);
static bool shown = false;
if(!shown)
{
ROS_WARN("Use depth image with \"unsigned short\" type to "
"avoid conversion. This message is only printed once...");
shown = true;
}
}
cv::Mat depth;
if(depthMsg.get())
// initialize
if(rgb.empty())
{
depth = cv_bridge::toCvCopy(depthMsg)->image;
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
{
ROS_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
{
ROS_ERROR("Some Depth images are not the same type!");
return;
}
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsgs[i]);
cameraModels.push_back(rtabmap::CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
localTransform));
}
cv::Mat scan;
@@ -540,7 +628,7 @@ void GuiWrapper::commonDepthCallback(
scanMsg.get()?(int)scanMsg->ranges.size():0,
rgb,
depth,
cameraModel,
cameraModels,
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomT,
@@ -754,6 +842,34 @@ void GuiWrapper::depthCallback(
rtabmap_ros::OdomInfoConstPtr());
}
void GuiWrapper::depth2Callback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& image1Msg,
const sensor_msgs::ImageConstPtr& depth1Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
const sensor_msgs::ImageConstPtr& image2Msg,
const sensor_msgs::ImageConstPtr& depth2Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg)
{
std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
imageMsgs.push_back(image1Msg);
imageMsgs.push_back(image2Msg);
depthMsgs.push_back(depth1Msg);
depthMsgs.push_back(depth2Msg);
cameraInfoMsgs.push_back(cameraInfo1Msg);
cameraInfoMsgs.push_back(cameraInfo2Msg);
commonDepthCallback(
odomMsg,
imageMsgs,
depthMsgs,
cameraInfoMsgs,
sensor_msgs::LaserScanConstPtr(),
rtabmap_ros::OdomInfoConstPtr());
}
void GuiWrapper::depthOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -770,6 +886,35 @@ void GuiWrapper::depthOdomInfoCallback(
odomInfoMsg);
}
void GuiWrapper::depthOdomInfo2Callback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& image1Msg,
const sensor_msgs::ImageConstPtr& depth1Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
const sensor_msgs::ImageConstPtr& image2Msg,
const sensor_msgs::ImageConstPtr& depth2Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg)
{
std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
imageMsgs.push_back(image1Msg);
imageMsgs.push_back(image2Msg);
depthMsgs.push_back(depth1Msg);
depthMsgs.push_back(depth2Msg);
cameraInfoMsgs.push_back(cameraInfo1Msg);
cameraInfoMsgs.push_back(cameraInfo2Msg);
commonDepthCallback(
odomMsg,
imageMsgs,
depthMsgs,
cameraInfoMsgs,
sensor_msgs::LaserScanConstPtr(),
odomInfoMsg);
}
void GuiWrapper::depthScanCallback(
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -939,7 +1084,8 @@ void GuiWrapper::setupCallbacks(
bool subscribeLaserScan,
bool subscribeOdomInfo,
bool subscribeStereo,
int queueSize)
int queueSize,
int depthCameras)
{
ros::NodeHandle nh; // public
ros::NodeHandle pnh("~"); // private
@@ -952,21 +1098,49 @@ void GuiWrapper::setupCallbacks(
{
ROS_WARN("Cannot subscribe to laser scan without depth or stereo subscription...");
}
if(depthCameras <= 0)
{
depthCameras = 1;
}
if(depthCameras > 2)
{
ROS_WARN("Cannot subscribe to more than 2 cameras yet...");
depthCameras = 2;
}
if(subscribeDepth)
{
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
UASSERT(depthCameras >= 1 && depthCameras <= 2);
UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan || !odomFrameId_.empty()), "Not yet supported!");
imageSubs_.resize(depthCameras);
imageDepthSubs_.resize(depthCameras);
cameraInfoSubs_.resize(depthCameras);
for(int i=0; i<depthCameras; ++i)
{
std::string rgbPrefix = "rgb";
std::string depthPrefix = "depth";
if(depthCameras>1)
{
rgbPrefix += uNumber2Str(i);
depthPrefix += uNumber2Str(i);
}
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);
imageSubs_[i] = new image_transport::SubscriberFilter;
imageDepthSubs_[i] = new image_transport::SubscriberFilter;
cameraInfoSubs_[i] = new message_filters::Subscriber<sensor_msgs::CameraInfo>;
imageSubs_[i]->subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
imageDepthSubs_[i]->subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSubs_[i]->subscribe(rgb_nh, "camera_info", 1);
}
if(odomFrameId_.empty())
{
@@ -974,83 +1148,168 @@ void GuiWrapper::setupCallbacks(
if(subscribeLaserScan)
{
scanSub_.subscribe(nh, "scan", 1);
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), scanSub_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
MyDepthScanSyncPolicy(queueSize),
scanSub_,
odomSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
odomSub_.getTopic().c_str(),
scanSub_.getTopic().c_str());
}
else if(subscribeOdomInfo)
{
odomInfoSub_.subscribe(nh, "odom_info", 1);
depthOdomInfoSync_ = new message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy>(MyDepthOdomInfoSyncPolicy(queueSize), odomInfoSub_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
depthOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoCallback, this, _1, _2, _3, _4, _5));
if(depthCameras > 1)
{
depthOdomInfo2Sync_ = new message_filters::Synchronizer<MyDepthOdomInfo2SyncPolicy>(
MyDepthOdomInfo2SyncPolicy(queueSize),
odomInfoSub_,
odomSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0],
*imageSubs_[1],
*imageDepthSubs_[1],
*cameraInfoSubs_[1]);
depthOdomInfo2Sync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfo2Callback, this, _1, _2, _3, _4, _5, _6, _7, _8));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
imageSubs_[1]->getTopic().c_str(),
imageDepthSubs_[1]->getTopic().c_str(),
cameraInfoSubs_[1]->getTopic().c_str(),
odomSub_.getTopic().c_str(),
odomInfoSub_.getTopic().c_str());
}
else
{
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
depthOdomInfoSync_ = new message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy>(
MyDepthOdomInfoSyncPolicy(queueSize),
odomInfoSub_,
odomSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
odomSub_.getTopic().c_str(),
odomInfoSub_.getTopic().c_str());
}
}
else
{
if(depthCameras > 1)
{
depth2Sync_ = new message_filters::Synchronizer<MyDepth2SyncPolicy>(
MyDepth2SyncPolicy(queueSize),
odomSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0],
*imageSubs_[1],
*imageDepthSubs_[1],
*cameraInfoSubs_[1]);
depth2Sync_->registerCallback(boost::bind(&GuiWrapper::depth2Callback, this, _1, _2, _3, _4, _5, _6, _7));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
imageSubs_[1]->getTopic().c_str(),
imageDepthSubs_[1]->getTopic().c_str(),
cameraInfoSubs_[1]->getTopic().c_str(),
odomSub_.getTopic().c_str());
}
else
{
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(
MyDepthSyncPolicy(queueSize),
odomSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthSync_->registerCallback(boost::bind(&GuiWrapper::depthCallback, this, _1, _2, _3, _4));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
odomSub_.getTopic().c_str());
}
}
}
else
{
// use TF as odom
if(subscribeLaserScan)
{
scanSub_.subscribe(nh, "scan", 1);
depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(MyDepthScanTFSyncPolicy(queueSize), scanSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(
MyDepthScanTFSyncPolicy(queueSize),
scanSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthScanTFSync_->registerCallback(boost::bind(&GuiWrapper::depthScanTFCallback, this, _1, _2, _3, _4));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
scanSub_.getTopic().c_str());
}
else if(subscribeOdomInfo)
{
odomInfoSub_.subscribe(nh, "odom_info", 1);
depthOdomInfoTFSync_ = new message_filters::Synchronizer<MyDepthOdomInfoTFSyncPolicy>(MyDepthOdomInfoTFSyncPolicy(queueSize), odomInfoSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
depthOdomInfoTFSync_ = new message_filters::Synchronizer<MyDepthOdomInfoTFSyncPolicy>(
MyDepthOdomInfoTFSyncPolicy(queueSize),
odomInfoSub_,
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthOdomInfoTFSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoTFCallback, this, _1, _2, _3, _4));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str(),
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
odomInfoSub_.getTopic().c_str());
}
else
{
depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(MyDepthTFSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
MyDepthTFSyncPolicy(queueSize),
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthTFSync_->registerCallback(boost::bind(&GuiWrapper::depthTFCallback, this, _1, _2, _3));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str());
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str());
}
}
}
@@ -1076,7 +1335,14 @@ void GuiWrapper::setupCallbacks(
if(subscribeLaserScan)
{
scanSub_.subscribe(nh, "scan", 1);
stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(MyStereoScanSyncPolicy(queueSize), scanSub_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(
MyStereoScanSyncPolicy(queueSize),
scanSub_,
odomSub_,
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
@@ -1091,7 +1357,14 @@ void GuiWrapper::setupCallbacks(
else if(subscribeOdomInfo)
{
odomInfoSub_.subscribe(nh, "odom_info", 1);
stereoOdomInfoSync_ = new message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy>(MyStereoOdomInfoSyncPolicy(queueSize), odomInfoSub_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
stereoOdomInfoSync_ = new message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy>(
MyStereoOdomInfoSyncPolicy(queueSize),
odomInfoSub_,
odomSub_,
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoOdomInfoCallback, this, _1, _2, _3, _4, _5, _6));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
@@ -1105,7 +1378,13 @@ void GuiWrapper::setupCallbacks(
}
else
{
stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(MyStereoSyncPolicy(queueSize), odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(
MyStereoSyncPolicy(queueSize),
odomSub_,
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoSync_->registerCallback(boost::bind(&GuiWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
@@ -1123,7 +1402,13 @@ void GuiWrapper::setupCallbacks(
if(subscribeLaserScan)
{
scanSub_.subscribe(nh, "scan", 1);
stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(MyStereoScanTFSyncPolicy(queueSize), scanSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
MyStereoScanTFSyncPolicy(queueSize),
scanSub_,
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoScanTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanTFCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
@@ -1137,7 +1422,13 @@ void GuiWrapper::setupCallbacks(
else if(subscribeOdomInfo)
{
odomInfoSub_.subscribe(nh, "odom_info", 1);
stereoOdomInfoTFSync_ = new message_filters::Synchronizer<MyStereoOdomInfoTFSyncPolicy>(MyStereoOdomInfoTFSyncPolicy(queueSize), odomInfoSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
stereoOdomInfoTFSync_ = new message_filters::Synchronizer<MyStereoOdomInfoTFSyncPolicy>(
MyStereoOdomInfoTFSyncPolicy(queueSize),
odomInfoSub_,
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoOdomInfoTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoOdomInfoTFCallback, this, _1, _2, _3, _4, _5));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
@@ -1150,7 +1441,12 @@ void GuiWrapper::setupCallbacks(
}
else
{
stereoTFSync_ = new message_filters::Synchronizer<MyStereoTFSyncPolicy>(MyStereoTFSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
stereoTFSync_ = new message_filters::Synchronizer<MyStereoTFSyncPolicy>(
MyStereoTFSyncPolicy(queueSize),
imageRectLeft_,
imageRectRight_,
cameraInfoLeft_,
cameraInfoRight_);
stereoTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoTFCallback, this, _1, _2, _3, _4));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
src/GuiWrapper.h
+55 -4
View File
@@ -73,7 +73,13 @@ protected:
private:
void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);
void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeOdomInfo, bool subscribeStereo, int queueSize);
void setupCallbacks(
bool subscribeDepth,
bool subscribeLaserScan,
bool subscribeOdomInfo,
bool subscribeStereo,
int queueSize,
int depthCameras);
void commonDepthCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -82,6 +88,13 @@ private:
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
void commonDepthCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
void commonStereoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
@@ -99,12 +112,29 @@ private:
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
void depth2Callback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& image1Msg,
const sensor_msgs::ImageConstPtr& depth1Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
const sensor_msgs::ImageConstPtr& image2Msg,
const sensor_msgs::ImageConstPtr& depth2Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg);
void depthOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
void depthOdomInfo2Callback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& image1Msg,
const sensor_msgs::ImageConstPtr& depth1Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
const sensor_msgs::ImageConstPtr& image2Msg,
const sensor_msgs::ImageConstPtr& depth2Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg);
void depthScanCallback(
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -185,9 +215,9 @@ private:
message_filters::Subscriber<rtabmap_ros::MapData> mapDataTopic_;
ros::Subscriber defaultSub_; // odometry only
image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
std::vector<image_transport::SubscriberFilter*> imageSubs_;
std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>* > cameraInfoSubs_;
message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
@@ -252,6 +282,27 @@ private:
sensor_msgs::CameraInfo> MyStereoOdomInfoSyncPolicy;
message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy> * stereoOdomInfoSync_;
typedef message_filters::sync_policies::ApproximateTime<
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepth2SyncPolicy;
message_filters::Synchronizer<MyDepth2SyncPolicy> * depth2Sync_;
typedef message_filters::sync_policies::ApproximateTime<
rtabmap_ros::OdomInfo,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthOdomInfo2SyncPolicy;
message_filters::Synchronizer<MyDepthOdomInfo2SyncPolicy> * depthOdomInfo2Sync_;
// with odom TF
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::LaserScan,
src/MapAssemblerNode.cpp
-1
View File
@@ -32,7 +32,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/util3d_conversions.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/utilite/ULogger.h>
src/RGBDOdometryNode.cpp
+214 -2
View File
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_ros/MsgConversion.h"
#include <rtabmap/core/util3d.h>
#include <rtabmap/utilite/ULogger.h>
using namespace rtabmap;
@@ -51,14 +52,74 @@ class RGBDOdometry : public rtabmap_ros::OdometryROS
public:
RGBDOdometry(int argc, char * argv[]) :
rtabmap_ros::OdometryROS(argc, argv),
sync_(0)
sync_(0),
sync2_(0)
{
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
int queueSize = 5;
int depthCameras = 1;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("depth_cameras", depthCameras, depthCameras);
if(depthCameras <= 0)
{
depthCameras = 1;
}
if(depthCameras > 2)
{
ROS_FATAL("Only 2 cameras maximum supported yet.");
}
if(depthCameras == 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);
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);
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\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());
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));
}
else
{
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
@@ -81,13 +142,22 @@ public:
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_mono_sub_, image_depth_sub_, info_sub_);
sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
}
~RGBDOdometry()
{
if(sync_)
{
delete sync_;
}
if(sync2_)
{
delete sync2_;
}
}
void callback(const sensor_msgs::ImageConstPtr& image,
void callback(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
@@ -159,12 +229,154 @@ public:
}
}
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)
{
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);
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::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))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight);
UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight);
tf::StampedTransform localTransform;
try
{
if(this->waitForTransform())
{
if(!this->tfListener().waitForTransform(this->frameId(), imageMsgs[i]->header.frame_id, imageMsgs[i]->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), imageMsgs[i]->header.frame_id.c_str());
return;
}
}
this->tfListener().lookupTransform(this->frameId(), imageMsgs[i]->header.frame_id, imageMsgs[i]->header.stamp, localTransform);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
cv_bridge::CvImageConstPtr ptrImage;
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;
if(subDepth.type() == CV_32FC1)
{
subDepth = rtabmap::util3d::cvtDepthFromFloat(subDepth);
static bool shown = false;
if(!shown)
{
ROS_WARN("Use depth image with \"unsigned short\" type to "
"avoid conversion. This message is only printed once...");
shown = true;
}
}
// 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
{
ROS_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
{
ROS_ERROR("Some Depth images are not the same type!");
return;
}
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*infoMsgs[i]);
cameraModels.push_back(rtabmap::CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
rtabmap_ros::transformFromTF(localTransform)));
}
rtabmap::SensorData data(
rgb,
depth,
cameraModels,
0,
rtabmap_ros::timestampFromROS(image->header.stamp));
this->processData(data, image->header);
}
}
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_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> * sync_;
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_;
};
int main(int argc, char *argv[])