How to add Radar sensor to TeslaModel3?

[sachinkum0009]

Discussed in https://github.com/cyberbotics/webots_ros2/discussions/876

^{Originally posted by **sachinkum0009** December 18, 2023} Hello, I want to add Radar Sensor to TeslaModel3. I tried to follow [link](https://cyberbotics.com/doc/reference/radar) But still it doesn't show the target ros2 topic. Here is the code ```wbt TeslaModel3 { translation 31.4381 47.0076 0.400134 rotation 0 0 1 3.1415 controller "" sensorsSlotFront [ Camera { translation -2.12 0 0.93 fieldOfView 1 width 360 height 240 recognition Recognition { occlusion 0 frameThickness 0 segmentation TRUE } } Radar { translation -2.12 0 0.93 minRange 1 # [0, maxRange) maxRange 50.0 # (minRange, inf) horizontalFieldOfView 0.78 # [0, pi] verticalFieldOfView 0.1 # [0, pi] minAbsoluteRadialSpeed 0.0 # [0, inf) minRadialSpeed 1 # [0, maxRadialSpeed] maxRadialSpeed -1 # {-1, [minRadialSpeed, inf)} cellDistance 0.0 # [0, inf) cellSpeed 0.0 # [0, inf) rangeNoise 0.0 # [0, inf) speedNoise 0.0 # [0, inf) angularNoise 0.0 # [0, inf) antennaGain 20.0 # (-inf, inf) frequency 24.0 # [0, inf) transmittedPower 1.0 # (-inf, inf) minDetectableSignal -100 # (-inf, inf) occlusion FALSE # {TRUE, FALSE} } GPS { } ] } ``` ROS2 topics list ```bash /Ros2Supervisor/remove_node /clock /cmd_ackermann /parameter_events /remove_urdf_robot /rosout /vehicle/antifog_lights /vehicle/backwards_lights /vehicle/brake_lights /vehicle/camera/camera_info /vehicle/camera/image_color /vehicle/camera/recognitions /vehicle/camera/recognitions/webots /vehicle/front_lights /vehicle/gps /vehicle/gps/speed /vehicle/gps/speed_vector /vehicle/left_indicators /vehicle/rear_lights /vehicle/right_indicators ``` I am very confused. Please help me with this issue. Thanks in advance :pray:

[sachinkum0009]

I also tried to use the tesla driver.py file to add a publisher to publish the radar sensor data, but unfortunately it is also not working.

"""ROS2 Tesla driver."""

import rclpy
from ackermann_msgs.msg import AckermannDrive
from radar_msgs.msg import RadarScan

class TeslaDriver:
    def init(self, webots_node, properties):
        self.__robot = webots_node.robot
        self.__radar = self.__robot.getDevice('radar')
        self.__lidar = self.__robot.getDevice('LDS-01')
        self.__radar.enableRadar()

        # ROS interface
        rclpy.init(args=None)
        self.__node = rclpy.create_node('tesla_node')
        self.__node.create_subscription(AckermannDrive, 'cmd_ackermann', self.__cmd_ackermann_callback, 1)
        self.__radar_publisher = self.__node.create_publisher(RadarScan, '/radar_targets', 10)  
        # self.__publish_radar()
        self.__node.create_timer(0.5, self.__publish_radar)

    def __cmd_ackermann_callback(self, message):
        self.__robot.setCruisingSpeed(message.speed)
        self.__robot.setSteeringAngle(message.steering_angle)
        # print("cmd_ackermann_called")
        # self.__node.get_logger().info("cmd ackermann called")

    def __publish_radar(self):
        # self.__node.get_logger().info("radar publishing")
        # distance = self.__robot.getRadarTarget("radar")
        targetsList = self.__radar.getTargets()
        for target in targetsList:
            self.__node.get_logger().info("radar: ", target.distance)
        pass
        self.__node.get_logger().info("num of targets from getTarget is %d" % (len(targetsList)))

        num_of_points = self.__lidar.getMaxRange()
        self.__node.get_logger().info("max range is %f" % (num_of_points))
        targets = self.__radar.getNumberOfTargets()
        self.__node.get_logger().info("num of targets is %d" % (targets))

    def step(self):
        # self.__publish_radar()
        rclpy.spin_once(self.__node, timeout_sec=0)

[sachinkum0009]

Now, its working fine.

But unfortunately there is only one object target.

I tried different Radar Sensors but they all give one output for one target. Any idea how to fix this to have multiple targets?

Thanks a lot

[sachinkum0009]

I am able to get some values from the radar sensor by using the python api to get the data from the radar sensor and then publish the data to ros2

"""ROS2 Tesla driver."""

import rclpy
from ackermann_msgs.msg import AckermannDrive
from radar_msgs.msg import RadarScan

class TeslaDriver:
    def init(self, webots_node, properties):
        self.__robot = webots_node.robot
        self.__radar = self.__robot.getDevice('Sms UMRR 0a31')
        # self.__radar = self.__robot.getDevice('Sms UMRR 0a29')
        # self.__radar = self.__robot.getDevice('radar')

        # self.__radar = self.__robot.getDevice('Delphi ESR')
        # self.__lidar = self.__robot.getDevice('LDS-01')
        self.__radar.enableRadar()

        # ROS interface
        rclpy.init(args=None)
        self.__node = rclpy.create_node('tesla_node')
        self.__node.create_subscription(AckermannDrive, 'cmd_ackermann', self.__cmd_ackermann_callback, 1)
        self.__radar_publisher = self.__node.create_publisher(RadarScan, '/radar_targets', 10)  # Change '/radar_targets' to your desired topic name
        # self.__publish_radar()
        self.__node.create_timer(0.5, self.__publish_radar)

    def __cmd_ackermann_callback(self, message):
        self.__robot.setCruisingSpeed(message.speed)
        self.__robot.setSteeringAngle(message.steering_angle)
        # print("cmd_ackermann_called")
        # self.__node.get_logger().info("cmd ackermann called")

    def __publish_radar(self):
        # self.__node.get_logger().info("radar publishing")
        # distance = self.__robot.getRadarTarget("radar")
        targetsList = self.__radar.getTargets()
        self.__node.get_logger().info("------")

        for target in targetsList:
            self.__node.get_logger().info("distance: %f" % target.distance)
            self.__node.get_logger().info("receiver power: %f" % target.receiver_power)
            self.__node.get_logger().info("speed: %f" % target.speed)
            self.__node.get_logger().info("azimuth: %f" % target.azimuth)
        pass
        self.__node.get_logger().info("num of targets from getTarget is %d" % (len(targetsList)))

        max_range = self.__radar.getMaxRange()
        self.__node.get_logger().info("max range is %f" % (max_range))
        targets = self.__radar.getNumberOfTargets()
        self.__node.get_logger().info("num of targets is %d" % (targets))

    def step(self):
        # self.__publish_radar()
        rclpy.spin_once(self.__node, timeout_sec=0)

but the issue is that the distance from the radar is not good. For example, I am getting the same distance values from the radar for two objects and it gives the value of the radar after a lot of values. Is there some problem with the code? How can I solve this issue?

Thanks :pray: