aquanaute_gazebo.urdf.xacro 3.7 KB
<?xml version="1.0"?>
<!-- Basic WAM-V with gazebo plugins for dynamics -->
<robot xmlns:xacro="http://ros.org/wiki/xacro">
    <xacro:arg name="testing" default="false" />
    <!-- Include macros for dynamics plugins -->
    <xacro:unless value="$(arg testing)">
        <xacro:include filename="$(find aquanaute_gazebo)/urdf/macros.xacro" />
    </xacro:unless>

    <!-- Defines the base vehicle -->
    <xacro:include filename="$(find aquanaute_description)/urdf/aquanaute_description.urdf.xacro"/>

    <!--Gazebo References-->

    <gazebo reference="base_link">
        <kp>100000.0</kp>
        <kd>100000.0</kd>
        <mu1>10.0</mu1>
        <mu2>10.0</mu2>
        <material>Gazebo/Grey</material>
    </gazebo>

    <gazebo reference="platform_link">
        <kp>100000.0</kp>
        <kd>100000.0</kd>
        <mu1>10.0</mu1>
        <mu2>10.0</mu2>
        <material>Gazebo/Grey</material>
    </gazebo>

    <gazebo reference="right_hull_link">
        <kp>100000.0</kp>
        <kd>100000.0</kd>
        <mu1>10.0</mu1>
        <mu2>10.0</mu2>
        <material>Gazebo/Red</material>
    </gazebo>

    <gazebo reference="left_hull_link">
        <kp>100000.0</kp>
        <kd>100000.0</kd>
        <mu1>10.0</mu1>
        <mu2>10.0</mu2>
        <material>Gazebo/Red</material>
    </gazebo>

    <gazebo reference="motor_link">
        <material>Gazebo/Black</material>
    </gazebo>

    <gazebo reference="propeller_link">
        <material>Gazebo/Blue</material>
    </gazebo>

    <gazebo reference="sensor_frame_link">
        <kp>100000.0</kp>
        <kd>100000.0</kd>
        <mu1>10.0</mu1>
        <mu2>10.0</mu2>
        <material>Gazebo/White</material>
    </gazebo>

    <gazebo reference="imu_link">
        <sensor name="imu_sensor" type="imu">
            <pose>0 0 0 3.141593 0 0</pose>
            <always_on>1</always_on>
            <update_rate>1000.0</update_rate>
        </sensor>
        <material>Gazebo/Yellow</material>
    </gazebo>
    
    <!-- * * * GAZEBO Plugins * * * -->
    <gazebo>
        <plugin name="ardupilot_plugin" filename="libArduPilotPlugin.so">
            <fdm_addr>127.0.0.1</fdm_addr>
            <fdm_port_in>9002</fdm_port_in>
            <fdm_port_out>9003</fdm_port_out>
            <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
            <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
            <imuName>imu_sensor</imuName>
            <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>

            <control channel="0">
                <type>POSITION</type>
                <multiplier>100</multiplier> <!--ARBITRARY VALUES-->
                <offset>0</offset>
                <p_gain>1.0</p_gain>
                <i_gain>0</i_gain>
                <d_gain>0</d_gain>
                <i_max>0</i_max>
                <i_min>0</i_min>
                <cmd_max>3.5</cmd_max>
                <cmd_min>-3.5</cmd_min>
                <jointName>aquanaute::motor_platform_joint</jointName>
            </control>
            <control channel="2">
                <type>VELOCITY</type>
                <multiplier>800</multiplier> <!--ARBITRARY VALUES-->
                <offset>0</offset>
                <p_gain>0.20</p_gain>
                <i_gain>0</i_gain>
                <d_gain>0</d_gain>
                <i_max>0</i_max>
                <i_min>0</i_min>
                <cmd_max>2.5</cmd_max>
                <cmd_min>-2.5</cmd_min>
                <jointName>aquanaute::propeller_motor_joint</jointName>
            </control>
<!--
-->
        </plugin>
    </gazebo>
    <!-- Attach hydrodynamics plugin -->
    <xacro:unless value="$(arg testing)">
        <xacro:usv_dynamics_gazebo name="aquanaute_dynamics_plugin"/>
    </xacro:unless>
</robot>