DuongTD/laser_geometry
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

adds python port (only simple projection)

Browse Source
This commit is contained in:
enriquefernandez 2014-05-20 17:45:09 +02:00 committed by Vincent Rabaud
parent 43bb1c4191
commit d4785e879a
4 changed files with 235 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -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

11
setup.py Normal file
View File

@ -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)

1
src/laser_geometry/__init__.py Normal file
View File

@ -0,0 +1 @@
from .laser_geometry import LaserProjection

221
src/laser_geometry/laser_geometry.py Normal file
View File

@ -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