laser_geometry.py
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 # Enable no channels
00071         INTENSITY = 0x01 # Enable "intensities" channel
00072         INDEX     = 0x02 # Enable "index"       channel
00073         DISTANCE  = 0x04 # Enable "distances"   channel
00074         TIMESTAMP = 0x08 # Enable "stamps"      channel
00075         VIEWPOINT = 0x10 # Enable "viewpoint"   channel
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         # Set the output cloud accordingly
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 


laser_geometry
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Tue Jul 2 2019 20:04:25