I attempted to add a ring and time to the PointField data to make it compatible with LIO SAM. Here is my code.
'
import numpy
from carla_ros_bridge.sensor import Sensor, create_cloud
from sensor_msgs.msg import PointCloud2, PointField
class Lidar(Sensor):
def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode):
"""
Constructor
:param uid: unique identifier for this object
:type uid: int
:param name: name identiying this object
:type name: string
:param parent: the parent of this
:type parent: carla_ros_bridge.Parent
:param relative_spawn_pose: the spawn pose of this
:type relative_spawn_pose: geometry_msgs.Pose
:param node: node-handle
:type node: CompatibleNode
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
:param synchronous_mode: use in synchronous mode?
:type synchronous_mode: bool
"""
super(Lidar, self).__init__(uid=uid,
name=name,
parent=parent,
relative_spawn_pose=relative_spawn_pose,
node=node,
carla_actor=carla_actor,
synchronous_mode=synchronous_mode)
self.lidar_publisher = node.new_publisher(PointCloud2,
self.get_topic_prefix(),
qos_profile=10)
self.channels = int(self.carla_actor.attributes.get('channels'))
rotation_frequency = float(self.carla_actor.attributes.get('rotation_frequency'))
self.rotation_period = 1.0/rotation_frequency
self.listen()
def destroy(self):
super(Lidar, self).destroy()
self.node.destroy_publisher(self.lidar_publisher)
# pylint: disable=arguments-differ
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),
PointField(name='ring', offset=16, datatype=PointField.UINT16, count=1),
# PointField(name='time', offset=18, datatype=PointField.FLOAT32, count=1)
]
ring = None
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))
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, numpy.uint16)))
ring = numpy.delete(ring, 0, axis=0)
# lidar_data = numpy.hstack((lidar_data, ring, time_point))
lidar_data = numpy.hstack((lidar_data, ring))
# 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
theta = numpy.arctan2(lidar_data[:, 1], lidar_data[:, 0])
time_point = self.atan2bearing(theta) * (self.rotation_period / (2 * numpy.pi))
time_point = time_point.astype(numpy.float32)
point_cloud_msg = create_cloud(header, fields, lidar_data)
self.lidar_publisher.publish(point_cloud_msg)
def atan2bearing(self, data):
data[data<0] = data[data<0] + 2*numpy.pi
return data
The ring data is working as expected, but the time data is not. I receive this error when executing the code.
[bridge-1] theta = numpy.arctan2(lidar_data[:, 1], lidar_data[:, 0])
[bridge-1] AttributeError: 'float' object has no attribute 'arctan2'
How to fix this.
I attempted to add a ring and time to the PointField data to make it compatible with LIO SAM. Here is my code. ' import numpy
from carla_ros_bridge.sensor import Sensor, create_cloud
from sensor_msgs.msg import PointCloud2, PointField
class Lidar(Sensor):
The ring data is working as expected, but the time data is not. I receive this error when executing the code. [bridge-1] theta = numpy.arctan2(lidar_data[:, 1], lidar_data[:, 0]) [bridge-1] AttributeError: 'float' object has no attribute 'arctan2' How to fix this.