00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <ros/ros.h>
00029 #include <pluginlib/class_list_macros.h>
00030 #include <nodelet/nodelet.h>
00031 #include <ros/publisher.h>
00032 #include <ros/subscriber.h>
00033
00034 #include <rtabmap_ros/MsgConversion.h>
00035 #include <rtabmap/core/util3d.h>
00036 #include <rtabmap/core/util2d.h>
00037 #include <rtabmap/utilite/ULogger.h>
00038
00039 #include <sensor_msgs/PointCloud2.h>
00040 #include <sensor_msgs/Image.h>
00041 #include <sensor_msgs/image_encodings.h>
00042 #include <sensor_msgs/CameraInfo.h>
00043
00044 #include <image_transport/image_transport.h>
00045
00046 #include <pcl/point_cloud.h>
00047 #include <pcl/point_types.h>
00048 #include <pcl_conversions/pcl_conversions.h>
00049
00050 #include <message_filters/sync_policies/approximate_time.h>
00051 #include <message_filters/sync_policies/exact_time.h>
00052 #include <message_filters/subscriber.h>
00053
00054 namespace rtabmap_ros
00055 {
00056
00057 class PointCloudToDepthImage : public nodelet::Nodelet
00058 {
00059 public:
00060 PointCloudToDepthImage() :
00061 listener_(0),
00062 waitForTransform_(0.1),
00063 fillHolesSize_ (0),
00064 fillHolesError_(0.1),
00065 fillIterations_(1),
00066 decimation_(1),
00067 approxSync_(0),
00068 exactSync_(0)
00069 {}
00070
00071 virtual ~PointCloudToDepthImage()
00072 {
00073 delete listener_;
00074 if(approxSync_)
00075 {
00076 delete approxSync_;
00077 }
00078 if(exactSync_)
00079 {
00080 delete exactSync_;
00081 }
00082 }
00083
00084 private:
00085 virtual void onInit()
00086 {
00087 listener_ = new tf::TransformListener();
00088
00089 ros::NodeHandle & nh = getNodeHandle();
00090 ros::NodeHandle & pnh = getPrivateNodeHandle();
00091
00092 int queueSize = 10;
00093 bool approx = true;
00094 pnh.param("queue_size", queueSize, queueSize);
00095 pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
00096 pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
00097 pnh.param("fill_holes_size", fillHolesSize_, fillHolesSize_);
00098 pnh.param("fill_holes_error", fillHolesError_, fillHolesError_);
00099 pnh.param("fill_iterations", fillIterations_, fillIterations_);
00100 pnh.param("decimation", decimation_, decimation_);
00101 pnh.param("approx", approx, approx);
00102
00103 if(fixedFrameId_.empty() && approx)
00104 {
00105 ROS_FATAL("fixed_frame_id should be set when using approximate "
00106 "time synchronization (approx=true)! If the robot "
00107 "is moving, it could be \"odom\". If not moving, it "
00108 "could be \"base_link\".");
00109 }
00110
00111 ROS_INFO("Params:");
00112 ROS_INFO(" approx=%s", approx?"true":"false");
00113 ROS_INFO(" queue_size=%d", queueSize);
00114 ROS_INFO(" fixed_frame_id=%s", fixedFrameId_.c_str());
00115 ROS_INFO(" wait_for_transform=%fs", waitForTransform_);
00116 ROS_INFO(" fill_holes_size=%d pixels (0=disabled)", fillHolesSize_);
00117 ROS_INFO(" fill_holes_error=%f", fillHolesError_);
00118 ROS_INFO(" fill_iterations=%d", fillIterations_);
00119 ROS_INFO(" decimation=%d", decimation_);
00120
00121 image_transport::ImageTransport it(nh);
00122 depthImage16Pub_ = it.advertise("image_raw", 1);
00123 depthImage32Pub_ = it.advertise("image", 1);
00124
00125 if(approx)
00126 {
00127 approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
00128 approxSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, _1, _2));
00129 }
00130 else
00131 {
00132 fixedFrameId_.clear();
00133 exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
00134 exactSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, _1, _2));
00135 }
00136
00137 pointCloudSub_.subscribe(nh, "cloud", 1);
00138 cameraInfoSub_.subscribe(nh, "camera_info", 1);
00139 }
00140
00141 void callback(
00142 const sensor_msgs::PointCloud2ConstPtr & pointCloud2Msg,
00143 const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg)
00144 {
00145 if(depthImage32Pub_.getNumSubscribers() > 0 || depthImage16Pub_.getNumSubscribers() > 0)
00146 {
00147 rtabmap::Transform cloudDisplacement = rtabmap::Transform::getIdentity();
00148 if(!fixedFrameId_.empty())
00149 {
00150
00151 cloudDisplacement = rtabmap_ros::getTransform(
00152 pointCloud2Msg->header.frame_id,
00153 fixedFrameId_,
00154 pointCloud2Msg->header.stamp,
00155 cameraInfoMsg->header.stamp,
00156 *listener_,
00157 waitForTransform_);
00158 }
00159
00160 if(cloudDisplacement.isNull())
00161 {
00162 return;
00163 }
00164
00165 rtabmap::Transform cloudToCamera = rtabmap_ros::getTransform(
00166 pointCloud2Msg->header.frame_id,
00167 cameraInfoMsg->header.frame_id,
00168 cameraInfoMsg->header.stamp,
00169 *listener_,
00170 waitForTransform_);
00171
00172 if(cloudToCamera.isNull())
00173 {
00174 return;
00175 }
00176
00177 rtabmap::Transform localTransform = cloudDisplacement.inverse()*cloudToCamera;
00178
00179 rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform);
00180
00181 if(decimation_ > 1)
00182 {
00183 if(model.imageWidth()%decimation_ == 0 && model.imageHeight()%decimation_ == 0)
00184 {
00185 model = model.scaled(1.0f/float(decimation_));
00186 }
00187 else
00188 {
00189 ROS_ERROR("decimation (%d) not valid for image size %dx%d",
00190 decimation_,
00191 model.imageWidth(),
00192 model.imageHeight());
00193 }
00194 }
00195
00196 pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
00197 pcl_conversions::toPCL(*pointCloud2Msg, *cloud);
00198
00199 cv_bridge::CvImage depthImage;
00200 depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
00201
00202 if(fillHolesSize_ > 0 && fillIterations_ > 0)
00203 {
00204 for(int i=0; i<fillIterations_;++i)
00205 {
00206 depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
00207 }
00208 }
00209
00210 depthImage.header = cameraInfoMsg->header;
00211
00212 if(depthImage32Pub_.getNumSubscribers())
00213 {
00214 depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00215 depthImage32Pub_.publish(depthImage.toImageMsg());
00216 }
00217
00218 if(depthImage16Pub_.getNumSubscribers())
00219 {
00220 depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00221 depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
00222 depthImage16Pub_.publish(depthImage.toImageMsg());
00223 }
00224 }
00225 }
00226
00227 private:
00228 image_transport::Publisher depthImage16Pub_;
00229 image_transport::Publisher depthImage32Pub_;
00230 message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudSub_;
00231 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
00232 std::string fixedFrameId_;
00233 tf::TransformListener * listener_;
00234 double waitForTransform_;
00235 int fillHolesSize_;
00236 double fillHolesError_;
00237 int fillIterations_;
00238 int decimation_;
00239
00240 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
00241 message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
00242 typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::CameraInfo> MyExactSyncPolicy;
00243 message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
00244 };
00245
00246 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudToDepthImage, nodelet::Nodelet);
00247 }