Commit 5140bb7f authored by Ricardo Rico Uribe's avatar Ricardo Rico Uribe
Browse files

general order and small fixes - missing fix for ardupilot gazebo error where... 

general order and small fixes - missing fix for ardupilot gazebo error where motors turn without control
parent bb89241c

README.md

+33 −17
Original line number Original line Diff line number Diff line 
# Aquanaute

# Aquanaute





Repository to simulate the Aquanaute boat in maritime conditions such as wind and waves with the plugins created for the VRX and VRX-Challenge in Gazebo and ROS.

Repository to simulate the Aquanaute boat in maritime conditions such as wind and waves and with autopilot provided by ardupilot, using the plugins created for the VRX and VRX-Challenge in Gazebo and ROS and the plugin ardupilot-gazebo.



---





## Installation

## Installation





* Clone this repo to a Catkin Workspace

* Clone this repo to a Catkin Workspace





* Clone the VRX repo (hosted at ensta) to the same Workspace (http://git-u2is.ensta.fr/ssh/vrx) and checkout to the branch "aquanaute"

* Clone the VRX repo (hosted at ensta) to the same Workspace (<http://git-u2is.ensta.fr/ssh/vrx>) and change to the branch "aquanaute"



### ardupilot parameters



in the file "vehiculeinfo.py" (/ardupilot/Tools/autotest/pysim) you need to change the default parameters file for the "gazebo-rover" in line 280 to the aquanaute.parm file provided in /aquanaute/aquanaute_description/ardu_params you can copy and paste the file in the default_params folder located inside (/ardupilot/Tools/autotest).

*There is an error when you launch ardupilot, the motors in the gazebo simulation will start to turn in a fixed direction and wont respond to commands, this issue has not yet been fixed and it's root cause it's not yet defined, it may be the ardupilot_gazebo plugin or the ardupilot frame*



---





## Usage

## Usage



### modify wind and waves

### modify wind and waves



in the ocean.world.xacro (/vrx_gazebo/worlds) you have

in the ocean.world.xacro (/vrx_gazebo/worlds) you have

  ```xml



    <!--Waves-->

    <!--Waves-->

    <xacro:include filename="$(find wave_gazebo)/world_models/ocean_waves/model.xacro"/>

    <xacro:include filename="$(find wave_gazebo)/world_models/ocean_waves/model.xacro"/>

    <xacro:ocean_waves gain="0.0" period="5" direction_x="-1.0" direction_y="0.0" angle="0.0"/>

    <xacro:ocean_waves gain="0.0" period="5" direction_x="-1.0" direction_y="0.0" angle="0.0"/>
@@ -27,31 +38,36 @@ Repository to simulate the Aquanaute boat in maritime conditions such as wind an 
        </wind_obj>

        </wind_obj>

      </wind_objs>

      </wind_objs>

    </xacro:usv_wind_gazebo>

    </xacro:usv_wind_gazebo>

  ```



  it is configured to have no waves and no wind, also the wind and the waves move in the same direction, to activate wind and waves change the "mean_vel" and "gain" parameters respectively (recomended values are smaller than 1.0).

It is configured to have no waves and no wind, also the wind and the waves move in the same direction, to activate wind and waves change the "mean_vel" and "gain" parameters respectively (recomended values are smaller than 1.0).  

Even when these values are set to 0, the boat will move from right to left.



---





### run

### run

in the workspace run the command at the beginning (and every time you change a parameter)





  ```bash

#### compilation



in the workspace run the command at the beginning (and every time you change a parameter in the vrx xacros)



    catkin_make

    catkin_make

  ```



#### gazebo simulation





then launch the file you created

then launch the file you created





  ```bash

    roslaunch aquanaute_gazebo aquanaute.launch testing:=false

    roslaunch aquanaute_gazebo aquanaute.launch testing:=false

  ```



  this parameter can be changed to true to deactivate the plugins from vrx (the world will be changed to an empty world)

this parameter can be changed to true to deactivate the plugins from vrx (the world will be changed to an empty world without gravity)



#### ardupilot





on another terminal (inside the ardupilot root) run

on another terminal (inside the ardupilot root) run

  ```bash

  ./Tools/autotest/sim_vehicle.py --vehicle=Rover --frame=sailboat-motor --map --console --location=Xlac

  ```





## Author

    ./Tools/autotest/sim_vehicle.py -v APMrover2 -f gazebo-rover --map --console -l 48.71603264538473,2.213283777236939,155,0 

Ricardo RICO URIBE intern at U2IS in the summer of 2020





## License

---





## Author





Ricardo RICO URIBE intern at U2IS in the summer of 2020

aquanaute_description/ardu_params/aquanaute.parm

0 → 100644
+6 −0
Original line number Original line Diff line number Diff line 
FRAME_CLASS 2

AHRS_ORIENTATIONM 0

GCS_PID_MASK 1

SERVO1_FUNCTION 26

SERVO3_FUNCTION 70

ARMING_CHECK 0

aquanaute_description/urdf/aquanaute_description.urdf.xacro

+4 −2
Original line number Original line Diff line number Diff line
@@ -212,7 +212,8 @@ 
        <child link="motor_link"/>

        <child link="motor_link"/>

        <origin xyz="-0.91 0.0 -0.09" rpy="0 0 0"/>

        <origin xyz="-0.91 0.0 -0.09" rpy="0 0 0"/>

        <axis xyz="0 0 1"/>

        <axis xyz="0 0 1"/>

        <limit effort="1" velocity="1" lower="-0.9" upper="0.9"/>

        <limit effort="-1" velocity="-1" lower="-0.9" upper="0.9"/>

        <dynamics damping="0.0000001"/>

    </joint>

    </joint>





    <joint name="propeller_motor_joint" type="revolute">

    <joint name="propeller_motor_joint" type="revolute">
@@ -220,7 +221,8 @@ 
        <child link="propeller_link"/>

        <child link="propeller_link"/>

        <origin xyz="-0.265 0 -0.42" rpy="0 0 0"/>

        <origin xyz="-0.265 0 -0.42" rpy="0 0 0"/>

        <axis xyz="1 0 0"/>

        <axis xyz="1 0 0"/>

        <limit effort="1" velocity="1" lower="-1e+16" upper="1e+16"/>

        <limit effort="-1" velocity="-1" lower="-1e+16" upper="1e+16"/>

        <dynamics damping="0.0000001"/>

    </joint>

    </joint>





    <joint name="sensor_frame_platform_joint" type="fixed">

    <joint name="sensor_frame_platform_joint" type="fixed">

aquanaute_description/urdf/tmp.urdf

deleted100644 → 0
+0 −281
Original line number Original line Diff line number Diff line 
<?xml version="1.0" encoding="utf-8"?>

<!-- =================================================================================== -->

<!-- |    This document was autogenerated by xacro from aquanaute_description.urdf.xacro | -->

<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->

<!-- =================================================================================== -->

<robot name="aquanaute">

  <material name="blue">

    <color rgba="0 0 0.8 1"/>

  </material>

  <material name="red">

    <color rgba="0.8 0 0 1"/>

  </material>

  <material name="green">

    <color rgba="0 0.8 0 0.5"/>

  </material>

  <material name="cyan">

    <color rgba="0 0.6 0.6 1"/>

  </material>

  <material name="yellow">

    <color rgba="0.8 0.8 0 1"/>

  </material>

  <material name="purple">

    <color rgba="0.5 0 0.5 1"/>

  </material>

  <material name="white">

    <color rgba="1 1 1 1"/>

  </material>

  <material name="gray">

    <color rgba="0.35 0.35 0.35 1"/>

  </material>

  <material name="black">

    <color rgba="0 0 0 1"/>

  </material>

  <!-- * * * Link Definitions * * * -->

  <link name="base_link">

    <visual>

      <origin rpy="0.0 0 0" xyz="0 0 0"/>

      <geometry>

        <sphere radius="0.001"/>

      </geometry>

      <material name="blue"/>

    </visual>

  </link>

  <link name="platform_link">

    <inertial>

      <origin rpy="0 0 0" xyz="-0.024 0 -0007"/>

      <mass value="16"/>

      <inertia ixx="4.206" ixy="0.0" ixz="-0.031" iyy="4.588" iyz="0.0" izz="8.785"/>

    </inertial>

    <collision>

      <origin rpy="0 0 0" xyz="0 0 0"/>

      <geometry>

        <mesh filename="package://aquanaute_description/models/meshes/Platform.stl"/>

      </geometry>

    </collision>

    <visual>

      <origin rpy="0.0 0 0" xyz="0 0 0"/>

      <geometry>

        <mesh filename="package://aquanaute_description/models/meshes/Platform.stl"/>

      </geometry>

      <material name="yellow"/>

    </visual>

  </link>

  <link name="right_hull_link">

    <inertial>

      <origin rpy="0 0 0" xyz="-0.124 0 -0.168"/>

      <mass value="10.45"/>

      <inertia ixx="0.244" ixy="3.346" ixz="0.009" iyy="4.974" iyz="-8.274" izz="4.965"/>

    </inertial>

    <collision>

      <origin rpy="0 0 0" xyz="0 0 0"/>

      <geometry>

        <mesh filename="package://aquanaute_description/models/meshes/Hull.stl"/>

      </geometry>

    </collision>

    <visual>

      <origin rpy="0.0 0 0" xyz="0 0 0"/>

      <geometry>

        <mesh filename="package://aquanaute_description/models/meshes/Hull.stl"/>

      </geometry>

      <material name="red"/>

    </visual>

  </link>

  <link name="left_hull_link">

    <inertial>

      <origin rpy="0 0 0" xyz="-0.124 0 -0.168"/>

      <mass value="10.45"/>

      <inertia ixx="0.244" ixy="3.346" ixz="0.009" iyy="4.974" iyz="-8.274" izz="4.965"/>

    </inertial>

    <collision>

      <origin rpy="0 0 0" xyz="0 0 0"/>

      <geometry>

        <mesh filename="package://aquanaute_description/models/meshes/Hull.stl"/>

      </geometry>

    </collision>

    <visual>

      <origin rpy="0.0 0 0" xyz="0 0 0"/>

      <geometry>

        <mesh filename="package://aquanaute_description/models/meshes/Hull.stl"/>

      </geometry>

      <material name="red"/>

    </visual>

  </link>

  <link name="motor_link">

    <inertial>

      <origin rpy="0 0 0" xyz="-0.133 0 -0.391"/>

      <mass value="6.6"/>

      <inertia ixx="0.072" ixy="3.604" ixz="-0.011" iyy="0.111" iyz="5.484" izz="0.042"/>

    </inertial>

    <collision>

      <origin rpy="0 0 0" xyz="0 0 0"/>

      <geometry>

        <mesh filename="package://aquanaute_description/models/meshes/Motor.stl"/>

      </geometry>

    </collision>

    <visual>

      <origin rpy="0.0 0 0" xyz="0 0 0"/>

      <geometry>

        <mesh filename="package://aquanaute_description/models/meshes/Motor.stl"/>

      </geometry>

      <material name="purple"/>

    </visual>

  </link>

  <link name="propeller_link">

    <collision>

      <origin rpy="0 0 0" xyz="0 0 0"/>

      <geometry>

        <cylinder length="0.04" radius="0.05"/>

      </geometry>

    </collision>

    <visual>

      <origin rpy="0 0 0" xyz="0 0 0"/>

      <geometry>

        <cylinder length="0.02" radius="0.01"/>

      </geometry>

      <material name="green"/>

    </visual>

  </link>

  <link name="sensor_frame_link">

    <inertial>

      <origin rpy="0 0 0" xyz="-0.007 0 0.379"/>

      <mass value="1.8"/>

      <inertia ixx="0.308" ixy="0.00" ixz="0.007" iyy="0.297" iyz="0.00" izz="0.022"/>

    </inertial>

    <collision>

      <origin rpy="0 0 0" xyz="0 0 0"/>

      <geometry>

        <mesh filename="package://aquanaute_description/models/meshes/sensor_frame.stl"/>

      </geometry>

    </collision>

    <visual>

      <origin rpy="0.0 0 0" xyz="0 0 0"/>

      <geometry>

        <mesh filename="package://aquanaute_description/models/meshes/sensor_frame.stl"/>

      </geometry>

      <material name="cyan"/>

    </visual>

  </link>

  <!-- * * * Joint Definitions * * * -->

  <joint name="platform_base_joint" type="fixed">

    <parent link="base_link"/>

    <child link="platform_link"/>

    <origin rpy="0 0 0" xyz="0 0 0.35"/>

  </joint>

  <joint name="right_hull_platform_joint" type="fixed">

    <parent link="platform_link"/>

    <child link="right_hull_link"/>

    <origin rpy="0 0 0" xyz="-0.05 -0.7 -0.036"/>

  </joint>

  <joint name="left_hull_platform_joint" type="fixed">

    <parent link="platform_link"/>

    <child link="left_hull_link"/>

    <origin rpy="0 0 0" xyz="-0.05 0.7 -0.036"/>

  </joint>

  <joint name="motor_platform_joint" type="revolute">

    <parent link="platform_link"/>

    <child link="motor_link"/>

    <origin rpy="0 0 0" xyz="-0.91 0.0 -0.09"/>

    <axis xyz="0 0 1"/>

    <limit effort="-1" lower="-0.9" upper="0.9" velocity="-1"/>

  </joint>

  <joint name="propeller_motor_joint" type="revolute">

    <parent link="motor_link"/>

    <child link="propeller_link"/>

    <origin rpy="0 1.57 0" xyz="-0.28 0 -0.42"/>

    <axis xyz="0 0 1"/>

    <limit effort="-1" lower="-1e+12" upper="1e+12" velocity="-1"/>

  </joint>

  <joint name="sensor_frame_platform_joint" type="fixed">

    <parent link="platform_link"/>

    <child link="sensor_frame_link"/>

    <origin rpy="0 0 0" xyz="-0.55 0.0 0.0"/>

  </joint>

  <!-- * * * GAZEBO Definitions * * * -->

  <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">

    <kp>100000.0</kp>

    <kd>100000.0</kd>

    <mu1>10.0</mu1>

    <mu2>10.0</mu2>

    <material>Gazebo/Black</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 Plugins * * * -->

  <plugin filename="libArduPilotPlugin.so" name="ardupilot_plugin">

    <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></imuName>-->

    <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>

    <!--

          incoming control command [0, 1]

          so offset it by 0 to get [0, 1]

          and divide max target by 1.

          offset = 0

          multiplier = 838 max rpm / 1 = 838

        -->

    <control channel="0">

      <multiplier>1</multiplier>

      <offset>-0.5</offset>

      <type>RIC</type>

      <p_gain>1</p_gain>

      <i_gain>0</i_gain>

      <d_gain>0</d_gain>

      <i_max>0</i_max>

      <i_min>0</i_min>

      <cmd_max>1</cmd_max>

      <cmd_min>-1</cmd_min>

      <jointName>aquanaute::motor_platform_joint</jointName>

    </control>

    <control channel="1">

      <multiplier>1</multiplier>

      <offset>-0.5</offset>

      <type>THOMAS</type>

      <p_gain>1</p_gain>

      <i_gain>0</i_gain>

      <d_gain>0</d_gain>

      <i_max>0</i_max>

      <i_min>0</i_min>

      <cmd_max>1</cmd_max>

      <cmd_min>-1</cmd_min>

      <jointName>aquanaute::propeller_motor_joint</jointName>

    </control>

  </plugin>

</robot>

aquanaute_gazebo/urdf/aquanaute_gazebo.urdf.xacro

+8 −36
Original line number Original line Diff line number Diff line
@@ -45,18 +45,10 @@ 
    </gazebo>

    </gazebo>





    <gazebo reference="motor_link">

    <gazebo reference="motor_link">

        <kp>100000.0</kp>

        <kd>100000.0</kd>

        <mu1>10.0</mu1>

        <mu2>10.0</mu2>

        <material>Gazebo/Black</material>

        <material>Gazebo/Black</material>

    </gazebo>

    </gazebo>





    <gazebo reference="propeller_link">

    <gazebo reference="propeller_link">

        <kp>1.0</kp>

        <kd>1.0</kd>

        <mu1>1.0</mu1>

        <mu2>1.0</mu2>

        <material>Gazebo/Blue</material>

        <material>Gazebo/Blue</material>

    </gazebo>

    </gazebo>
@@ -87,41 +79,21 @@ 
            <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>

            <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>

            <imuName>imu_sensor</imuName>

            <imuName>imu_sensor</imuName>

            <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>

            <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>

            <!--



            incoming control command [0, 1]

            so offset it by 0 to get [0, 1]

            and divide max target by 1.

            offset = 0

            multiplier = 838 max rpm / 1 = 838

            -->

            <control channel="0">

            <control channel="0">

                <multiplier>1</multiplier>

                <offset>-0.5</offset>

                <type>POSITION</type>

                <type>POSITION</type>

                <p_gain>1</p_gain>

                <multiplier>1</multiplier>

                <i_gain>0</i_gain>

                <offset>0</offset>

                <d_gain>0</d_gain>

                <i_max>0</i_max>

                <i_min>0</i_min>

                <cmd_max>1</cmd_max>

                <cmd_min>-1</cmd_min>

                <jointName>aquanaute::motor_platform_joint</jointName>

                <jointName>aquanaute::motor_platform_joint</jointName>

            </control>

            </control>



            <control channel="0">

            <control channel="1">

                <type>VELOCITY</type>

                <type>VELOCITY</type>

                <offset>-0.5</offset>

                <multiplier>1</multiplier>

                <p_gain>1</p_gain>

                <offset>0</offset>

                <i_gain>0</i_gain>

                <d_gain>0</d_gain>

                <i_max>0</i_max>

                <i_min>0</i_min>

                <cmd_max>10</cmd_max>

                <cmd_min>-10</cmd_min>

                <jointName>aquanaute::propeller_motor_joint</jointName>

                <jointName>aquanaute::propeller_motor_joint</jointName>

                <multiplier>70</multiplier>

            </control>

            </control>



<!--

-->

        </plugin>

        </plugin>

    </gazebo>

    </gazebo>

    <!-- Attach hydrodynamics plugin -->

    <!-- Attach hydrodynamics plugin -->