You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
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.
The text was updated successfully, but these errors were encountered:
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.
The text was updated successfully, but these errors were encountered: