SITL with Gazebo for Rover

Hello everyone,
I am trying to use the SITL to simulate a Rover, and I would like to link GAZEBO to the simulation. My SITL works well but the link between gazebo and the simulation is not made… I don’t know how to do.

I do these two commands in parallel :

For SITL

sim_vehicle.py -v Rover -f rover-skid --map --console

For GAZEBO

gz sim -v4 -r ackermann_steering.sdf

I don’t know how to link a rover models to the simulation :confused:

If someone has any idea, please help me :slight_smile:

THANK YOU !!

1 Like

please someone can help me :smiling_face_with_tear:

Ah… I’m interested in this topic also. :smiley:

Hi @sarah_gress, if this is the ackermann_steering.sdf example in gz-sim/test/worlds, it doesn’t have any integration with ArduPilot, so you’d need to add the ardupilot_gazebo plugin and configure it to forward commands to the topics expected by the AckermannSteering controller plugin. In this case you’d also want to use a standard rover rather than a skid steer one, as the Ackermann controller expects a steering and throttle input, where a skid-steering rover requires a left and right throttle.

If you would like a skid-steering example you could look at the example here: SITL_Models/Gazebo/docs/WildThumper.md at master · ArduPilot/SITL_Models · GitHub.

On the other hand if it’s an Ackermann (i,e, car steering) rover you want then I could look into configuring the plugin to support the gz-sim example.

Update

The zip file below contains the modifications to gz-sim/examples/worlds/ackermann_steering.sdf needed to get it working with the ardupilot_gazebo plugin. It is assumed that you have built the plugin following the instructions here: Using SITL with Gazebo — Dev documentation

ackermann_steering.sdf.zip (3.2 KB)

I’ve annotated the changes in the file, but to summarise the things required are:

  1. Add the IMU system to the world. Without this you will get no link errors in the console as the FC will wait for a valid IMU reading which never arrives.
<plugin filename="gz-sim-imu-system"
  name="gz::sim::systems::Imu">
</plugin>
  1. Add an IMU to the base link (or chassis in this case). Note the orientation. The z-axis must be pointing down.
<sensor name="imu_sensor" type="imu">
  <pose degrees="true">0 0 0 180 0 0</pose>
  <always_on>1</always_on>
  <update_rate>1000</update_rate>
</sensor>
  1. Modify the AckermannSteering plugin to be steering only as the ArduPilot plugin does not supply cmd_vel, but forwards steering and throttle commands separately.
<plugin
  filename="gz-sim-ackermann-steering-system"
  name="gz::sim::systems::AckermannSteering">
  <steering_only>true</steering_only>
  <use_actuator_msg>false</use_actuator_msg>
  <left_joint>front_left_wheel_joint</left_joint>
  <left_joint>rear_left_wheel_joint</left_joint>
  <right_joint>front_right_wheel_joint</right_joint>
  <right_joint>rear_right_wheel_joint</right_joint>
  <left_steering_joint>front_left_wheel_steering_joint</left_steering_joint>
  <right_steering_joint>front_right_wheel_steering_joint</right_steering_joint>
  <kingpin_width>1.0</kingpin_width>
  <steering_limit>0.5</steering_limit>
  <wheel_base>1.0</wheel_base>
  <wheel_separation>1.25</wheel_separation>
  <wheel_radius>0.3</wheel_radius>
  <min_velocity>-1</min_velocity>
  <max_velocity>1</max_velocity>
  <min_acceleration>-3</min_acceleration>
  <max_acceleration>3</max_acceleration>
</plugin>
  1. Add velocity controllers for the rear wheels. You could make the vehicle
    4-wheel drive by adding controllers for the front wheels also.
<plugin
  filename="gz-sim-joint-controller-system"
  name="gz::sim::systems::JointController">
  <joint_name>rear_left_wheel_joint</joint_name>
  <use_force_commands>true</use_force_commands>
  <use_actuator_msg>false</use_actuator_msg>
  <p_gain>0.1</p_gain>
  <i_gain>0.0</i_gain>
  <d_gain>0.0</d_gain>
</plugin>

<plugin
  filename="gz-sim-joint-controller-system"
  name="gz::sim::systems::JointController">
  <joint_name>rear_right_wheel_joint</joint_name>
  <use_force_commands>true</use_force_commands>
  <use_actuator_msg>false</use_actuator_msg>
  <p_gain>0.1</p_gain>
  <i_gain>0.0</i_gain>
  <d_gain>0.0</d_gain>
</plugin>
  1. Add the ArduPilot plugin. Note that both steering and throttle are ‘neutral’ at PWM 1500, so we need to set the offset element to -0.5. Also for the steering the multiplier is negative to ensure the wheels turn left when the PWM is low, and right when the PWM is high. The throttle is scaled to a force of +/- 50N for the velocity joint control command, and the steering range is scaled to +/- 45 deg (but the joints will limit this to +/- 35 deg). Both steering and throttle scaled commands are forwarded onto the gz-transport message bus where they are picked up by the AckermannSteering and JointController plugins. The topic names are the default values as documented in the controller plugin header files.
<plugin name="ArduPilotPlugin"
  filename="ArduPilotPlugin">
  <fdm_addr>127.0.0.1</fdm_addr>
  <fdm_port_in>9002</fdm_port_in>
  <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
  <lock_step>1</lock_step>
  <gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
  <modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 0</modelXYZToAirplaneXForwardZDown>
  <imuName>chassis::imu_sensor</imuName>

  <!-- steering -->
  <control channel="0">
    <jointName>front_left_wheel_steering_joint</jointName>
    <useForce>1</useForce>
    <multiplier>-1.571</multiplier>
    <offset>-0.5</offset>
    <servo_min>1000</servo_min>
    <servo_max>2000</servo_max>
    <type>COMMAND</type>
    <cmd_topic>/model/vehicle_blue/steer_angle</cmd_topic>
  </control>

  <!-- throttle -->
  <control channel="2">
    <jointName>rear_left_wheel_joint</jointName>
    <multiplier>100.0</multiplier>
    <offset>-0.5</offset>
    <servo_min>500</servo_min>
    <servo_max>2000</servo_max>
    <type>COMMAND</type>
    <cmd_topic>/model/vehicle_blue/joint/rear_left_wheel_joint/cmd_vel</cmd_topic>
  </control>
  <control channel="2">
    <jointName>rear_right_wheel_joint</jointName>
    <multiplier>100.0</multiplier>
    <offset>-0.5</offset>
    <servo_min>1000</servo_min>
    <servo_max>2000</servo_max>
    <type>COMMAND</type>
    <cmd_topic>/model/vehicle_blue/joint/rear_right_wheel_joint/cmd_vel</cmd_topic>
  </control>

</plugin>

The default PID controllers require tuning for the vehicle to respond well. These values are are reasonable start:

ATC_STR_RAT_P    2.500000
ATC_STR_RAT_D    0.010000
ATC_STR_RAT_FF   2.000000
ATC_SPEED_P      1.000000
ATC_SPEED_I      0.100000

Here is the vehicle running an AUTO mission around a large rectangle. The steering tune is acceptable and the nav controller seems to work well with defaults.

ackermann_steering

Thank you for your time.

I am a newbie in gazebo, and ardupilot sitl, then I am not sure to know how to add good plugins and setup a good configuration… could you help me with precisions please ??

Actually, I would like to use a steering-car yes.

thank you :slight_smile:

@sarah_gress Sorry for cut in.

@rhys I’m interested in Ackermann (i,e, car steering) rover. And I hope to setup an simulation environment with obstacles. Can you guide us through the setup?

As I have one AKM Rover with ArduRover 4.5.2, see below: