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 ranges = np.array([ranges, ranges])
00107
00108 if (self.__cos_sin_map.shape[1] != N or
00109 self.__angle_min != scan_in.angle_min or
00110 self.__angle_max != scan_in.angle_max):
00111 rospy.logdebug("No precomputed map given. Computing one.")
00112
00113 self.__angle_min = scan_in.angle_min
00114 self.__angle_max = scan_in.angle_max
00115
00116 cos_map = [np.cos(scan_in.angle_min + i * scan_in.angle_increment)
00117 for i in range(N)]
00118 sin_map = [np.sin(scan_in.angle_min + i * scan_in.angle_increment)
00119 for i in range(N)]
00120
00121 self.__cos_sin_map = np.array([cos_map, sin_map])
00122
00123 output = ranges * self.__cos_sin_map
00124
00125
00126 cloud_out = PointCloud2()
00127
00128 fields = [pc2.PointField() for _ in range(3)]
00129
00130 fields[0].name = "x"
00131 fields[0].offset = 0
00132 fields[0].datatype = pc2.PointField.FLOAT32
00133 fields[0].count = 1
00134
00135 fields[1].name = "y"
00136 fields[1].offset = 4
00137 fields[1].datatype = pc2.PointField.FLOAT32
00138 fields[1].count = 1
00139
00140 fields[2].name = "z"
00141 fields[2].offset = 8
00142 fields[2].datatype = pc2.PointField.FLOAT32
00143 fields[2].count = 1
00144
00145 idx_intensity = idx_index = idx_distance = idx_timestamp = -1
00146 idx_vpx = idx_vpy = idx_vpz = -1
00147
00148 offset = 12
00149
00150 if (channel_options & self.ChannelOption.INTENSITY and
00151 len(scan_in.intensities) > 0):
00152 field_size = len(fields)
00153 fields.append(pc2.PointField())
00154 fields[field_size].name = "intensity"
00155 fields[field_size].datatype = pc2.PointField.FLOAT32
00156 fields[field_size].offset = offset
00157 fields[field_size].count = 1
00158 offset += 4
00159 idx_intensity = field_size
00160
00161 if channel_options & self.ChannelOption.INDEX:
00162 field_size = len(fields)
00163 fields.append(pc2.PointField())
00164 fields[field_size].name = "index"
00165 fields[field_size].datatype = pc2.PointField.INT32
00166 fields[field_size].offset = offset
00167 fields[field_size].count = 1
00168 offset += 4
00169 idx_index = field_size
00170
00171 if channel_options & self.ChannelOption.DISTANCE:
00172 field_size = len(fields)
00173 fields.append(pc2.PointField())
00174 fields[field_size].name = "distances"
00175 fields[field_size].datatype = pc2.PointField.FLOAT32
00176 fields[field_size].offset = offset
00177 fields[field_size].count = 1
00178 offset += 4
00179 idx_distance = field_size
00180
00181 if channel_options & self.ChannelOption.TIMESTAMP:
00182 field_size = len(fields)
00183 fields.append(pc2.PointField())
00184 fields[field_size].name = "stamps"
00185 fields[field_size].datatype = pc2.PointField.FLOAT32
00186 fields[field_size].offset = offset
00187 fields[field_size].count = 1
00188 offset += 4
00189 idx_timestamp = field_size
00190
00191 if channel_options & self.ChannelOption.VIEWPOINT:
00192 field_size = len(fields)
00193 fields.extend([pc2.PointField() for _ in range(3)])
00194 fields[field_size].name = "vp_x"
00195 fields[field_size].datatype = pc2.PointField.FLOAT32
00196 fields[field_size].offset = offset
00197 fields[field_size].count = 1
00198 offset += 4
00199 idx_vpx = field_size
00200 field_size += 1
00201
00202 fields[field_size].name = "vp_y"
00203 fields[field_size].datatype = pc2.PointField.FLOAT32
00204 fields[field_size].offset = offset
00205 fields[field_size].count = 1
00206 offset += 4
00207 idx_vpy = field_size
00208 field_size += 1
00209
00210 fields[field_size].name = "vp_z"
00211 fields[field_size].datatype = pc2.PointField.FLOAT32
00212 fields[field_size].offset = offset
00213 fields[field_size].count = 1
00214 offset += 4
00215 idx_vpz = field_size
00216
00217 if range_cutoff < 0:
00218 range_cutoff = scan_in.range_max
00219 else:
00220 range_cutoff = min(range_cutoff, scan_in.range_max)
00221
00222 points = []
00223 for i in range(N):
00224 ri = scan_in.ranges[i]
00225 if ri < range_cutoff and ri >= scan_in.range_min:
00226 point = output[:, i].tolist()
00227 point.append(0)
00228
00229 if idx_intensity != -1:
00230 point.append(scan_in.intensities[i])
00231
00232 if idx_index != -1:
00233 point.append(i)
00234
00235 if idx_distance != -1:
00236 point.append(scan_in.ranges[i])
00237
00238 if idx_timestamp != -1:
00239 point.append(i * scan_in.time_increment)
00240
00241 if idx_vpx != -1 and idx_vpy != -1 and idx_vpz != -1:
00242 point.extend([0 for _ in range(3)])
00243
00244 points.append(point)
00245
00246 cloud_out = pc2.create_cloud(scan_in.header, fields, points)
00247
00248 return cloud_out
00249