From e7cbcc65bc345a7c8267837789240ce586a30d9b Mon Sep 17 00:00:00 2001 From: opletts Date: Wed, 9 Dec 2020 00:30:30 +0530 Subject: [PATCH 1/2] Adds a ring field to the published pointcloud --- .../src/carla_ros_bridge/lidar.py | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/carla_ros_bridge/src/carla_ros_bridge/lidar.py b/carla_ros_bridge/src/carla_ros_bridge/lidar.py index 7d50e326..f9a96b81 100644 --- a/carla_ros_bridge/src/carla_ros_bridge/lidar.py +++ b/carla_ros_bridge/src/carla_ros_bridge/lidar.py @@ -63,15 +63,38 @@ def sensor_data_updated(self, carla_lidar_measurement): PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('intensity', 12, PointField.FLOAT32, 1), + PointField('ring', 16, PointField.UINT16, 1) ] lidar_data = numpy.fromstring( bytes(carla_lidar_measurement.raw_data), dtype=numpy.float32) lidar_data = numpy.reshape( lidar_data, (int(lidar_data.shape[0] / 4), 4)) + + channels = float(self.carla_actor.attributes.get('channels')) + lower_fov = float(self.carla_actor.attributes.get('lower_fov')) + upper_fov = float(self.carla_actor.attributes.get('upper_fov')) + + norm = numpy.linalg.norm(lidar_data[:, :3], 2, axis=1) + pitch = numpy.arcsin(lidar_data[:, 2] / norm) + + fov_down = lower_fov / 180.0 * numpy.pi + fov = (abs(lower_fov) + abs(upper_fov)) / 180.0 * numpy.pi + + ring = (pitch + abs(fov_down)) / fov + ring *= channels + ring = numpy.floor(ring) + ring = numpy.minimum(channels - 1, ring) + ring = numpy.maximum(0, ring).astype(numpy.uint16) + ring = ring.reshape(-1, 1) + # we take the oposite of y axis # (as lidar point are express in left handed coordinate system, and ros need right handed) lidar_data[:, 1] *= -1 + + lidar_data = lidar_data.tolist() + [lidar_data[i].insert(4, *ring[i]) for i in range(len(lidar_data))] + point_cloud_msg = create_cloud(header, fields, lidar_data) self.lidar_publisher.publish(point_cloud_msg) From 964c0a461d8d0b222892f0cef54373f456d408f9 Mon Sep 17 00:00:00 2001 From: opletts Date: Thu, 7 Jan 2021 17:56:40 +0530 Subject: [PATCH 2/2] Using get_point_count() to calculate ring according to (#432) and moved static calculation to init --- .../src/carla_ros_bridge/lidar.py | 25 ++++++------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/carla_ros_bridge/src/carla_ros_bridge/lidar.py b/carla_ros_bridge/src/carla_ros_bridge/lidar.py index f9a96b81..85c057ff 100644 --- a/carla_ros_bridge/src/carla_ros_bridge/lidar.py +++ b/carla_ros_bridge/src/carla_ros_bridge/lidar.py @@ -47,6 +47,8 @@ def __init__(self, carla_actor, parent, node, synchronous_mode): "/point_cloud", PointCloud2, queue_size=10) + + self.channels = int(self.carla_actor.attributes.get('channels')) self.listen() # pylint: disable=arguments-differ @@ -71,30 +73,19 @@ def sensor_data_updated(self, carla_lidar_measurement): lidar_data = numpy.reshape( lidar_data, (int(lidar_data.shape[0] / 4), 4)) - channels = float(self.carla_actor.attributes.get('channels')) - lower_fov = float(self.carla_actor.attributes.get('lower_fov')) - upper_fov = float(self.carla_actor.attributes.get('upper_fov')) - - norm = numpy.linalg.norm(lidar_data[:, :3], 2, axis=1) - pitch = numpy.arcsin(lidar_data[:, 2] / norm) + ring = None - fov_down = lower_fov / 180.0 * numpy.pi - fov = (abs(lower_fov) + abs(upper_fov)) / 180.0 * numpy.pi + for i in range(self.channels): + current_ring_points_count = carla_lidar_measurement.get_point_count(i) + ring = numpy.vstack((ring, numpy.full((current_ring_points_count, 1), i))) - ring = (pitch + abs(fov_down)) / fov - ring *= channels - ring = numpy.floor(ring) - ring = numpy.minimum(channels - 1, ring) - ring = numpy.maximum(0, ring).astype(numpy.uint16) - ring = ring.reshape(-1, 1) + ring = numpy.delete(ring, 0, axis=0) + lidar_data = numpy.hstack((lidar_data, ring)) # we take the oposite of y axis # (as lidar point are express in left handed coordinate system, and ros need right handed) lidar_data[:, 1] *= -1 - lidar_data = lidar_data.tolist() - [lidar_data[i].insert(4, *ring[i]) for i in range(len(lidar_data))] - point_cloud_msg = create_cloud(header, fields, lidar_data) self.lidar_publisher.publish(point_cloud_msg)