From 2454df76288ebc77ce14f4f1fb799db52e267270 Mon Sep 17 00:00:00 2001 From: Laura Lindzey Date: Wed, 11 Dec 2013 17:04:32 -0800 Subject: [PATCH] and, now that the debs are correct, using sensor_msgs.point_cloud2 rather tahn my local copy of it --- clear_table/scripts/run_segmentation.py | 2 +- .../src/clear_table/handle_point_cloud2.py | 144 ------------------ clear_table/src/clear_table/scene_handler.py | 2 +- clear_table/src/clear_table/sensor_handler.py | 3 +- 4 files changed, 3 insertions(+), 148 deletions(-) delete mode 100644 clear_table/src/clear_table/handle_point_cloud2.py diff --git a/clear_table/scripts/run_segmentation.py b/clear_table/scripts/run_segmentation.py index 443d0a9..6dc6721 100755 --- a/clear_table/scripts/run_segmentation.py +++ b/clear_table/scripts/run_segmentation.py @@ -6,7 +6,7 @@ import actionlib from cv_bridge import CvBridge -import clear_table.handle_point_cloud2 as pts +import sensor_msgs.point_cloud2 as pts from sensor_msgs.msg import Image, CameraInfo, PointCloud2 from shared_autonomy_msgs.msg import SegmentGoal, SegmentAction diff --git a/clear_table/src/clear_table/handle_point_cloud2.py b/clear_table/src/clear_table/handle_point_cloud2.py deleted file mode 100644 index b6039ba..0000000 --- a/clear_table/src/clear_table/handle_point_cloud2.py +++ /dev/null @@ -1,144 +0,0 @@ -#! /usr/bin/env python - -import ctypes -import math -import struct - -from sensor_msgs.msg import PointCloud2, PointField - -_DATATYPES = {} -_DATATYPES[PointField.INT8] = ('b', 1) -_DATATYPES[PointField.UINT8] = ('B', 1) -_DATATYPES[PointField.INT16] = ('h', 2) -_DATATYPES[PointField.UINT16] = ('H', 2) -_DATATYPES[PointField.INT32] = ('i', 4) -_DATATYPES[PointField.UINT32] = ('I', 4) -_DATATYPES[PointField.FLOAT32] = ('f', 4) -_DATATYPES[PointField.FLOAT64] = ('d', 8) - -def read_points(cloud, field_names=None, skip_nans=False, uvs=[]): - """ - Read points from a L{sensor_msgs.PointCloud2} message. - - @param cloud: The point cloud to read from. - @type cloud: L{sensor_msgs.PointCloud2} - @param field_names: The names of fields to read. If None, read all fields. [default: None] - @type field_names: iterable - @param skip_nans: If True, then don't return any point with a NaN value. - @type skip_nans: bool [default: False] - @param uvs: If specified, then only return the points at the given coordinates. [default: empty list] - @type uvs: iterable - @return: Generator which yields a list of values for each point. - @rtype: generator - """ - #assert isinstance(cloud, roslib.message.Message) and cloud._type == 'sensor_msgs/PointCloud2', 'cloud is not a sensor_msgs.msg.PointCloud2' - fmt = _get_struct_fmt(cloud.is_bigendian, cloud.fields, field_names) - width, height, point_step, row_step, data, isnan = cloud.width, cloud.height, cloud.point_step, cloud.row_step, cloud.data, math.isnan - unpack_from = struct.Struct(fmt).unpack_from - - if skip_nans: - if uvs: - for u, v in uvs: - p = unpack_from(data, (row_step * v) + (point_step * u)) - has_nan = False - for pv in p: - if isnan(pv): - has_nan = True - break - if not has_nan: - yield p - else: - for v in xrange(height): - offset = row_step * v - for u in xrange(width): - p = unpack_from(data, offset) - has_nan = False - for pv in p: - if isnan(pv): - has_nan = True - break - if not has_nan: - yield p - offset += point_step - else: - if uvs: - for u, v in uvs: - yield unpack_from(data, (row_step * v) + (point_step * u)) - else: - for v in xrange(height): - offset = row_step * v - for u in xrange(width): - yield unpack_from(data, offset) - offset += point_step - -def create_cloud(header, fields, points): - """ - Create a L{sensor_msgs.msg.PointCloud2} message. - - @param header: The point cloud header. - @type header: L{std_msgs.msg.Header} - @param fields: The point cloud fields. - @type fields: iterable of L{sensor_msgs.msg.PointField} - @param points: The point cloud points. - @type points: list of iterables, i.e. one iterable for each point, with the - elements of each iterable being the values of the fields for - that point (in the same order as the fields parameter) - @return: The point cloud. - @rtype: L{sensor_msgs.msg.PointCloud2} - """ - - cloud_struct = struct.Struct(_get_struct_fmt(False, fields)) - - buff = ctypes.create_string_buffer(cloud_struct.size * len(points)) - - point_step, pack_into = cloud_struct.size, cloud_struct.pack_into - offset = 0 - for p in points: - pack_into(buff, offset, *p) - offset += point_step - - return PointCloud2(header=header, - height=1, - width=len(points), - is_dense=False, - is_bigendian=False, - fields=fields, - point_step=cloud_struct.size, - row_step=cloud_struct.size * len(points), - data=buff.raw) - - - -def create_cloud_xyz32(header, points): - """ - Create a L{sensor_msgs.msg.PointCloud2} message with 3 float32 fields (x, y, z). - - @param header: The point cloud header. - @type header: L{std_msgs.msg.Header} - @param points: The point cloud points. - @type points: iterable - @return: The point cloud. - @rtype: L{sensor_msgs.msg.PointCloud2} - """ - fields = [PointField('x', 0, PointField.FLOAT32, 1), - PointField('y', 4, PointField.FLOAT32, 1), - PointField('z', 8, PointField.FLOAT32, 1)] - return create_cloud(header, fields, points) - - -def _get_struct_fmt(is_bigendian, fields, field_names=None): - fmt = '>' if is_bigendian else '<' - - offset = 0 - for field in (f for f in sorted(fields, key=lambda f: f.offset) if field_names is None or f.name in field_names): - if offset < field.offset: - fmt += 'x' * (field.offset - offset) - offset = field.offset - if field.datatype not in _DATATYPES: - print >> sys.stderr, 'Skipping unknown PointField datatype [%d]' % field.datatype - else: - datatype_fmt, datatype_length = _DATATYPES[field.datatype] - fmt += field.count * datatype_fmt - offset += field.count * datatype_length - - return fmt diff --git a/clear_table/src/clear_table/scene_handler.py b/clear_table/src/clear_table/scene_handler.py index 8feaa07..10da8e4 100644 --- a/clear_table/src/clear_table/scene_handler.py +++ b/clear_table/src/clear_table/scene_handler.py @@ -9,7 +9,7 @@ import rospy import tf -import clear_table.handle_point_cloud2 as pts +import sensor_msgs.point_cloud2 as pts from geometry_msgs.msg import Point, PoseStamped, Vector3 from moveit_msgs.msg import AttachedCollisionObject, CollisionObject, PlanningSceneWorld diff --git a/clear_table/src/clear_table/sensor_handler.py b/clear_table/src/clear_table/sensor_handler.py index be0c3ec..9eee62f 100644 --- a/clear_table/src/clear_table/sensor_handler.py +++ b/clear_table/src/clear_table/sensor_handler.py @@ -6,7 +6,7 @@ import actionlib from cv_bridge import CvBridge -import clear_table.handle_point_cloud2 as pts +import sensor_msgs.point_cloud2 as pts from sensor_msgs.msg import PointCloud2 from shared_autonomy_msgs.msg import SegmentGoal, SegmentAction @@ -68,7 +68,6 @@ def get_point_cloud(self, data, mask): print e return [] - # TODO: depend on the read_points that should be in common_msgs? # TODO: why didn't the uvs=idxs code work? x pt_gen = pts.read_points(data.points)#, uvs=idxs, skip_nans=True) # this didn't work!!