def sensor_data_updated(self, carla_lidar_measurement):
"""
Function to transform a received semantic lidar measurement into a ROS point cloud message
:param carla_lidar_measurement: carla semantic lidar measurement object
:type carla_lidar_measurement: carla.SemanticLidarMeasurement
"""
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='CosAngle', offset=12, datatype=PointField.FLOAT32, count=1),
PointField(name='ObjIdx', offset=16, datatype=PointField.UINT32, count=1),
PointField(name='ObjTag', offset=20, datatype=PointField.UINT32, count=1)
]
lidar_data = numpy.fromstring(bytes(carla_lidar_measurement.raw_data),
dtype=numpy.dtype([
('x', numpy.float32),
('y', numpy.float32),
('z', numpy.float32),
('CosAngle', numpy.float32),
('ObjIdx', numpy.uint32),
('ObjTag', numpy.uint32)
]))
# we take the oposite of y axis
# (as lidar point are express in left handed coordinate system, and ros need right handed)
lidar_data['y'] *= -1
point_cloud_msg = create_cloud(header, fields, lidar_data.tolist())
self.semantic_lidar_publisher.publish(point_cloud_msg)
def sensor_data_updated(self, carla_dvs_event_array):
"""
Function to transform the received DVS event array into a ROS message
:param carla_dvs_event_array: dvs event array object
:type carla_image: carla.DVSEventArray
"""
super(DVSCamera, self).sensor_data_updated(carla_dvs_event_array)
header = self.get_msg_header(timestamp=carla_dvs_event_array.timestamp)
fields = [
PointField(name='x', offset=0, datatype=PointField.UINT16,
count=1),
PointField(name='y', offset=2, datatype=PointField.UINT16,
count=1),
PointField(name='t',
offset=4,
datatype=PointField.FLOAT64,
count=1),
PointField(name='pol',
offset=12,
datatype=PointField.INT8,
count=1)
]
dvs_events_msg = create_cloud(header, fields,
self._dvs_events.tolist())
self.dvs_camera_publisher.publish(dvs_events_msg)
def sensor_data_updated(self, carla_radar_measurement):
"""
Function to transform the a received Radar measurement into a ROS message
:param carla_radar_measurement: carla Radar measurement object
:type carla_radar_measurement: carla.RadarMeasurement
"""
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='Range',
offset=12,
datatype=PointField.FLOAT32,
count=1),
PointField(name='Velocity',
offset=16,
datatype=PointField.FLOAT32,
count=1),
PointField(name='AzimuthAngle',
offset=20,
datatype=PointField.FLOAT32,
count=1),
PointField(name='ElevationAngle',
offset=28,
datatype=PointField.FLOAT32,
count=1)
]
points = []
for detection in carla_radar_measurement:
points.append([
detection.depth * np.cos(detection.azimuth) *
np.cos(-detection.altitude), detection.depth *
np.sin(-detection.azimuth) * np.cos(detection.altitude),
detection.depth * np.sin(detection.altitude), detection.depth,
detection.velocity, detection.azimuth, detection.altitude
])
radar_msg = create_cloud(
self.get_msg_header(timestamp=carla_radar_measurement.timestamp),
fields, points)
self.radar_publisher.publish(radar_msg)
def sensor_data_updated(self, carla_lidar_measurement):
"""
Function to transform the a received lidar measurement into a ROS point cloud message
: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_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)