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
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
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
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
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_;
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 }
