ArduPilot / SITL_Models

Models of aircraft for SITL
70 stars 111 forks source link

Blue square appears under blueboat #124

Closed dlaboy closed 3 months ago

dlaboy commented 3 months ago

Hello! Any idea why a blue square appears under blueboat?

This is current blueboat model.

<sdf version="1.9">
  <model name="blueboat">
    <link name="base_link">
      <inertial>
        <pose>-0.05 0 0 0 0 0</pose>
        <mass>30</mass>
       <inertia>
            <ixx>86.28907821859966</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>86.28907821859966</iyy>
            <iyz>0</iyz>
            <izz>5.026548245743671</izz>
        </inertia>
      </inertial>
      <collision name="hull_collision">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_hull_collision.stl</uri>
          </mesh>
        </geometry>
      </collision>
      <visual name="hull_visual">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_hull.dae</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0 0 1 1</diffuse>
          <ambient>1 0 1 1</ambient>
          <specular>0.7 0.7 0.7 1</specular>
          <pbr>
            <metal>
              <metalness>0.5</metalness>
              <roughness>0.5</roughness>
            </metal>
          </pbr>
        </material>
      </visual>
      <visual name="aerial_visual">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_aerial.dae</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.01 0.01 0.01 1</diffuse>
          <ambient>0.01 0.01 0.01 1</ambient>
          <specular>0.7 0.7 0.7 1</specular>
          <pbr>
            <metal>
              <metalness>0.5</metalness>
              <roughness>0.5</roughness>
            </metal>
          </pbr>
        </material>
      </visual>
      <visual name="crosstube_visual">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_crosstube.dae</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.01 0.01 0.01 1</diffuse>
          <ambient>0.01 0.01 0.01 1</ambient>
          <specular>0.7 0.7 0.7 1</specular>
          <pbr>
            <metal>
              <metalness>0.5</metalness>
              <roughness>0.5</roughness>
            </metal>
          </pbr>
        </material>
      </visual>
      <visual name="flag_visual">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_flag.dae</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>1 1 0 1</diffuse>
          <ambient>1 1 0 1</ambient>
          <specular>0.7 0.7 0.7 1</specular>
          <pbr>
            <metal>
              <metalness>0.5</metalness>
              <roughness>0.5</roughness>
            </metal>
          </pbr>
        </material>
      </visual>
      <visual name="frame_asm_fore_visual">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_frame_asm_fore.dae</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.01 0.01 0.01 1</diffuse>
          <ambient>0.01 0.01 0.01 1</ambient>
          <specular>0.7 0.7 0.7 1</specular>
          <pbr>
            <metal>
              <metalness>0.9</metalness>
              <roughness>0.1</roughness>
            </metal>
          </pbr>
        </material>
      </visual>
      <visual name="frame_asm_aft_visual">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_frame_asm_aft.dae</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.01 0.01 0.01 1</diffuse>
          <ambient>0.01 0.01 0.01 1</ambient>
          <specular>0.7 0.7 0.7 1</specular>
          <pbr>
            <metal>
              <metalness>0.9</metalness>
              <roughness>0.1</roughness>
            </metal>
          </pbr>
        </material>
      </visual>
      <visual name="hatch_asm_stbd">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_hatch_asm_stbd.dae</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.01 0.01 0.01 1</diffuse>
          <ambient>0.01 0.01 0.01 1</ambient>
          <specular>0.7 0.7 0.7 1</specular>
          <pbr>
            <metal>
              <metalness>0.5</metalness>
              <roughness>0.5</roughness>
            </metal>
          </pbr>
        </material>
      </visual>
      <visual name="hatch_asm_port">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_hatch_asm_port.dae</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.01 0.01 0.01 1</diffuse>
          <ambient>0.01 0.01 0.01 1</ambient>
          <specular>0.7 0.7 0.7 1</specular>
          <pbr>
            <metal>
              <metalness>0.5</metalness>
              <roughness>0.5</roughness>
            </metal>
          </pbr>
        </material>
      </visual>
      <visual name="motor_stbd_visual">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_motor_stbd.dae</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.01 0.01 0.01 1</diffuse>
          <ambient>0.01 0.01 0.01 1</ambient>
          <specular>0.7 0.7 0.7 1</specular>
          <pbr>
            <metal>
              <metalness>0.5</metalness>
              <roughness>0.5</roughness>
            </metal>
          </pbr>
        </material>
      </visual>
      <visual name="motor_port_visual">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_motor_port.dae</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.01 0.01 0.01 1</diffuse>
          <ambient>0.01 0.01 0.01 1</ambient>
          <specular>0.7 0.7 0.7 1</specular>
          <pbr>
            <metal>
              <metalness>0.5</metalness>
              <roughness>0.5</roughness>
            </metal>
          </pbr>
        </material>
      </visual>
    </link>

    <link name="motor_stbd_link">
      <pose degrees="true">-0.488 -0.295 -0.272 -90 0 90</pose>
      <inertial>
        <mass>0.2</mass>
        <inertia>
          <ixx>0.1</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.1</iyy>
          <iyz>0</iyz>
          <izz>0.1</izz>
        </inertia>
      </inertial>
      <collision name="prop_collision">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_prop_stbd.dae</uri>
          </mesh>
        </geometry>
      </collision>
      <visual name="prop_visual">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_prop_stbd.dae</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.01 0.01 0.01 1</diffuse>
          <ambient>0.01 0.01 0.01 1</ambient>
          <specular>0.7 0.7 0.7 1</specular>
          <pbr>
            <metal>
              <metalness>0.5</metalness>
              <roughness>0.5</roughness>
            </metal>
          </pbr>
        </material>
      </visual>
    </link>
    <joint name="motor_stbd_joint" type="revolute">
      <parent>base_link</parent>
      <child>motor_stbd_link</child>
      <axis>
        <xyz>0 0 -1</xyz>
        <limit>
          <lower>-1e16</lower>
          <upper>1e16</upper>
        </limit>
        <dynamics>
          <damping>0.02</damping>
        </dynamics>
      </axis>
    </joint>

    <link name="motor_port_link">
      <pose degrees="true">-0.488 0.295 -0.272 -90 0 90</pose>
      <inertial>
        <mass>0.2</mass>
        <inertia>
          <ixx>0.1</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.1</iyy>
          <iyz>0</iyz>
          <izz>0.1</izz>
        </inertia>
      </inertial>
      <collision name="prop_collision">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_prop_port.dae</uri>
          </mesh>
        </geometry>
      </collision>
      <visual name="prop_visual">
        <geometry>
          <mesh>
            <uri>models://blueboat/meshes/blueboat_prop_port.dae</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.01 0.01 0.01 1</diffuse>
          <ambient>0.01 0.01 0.01 1</ambient>
          <specular>0.7 0.7 0.7 1</specular>
          <pbr>
            <metal>
              <metalness>0.5</metalness>
              <roughness>0.5</roughness>
            </metal>
          </pbr>
        </material>
      </visual>
    </link>
    <joint name="motor_port_joint" type="revolute">
      <parent>base_link</parent>
      <child>motor_port_link</child>
      <axis>
        <xyz>0 0 -1</xyz>
        <limit>
          <lower>-1e16</lower>
          <upper>1e16</upper>
        </limit>
        <dynamics>
          <damping>0.02</damping>
        </dynamics>
      </axis>
    </joint>

    <!-- sensors -->
    <link name="imu_link">
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.0001</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0001</iyy>
          <iyz>0</iyz>
          <izz>0.0001</izz>
        </inertia>
      </inertial>
      <sensor name="imu_sensor" type="imu">
        <pose degrees="true">0 0 0 180 0 0</pose>
        <always_on>1</always_on>
        <update_rate>1000.0</update_rate>
      </sensor>
    </link>
    <joint name="imu_joint" type="revolute">
      <child>imu_link</child>
      <parent>base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>0</lower>
          <upper>0</upper>
        </limit>
        <dynamics>
          <damping>1.0</damping>
        </dynamics>
      </axis>
    </joint>

    <!-- plugins -->
    <plugin name="gz::sim::systems::JointStatePublisher"
      filename="gz-sim-joint-state-publisher-system">
    </plugin>
    <plugin name="gz::sim::systems::OdometryPublisher"
      filename="gz-sim-odometry-publisher-system">
      <odom_frame>odom</odom_frame>
      <robot_base_frame>base_link</robot_base_frame>
      <dimensions>3</dimensions>
    </plugin>

    <!-- left motor thruster cw -->
    <plugin name="gz::sim::systems::Thruster"
      filename="gz-sim-thruster-system">
      <namespace>blueboat</namespace>
      <joint_name>motor_port_joint</joint_name>
      <thrust_coefficient>-0.02</thrust_coefficient>
      <fluid_density>1025</fluid_density>
      <propeller_diameter>0.112</propeller_diameter>
      <velocity_control>0</velocity_control>
      <use_angvel_cmd>0</use_angvel_cmd>
    </plugin>

    <!-- right motor thruster ccw -->
    <plugin name="gz::sim::systems::Thruster"
      filename="gz-sim-thruster-system">
      <namespace>blueboat</namespace>
      <joint_name>motor_stbd_joint</joint_name>
      <thrust_coefficient>0.02</thrust_coefficient>
      <fluid_density>1025</fluid_density>
      <propeller_diameter>0.112</propeller_diameter>
      <velocity_control>0</velocity_control>
      <use_angvel_cmd>0</use_angvel_cmd>
    </plugin>

    <!-- hydrodynamics plugin -->

     <plugin name="gz::sim::systems::Hydrodynamics" 
        filename="gz-waves1-hydrodynamics-system">
        <enable>blueboat::base_link</enable>
        <hydrodynamics>
          <damping_on>1</damping_on>
          <viscous_drag_on>1</viscous_drag_on>
          <pressure_drag_on>1</pressure_drag_on>
        </hydrodynamics>
        <markers>
          <update_rate>10</update_rate>
          <water_patch>1</water_patch>
          <waterline>1</waterline>
          <underwater_surface>1</underwater_surface>
        </markers>
    </plugin>

      <!-- ardupilot plugin -->
    <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>imu_link::imu_sensor</imuName>

      <!--
        port motor cw

        SERVO1_FUNCTION 73 (ThrottleLeft)
        SERVO1_MAX 2000
        SERVO1_MIN 1000
      -->
      <control channel="0">
        <jointName>motor_port_joint</jointName>
        <multiplier>70</multiplier>
        <offset>-0.5</offset>
        <servo_min>1000</servo_min>
        <servo_max>2000</servo_max>
        <type>COMMAND</type>
        <cmd_topic>/model/blueboat/joint/motor_port_joint/cmd_thrust</cmd_topic>
      </control>

      <!--
        stbd motor ccw

        SERVO3_FUNCTION 74 (ThrottleRight)
        SERVO3_MAX 2000
        SERVO3_MIN 1000
      -->
      <control channel="2">
        <jointName>motor_stbd_joint</jointName>
        <multiplier>70</multiplier>
        <offset>-0.5</offset>
        <servo_min>1000</servo_min>
        <servo_max>2000</servo_max>
        <type>COMMAND</type>
        <cmd_topic>/model/blueboat/joint/motor_stbd_joint/cmd_thrust</cmd_topic>
      </control>
    </plugin>

  </model>
</sdf>
Screenshot 2024-03-07 at 4 19 18 PM
srmainwaring commented 3 months ago

@dlaboy, you have the <water_patch> and other markers enabled in the hydrodynamics plugin XML. See the <markers> element in: https://github.com/srmainwaring/asv_wave_sim?tab=readme-ov-file#hydrodynamics-plugin

The model checked into the repo does not have these set: https://github.com/ArduPilot/SITL_Models/blob/master/Gazebo/models/blueboat/model.sdf