From d4785e879a06f2dc7c66cae58c742021677e0966 Mon Sep 17 00:00:00 2001 From: enriquefernandez Date: Tue, 20 May 2014 17:45:09 +0200 Subject: [PATCH] adds python port (only simple projection) --- CMakeLists.txt | 2 + setup.py | 11 ++ src/laser_geometry/__init__.py | 1 + src/laser_geometry/laser_geometry.py | 221 +++++++++++++++++++++++++++ 4 files changed, 235 insertions(+) create mode 100644 setup.py create mode 100644 src/laser_geometry/__init__.py create mode 100644 src/laser_geometry/laser_geometry.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 3f5509b..1260dc0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,8 @@ find_package(Boost REQUIRED COMPONENTS system thread) find_package(Eigen REQUIRED) +catkin_python_setup() + catkin_package( INCLUDE_DIRS include LIBRARIES laser_geometry diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..44edf6d --- /dev/null +++ b/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['laser_geometry'], + package_dir={'': 'src'} +) + +setup(**d) diff --git a/src/laser_geometry/__init__.py b/src/laser_geometry/__init__.py new file mode 100644 index 0000000..ff0168c --- /dev/null +++ b/src/laser_geometry/__init__.py @@ -0,0 +1 @@ +from .laser_geometry import LaserProjection diff --git a/src/laser_geometry/laser_geometry.py b/src/laser_geometry/laser_geometry.py new file mode 100644 index 0000000..617bc15 --- /dev/null +++ b/src/laser_geometry/laser_geometry.py @@ -0,0 +1,221 @@ + +import rospy +from sensor_msgs.msg import PointCloud2 +import sensor_msgs.point_cloud2 as pc2 + +import numpy as np + +class LaserProjection: + """ + A class to Project Laser Scan + + This calls will project laser scans into point clouds. It caches + unit vectors between runs (provided the angular resolution of + your scanner is not changing) to avoid excess computation. + + By default all range values less thatn the scanner min_range, + greater than the scanner max_range are removed from the generated + point cloud, as these are assumed to be invalid. + + If it is important to preserve a mapping between the index of + range values and points in the cloud, the recommended approach is to + pre-filter your laser scan message to meet the requirement that all + ranges are between min and max_range. + + The generate PointClouds have a number of channels which can be enabled + through the use of ChannelOption. + - ChannelOption.INTENSITY - Create a channel named "intensities" with the + intensity of the return for each point. + - ChannelOption.INDEX - Create a channel named "index" containing the + index from the original array for each point. + - ChannelOption.DISTANCE - Create a channel named "distance" containing + the distance from the laser to each point. + - ChannelOption.TIMESTAMP - Create a channel named "stamps" containing the + specific timestamp at which each point was measured. + """ + + LASER_SCAN_INVALID = -1.0 + LASER_SCAN_MIN_RANGE = -2.0 + LASER_SCAN_MAX_RANGE = -3.0 + + class ChannelOption: + NONE = 0x00 # Enable no channels + INTENSITY = 0x01 # Enable "intensities" channel + INDEX = 0x02 # Enable "index" channel + DISTANCE = 0x04 # Enable "distances" channel + TIMESTAMP = 0x08 # Enable "stamps" channel + VIEWPOINT = 0x10 # Enable "viewpoint" channel + DEFAULT = (INTENSITY | INDEX) + + def __init__(self): + self.__angle_min = 0.0 + self.__angle_max = 0.0 + + self.__cos_sin_map = np.array([[]]) + + def projectLaser(self, scan_in, + range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT): + """ + Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2. + + Project a single laser scan from a linear array into a 3D + point cloud. The generated cloud will be in the same frame + as the original laser scan. + + Keyword arguments: + scan_in -- The input laser scan. + range_cutoff -- An additional range cutoff which can be + applied which is more limiting than max_range in the scan + (default -1.0). + channel_options -- An OR'd set of channels to include. + """ + return self.__projectLaser(scan_in, range_cutoff, channel_options) + + def __projectLaser(self, scan_in, range_cutoff, channel_options): + N = len(scan_in.ranges) + + ranges = np.array(scan_in.ranges) + ranges = np.array([ranges, ranges]) + + if (self.__cos_sin_map.shape[1] != N or + self.__angle_min != scan_in.angle_min or + self.__angle_max != scan_in.angle_max): + rospy.logdebug("No precomputed map given. Computing one.") + + self.__angle_min = scan_in.angle_min + self.__angle_max = scan_in.angle_max + + cos_map = [np.cos(scan_in.angle_min + i * scan_in.angle_increment) + for i in range(N)] + sin_map = [np.sin(scan_in.angle_min + i * scan_in.angle_increment) + for i in range(N)] + + self.__cos_sin_map = np.array([cos_map, sin_map]) + + output = ranges * self.__cos_sin_map + + # Set the output cloud accordingly + cloud_out = PointCloud2() + + fields = [pc2.PointField() for _ in range(3)] + + fields[0].name = "x" + fields[0].offset = 0 + fields[0].datatype = pc2.PointField.FLOAT32 + fields[0].count = 1 + + fields[1].name = "y" + fields[1].offset = 4 + fields[1].datatype = pc2.PointField.FLOAT32 + fields[1].count = 1 + + fields[2].name = "z" + fields[2].offset = 8 + fields[2].datatype = pc2.PointField.FLOAT32 + fields[2].count = 1 + + idx_intensity = idx_index = idx_distance = idx_timestamp = -1 + idx_vpx = idx_vpy = idx_vpz = -1 + + offset = 12 + + if (channel_options & self.ChannelOption.INTENSITY and + len(scan_in.intensities) > 0): + field_size = len(fields) + fields.append(pc2.PointField()) + fields[field_size].name = "intensity" + fields[field_size].datatype = pc2.PointField.FLOAT32 + fields[field_size].offset = offset + fields[field_size].count = 1 + offset += 4 + idx_intensity = field_size + + if channel_options & self.ChannelOption.INDEX: + field_size = len(fields) + fields.append(pc2.PointField()) + fields[field_size].name = "index" + fields[field_size].datatype = pc2.PointField.INT32 + fields[field_size].offset = offset + fields[field_size].count = 1 + offset += 4 + idx_index = field_size + + if channel_options & self.ChannelOption.DISTANCE: + field_size = len(fields) + fields.append(pc2.PointField()) + fields[field_size].name = "distances" + fields[field_size].datatype = pc2.PointField.FLOAT32 + fields[field_size].offset = offset + fields[field_size].count = 1 + offset += 4 + idx_distance = field_size + + if channel_options & self.ChannelOption.TIMESTAMP: + field_size = len(fields) + fields.append(pc2.PointField()) + fields[field_size].name = "stamps" + fields[field_size].datatype = pc2.PointField.FLOAT32 + fields[field_size].offset = offset + fields[field_size].count = 1 + offset += 4 + idx_timestamp = field_size + + if channel_options & self.ChannelOption.VIEWPOINT: + field_size = len(fields) + fields.extend([pc2.PointField() for _ in range(3)]) + fields[field_size].name = "vp_x" + fields[field_size].datatype = pc2.PointField.FLOAT32 + fields[field_size].offset = offset + fields[field_size].count = 1 + offset += 4 + idx_vpx = field_size + field_size += 1 + + fields[field_size].name = "vp_y" + fields[field_size].datatype = pc2.PointField.FLOAT32 + fields[field_size].offset = offset + fields[field_size].count = 1 + offset += 4 + idx_vpy = field_size + field_size += 1 + + fields[field_size].name = "vp_z" + fields[field_size].datatype = pc2.PointField.FLOAT32 + fields[field_size].offset = offset + fields[field_size].count = 1 + offset += 4 + idx_vpz = field_size + + if range_cutoff < 0: + range_cutoff = scan_in.range_max + else: + range_cutoff = min(range_cutoff, scan_in.range_max) + + points = [] + for i in range(N): + ri = scan_in.ranges[i] + if ri < range_cutoff and ri >= scan_in.range_min: + point = output[:, i].tolist() + point.append(0) + + if idx_intensity != -1: + point.append(scan_in.intensities[i]) + + if idx_index != -1: + point.append(i) + + if idx_distance != -1: + point.append(scan_in.ranges[i]) + + if idx_timestamp != -1: + point.append(i * scan_in.time_increment) + + if idx_vpx != -1 and idx_vpy != -1 and idx_vpz != -1: + point.extend([0 for _ in range(3)]) + + points.append(point) + + cloud_out = pc2.create_cloud(scan_in.header, fields, points) + + return cloud_out +