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

ros-pkg: added disparity to cloud in point_cloud_xyz nodelet

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1916 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-22 20:34:07 +00:00
parent fae192fc26
commit dcb4f3e928
3 changed files with 78 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/azimut3/az3_mapping_robot_stereo_nav.launch
+10 -3
View File
@@ -69,7 +69,7 @@
<!-- Run the ROS package stereo_image_proc for image rectification-->
<group ns="/stereo_camera" >
<!-- HACK: the fps parameter on camera1394stereo_node doesn't work for my camera!?!?
Throttle camera images to 10 Hz () -->
Throttle camera images to 5 Hz (), odometry on azimut cannot run over 5Hz -->
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap/stereo_throttle">
<remap from="left/image" to="left/image_raw"/>
<remap from="right/image" to="right/image_raw"/>
@@ -77,7 +77,7 @@
<remap from="right/camera_info" to="right/camera_info"/>
<param name="queue_size" type="int" value="10"/>
<param name="rate" type="double" value="10"/>
<param name="rate" type="double" value="5"/>
</node>
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
@@ -85,7 +85,14 @@
<remap from="left/camera_info" to="left/camera_info_throttle"/>
<remap from="right/image_raw" to="right/image_raw_throttle"/>
<remap from="right/camera_info" to="right/camera_info_throttle"/>
<remap from="points2" to="/planner_cloud"/>
</node>
<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="standalone rtabmap/point_cloud_xyz">
<remap from="disparity/image" to="disparity"/>
<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
<remap from="cloud" to="/planner_cloud"/>
<param name="voxel_size" type="double" value="0.05"/>
<param name="decimation" type="int" value="4"/>
</node>
</group>
src/nodelets/point_cloud_xyz.cpp
+64 -4
View File
@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <stereo_msgs/DisparityImage.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
@@ -62,6 +63,7 @@ public:
virtual ~PointCloudXYZ()
{
delete sync_;
delete syncDisparity_;
}
private:
@@ -77,12 +79,18 @@ private:
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
sync_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
syncDisparity_ = new message_filters::Synchronizer<MySyncDispPolicy>(MySyncDispPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
syncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
image_transport::ImageTransport it(nh);
imageDepthSub_.subscribe(it, "depth/image", 1);
cameraInfoSub_.subscribe(nh, "depth/camera_info", 1);
imageDepthSub_.subscribe(it, "depth", 1);
cameraInfoSub_.subscribe(nh, "camera_info", 1);
disparitySub_.subscribe(nh, "disparity/image", 1);
disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1);
cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
}
@@ -137,6 +145,54 @@ private:
}
}
void callbackDisparity(
const stereo_msgs::DisparityImageConstPtr& disparityMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0)
{
ROS_ERROR("Input type must be disparity=32FC1");
return;
}
// sensor_msgs::image_encodings::TYPE_32FC1
cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
if(cloudPub_.getNumSubscribers())
{
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
float cx = model.cx();
float cy = model.cy();
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromDisparity(
disparity,
cx,
cy,
disparityMsg->f,
disparityMsg->T,
decimation_);
if(voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
}
//*********************
// Publish Map
//*********************
sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(*pclCloud, rosCloud);
rosCloud.header.stamp = disparityMsg->header.stamp;
rosCloud.header.frame_id = disparityMsg->header.frame_id;
//publish the message
cloudPub_.publish(rosCloud);
}
}
private:
double voxelSize_;
@@ -146,10 +202,14 @@ private:
image_transport::SubscriberFilter imageDepthSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
message_filters::Subscriber<stereo_msgs::DisparityImage> disparitySub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> disparityCameraInfoSub_;
// without odometry subscription (odometry is computed by this node)
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> * sync_;
typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MySyncDispPolicy;
message_filters::Synchronizer<MySyncDispPolicy> * syncDisparity_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap::PointCloudXYZ, nodelet::Nodelet);
src/nodelets/point_cloud_xyzrgb.cpp
-2
View File
@@ -162,8 +162,6 @@ private:
image_transport::SubscriberFilter imageDepthSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
// without odometry subscription (odometry is computed by this node)
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> * sync_;
};