From fae192fc2650d94a133e49d6e50969b813130bde Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 22 Oct 2014 19:32:27 +0000 Subject: [PATCH] ros-pkg: added point_cloud_xyz nodelet git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1914 f169173b-cf89-36c8-b27e-44dbe73f0c83 --- CMakeLists.txt | 1 + .../az3_mapping_robot_stereo_nav.launch | 1 + nodelet_plugins.xml | 8 + src/nodelets/point_cloud_xyz.cpp | 157 ++++++++++++++++++ 4 files changed, 167 insertions(+) create mode 100644 src/nodelets/point_cloud_xyz.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 3a8ba07d..3e1b3759 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -122,6 +122,7 @@ add_library(rtabmap_ros src/nodelets/stereo_throttle.cpp src/nodelets/data_odom_sync.cpp src/nodelets/point_cloud_xyzrgb.cpp + src/nodelets/point_cloud_xyz.cpp src/nodelets/disparity_to_depth.cpp src/MsgConversion.cpp src/OdometryROS.cpp diff --git a/launch/azimut3/az3_mapping_robot_stereo_nav.launch b/launch/azimut3/az3_mapping_robot_stereo_nav.launch index 2b8ac52e..d8e5e61f 100644 --- a/launch/azimut3/az3_mapping_robot_stereo_nav.launch +++ b/launch/azimut3/az3_mapping_robot_stereo_nav.launch @@ -85,6 +85,7 @@ + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index 03b4ca6b..430882ee 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -31,6 +31,14 @@ + + + This is my nodelet. + + + diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp new file mode 100644 index 00000000..dcf2739e --- /dev/null +++ b/src/nodelets/point_cloud_xyz.cpp @@ -0,0 +1,157 @@ +/* +Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include + +#include +#include + +#include "rtabmap/core/util3d.h" + +namespace rtabmap +{ + +class PointCloudXYZ : public nodelet::Nodelet +{ +public: + PointCloudXYZ() : voxelSize_(0.0), decimation_(1) {} + + virtual ~PointCloudXYZ() + { + delete sync_; + } + +private: + virtual void onInit() + { + ros::NodeHandle & nh = getNodeHandle(); + ros::NodeHandle & pnh = getPrivateNodeHandle(); + + int queueSize = 10; + pnh.param("queue_size", queueSize, queueSize); + pnh.param("voxel_size", voxelSize_, voxelSize_); + pnh.param("decimation", decimation_, decimation_); + + sync_ = new message_filters::Synchronizer(MySyncPolicy(queueSize), imageDepthSub_, cameraInfoSub_); + sync_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2)); + cloudPub_ = nh.advertise("cloud", 1); + + image_transport::ImageTransport it(nh); + + imageDepthSub_.subscribe(it, "depth", 1); + cameraInfoSub_.subscribe(nh, "camera_info", 1); + } + + + + void callback( + const sensor_msgs::ImageConstPtr& depth, + const sensor_msgs::CameraInfoConstPtr& cameraInfo) + { + if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 && + depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0) + { + ROS_ERROR("Input type depth=32FC1,16UC1"); + return; + } + + if(cloudPub_.getNumSubscribers()) + { + cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth); + + image_geometry::PinholeCameraModel model; + model.fromCameraInfo(*cameraInfo); + float fx = model.fx(); + float fy = model.fy(); + float cx = model.cx(); + float cy = model.cy(); + + pcl::PointCloud::Ptr pclCloud; + pclCloud = rtabmap::util3d::cloudFromDepth( + imageDepthPtr->image, + cx, + cy, + fx, + fy, + decimation_); + + if(voxelSize_ > 0.0) + { + pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_); + } + + //********************* + // Publish Map + //********************* + + sensor_msgs::PointCloud2 rosCloud; + pcl::toROSMsg(*pclCloud, rosCloud); + rosCloud.header.stamp = depth->header.stamp; + rosCloud.header.frame_id = depth->header.frame_id; + + //publish the message + cloudPub_.publish(rosCloud); + } +} + +private: + + double voxelSize_; + int decimation_; + ros::Publisher cloudPub_; + + image_transport::SubscriberFilter imageDepthSub_; + message_filters::Subscriber cameraInfoSub_; + + + // without odometry subscription (odometry is computed by this node) + typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; + message_filters::Synchronizer * sync_; +}; + +PLUGINLIB_EXPORT_CLASS(rtabmap::PointCloudXYZ, nodelet::Nodelet); +} +