How to make sonar gazebo plugin publish ROS topic

Would love to get some guidance on this.

I am running the sitl simulation with iris drone. I notice the drone is publishing gazebo topic /gazebo/default/iris/link/sonar. However, it is not publishing any ros topic.

In general, how do I make a gazebo plugin located in /PX4-Autopilot/Tools/sitl_gazebo/src to publish ros topic? Do I need to use mavros?

@kolohe This might help: Gazebo : Tutorial : Intermediate: Connect to ROS

1 Like

@kolohe It should be possible with mavros, yes:

  1. Make sure that SITL converts your mentioned gazebo topic into a MAVLink message. See PX4-SITL_gazebo/gazebo_mavlink_interface.cpp at 3e5fed04d8e574b10e17e446d2938346bc6152ca · PX4/PX4-SITL_gazebo · GitHub with static const std::regex kDefaultSonarModelNaming(".*(sonar|mb1240-xl-ez4)(.*)"); which is obviously matching your topic. As you can see in line PX4-SITL_gazebo/gazebo_mavlink_interface.cpp at 3e5fed04d8e574b10e17e446d2938346bc6152ca · PX4/PX4-SITL_gazebo · GitHub, a MAVLink message called DISTANCE_SENSOR is being published towards PX4.
  2. In PX4 you can find the counterpart in PX4-Autopilot/simulator_mavlink.cpp at f65781025ebb776fb5c406d96fbbfa52360f8a30 · PX4/PX4-Autopilot · GitHub, which is publishing the uORB topic distance_sensor whenever it receives a DISTANCE_SENSOR MAVLink message from SITL Gazebo. Make sure that you have the latest firmware or at least that you include this PR: simulator: fix lidar sensors by dayjaby · Pull Request #17058 · PX4/PX4-Autopilot · GitHub. You can check the uORB topics with listener distance_sensor in the PX4 shell.
  3. This uORB topic gets subscribed by the PX4 module mavlink: PX4-Autopilot/DISTANCE_SENSOR.hpp at master · PX4/PX4-Autopilot · GitHub which sends the MAVLink message DISTANCE_SENSOR to all MAVLink channels. Just to be safe, you can check whether DISTANCE_SENSOR is included in all channels by typing in the PX4 shell: mavlink status streams.
  4. If you connect with QGroundControl to the simulation (usually on UDP port 14550), do you receive the DISTANCE_SENSOR in Analyze > MAVLink inspector?
  5. What is the result for rosparam get /mavros/distance_sensor? Make sure that the orientation (e.g. PITCH_270 for downwards) matches the orientation within the MAVLink message (compare step 4). See mavros/Enumerations - ROS Wiki
  6. As a result, you should get published messages on the topic /mavros/distance_sensor

This might seem a bit too complex for such a simple thing as publishing the sonar topic. However, on a real drone you most likely would have a similar setup where the sonar sensors are connected to your autopilot directly. All the mentioned steps would work the same (except for 1-2 which are SITL-related)

2 Likes

Thanks you very much for the solution.
If running on a real drone, I can subscribe the distance_sensor topic:
/mavros/distance_sensor/hrlv_ez4_pub
However,
If running on a simulated drone (such as typhoon_h480), there is error like the below picture

and there is no published data from /mavros/distance_sensor/hrlv_ez4_pub
like this: Received sensor data has different orientation · Issue #1203 · mavlink/mavros · GitHub