velodyne_laserscan.cpp
Go to the documentation of this file.
1 // Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
2 // All rights reserved.
3 //
4 // Software License Agreement (BSD License 2.0)
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions
8 // are met:
9 //
10 // * Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 // * Redistributions in binary form must reproduce the above
13 // copyright notice, this list of conditions and the following
14 // disclaimer in the documentation and/or other materials provided
15 // with the distribution.
16 // * Neither the name of {copyright_holder} nor the names of its
17 // contributors may be used to endorse or promote products derived
18 // from this software without specific prior written permission.
19 //
20 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 // POSSIBILITY OF SUCH DAMAGE.
32 
35 
36 namespace velodyne_laserscan
37 {
38 
40  nh_(nh), srv_(nh_priv), ring_count_(0)
41 {
42  ros::SubscriberStatusCallback connect_cb = boost::bind(&VelodyneLaserScan::connectCb, this);
43  pub_ = nh.advertise<sensor_msgs::LaserScan>("scan", 10, connect_cb, connect_cb);
44 
45  srv_.setCallback(boost::bind(&VelodyneLaserScan::reconfig, this, _1, _2));
46 }
47 
49 {
50  boost::lock_guard<boost::mutex> lock(connect_mutex_);
51  if (!pub_.getNumSubscribers())
52  {
53  sub_.shutdown();
54  }
55  else if (!sub_)
56  {
57  sub_ = nh_.subscribe("velodyne_points", 10, &VelodyneLaserScan::recvCallback, this);
58  }
59 }
60 
61 void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
62 {
63  // Latch ring count
64  if (!ring_count_)
65  {
66  // Check for PointCloud2 field 'ring'
67  bool found = false;
68  for (size_t i = 0; i < msg->fields.size(); i++)
69  {
70  if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16)
71  {
72  if (msg->fields[i].name == "ring")
73  {
74  found = true;
75  break;
76  }
77  }
78  }
79  if (!found)
80  {
81  ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
82  return;
83  }
84  for (sensor_msgs::PointCloud2ConstIterator<uint16_t> it(*msg, "ring"); it != it.end(); ++it)
85  {
86  const uint16_t ring = *it;
87 
88  if (ring + 1 > ring_count_)
89  {
90  ring_count_ = ring + 1;
91  }
92  }
93  if (ring_count_)
94  {
95  ROS_INFO("VelodyneLaserScan: Latched ring count of %u", ring_count_);
96  }
97  else
98  {
99  ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
100  return;
101  }
102  }
103 
104  // Select ring to use
105  uint16_t ring;
106 
107  if ((cfg_.ring < 0) || (cfg_.ring >= ring_count_))
108  {
109  // Default to ring closest to being level for each known sensor
110  if (ring_count_ > 32)
111  {
112  ring = 57; // HDL-64E
113  }
114  else if (ring_count_ > 16)
115  {
116  ring = 23; // HDL-32E
117  }
118  else
119  {
120  ring = 8; // VLP-16
121  }
122  }
123  else
124  {
125  ring = cfg_.ring;
126  }
127 
128  ROS_INFO_ONCE("VelodyneLaserScan: Extracting ring %u", ring);
129 
130  // Load structure of PointCloud2
131  int offset_x = -1;
132  int offset_y = -1;
133  int offset_z = -1;
134  int offset_i = -1;
135  int offset_r = -1;
136 
137  for (size_t i = 0; i < msg->fields.size(); i++)
138  {
139  if (msg->fields[i].datatype == sensor_msgs::PointField::FLOAT32)
140  {
141  if (msg->fields[i].name == "x")
142  {
143  offset_x = msg->fields[i].offset;
144  }
145  else if (msg->fields[i].name == "y")
146  {
147  offset_y = msg->fields[i].offset;
148  }
149  else if (msg->fields[i].name == "z")
150  {
151  offset_z = msg->fields[i].offset;
152  }
153  else if (msg->fields[i].name == "intensity")
154  {
155  offset_i = msg->fields[i].offset;
156  }
157  }
158  else if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16)
159  {
160  if (msg->fields[i].name == "ring")
161  {
162  offset_r = msg->fields[i].offset;
163  }
164  }
165  }
166 
167  // Construct LaserScan message
168  if ((offset_x >= 0) && (offset_y >= 0) && (offset_r >= 0))
169  {
170  const float RESOLUTION = std::abs(cfg_.resolution);
171  const size_t SIZE = 2.0 * M_PI / RESOLUTION;
172  sensor_msgs::LaserScanPtr scan(new sensor_msgs::LaserScan());
173  scan->header = msg->header;
174  scan->angle_increment = RESOLUTION;
175  scan->angle_min = -M_PI;
176  scan->angle_max = M_PI;
177  scan->range_min = 0.0;
178  scan->range_max = 200.0;
179  scan->time_increment = 0.0;
180  scan->ranges.resize(SIZE, INFINITY);
181 
182  if ((offset_x == 0) &&
183  (offset_y == 4) &&
184  (offset_i % 4 == 0) &&
185  (offset_r % 4 == 0))
186  {
187  scan->intensities.resize(SIZE);
188 
189  const size_t X = 0;
190  const size_t Y = 1;
191  const size_t I = offset_i / 4;
192  const size_t R = offset_r / 4;
193  for (sensor_msgs::PointCloud2ConstIterator<float> it(*msg, "x"); it != it.end(); ++it)
194  {
195  const uint16_t r = *((const uint16_t*)(&it[R])); // ring
196 
197  if (r == ring)
198  {
199  const float x = it[X]; // x
200  const float y = it[Y]; // y
201  const float i = it[I]; // intensity
202  const int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;
203 
204  if ((bin >= 0) && (bin < static_cast<int>(SIZE)))
205  {
206  scan->ranges[bin] = sqrtf(x * x + y * y);
207  scan->intensities[bin] = i;
208  }
209  }
210  }
211  }
212  else
213  {
214  ROS_WARN_ONCE("VelodyneLaserScan: PointCloud2 fields in unexpected order. Using slower generic method.");
215 
216  if (offset_i >= 0)
217  {
218  scan->intensities.resize(SIZE);
222  sensor_msgs::PointCloud2ConstIterator<float> iter_i(*msg, "intensity");
223  for ( ; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r, ++iter_i)
224  {
225  const uint16_t r = *iter_r; // ring
226 
227  if (r == ring)
228  {
229  const float x = *iter_x; // x
230  const float y = *iter_y; // y
231  const float i = *iter_i; // intensity
232  const int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;
233 
234  if ((bin >= 0) && (bin < static_cast<int>(SIZE)))
235  {
236  scan->ranges[bin] = sqrtf(x * x + y * y);
237  scan->intensities[bin] = i;
238  }
239  }
240  }
241  }
242  else
243  {
247 
248  for (; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r)
249  {
250  const uint16_t r = *iter_r; // ring
251 
252  if (r == ring)
253  {
254  const float x = *iter_x; // x
255  const float y = *iter_y; // y
256  const int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;
257 
258  if ((bin >= 0) && (bin < static_cast<int>(SIZE)))
259  {
260  scan->ranges[bin] = sqrtf(x * x + y * y);
261  }
262  }
263  }
264  }
265  }
266 
267  pub_.publish(scan);
268  }
269  else
270  {
271  ROS_ERROR("VelodyneLaserScan: PointCloud2 missing one or more required fields! (x,y,ring)");
272  }
273 }
274 
275 void VelodyneLaserScan::reconfig(VelodyneLaserScanConfig& config, uint32_t level)
276 {
277  cfg_ = config;
278 }
279 
280 } // namespace velodyne_laserscan
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
void publish(const boost::shared_ptr< M > &message) const
#define ROS_INFO_ONCE(...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
dynamic_reconfigure::Server< VelodyneLaserScanConfig > srv_
#define ROS_WARN_ONCE(...)
#define ROS_INFO(...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
uint32_t getNumSubscribers() const
VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv)
void recvCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
#define ROS_ERROR(...)
void reconfig(VelodyneLaserScanConfig &config, uint32_t level)


velodyne_laserscan
Author(s): Micho Radovnikovich, Kevin Hallenbeck
autogenerated on Sun Sep 6 2020 03:25:25