Go to the documentation of this file.00001 """
00002 Copyright (c) 2014, Enrique Fernandez
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 import rospy
00031 from sensor_msgs.msg import PointCloud2
00032 import sensor_msgs.point_cloud2 as pc2
00033
00034 import numpy as np
00035
00036 class LaserProjection:
00037 """
00038 A class to Project Laser Scan
00039
00040 This calls will project laser scans into point clouds. It caches
00041 unit vectors between runs (provided the angular resolution of
00042 your scanner is not changing) to avoid excess computation.
00043
00044 By default all range values less thatn the scanner min_range,
00045 greater than the scanner max_range are removed from the generated
00046 point cloud, as these are assumed to be invalid.
00047
00048 If it is important to preserve a mapping between the index of
00049 range values and points in the cloud, the recommended approach is to
00050 pre-filter your laser scan message to meet the requirement that all
00051 ranges are between min and max_range.
00052
00053 The generate PointClouds have a number of channels which can be enabled
00054 through the use of ChannelOption.
00055 - ChannelOption.INTENSITY - Create a channel named "intensities" with the
00056 intensity of the return for each point.
00057 - ChannelOption.INDEX - Create a channel named "index" containing the
00058 index from the original array for each point.
00059 - ChannelOption.DISTANCE - Create a channel named "distance" containing
00060 the distance from the laser to each point.
00061 - ChannelOption.TIMESTAMP - Create a channel named "stamps" containing the
00062 specific timestamp at which each point was measured.
00063 """
00064
00065 LASER_SCAN_INVALID = -1.0
00066 LASER_SCAN_MIN_RANGE = -2.0
00067 LASER_SCAN_MAX_RANGE = -3.0
00068
00069 class ChannelOption:
00070 NONE = 0x00
00071 INTENSITY = 0x01
00072 INDEX = 0x02
00073 DISTANCE = 0x04
00074 TIMESTAMP = 0x08
00075 VIEWPOINT = 0x10
00076 DEFAULT = (INTENSITY | INDEX)
00077
00078 def __init__(self):
00079 self.__angle_min = 0.0
00080 self.__angle_max = 0.0
00081
00082 self.__cos_sin_map = np.array([[]])
00083
00084 def projectLaser(self, scan_in,
00085 range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT):
00086 """
00087 Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2.
00088
00089 Project a single laser scan from a linear array into a 3D
00090 point cloud. The generated cloud will be in the same frame
00091 as the original laser scan.
00092
00093 Keyword arguments:
00094 scan_in -- The input laser scan.
00095 range_cutoff -- An additional range cutoff which can be
00096 applied which is more limiting than max_range in the scan
00097 (default -1.0).
00098 channel_options -- An OR'd set of channels to include.
00099 """
00100 return self.__projectLaser(scan_in, range_cutoff, channel_options)
00101
00102 def __projectLaser(self, scan_in, range_cutoff, channel_options):
00103 N = len(scan_in.ranges)
00104
00105 ranges = np.array(scan_in.ranges)
00106
00107 if (self.__cos_sin_map.shape[1] != N or
00108 self.__angle_min != scan_in.angle_min or
00109 self.__angle_max != scan_in.angle_max):
00110 rospy.logdebug("No precomputed map given. Computing one.")
00111
00112 self.__angle_min = scan_in.angle_min
00113 self.__angle_max = scan_in.angle_max
00114
00115 angles = scan_in.angle_min + np.arange(N) * scan_in.angle_increment
00116 self.__cos_sin_map = np.array([np.cos(angles), np.sin(angles)])
00117
00118 output = ranges * self.__cos_sin_map
00119
00120
00121 cloud_out = PointCloud2()
00122
00123 fields = [pc2.PointField() for _ in range(3)]
00124
00125 fields[0].name = "x"
00126 fields[0].offset = 0
00127 fields[0].datatype = pc2.PointField.FLOAT32
00128 fields[0].count = 1
00129
00130 fields[1].name = "y"
00131 fields[1].offset = 4
00132 fields[1].datatype = pc2.PointField.FLOAT32
00133 fields[1].count = 1
00134
00135 fields[2].name = "z"
00136 fields[2].offset = 8
00137 fields[2].datatype = pc2.PointField.FLOAT32
00138 fields[2].count = 1
00139
00140 idx_intensity = idx_index = idx_distance = idx_timestamp = -1
00141 idx_vpx = idx_vpy = idx_vpz = -1
00142
00143 offset = 12
00144
00145 if (channel_options & self.ChannelOption.INTENSITY and
00146 len(scan_in.intensities) > 0):
00147 field_size = len(fields)
00148 fields.append(pc2.PointField())
00149 fields[field_size].name = "intensity"
00150 fields[field_size].datatype = pc2.PointField.FLOAT32
00151 fields[field_size].offset = offset
00152 fields[field_size].count = 1
00153 offset += 4
00154 idx_intensity = field_size
00155
00156 if channel_options & self.ChannelOption.INDEX:
00157 field_size = len(fields)
00158 fields.append(pc2.PointField())
00159 fields[field_size].name = "index"
00160 fields[field_size].datatype = pc2.PointField.INT32
00161 fields[field_size].offset = offset
00162 fields[field_size].count = 1
00163 offset += 4
00164 idx_index = field_size
00165
00166 if channel_options & self.ChannelOption.DISTANCE:
00167 field_size = len(fields)
00168 fields.append(pc2.PointField())
00169 fields[field_size].name = "distances"
00170 fields[field_size].datatype = pc2.PointField.FLOAT32
00171 fields[field_size].offset = offset
00172 fields[field_size].count = 1
00173 offset += 4
00174 idx_distance = field_size
00175
00176 if channel_options & self.ChannelOption.TIMESTAMP:
00177 field_size = len(fields)
00178 fields.append(pc2.PointField())
00179 fields[field_size].name = "stamps"
00180 fields[field_size].datatype = pc2.PointField.FLOAT32
00181 fields[field_size].offset = offset
00182 fields[field_size].count = 1
00183 offset += 4
00184 idx_timestamp = field_size
00185
00186 if channel_options & self.ChannelOption.VIEWPOINT:
00187 field_size = len(fields)
00188 fields.extend([pc2.PointField() for _ in range(3)])
00189 fields[field_size].name = "vp_x"
00190 fields[field_size].datatype = pc2.PointField.FLOAT32
00191 fields[field_size].offset = offset
00192 fields[field_size].count = 1
00193 offset += 4
00194 idx_vpx = field_size
00195 field_size += 1
00196
00197 fields[field_size].name = "vp_y"
00198 fields[field_size].datatype = pc2.PointField.FLOAT32
00199 fields[field_size].offset = offset
00200 fields[field_size].count = 1
00201 offset += 4
00202 idx_vpy = field_size
00203 field_size += 1
00204
00205 fields[field_size].name = "vp_z"
00206 fields[field_size].datatype = pc2.PointField.FLOAT32
00207 fields[field_size].offset = offset
00208 fields[field_size].count = 1
00209 offset += 4
00210 idx_vpz = field_size
00211
00212 if range_cutoff < 0:
00213 range_cutoff = scan_in.range_max
00214 else:
00215 range_cutoff = min(range_cutoff, scan_in.range_max)
00216
00217 points = []
00218 for i in range(N):
00219 ri = scan_in.ranges[i]
00220 if ri < range_cutoff and ri >= scan_in.range_min:
00221 point = output[:, i].tolist()
00222 point.append(0)
00223
00224 if idx_intensity != -1:
00225 point.append(scan_in.intensities[i])
00226
00227 if idx_index != -1:
00228 point.append(i)
00229
00230 if idx_distance != -1:
00231 point.append(scan_in.ranges[i])
00232
00233 if idx_timestamp != -1:
00234 point.append(i * scan_in.time_increment)
00235
00236 if idx_vpx != -1 and idx_vpy != -1 and idx_vpz != -1:
00237 point.extend([0 for _ in range(3)])
00238
00239 points.append(point)
00240
00241 cloud_out = pc2.create_cloud(scan_in.header, fields, points)
00242
00243 return cloud_out
00244