cloud_to_scan.cpp
Go to the documentation of this file.
00001 /* Copyright (c) 2010, Willow Garage Inc.
00002  * Copyright (c) 2012, Tang Tiong Yew
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "ros/ros.h"
00031 #include "pluginlib/class_list_macros.h"
00032 #include "nodelet/nodelet.h"
00033 #include "sensor_msgs/LaserScan.h"
00034 #include "pcl/point_cloud.h"
00035 #include "pcl_ros/point_cloud.h"
00036 #include "pcl/point_types.h"
00037 #include "pcl/ros/conversions.h"
00038 #include "dynamic_reconfigure/server.h"
00039 #include "pointcloud_to_laserscan/CloudScanConfig.h"
00040 
00041 namespace pointcloud_to_laserscan
00042 {
00043 class CloudToScan : public nodelet::Nodelet
00044 {
00045 public:
00046   //Constructor
00047   CloudToScan(): min_height_(0.10),
00048                  max_height_(0.15),
00049                  angle_min_(-M_PI/2),
00050                  angle_max_(M_PI/2),
00051                  angle_increment_(M_PI/180.0/2.0),
00052                  scan_time_(1.0/30.0),
00053                  range_min_(0.45),
00054                  range_max_(10.0),
00055                  output_frame_id_("/kinect_depth_frame")
00056   {
00057   };
00058 
00059   ~CloudToScan()
00060   {
00061     delete srv_;
00062   }
00063 
00064 private:
00065   typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00066 
00067   boost::mutex connect_mutex_;
00068   // Dynamic reconfigure server
00069   dynamic_reconfigure::Server<pointcloud_to_laserscan::CloudScanConfig>* srv_;
00070 
00071   virtual void onInit()
00072   {
00073     nh_ = getNodeHandle();
00074     ros::NodeHandle& private_nh = getPrivateNodeHandle();
00075 
00076     private_nh.getParam("min_height", min_height_);
00077     private_nh.getParam("max_height", max_height_);
00078 
00079     private_nh.getParam("angle_min", angle_min_);
00080     private_nh.getParam("angle_max", angle_max_);
00081     private_nh.getParam("angle_increment", angle_increment_);
00082     private_nh.getParam("scan_time", scan_time_);
00083     private_nh.getParam("range_min", range_min_);
00084     private_nh.getParam("range_max", range_max_);
00085 
00086     range_min_sq_ = range_min_ * range_min_;
00087 
00088     private_nh.getParam("output_frame_id", output_frame_id_);
00089 
00090     srv_ = new dynamic_reconfigure::Server<pointcloud_to_laserscan::CloudScanConfig>(private_nh);
00091     dynamic_reconfigure::Server<pointcloud_to_laserscan::CloudScanConfig>::CallbackType f = boost::bind(&CloudToScan::reconfigure, this, _1, _2);
00092     srv_->setCallback(f);
00093 
00094     // Lazy subscription to point cloud topic
00095     ros::AdvertiseOptions scan_ao = ros::AdvertiseOptions::create<sensor_msgs::LaserScan>(
00096       "scan", 10,
00097       boost::bind( &CloudToScan::connectCB, this),
00098       boost::bind( &CloudToScan::disconnectCB, this), ros::VoidPtr(), nh_.getCallbackQueue());
00099 
00100     boost::lock_guard<boost::mutex> lock(connect_mutex_);
00101     pub_ = nh_.advertise(scan_ao);
00102   };
00103 
00104   void connectCB() {
00105       boost::lock_guard<boost::mutex> lock(connect_mutex_);
00106       if (pub_.getNumSubscribers() > 0) {
00107           NODELET_DEBUG("Connecting to point cloud topic.");
00108           sub_ = nh_.subscribe<PointCloud>("cloud", 10, &CloudToScan::callback, this);
00109       }
00110   }
00111 
00112   void disconnectCB() {
00113       boost::lock_guard<boost::mutex> lock(connect_mutex_);
00114       if (pub_.getNumSubscribers() == 0) {
00115           NODELET_DEBUG("Unsubscribing from point cloud topic.");
00116           sub_.shutdown();
00117       }
00118   }
00119 
00120   void reconfigure(pointcloud_to_laserscan::CloudScanConfig &config, uint32_t level)
00121   {
00122     min_height_ = config.min_height;
00123     max_height_ = config.max_height;
00124     angle_min_ = config.angle_min;
00125     angle_max_ = config.angle_max;
00126     angle_increment_ = config.angle_increment;
00127     scan_time_ = config.scan_time;
00128     range_min_ = config.range_min;
00129     range_max_ = config.range_max;
00130 
00131     range_min_sq_ = range_min_ * range_min_;
00132   }
00133 
00134   void callback(const PointCloud::ConstPtr& cloud)
00135   {
00136     sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
00137     output->header = cloud->header;
00138     output->header.frame_id = output_frame_id_; // Set output frame. Point clouds come from "optical" frame, scans come from corresponding mount frame
00139     output->angle_min = angle_min_;
00140     output->angle_max = angle_max_;
00141     output->angle_increment = angle_increment_;
00142     output->time_increment = 0.0;
00143     output->scan_time = scan_time_;
00144     output->range_min = range_min_;
00145     output->range_max = range_max_;
00146 
00147     uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment);
00148     output->ranges.assign(ranges_size, output->range_max + 1.0);
00149 
00150     for (PointCloud::const_iterator it = cloud->begin(); it != cloud->end(); ++it)
00151     {
00152       const float &x = it->x;
00153       const float &y = it->y;
00154       const float &z = it->z;
00155 
00156       if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
00157       {
00158         NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
00159         continue;
00160       }
00161 
00162       if (-y > max_height_ || -y < min_height_)
00163       {
00164         NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", x, min_height_, max_height_);
00165         continue;
00166       }
00167 
00168       double range_sq = z*z+x*x;
00169       if (range_sq < range_min_sq_) {
00170         NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range_sq, range_min_sq_, x, y, z);
00171         continue;
00172       }
00173 
00174       double angle = -atan2(x, z);
00175       if (angle < output->angle_min || angle > output->angle_max)
00176       {
00177         NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output->angle_min, output->angle_max);
00178         continue;
00179       }
00180       int index = (angle - output->angle_min) / output->angle_increment;
00181 
00182 
00183       if (output->ranges[index] * output->ranges[index] > range_sq)
00184         output->ranges[index] = sqrt(range_sq);
00185       }
00186 
00187     pub_.publish(output);
00188   }
00189 
00190 
00191   double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_, range_min_sq_;
00192   std::string output_frame_id_;
00193 
00194   ros::NodeHandle nh_;
00195   ros::Publisher pub_;
00196   ros::Subscriber sub_;
00197 
00198 };
00199 
00200 PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, CloudToScan, pointcloud_to_laserscan::CloudToScan, nodelet::Nodelet);
00201 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


pointcloud_to_laserscan
Author(s): Tang Tiong Yew
autogenerated on Thu Jan 3 2013 15:31:41