cyberbotics / webots_ros2

Webots ROS 2 packages
Apache License 2.0
389 stars 141 forks source link

tesla imu support #920

Closed abdulkadrtr closed 2 months ago

abdulkadrtr commented 2 months ago

Hello, I want to add an imu to the Tesla model. To do this, I first tried to add the inertial unit from the simulation interface, but this does not work. Afterwards, I examined the mavic model and in the mavic_webots.urdf file

         <plugin type="webots_ros2_driver::Ros2IMU">
             <enabled>true</enabled>
             <topicName>/imu</topicName>
             <alwaysOn>true</alwaysOn>
             <frameName>imu_link</frameName>
             <inertialUnitName>inertial unit</inertialUnitName>
             <gyroName>gyro</gyroName>
         </plugin>

I noticed your code. I added this code to the tesla_webots.urdf file. When you run the simulation

[webots_controller_vehicle-3] Warning: "inertial unit" device not found.
[webots_controller_vehicle-3] terminate called after throwing an instance of 'std::runtime_error'
[webots_controller_vehicle-3] what(): Cannot find InertialUnit with name inertial unit
[webots_controller_vehicle-3] [ros2run]: Aborted

I'm getting the error. How can I add imu to a Tesla model?

Ros2 Humble Ubuntu 22.04

abdulkadrtr commented 2 months ago

Hello again, I just solved the problem. Add the following codes to the tesla_webots.urdf file.

 <device reference="gps" type="GPS">
     <ros>
         <enabled>true</enabled>
         <alwaysOn>true</alwaysOn>
     </ros>
 </device>

 <plugin type="webots_ros2_driver::Ros2IMU">
     <enabled>true</enabled>
     <topicName>/imu</topicName>
     <alwaysOn>true</alwaysOn>
     <frameName>imu_link</frameName>
     <inertialUnitName>inertial unit</inertialUnitName>
     <gyroName>gyro</gyroName>
 </plugin>

Then you need to revise TeslaModel3 in the tesla_world.wbt file. The new TeslaModel3 code is as follows.

TeslaModel3 {
   translation 31.4381 47.0076 0.400134
   rotation 0 0 1 3.1415
   controller "<extern>"
   sensorsSlotFront [
     InertialUnit {
     }
     Gyro {
     }
     GPS {
     }
     Camera {
       translation -2.12 0 0.93
       fieldOfView 1
       width 360
       height 240
       recognition {
         occlusion 0
         frameThickness 0
         segmentation TRUE
       }
     }
   ]
}

Screenshot from 2024-04-29 12-24-52

This way you can get data from imu and gps sensor.