Gazebo Harmonic slow real time factor on baylands world

ArduCopter Copter Simulation
1

I’m attempting to run an Ardupilot-Gazebo simulation using the baylands world downloaded from fuel (https://app.gazebosim.org/VentuRobotics/fuel/models/baylands).

The simulation successfully loads and renders (image below), but the real time factor is extremely slow (~5%). The standard iris_runway world runs at ~90%.

Are there any particular techniques I need to be using to have this run quicker on my machine? I’ve left my specs below. What’s confusing me is that the sim doesn’t appear to be using all of my system resources (~40% CPU and memory). Do I have to explicitly set gazebo to use my system’s gpu?

I’ve tried adjusting reducing the <max_step_size> from 250hz to 100hz, but the copter becomes unusable. I’ve included my base .sdf file below for reference on the plugins being employed.

I’m new to Gazebo simulations so thank you in advance for any support!

System:

CPU: 12th Gen Intel i7-12650H (16) @ 4.600G GPU: NVIDIA GeForce RTX 3060 GPU: Intel Alder Lake-P GT1 Memory: 32gb DDR4

OS: Ubuntu 22.04.5 LTS x86_64 (6.8.0-47-generic)

Gazebo Harmonic

ROS2 Humble

Screenshot from 2024-10-22 19-12-47
Screenshot from 2024-10-22 19-12-471837×1054 680 KB

Screenshot from 2024-10-22 19-28-45
Screenshot from 2024-10-22 19-28-45686×819 98.1 KB

<?xml version="1.0" ?>
<sdf version='1.9'>
  <world name='baylands'>
    <physics type="ode">
      <max_step_size>0.004</max_step_size>
      <real_time_factor>1.0</real_time_factor>
      <!-- <real_time_update_rate>250</real_time_update_rate> -->
      <!-- <real_time_update_rate>0</real_time_update_rate> -->
    </physics>

    <plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
    <plugin name='gz::sim::systems::Sensors' filename='gz-sim-sensors-system'>
      <render_engine>ogre2</render_engine>
    </plugin>
    <plugin name='gz::sim::systems::UserCommands' filename='gz-sim-user-commands-system'/>
    <plugin name='gz::sim::systems::SceneBroadcaster' filename='gz-sim-scene-broadcaster-system'/>
    <plugin name='gz::sim::systems::AirPressure' filename='gz-sim-air-pressure-system'/>
    <plugin filename="gz-sim-air-speed-system"
      name="gz::sim::systems::AirSpeed">
    </plugin>
        <plugin filename="gz-sim-altimeter-system"
      name="gz::sim::systems::Altimeter">
    </plugin>
    <plugin name='gz::sim::systems::Imu' filename='gz-sim-imu-system'/>
    <plugin filename="gz-sim-magnetometer-system"
      name="gz::sim::systems::Magnetometer">
    </plugin>
    <plugin name='gz::sim::systems::NavSat' filename='gz-sim-navsat-system'/>

    <!-- <plugin name='gz::sim::systems::Contact' filename='gz-sim-contact-system'/> -->
    <!-- <plugin name='gz::sim::systems::ApplyLinkWrench' filename='gz-sim-apply-link-wrench-system'/> -->

    <gui fullscreen='false'>

      <!-- 3D scene -->
      <plugin filename="MinimalScene" name="3D View">
        <gz-gui>
          <title>3D View</title>
          <property type="bool" key="showTitleBar">false</property>
          <property type="string" key="state">docked</property>
        </gz-gui>

        <engine>ogre2</engine>
        <scene>scene</scene>
        <ambient_light>0.4 0.4 0.4</ambient_light>
        <background_color>0.8 0.8 0.8</background_color>
        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
        <camera_clip>
          <near>0.25</near>
          <far>25000</far>
        </camera_clip>
      </plugin>

      <!-- Plugins that add functionality to the scene -->
      <plugin filename="EntityContextMenuPlugin" name="Entity context menu">
        <gz-gui>
          <property key="state" type="string">floating</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="GzSceneManager" name="Scene Manager">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="InteractiveViewControl" name="Interactive view control">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="CameraTracking" name="Camera Tracking">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="MarkerManager" name="Marker manager">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="SelectEntities" name="Select Entities">
        <gz-gui>
          <anchors target="Select entities">
            <line own="right" target="right"/>
            <line own="top" target="top"/>
          </anchors>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="VisualizationCapabilities" name="Visualization Capabilities">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="Spawn" name="Spawn Entities">
        <gz-gui>
          <anchors target="Select entities">
            <line own="right" target="right"/>
            <line own="top" target="top"/>
          </anchors>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>

      <plugin name='World control' filename='WorldControl'>
        <gz-gui>
          <title>World control</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
          <property type='double' key='height'>72</property>
          <property type='double' key='width'>121</property>
          <property type='double' key='z'>1</property>
          <property type='string' key='state'>floating</property>
          <anchors target='3D View'>
            <line own='left' target='left'/>
            <line own='bottom' target='bottom'/>
          </anchors>
        </gz-gui>
        <play_pause>1</play_pause>
        <step>1</step>
        <start_paused>1</start_paused>
      </plugin>
      <plugin name='World stats' filename='WorldStats'>
        <gz-gui>
          <title>World stats</title>
          <property type='bool' key='showTitleBar'>0</property>
          <property type='bool' key='resizable'>0</property>
          <property type='double' key='height'>110</property>
          <property type='double' key='width'>290</property>
          <property type='double' key='z'>1</property>
          <property type='string' key='state'>floating</property>
          <anchors target='3D View'>
            <line own='right' target='right'/>
            <line own='bottom' target='bottom'/>
          </anchors>
        </gz-gui>
        <sim_time>1</sim_time>
        <real_time>1</real_time>
        <real_time_factor>1</real_time_factor>
        <iterations>1</iterations>
      </plugin>
      <plugin name='Entity tree' filename='EntityTree'/>
    </gui>
    <gravity>0 0 -9.8</gravity>
    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
    <atmosphere type='adiabatic'/>
    <scene>
      <ambient>0.8 0.5 1</ambient>
      <grid>false</grid>
      <sky>
        <clouds>true</clouds>
      </sky>
      <shadows>1</shadows>
    </scene>
    <light name='sunUTC' type='directional'>
      <pose>0 0 500 0 -0 0</pose>
      <cast_shadows>false</cast_shadows>
      <intensity>1</intensity>
      <direction>0.001 0.625 -0.78</direction>
      <diffuse>0.904 0.904 0.904 1</diffuse>
      <specular>0.271 0.271 0.271 1</specular>
      <attenuation>
        <range>2000</range>
        <linear>0</linear>
        <constant>1</constant>
        <quadratic>0</quadratic>
      </attenuation>
    </light>
    <include>
      <uri>
        model://baylands
      </uri>
      <name>park</name>
      <pose>205 155 -1 0 0 0</pose>
    </include>

    <include>
      <uri>model://iris_with_gimbal</uri>
      <name>iris</name>
      <pose degrees="true">200 150 0.195 0 0 90</pose>
    </include>
    <spherical_coordinates>
      <surface_model>EARTH_WGS84</surface_model>
      <world_frame_orientation>ENU</world_frame_orientation>
      <latitude_deg>37.412173071650805</latitude_deg>
      <longitude_deg>-121.998878727967</longitude_deg>
      <elevation>38</elevation>
    </spherical_coordinates>
  </world>
</sdf>
2

Hi @jamisonpereira, the slowdown is caused by the collision mesh of the gimbal in the iris_with_gimbal model. Using the model without a gimbal, which uses primitives for collisions, there is almost no slowdown:

    <include>
      <uri>model://iris_with_ardupilot</uri>
      <pose degrees="true">0 0 0.5 0 0 90</pose>
    </include>

The problem collision objects are in gimbal_small_3d

      <collision name="roll_collision">
        <geometry>
          <mesh>
            <uri>model://gimbal_small_3d/meshes/roll_arm.dae</uri>
          </mesh>
        </geometry>
      </collision>

and so on. The solution is for us to add a low poly mesh, or better collection of primitives to approximate the object.

02-baylands-iris-with-ardupilot
02-baylands-iris-with-ardupilot1024×734 520 KB

Issue raised here:

3

Thank you @rhys. Looking forward to the new feature.

I got the same results as you when running with iris_with_ardupilot – the sim runs normally. FYI, in my iris_with_gimbal.sdf the gimbal_small_2d is attached (not the 3d; see below). But I assume it would have the same issue with collision objects you described.

One additional question: Is there no longer a default copter model with a camera pointed straight down (i.e. no gimbal)? I seem to recall this was available in older ardupilot_gazebo packages.

<?xml version='1.0'?>
<sdf version="1.9">
  <model name="iris_with_gimbal">
    <include merge="true">
      <uri>package://ardupilot_gazebo/models/iris_with_standoffs</uri>
      <name>iris</name>
    </include>

    <include merge="true">
      <uri>package://ardupilot_gazebo/models/gimbal_small_2d</uri>
      <name>gimbal</name>
      <pose degrees="true">0 -0.01 -0.124923 90 0 90</pose>    
    </include>
4

Yes, though the performance for the 3DoF gimbal will be worse as it has an additional high poly collision mesh.

No, the default is now the 3DoF gimbal in the main branch. The ros2 branch is lagging somewhat and still uses the 2DoF gimbal. The 3DoF gimbal can be positioned to point directly down, the advantage of the gimbal is that it can maintain attitude in the earth frame.

5

Thanks @rhys.

I have successfully controlled the gimbal per your PR and using the MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW MAVLink command listed in this guide. Is this the best way to control the gimbal? Or is there a way to control it with ROS2? Using mavros bridge i’d assume.

Also, should the 2DoF gimbal also be capable of tracking the earth frame or that just a 3DoF feature? I’ve tried both ways in the below code and both settings seem to be body frame (as far as I can tell… i’m assuming earth frame should be tracking the earth).

def control_gimbal_pitch_yaw(vehicle, pitch=90, yaw=0, follow_body_frame=False):
    """
    Controls the gimbal's pitch and yaw using MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW to point the gimbal straight down.

    Args:
        vehicle (Vehicle): The connected vehicle object.
        pitch (float): Pitch angle in degrees (-90 for straight down).
        yaw (float): Yaw angle in degrees (0 is forward).
        follow_body_frame (bool): If True, gimbal yaw follows the vehicle body frame; if False, yaw locks to the earth frame.

    Returns:
        None
    """
    print(f"Setting gimbal pitch={pitch}, yaw={yaw}, follow_body_frame={follow_body_frame}")
    try:
        # Define the flags for yaw control (0 = follow body frame, 16 = lock to earth frame)
        yaw_control_flag = 0 if follow_body_frame else 16

        # Send MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW command
        vehicle._master.mav.command_long_send(
            vehicle._master.target_system,    # Target system ID
            vehicle._master.target_component, # Target component ID
            mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW,  # Command ID
            0,                                # Confirmation
            pitch,                            # Pitch angle in degrees (-90 for straight down)
            yaw,                              # Yaw angle in degrees (0 = forward)
            float('nan'),                     # Pitch rate (not used)
            float('nan'),                     # Yaw rate (not used)
            yaw_control_flag,                 # 0=Follow body frame, 16=Lock to earth frame
            0,                                # Not used
            0                                 # Gimbal device ID (0 = primary gimbal)
        )
        print(f"Setting gimbal (Pitch={pitch}, Yaw={yaw})")
    except Exception as e:
        print(f"Error setting gimbal pitch and yaw: {e}")
6

We don’t yet support a direct interface in XRCE DDS. See the feature request here: