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

Updated rtabmap.launch to subscribe only once to an external rgbd_image

Browse Source
This commit is contained in:
matlabbe
2018-05-29 13:00:40 -04:00
parent 2fce7bf64b
commit 71f422bf5e
4 changed files with 222 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/MsgConversion.h
+1
View File
@@ -72,6 +72,7 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg);
void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth); void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth);
void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth); void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & image);
// copy data // copy data
void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes); void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes);
launch/rtabmap.launch
+58 -38
View File
@@ -73,7 +73,7 @@
<arg name="compressed" default="false"/> <!-- If you want to subscribe to compressed image topics --> <arg name="compressed" default="false"/> <!-- If you want to subscribe to compressed image topics -->
<arg name="rgb_image_transport" default="compressed"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") --> <arg name="rgb_image_transport" default="compressed"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
<arg name="depth_image_transport" default="compressedDepth"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") --> <arg name="depth_image_transport" default="compressedDepth"/> <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
<arg name="subscribe_scan" default="false"/> <arg name="subscribe_scan" default="false"/>
<arg name="scan_topic" default="/scan"/> <arg name="scan_topic" default="/scan"/>
@@ -112,34 +112,48 @@
<!-- relays --> <!-- relays -->
<group unless="$(arg stereo)"> <group unless="$(arg stereo)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" /> <group unless="$(arg subscribe_rgbd)">
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" /> <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
<node if="$(arg rgbd_sync)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="$(arg output)"> </group>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/> <group if="$(arg rgbd_sync)">
<remap from="depth/image" to="$(arg depth_topic_relay)"/> <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
<remap from="rgbd_image" to="$(arg rgbd_topic)"/> <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="$(arg output)">
<param name="approx_sync" type="bool" value="$(arg approx_rgbd_sync)"/> <remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/> <remap from="depth/image" to="$(arg depth_topic_relay)"/>
<param name="depth_scale" type="double" value="$(arg depth_scale)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
</node> <remap from="rgbd_image" to="$(arg rgbd_topic)"/>
<param name="approx_sync" type="bool" value="$(arg approx_rgbd_sync)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="depth_scale" type="double" value="$(arg depth_scale)"/>
</node>
</group>
</group> </group>
<group if="$(arg stereo)"> <group if="$(arg stereo)">
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="compressed in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" /> <group unless="$(arg subscribe_rgbd)">
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" /> <node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
<node if="$(arg rgbd_sync)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/stereo_sync" output="$(arg output)"> </group>
<remap from="left/image_rect" to="$(arg left_image_topic)"/> <group if="$(arg rgbd_sync)">
<remap from="right/image_rect" to="$(arg right_image_topic)"/> <node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/> <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/> <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/stereo_sync" output="$(arg output)">
<remap from="rgbd_image" to="$(arg rgbd_topic)"/> <remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<param name="approx_sync" type="bool" value="$(arg approx_rgbd_sync)"/> <remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/> <remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
</node> <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic)"/>
<param name="approx_sync" type="bool" value="$(arg approx_rgbd_sync)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
</node>
</group>
</group> </group>
<group unless="$(arg rgbd_sync)">
<node if="$(arg subscribe_rgbd)" name="republish_rgbd_image" type="relay" pkg="topic_tools" args="$(arg rgbd_topic) $(arg rgbd_topic)_relay" />
</group>
<!-- Visual odometry --> <!-- Visual odometry -->
<group unless="$(arg icp_odometry)"> <group unless="$(arg icp_odometry)">
<group if="$(arg visual_odometry)"> <group if="$(arg visual_odometry)">
@@ -149,7 +163,8 @@
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/> <remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/> <remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic)"/> <remap if="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)"/>
<remap unless="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)_relay"/>
<remap from="odom" to="$(arg odom_topic)"/> <remap from="odom" to="$(arg odom_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
@@ -172,7 +187,8 @@
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/> <remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/> <remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/> <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic)"/> <remap if="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)"/>
<remap unless="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)_relay"/>
<remap from="odom" to="$(arg odom_topic)"/> <remap from="odom" to="$(arg odom_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
@@ -240,7 +256,8 @@
<remap from="depth/image" to="$(arg depth_topic_relay)"/> <remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic)"/> <remap if="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)"/>
<remap unless="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)_relay"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/> <remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/> <remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
@@ -279,7 +296,8 @@
<remap from="depth/image" to="$(arg depth_topic_relay)"/> <remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic)"/> <remap if="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)"/>
<remap unless="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)_relay"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/> <remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/> <remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
@@ -295,14 +313,16 @@
<!-- Visualization RVIZ --> <!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/> <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb"> <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">
<remap from="left/image" to="$(arg left_image_topic_relay)"/> <remap if="$(arg stereo)" from="left/image" to="$(arg left_image_topic_relay)"/>
<remap from="right/image" to="$(arg right_image_topic_relay)"/> <remap if="$(arg stereo)" from="right/image" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/> <remap if="$(arg stereo)" from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/> <remap if="$(arg stereo)" from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/> <remap unless="$(arg subscribe_rgbd)" from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/> <remap unless="$(arg subscribe_rgbd)" from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap unless="$(arg subscribe_rgbd)" from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap if="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)"/>
<remap unless="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)_relay"/>
<remap from="cloud" to="voxel_cloud" /> <remap from="cloud" to="voxel_cloud" />
<param name="decimation" type="double" value="4"/> <param name="decimation" type="double" value="4"/>
src/MsgConversion.cpp
+133 -1
View File
@@ -176,7 +176,7 @@ void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageC
else if(!image->rgbCompressed.data.empty()) else if(!image->rgbCompressed.data.empty())
{ {
#ifdef CV_BRIDGE_HYDRO #ifdef CV_BRIDGE_HYDRO
ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this."); ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
#else #else
rgb = cv_bridge::toCvCopy(image->rgbCompressed); rgb = cv_bridge::toCvCopy(image->rgbCompressed);
#endif #endif
@@ -218,6 +218,138 @@ void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageC
} }
} }
rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & image)
{
rtabmap::SensorData data;
cv_bridge::CvImageConstPtr imageMsg;
cv_bridge::CvImageConstPtr depthMsg;
toCvShare(image, imageMsg, depthMsg);
rtabmap::StereoCameraModel stereoModel = stereoCameraModelFromROS(image->rgbCameraInfo, image->depthCameraInfo, rtabmap::Transform::getIdentity());
if(stereoModel.isValidForProjection())
{
cv_bridge::CvImageConstPtr imageRectLeft = imageMsg;
cv_bridge::CvImageConstPtr imageRectRight = depthMsg;
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended), received types are %s (left) and %s (right)",
imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
return data;
}
if(!imageRectLeft->image.empty() && !imageRectRight->image.empty())
{
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
{
ROS_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
stereoModel.baseline());
shown = true;
}
}
cv::Mat left, right;
if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
left = cv_bridge::cvtColor(imageRectLeft, "mono8")->image;
}
else
{
left = cv_bridge::cvtColor(imageRectLeft, "bgr8")->image;
}
right = cv_bridge::cvtColor(imageRectRight, "mono8")->image;
//
data = rtabmap::SensorData(
left,
right,
stereoModel,
0,
rtabmap_ros::timestampFromROS(image->header.stamp));
}
else
{
ROS_WARN("Odom: input images empty?!?");
}
}
else //depth
{
ros::Time higherStamp;
int imageWidth = imageMsg->image.cols;
int imageHeight = imageMsg->image.rows;
int depthWidth = depthMsg->image.cols;
int depthHeight = depthMsg->image.rows;
UASSERT_MSG(
imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
imageWidth/depthWidth == imageHeight/depthHeight,
uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
cv::Mat rgb;
cv::Mat depth;
rtabmap::CameraModel cameraModels;
if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==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 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 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,bgra8,rgba8 and "
"image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
imageMsg->encoding.c_str(),
depthMsg->encoding.c_str());
return data;
}
cv_bridge::CvImageConstPtr ptrImage = imageMsg;
if(imageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
{
// do nothing
}
else if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrImage = cv_bridge::cvtColor(imageMsg, "mono8");
}
else
{
ptrImage = cv_bridge::cvtColor(imageMsg, "bgr8");
}
cv_bridge::CvImageConstPtr ptrDepth = depthMsg;
data = rtabmap::SensorData(
ptrImage->image,
ptrDepth->image,
rtabmap_ros::cameraModelFromROS(image->rgbCameraInfo),
0,
rtabmap_ros::timestampFromROS(image->header.stamp));
}
return data;
}
void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes) void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes)
{ {
UASSERT(compressed.empty() || compressed.type() == CV_8UC1); UASSERT(compressed.empty() || compressed.type() == CV_8UC1);
src/nodelets/point_cloud_xyzrgb.cpp
+30
View File
@@ -191,6 +191,8 @@ private:
cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1); cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
rgbdImageSub_ = nh.subscribe("rgbd_image", 1, &PointCloudXYZRGB::rgbdImageCallback, this);
if(approxSync) if(approxSync)
{ {
@@ -437,6 +439,32 @@ private:
} }
} }
void rgbdImageCallback(const rtabmap_ros::RGBDImageConstPtr & image)
{
if(cloudPub_.getNumSubscribers())
{
ros::WallTime time = ros::WallTime::now();
rtabmap::SensorData data = rtabmap_ros::rgbdImageFromROS(image);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pcl::IndicesPtr indices(new std::vector<int>);
if(data.isValid())
{
pclCloud = rtabmap::util3d::cloudRGBFromSensorData(
data,
decimation_,
maxDepth_,
minDepth_,
indices.get(),
stereoBMParameters_);
processAndPublish(pclCloud, indices, image->header);
}
NODELET_DEBUG("point_cloud_xyzrgb from rgbd_image time = %f s", (ros::WallTime::now() - time).toSec());
}
}
void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header) void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header)
{ {
if(indices->size() && voxelSize_ > 0.0) if(indices->size() && voxelSize_ > 0.0)
@@ -504,6 +532,8 @@ private:
ros::Publisher cloudPub_; ros::Publisher cloudPub_;
ros::Subscriber rgbdImageSub_;
image_transport::SubscriberFilter imageSub_; image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_; image_transport::SubscriberFilter imageDepthSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_; message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;