How to add Radar sensor to TeslaModel3?

[sachinkum0009]

Discussed in #876

^{Originally posted by sachinkum0009 December 18, 2023}
Hello,

I want to add Radar Sensor to TeslaModel3. I tried to follow link

But still it doesn't show the target ros2 topic.

Here is the code

TeslaModel3 {
  translation 31.4381 47.0076 0.400134
  rotation 0 0 1 3.1415
  controller "<extern>"
  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

/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 🙏

[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 🙏