laser_geometry.py
Go to the documentation of this file.
1 """
2 Copyright (c) 2014, Enrique Fernandez
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7 
8  * Redistributions of source code must retain the above copyright
9  notice, this list of conditions and the following disclaimer.
10  * Redistributions in binary form must reproduce the above copyright
11  notice, this list of conditions and the following disclaimer in the
12  documentation and/or other materials provided with the distribution.
13  * Neither the name of the Willow Garage, Inc. nor the names of its
14  contributors may be used to endorse or promote products derived from
15  this software without specific prior written permission.
16 
17 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 POSSIBILITY OF SUCH DAMAGE.
28 """
29 
30 import rospy
31 from sensor_msgs.msg import PointCloud2
32 import sensor_msgs.point_cloud2 as pc2
33 
34 import numpy as np
35 
37  """
38  A class to Project Laser Scan
39 
40  This calls will project laser scans into point clouds. It caches
41  unit vectors between runs (provided the angular resolution of
42  your scanner is not changing) to avoid excess computation.
43 
44  By default all range values less thatn the scanner min_range,
45  greater than the scanner max_range are removed from the generated
46  point cloud, as these are assumed to be invalid.
47 
48  If it is important to preserve a mapping between the index of
49  range values and points in the cloud, the recommended approach is to
50  pre-filter your laser scan message to meet the requirement that all
51  ranges are between min and max_range.
52 
53  The generate PointClouds have a number of channels which can be enabled
54  through the use of ChannelOption.
55  - ChannelOption.INTENSITY - Create a channel named "intensities" with the
56  intensity of the return for each point.
57  - ChannelOption.INDEX - Create a channel named "index" containing the
58  index from the original array for each point.
59  - ChannelOption.DISTANCE - Create a channel named "distance" containing
60  the distance from the laser to each point.
61  - ChannelOption.TIMESTAMP - Create a channel named "stamps" containing the
62  specific timestamp at which each point was measured.
63  """
64 
65  LASER_SCAN_INVALID = -1.0
66  LASER_SCAN_MIN_RANGE = -2.0
67  LASER_SCAN_MAX_RANGE = -3.0
68 
70  NONE = 0x00 # Enable no channels
71  INTENSITY = 0x01 # Enable "intensities" channel
72  INDEX = 0x02 # Enable "index" channel
73  DISTANCE = 0x04 # Enable "distances" channel
74  TIMESTAMP = 0x08 # Enable "stamps" channel
75  VIEWPOINT = 0x10 # Enable "viewpoint" channel
76  DEFAULT = (INTENSITY | INDEX)
77 
78  def __init__(self):
79  self.__angle_min = 0.0
80  self.__angle_max = 0.0
81 
82  self.__cos_sin_map = np.array([[]])
83 
84  def projectLaser(self, scan_in,
85  range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT):
86  """
87  Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2.
88 
89  Project a single laser scan from a linear array into a 3D
90  point cloud. The generated cloud will be in the same frame
91  as the original laser scan.
92 
93  Keyword arguments:
94  scan_in -- The input laser scan.
95  range_cutoff -- An additional range cutoff which can be
96  applied which is more limiting than max_range in the scan
97  (default -1.0).
98  channel_options -- An OR'd set of channels to include.
99  """
100  return self.__projectLaser(scan_in, range_cutoff, channel_options)
101 
102  def __projectLaser(self, scan_in, range_cutoff, channel_options):
103  N = len(scan_in.ranges)
104 
105  ranges = np.array(scan_in.ranges)
106 
107  if (self.__cos_sin_map.shape[1] != N or
108  self.__angle_min != scan_in.angle_min or
109  self.__angle_max != scan_in.angle_max):
110  rospy.logdebug("No precomputed map given. Computing one.")
111 
112  self.__angle_min = scan_in.angle_min
113  self.__angle_max = scan_in.angle_max
114 
115  angles = scan_in.angle_min + np.arange(N) * scan_in.angle_increment
116  self.__cos_sin_map = np.array([np.cos(angles), np.sin(angles)])
117 
118  output = ranges * self.__cos_sin_map
119 
120  # Set the output cloud accordingly
121  cloud_out = PointCloud2()
122 
123  fields = [pc2.PointField() for _ in range(3)]
124 
125  fields[0].name = "x"
126  fields[0].offset = 0
127  fields[0].datatype = pc2.PointField.FLOAT32
128  fields[0].count = 1
129 
130  fields[1].name = "y"
131  fields[1].offset = 4
132  fields[1].datatype = pc2.PointField.FLOAT32
133  fields[1].count = 1
134 
135  fields[2].name = "z"
136  fields[2].offset = 8
137  fields[2].datatype = pc2.PointField.FLOAT32
138  fields[2].count = 1
139 
140  idx_intensity = idx_index = idx_distance = idx_timestamp = -1
141  idx_vpx = idx_vpy = idx_vpz = -1
142 
143  offset = 12
144 
145  if (channel_options & self.ChannelOption.INTENSITY and
146  len(scan_in.intensities) > 0):
147  field_size = len(fields)
148  fields.append(pc2.PointField())
149  fields[field_size].name = "intensity"
150  fields[field_size].datatype = pc2.PointField.FLOAT32
151  fields[field_size].offset = offset
152  fields[field_size].count = 1
153  offset += 4
154  idx_intensity = field_size
155 
156  if channel_options & self.ChannelOption.INDEX:
157  field_size = len(fields)
158  fields.append(pc2.PointField())
159  fields[field_size].name = "index"
160  fields[field_size].datatype = pc2.PointField.INT32
161  fields[field_size].offset = offset
162  fields[field_size].count = 1
163  offset += 4
164  idx_index = field_size
165 
166  if channel_options & self.ChannelOption.DISTANCE:
167  field_size = len(fields)
168  fields.append(pc2.PointField())
169  fields[field_size].name = "distances"
170  fields[field_size].datatype = pc2.PointField.FLOAT32
171  fields[field_size].offset = offset
172  fields[field_size].count = 1
173  offset += 4
174  idx_distance = field_size
175 
176  if channel_options & self.ChannelOption.TIMESTAMP:
177  field_size = len(fields)
178  fields.append(pc2.PointField())
179  fields[field_size].name = "stamps"
180  fields[field_size].datatype = pc2.PointField.FLOAT32
181  fields[field_size].offset = offset
182  fields[field_size].count = 1
183  offset += 4
184  idx_timestamp = field_size
185 
186  if channel_options & self.ChannelOption.VIEWPOINT:
187  field_size = len(fields)
188  fields.extend([pc2.PointField() for _ in range(3)])
189  fields[field_size].name = "vp_x"
190  fields[field_size].datatype = pc2.PointField.FLOAT32
191  fields[field_size].offset = offset
192  fields[field_size].count = 1
193  offset += 4
194  idx_vpx = field_size
195  field_size += 1
196 
197  fields[field_size].name = "vp_y"
198  fields[field_size].datatype = pc2.PointField.FLOAT32
199  fields[field_size].offset = offset
200  fields[field_size].count = 1
201  offset += 4
202  idx_vpy = field_size
203  field_size += 1
204 
205  fields[field_size].name = "vp_z"
206  fields[field_size].datatype = pc2.PointField.FLOAT32
207  fields[field_size].offset = offset
208  fields[field_size].count = 1
209  offset += 4
210  idx_vpz = field_size
211 
212  if range_cutoff < 0:
213  range_cutoff = scan_in.range_max
214  else:
215  range_cutoff = min(range_cutoff, scan_in.range_max)
216 
217  points = []
218  for i in range(N):
219  ri = scan_in.ranges[i]
220  if ri < range_cutoff and ri >= scan_in.range_min:
221  point = output[:, i].tolist()
222  point.append(0)
223 
224  if idx_intensity != -1:
225  point.append(scan_in.intensities[i])
226 
227  if idx_index != -1:
228  point.append(i)
229 
230  if idx_distance != -1:
231  point.append(scan_in.ranges[i])
232 
233  if idx_timestamp != -1:
234  point.append(i * scan_in.time_increment)
235 
236  if idx_vpx != -1 and idx_vpy != -1 and idx_vpz != -1:
237  point.extend([0 for _ in range(3)])
238 
239  points.append(point)
240 
241  cloud_out = pc2.create_cloud(scan_in.header, fields, points)
242 
243  return cloud_out
244 
def __projectLaser(self, scan_in, range_cutoff, channel_options)
def projectLaser(self, scan_in, range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT)


laser_geometry
Author(s): Tully Foote, Radu Bogdan Rusu, William Woodall
autogenerated on Sun Aug 21 2022 02:54:21