pointcloud_to_laserscan_nodelet.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010-2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  *
35  */
36 
37 /*
38  * Author: Paul Bovbel
39  */
40 
42 #include <sensor_msgs/LaserScan.h>
46 
48 {
49 
51 
53  {
54  boost::mutex::scoped_lock lock(connect_mutex_);
56 
57  private_nh_.param<std::string>("target_frame", target_frame_, "");
58  private_nh_.param<double>("transform_tolerance", tolerance_, 0.01);
59  private_nh_.param<double>("min_height", min_height_, 0.0);
60  private_nh_.param<double>("max_height", max_height_, 1.0);
61 
62  private_nh_.param<double>("angle_min", angle_min_, -M_PI / 2.0);
63  private_nh_.param<double>("angle_max", angle_max_, M_PI / 2.0);
64  private_nh_.param<double>("angle_increment", angle_increment_, M_PI / 360.0);
65  private_nh_.param<double>("scan_time", scan_time_, 1.0 / 30.0);
66  private_nh_.param<double>("range_min", range_min_, 0.45);
67  private_nh_.param<double>("range_max", range_max_, 4.0);
68 
69  int concurrency_level;
70  private_nh_.param<int>("concurrency_level", concurrency_level, 1);
71  private_nh_.param<bool>("use_inf", use_inf_, true);
72 
73  //Check if explicitly single threaded, otherwise, let nodelet manager dictate thread pool size
74  if (concurrency_level == 1)
75  {
76  nh_ = getNodeHandle();
77  }
78  else
79  {
80  nh_ = getMTNodeHandle();
81  }
82 
83  // Only queue one pointcloud per running thread
84  if (concurrency_level > 0)
85  {
86  input_queue_size_ = concurrency_level;
87  }
88  else
89  {
90  input_queue_size_ = boost::thread::hardware_concurrency();
91  }
92 
93  // if pointcloud target frame specified, we need to filter by transform availability
94  if (!target_frame_.empty())
95  {
96  tf2_.reset(new tf2_ros::Buffer());
98  message_filter_.reset(new MessageFilter(sub_, *tf2_, target_frame_, input_queue_size_, nh_));
99  message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
100  message_filter_->registerFailureCallback(boost::bind(&PointCloudToLaserScanNodelet::failureCb, this, _1, _2));
101  }
102  else // otherwise setup direct subscription
103  {
105  }
106 
107  pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 10,
108  boost::bind(&PointCloudToLaserScanNodelet::connectCb, this),
109  boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this));
110  }
111 
113  {
114  boost::mutex::scoped_lock lock(connect_mutex_);
116  {
117  NODELET_INFO("Got a subscriber to scan, starting subscriber to pointcloud");
118  sub_.subscribe(nh_, "cloud_in", input_queue_size_);
119  }
120  }
121 
123  {
124  boost::mutex::scoped_lock lock(connect_mutex_);
125  if (pub_.getNumSubscribers() == 0)
126  {
127  NODELET_INFO("No subscibers to scan, shutting down subscriber to pointcloud");
128  sub_.unsubscribe();
129  }
130  }
131 
132  void PointCloudToLaserScanNodelet::failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
134  {
135  NODELET_WARN_STREAM_THROTTLE(1.0, "Can't transform pointcloud from frame " << cloud_msg->header.frame_id << " to "
136  << message_filter_->getTargetFramesString());
137  }
138 
139  void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
140  {
141 
142  //build laserscan output
143  sensor_msgs::LaserScan output;
144  output.header = cloud_msg->header;
145  if (!target_frame_.empty())
146  {
147  output.header.frame_id = target_frame_;
148  }
149 
150  output.angle_min = angle_min_;
151  output.angle_max = angle_max_;
152  output.angle_increment = angle_increment_;
153  output.time_increment = 0.0;
154  output.scan_time = scan_time_;
155  output.range_min = range_min_;
156  output.range_max = range_max_;
157 
158  //determine amount of rays to create
159  uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment);
160 
161  //determine if laserscan rays with no obstacle data will evaluate to infinity or max_range
162  if (use_inf_)
163  {
164  output.ranges.assign(ranges_size, std::numeric_limits<double>::infinity());
165  }
166  else
167  {
168  output.ranges.assign(ranges_size, output.range_max + 1.0);
169  }
170 
171  sensor_msgs::PointCloud2ConstPtr cloud_out;
172  sensor_msgs::PointCloud2Ptr cloud;
173 
174  // Transform cloud if necessary
175  if (!(output.header.frame_id == cloud_msg->header.frame_id))
176  {
177  try
178  {
179  cloud.reset(new sensor_msgs::PointCloud2);
180  tf2_->transform(*cloud_msg, *cloud, target_frame_, ros::Duration(tolerance_));
181  cloud_out = cloud;
182  }
183  catch (tf2::TransformException ex)
184  {
185  NODELET_ERROR_STREAM("Transform failure: " << ex.what());
186  return;
187  }
188  }
189  else
190  {
191  cloud_out = cloud_msg;
192  }
193 
194  // Iterate through pointcloud
196  iter_x(*cloud_out, "x"), iter_y(*cloud_out, "y"), iter_z(*cloud_out, "z");
197  iter_x != iter_x.end();
198  ++iter_x, ++iter_y, ++iter_z)
199  {
200 
201  if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z))
202  {
203  NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", *iter_x, *iter_y, *iter_z);
204  continue;
205  }
206 
207  if (*iter_z > max_height_ || *iter_z < min_height_)
208  {
209  NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", *iter_z, min_height_, max_height_);
210  continue;
211  }
212 
213  double range = hypot(*iter_x, *iter_y);
214  if (range < range_min_)
215  {
216  NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, *iter_x, *iter_y,
217  *iter_z);
218  continue;
219  }
220 
221  double angle = atan2(*iter_y, *iter_x);
222  if (angle < output.angle_min || angle > output.angle_max)
223  {
224  NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max);
225  continue;
226  }
227 
228  //overwrite range at laserscan ray if new range is smaller
229  int index = (angle - output.angle_min) / output.angle_increment;
230  if (range < output.ranges[index])
231  {
232  output.ranges[index] = range;
233  }
234 
235  }
236  pub_.publish(output);
237  }
238 
239 }
240 
#define NODELET_WARN_STREAM_THROTTLE(rate,...)
void publish(const boost::shared_ptr< M > &message) const
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
ros::NodeHandle & getMTNodeHandle() const
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
const ros::Subscriber & getSubscriber() const
ros::NodeHandle & getPrivateNodeHandle() const
void failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg, tf2_ros::filter_failure_reasons::FilterFailureReason reason)
#define NODELET_ERROR_STREAM(...)
tf2_ros::MessageFilter< sensor_msgs::PointCloud2 > MessageFilter
bool param(const std::string &param_name, T &param_val, const T &default_val) const
uint32_t getNumPublishers() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle & getNodeHandle() const
#define NODELET_INFO(...)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_
uint32_t getNumSubscribers() const
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, PointCloudToLaserScanNodelet, pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet)
boost::shared_ptr< tf2_ros::TransformListener > tf2_listener_
#define NODELET_DEBUG(...)
Connection registerCallback(const C &callback)


pointcloud_to_laserscan
Author(s): Paul Bovbel , Tully Foote
autogenerated on Sat Jun 8 2019 17:57:58