Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix issue #680 : incomplete lidar points cloud #682

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Fix issue #680 : incomplete lidar points cloud #682

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 2 additions & 3 deletions carla_ros_bridge/src/carla_ros_bridge/actor_factory.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
47 changes: 30 additions & 17 deletions carla_ros_bridge/src/carla_ros_bridge/lidar.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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):
Expand All @@ -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):
Expand Down
26 changes: 9 additions & 17 deletions carla_ros_bridge/src/carla_ros_bridge/sensor.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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(
Expand Down