carla-simulator / ros-bridge

ROS bridge for CARLA Simulator
MIT License
509 stars 408 forks source link

AttributeError: 'float' object has no attribute 'arctan2' #724

Open jarunyawat opened 4 months ago

jarunyawat commented 4 months ago

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.