aquanaute_gazebo.urdf.xacro 3.02 KB
Newer Older
1
2
<?xml version="1.0"?>
<!-- Basic WAM-V with gazebo plugins for dynamics -->
3
<robot xmlns:xacro="http://ros.org/wiki/xacro">
4
5
6
7
8
9
    <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>

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

13
    <!--Gazebo References-->
14
15
16
17
18

    <gazebo reference="base_link">
        <material>Gazebo/Grey</material>
    </gazebo>

19
    <gazebo reference="boat_link">
20
21
22
23
24
25
26
27
        <kp>100000.0</kp>
        <kd>100000.0</kd>
        <mu1>10.0</mu1>
        <mu2>10.0</mu2>
        <material>Gazebo/Grey</material>
    </gazebo>

    <gazebo reference="motor_link">
28
        <material>Gazebo/Blue</material>
29
30
    </gazebo>

31
    <gazebo reference="propeller_link">
32
33
34
        <material>Gazebo/White</material>
    </gazebo>

35
36
37
38
39
40
    <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>
41
        <material>Gazebo/White</material>
42
43
    </gazebo>
    
44
45
46
47
48
49
50
51
52
53
    <!-- * * * 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>
54

55
56
            <control channel="0">
                <type>POSITION</type>
57
                <multiplier>100</multiplier> <!--ARBITRARY VALUES-->
58
                <offset>0</offset>
59
60
61
62
63
64
65
                <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>
66
                <jointName>aquanaute::motor_boat_joint</jointName>
67
            </control>
68
            <control channel="2">
69
                <type>VELOCITY</type>
70
                <multiplier>800</multiplier> <!--ARBITRARY VALUES-->
71
                <offset>0</offset>
72
73
74
75
76
77
78
                <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>
79
80
                <jointName>aquanaute::propeller_motor_joint</jointName>
            </control>
81
82
<!--
-->
83
84
        </plugin>
    </gazebo>
85
86
87
88
    <!-- Attach hydrodynamics plugin -->
    <xacro:unless value="$(arg testing)">
        <xacro:usv_dynamics_gazebo name="aquanaute_dynamics_plugin"/>
    </xacro:unless>
89
</robot>