def send_lidar_msg(self,
point_cloud_stream,
simulator_pc,
timestamp,
lidar_setup,
ego_transform=None):
"""Transforms and sends a point cloud reading.
This method is transforms a point cloud from the challenge format
to the type Pylot uses, and it sends it on the point cloud stream.
"""
# Remove the intensity component of the point cloud.
simulator_pc = simulator_pc[:, :3]
point_cloud = PointCloud(simulator_pc, lidar_setup)
if self._last_point_cloud is not None:
# TODO(ionel): Should offset the last point cloud wrt to the
# current location.
# self._last_point_cloud.global_points = \
# ego_transform.transform_points(
# self._last_point_cloud.global_points)
# self._last_point_cloud.points = \
# self._last_point_cloud._to_camera_coordinates(
# self._last_point_cloud.global_points)
point_cloud.merge(self._last_point_cloud)
point_cloud_stream.send(
pylot.perception.messages.PointCloudMessage(
timestamp, point_cloud))
point_cloud_stream.send(erdos.WatermarkMessage(timestamp))
# global_pc = ego_transform.inverse_transform_points(simulator_pc)
self._last_point_cloud = PointCloud(simulator_pc, lidar_setup)
def process_point_clouds(self, simulator_pc):
""" Invoked when a point cloud is received from the simulator.
"""
game_time = int(simulator_pc.timestamp * 1000)
timestamp = erdos.Timestamp(coordinates=[game_time])
watermark_msg = erdos.WatermarkMessage(timestamp)
with erdos.profile(self.config.name + '.process_point_clouds',
self,
event_data={'timestamp': str(timestamp)}):
# Ensure that the code executes serially
with self._lock:
assert len(
simulator_pc.raw_data) > 0, 'Lidar did not send any points'
# Include the transform relative to the vehicle.
# simulator_pc.transform returns the world transform, but
# we do not use it directly.
msg = PointCloudMessage(
timestamp,
PointCloud.from_simulator_point_cloud(
simulator_pc, self._lidar_setup))
if self._release_data:
self._lidar_stream.send(msg)
self._lidar_stream.send(watermark_msg)
else:
# Pickle the data, and release it upon release msg receipt.
pickled_msg = pickle.dumps(
msg, protocol=pickle.HIGHEST_PROTOCOL)
with self._pickle_lock:
self._pickled_messages[msg.timestamp] = pickled_msg
self._notify_reading_stream.send(watermark_msg)
def test_point_cloud_get_pixel_location(lidar_points, pixel, expected):
camera_setup = CameraSetup(
'test_setup',
'test_type',
801,
601, # width, height
Transform(location=Location(0, 0, 0), rotation=Rotation(0, 0, 0)),
fov=90)
point_cloud = PointCloud(lidar_points, Transform(Location(), Rotation()))
location = point_cloud.get_pixel_location(pixel, camera_setup)
assert np.isclose(location.x,
expected.x), 'Returned x value is not the same '
'as expected'
assert np.isclose(location.y,
expected.y), 'Returned y value is not the same '
'as expected'
assert np.isclose(location.z,
expected.z), 'Returned z value is not the same '
'as expected'
def send_lidar_msg(self,
carla_pc,
transform,
timestamp,
ego_transform=None):
# Remove the intensity component of the point cloud.
carla_pc = carla_pc[:, :3]
point_cloud = PointCloud(carla_pc, self._lidar_setup)
if self._last_point_cloud is not None:
# TODO(ionel): Should offset the last point cloud wrt to the
# current location.
# self._last_point_cloud.global_points = \
# ego_transform.transform_points(
# self._last_point_cloud.global_points)
# self._last_point_cloud.points = \
# self._last_point_cloud._to_camera_coordinates(
# self._last_point_cloud.global_points)
point_cloud.merge(self._last_point_cloud)
self._point_cloud_stream.send(
pylot.perception.messages.PointCloudMessage(
timestamp, point_cloud))
self._point_cloud_stream.send(erdos.WatermarkMessage(timestamp))
# global_pc = ego_transform.inverse_transform_points(carla_pc)
self._last_point_cloud = PointCloud(carla_pc, self._lidar_setup)
def on_lidar_msg(carla_pc):
game_time = int(carla_pc.timestamp * 1000)
print("Received lidar msg {}".format(game_time))
lidar_transform = pylot.utils.Transform.from_carla_transform(
carla_pc.transform)
point_cloud = PointCloud.from_carla_point_cloud(carla_pc, lidar_transform)
camera_setup = CameraSetup("lidar_camera",
"sensor.camera.depth",
800,
600,
lidar_transform,
fov=90.0)
for (x, y) in pixels_to_check:
pixel = pylot.utils.Vector2D(x, y)
location = point_cloud.get_pixel_location(pixel, camera_setup)
print("{} Computed using lidar {}".format((x, y), location))
global lidar_pc
lidar_pc = point_cloud
def process_point_clouds(self, carla_pc):
""" Invoked when a pointcloud is received from the simulator.
Args:
carla_pc: a carla.SensorData object.
"""
# Ensure that the code executes serially
with self._lock:
game_time = int(carla_pc.timestamp * 1000)
timestamp = erdos.Timestamp(coordinates=[game_time])
watermark_msg = erdos.WatermarkMessage(timestamp)
# Include the transform relative to the vehicle.
# Carla carla_pc.transform returns the world transform, but
# we do not use it directly.
msg = PointCloudMessage(
PointCloud.from_carla_point_cloud(
carla_pc, self._lidar_setup.get_transform()), timestamp)
self._lidar_stream.send(msg)
# Note: The operator is set not to automatically propagate
# watermark messages received on input streams. Thus, we can
# issue watermarks only after the Carla callback is invoked.
self._lidar_stream.send(watermark_msg)
def test_initialize_point_cloud(points, expected):
point_cloud = PointCloud(points, Transform(Location(), Rotation()))
for i in range(len(expected)):
assert all(np.isclose(point_cloud.points[i], expected[i]))
def test_initialize_point_cloud(points, expected):
lidar_setup = LidarSetup('lidar', 'sensor.lidar.ray_cast',
Transform(Location(), Rotation()))
point_cloud = PointCloud(points, lidar_setup)
for i in range(len(expected)):
assert all(np.isclose(point_cloud.points[i], expected[i]))
def send_lidar_msg(self, carla_pc, transform, timestamp):
msg = pylot.perception.messages.PointCloudMessage(
timestamp, PointCloud(carla_pc, self._lidar_setup))
self._point_cloud_stream.send(msg)
self._point_cloud_stream.send(erdos.WatermarkMessage(timestamp))
def send_lidar_msg(self, data, transform, timestamp):
msg = pylot.perception.messages.PointCloudMessage(PointCloud(
data, transform),
timestamp=timestamp)
self._point_cloud_stream.send(msg)
self._point_cloud_stream.send(erdos.WatermarkMessage(timestamp))