diff --git a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py index a6963556..83390bc2 100755 --- a/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py +++ b/carla_ros_bridge/src/carla_ros_bridge/actor_factory.py @@ -387,9 +387,8 @@ def _create_object(self, uid, type_id, name, attach_to, spawn_pose, carla_actor= elif carla_actor.type_id.startswith("sensor.lidar"): if carla_actor.type_id.endswith("sensor.lidar.ray_cast"): actor = Lidar(uid, name, parent, spawn_pose, self.node, - carla_actor, self.sync_mode) - elif carla_actor.type_id.endswith( - "sensor.lidar.ray_cast_semantic"): + carla_actor, self.sync_mode, False, self.world.get_settings().fixed_delta_seconds) + elif carla_actor.type_id.endswith("sensor.lidar.ray_cast_semantic"): actor = SemanticLidar(uid, name, parent, spawn_pose, self.node, carla_actor, self.sync_mode) diff --git a/carla_ros_bridge/src/carla_ros_bridge/lidar.py b/carla_ros_bridge/src/carla_ros_bridge/lidar.py index 9cb39ab1..140e87e6 100644 --- a/carla_ros_bridge/src/carla_ros_bridge/lidar.py +++ b/carla_ros_bridge/src/carla_ros_bridge/lidar.py @@ -24,7 +24,7 @@ class Lidar(Sensor): Actor implementation details for lidars """ - def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode): + def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode, is_event_sensor, simulation_tick): """ Constructor @@ -54,6 +54,18 @@ def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, sy self.lidar_publisher = node.new_publisher(PointCloud2, self.get_topic_prefix(), qos_profile=10) + + self.packet_per_frame = 1/(float(carla_actor.attributes.get('rotation_frequency'))*simulation_tick) + + self.packet_list = [] + + self.fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1) + ] + self.listen() def destroy(self): @@ -68,23 +80,24 @@ def sensor_data_updated(self, carla_lidar_measurement): :param carla_lidar_measurement: carla lidar measurement object :type carla_lidar_measurement: carla.LidarMeasurement """ - header = self.get_msg_header(timestamp=carla_lidar_measurement.timestamp) - fields = [ - PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), - PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), - PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), - PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1) - ] + lidar_packet = numpy.fromstring(bytes(carla_lidar_measurement.raw_data), dtype=numpy.float32) - 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)) - # we take the opposite of y axis - # (as lidar point are express in left handed coordinate system, and ros need right handed) - lidar_data[:, 1] *= -1 - point_cloud_msg = create_cloud(header, fields, lidar_data) - self.lidar_publisher.publish(point_cloud_msg) + self.packet_list.append(lidar_packet) + + if len(self.packet_list) == self.packet_per_frame: + + pts_all = numpy.hstack(self.packet_list) + + header = self.get_msg_header(timestamp=carla_lidar_measurement.timestamp) + + lidar_data = numpy.reshape(pts_all, (int(pts_all.shape[0] / 4), 4)) + # we take the opposite of y axis + # (as lidar point are express in left handed coordinate system, and ros need right handed) + lidar_data[:, 1] *= -1 + point_cloud_msg = create_cloud(header, self.fields, lidar_data) + self.lidar_publisher.publish(point_cloud_msg) + + self.packet_list = [] class SemanticLidar(Sensor): diff --git a/carla_ros_bridge/src/carla_ros_bridge/sensor.py b/carla_ros_bridge/src/carla_ros_bridge/sensor.py index 1bdd36c3..660397bc 100644 --- a/carla_ros_bridge/src/carla_ros_bridge/sensor.py +++ b/carla_ros_bridge/src/carla_ros_bridge/sensor.py @@ -61,6 +61,7 @@ def __init__(self, # pylint: disable=too-many-arguments # if a sensor only delivers data on special events, # do not wait for it. That means you might get data from a # sensor, that belongs to a different frame + simulation_tick=None, ): """ Constructor @@ -219,26 +220,17 @@ def _update_synchronous_event_sensor(self, frame, timestamp): return def _update_synchronous_sensor(self, frame, timestamp): - while not self.next_data_expected_time or \ - (not self.queue.empty() or - self.next_data_expected_time and - self.next_data_expected_time < timestamp): + if not self.queue.empty(): while True: try: carla_sensor_data = self.queue.get(timeout=1.0) - if carla_sensor_data.frame == frame: - self.node.logdebug("{}({}): process {}".format(self.__class__.__name__, - self.get_id(), frame)) - self.publish_tf(trans.carla_transform_to_ros_pose( - carla_sensor_data.transform), timestamp) - self.sensor_data_updated(carla_sensor_data) - return - elif carla_sensor_data.frame < frame: - self.node.logwarn("{}({}): skipping old frame {}, expected {}".format( - self.__class__.__name__, - self.get_id(), - carla_sensor_data.frame, - frame)) + self.node.logdebug("{}({}): process {}".format(self.__class__.__name__, + self.get_id(), frame)) + self.publish_tf(trans.carla_transform_to_ros_pose( + carla_sensor_data.transform), timestamp) + self.sensor_data_updated(carla_sensor_data) + return + except queue.Empty: if roscomp.ok(): self.node.logwarn("{}({}): Expected Frame {} not received".format(