Gazebo Garden (GZ) Sim for Boats: Thruster Plugin

I am trying to integrate a Gazebo(Garden) simulation model of a differential drive of a catamaran type boat with PX4 SITL. So far I have managed to use the default differential drive rover model by modifying it. I am able to publish the throttle and yaw commands from a attached RC as a joystick into a Gazebo topic. However the commands are not getting linked to the thruster plugin I use in the gazebo vehicle model.

Following is my thruster code in model.sdf :

  <!-- Uncomment to enable the left thruster -->
    <plugin
      filename="gz-sim-thruster-system"
      name="gz::sim::systems::Thruster">
      <joint_name>left_engine_propeller_joint</joint_name>
      <thrust_coefficient>-0.000085</thrust_coefficient>
      <fluid_density>1000</fluid_density>
      <propeller_diameter>0.1</propeller_diameter>
      <velocity_control>true</velocity_control>
    </plugin>

  <!-- Uncomment to enable the right thruster -->
    <plugin
      filename="gz-sim-thruster-system"
      name="gz::sim::systems::Thruster">
      <joint_name>right_engine_propeller_joint</joint_name>
      <thrust_coefficient>0.000085</thrust_coefficient>
      <fluid_density>1000</fluid_density>
      <propeller_diameter>0.1</propeller_diameter>
      <velocity_control>true</velocity_control>
    </plugin>

I have tried the following implementations for getting the data from PX4_SITL into GZ model:
1.

<plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
    <jointName>left_engine_propeller_joint</jointName>
    <linkName>left_propeller_link</linkName>
    <turningDirection>ccw</turningDirection>
    <!--timeConstantUp>0.0125</timeConstantUp-->
    <!--timeConstantDown>0.025</timeConstantDown-->
    <maxRotVelocity>5000.0</maxRotVelocity>
    <motorConstant>0.000085</motorConstant>
    <!--momentConstant>0.016</momentConstant-->
    <commandSubTopic>command/motor_speed</commandSubTopic>
    <motorNumber>0</motorNumber>
    <!--rotorDragCoefficient>8.06428e-05</rotorDragCoefficient-->
    <!--rollingMomentCoefficient>1e-06</rollingMomentCoefficient-->
    <rotorVelocitySlowdownSim>100</rotorVelocitySlowdownSim>
    <motorType>velocity</motorType>
</plugin>
    <plugin
      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
      <joint_name>left_engine_propeller_joint</joint_name>
      <sub_topic>servo_0</sub_topic>
      <p_gain>1000.0</p_gain>
      <servo_min>1000</servo_min>
      <servo_max>2000</servo_max>
      <type>COMMAND</type>
      <cmd_topic>/model/r1_boat/joint/left_engine_propeller_joint/cmd_thrust</cmd_topic>
    </plugin>
    <plugin filename="gz-sim-joint-controller-system" name="gz::sim::systems::JointController">
			<joint_name>left_engine_propeller_joint</joint_name>
			<sub_topic>command/motor_speed</sub_topic>
			<use_actuator_msg>true</use_actuator_msg>
			<actuator_number>0</actuator_number>
			<p_gain>100.0</p_gain>
		</plugin>

In all three cases I do get an output in Gazebo topic. In case of 1 and 3 its on ‘/model/r1_boat_0/command/motor_speed’ topic while in case of 2 its on ‘/model/r1_boat_0/servo_0’ and ‘/model/r1_boat_0/servo_1’.
I have the following topics in Gazebo:

/model/r1_boat_0/command/motor_speed
/model/r1_boat_0/joint/left_engine_propeller_joint/ang_vel
/model/r1_boat_0/joint/right_engine_propeller_joint/ang_vel
/model/r1_boat_0/servo_0
/model/r1_boat_0/servo_1
/model/r1_boat_0/servo_2
/model/r1_boat_0/servo_3
/model/r1_boat_0/servo_4
/model/r1_boat_0/servo_5
/model/r1_boat_0/servo_6
/model/r1_boat_0/servo_7
/r1_boat_0/command/motor_speed

But nothing on ‘/model/r1_boat_0/joint/left_engine_propeller_joint/ang_vel’ and on ‘/model/r1_boat_0/joint/left_engine_propeller_joint/cmd_thrust’. Its the last one where I need the data to be published. If I manually publish on ‘/model/r1_boat_0/joint/left_engine_propeller_joint/cmd_thrust’ the boat moves.

How to make the PX4_SITL to publish data on ‘/model/r1_boat_0/joint/left_engine_propeller_joint/cmd_thrust’ topic?

Also why are there 7 servo topics in Gazebo when I have not defined any servo outputs?

I’d be very grateful if someone can guide me on this. I am willing to learn and write any additional code that is required for doing this.

I am also learning GZ simulation. Maybe you should read the module ‘gz_bridge’.

In QGC I set my motors as servos (it can be changed in the airframe config later as well to make changes permanent), so PX4 SITL publishes data on /model/{model_name}/servo_n. It is important, because servo publishes data in double format, which is required by thruster plugin. Then I added these tags to the thruster plugin:

<namespace>/model/usv_0</namespace>
<topic>servo_0</topic>

so plugin listens on topic /model/{model_name}/servo_n. Now motors work with PX4!

Here’s useful reference for the thruster plugin (see parameters section)
https://gazebosim.org/api/gazebo/6/classignition_1_1gazebo_1_1systems_1_1Thruster.html