Commit bb89241cd220eb1b6bc45c20e55106b85faacfbf

Authored by Ricardo Rico Uribe
1 parent 4120d958

fixed plugin - error in topic advertisements

@@ -10,7 +10,7 @@ Repository to simulate the Aquanaute boat in maritime conditions such as wind an @@ -10,7 +10,7 @@ Repository to simulate the Aquanaute boat in maritime conditions such as wind an
10 10
11 ## Usage 11 ## Usage
12 ### modify wind and waves 12 ### modify wind and waves
13 - in the ocean.world.xacro you have found in /vrx_gazebo/worlds 13 + in the ocean.world.xacro (/vrx_gazebo/worlds) you have
14 ```xml 14 ```xml
15 <!--Waves--> 15 <!--Waves-->
16 <xacro:include filename="$(find wave_gazebo)/world_models/ocean_waves/model.xacro"/> 16 <xacro:include filename="$(find wave_gazebo)/world_models/ocean_waves/model.xacro"/>
@@ -40,8 +40,15 @@ in the workspace run the command at the beginning (and every time you change a p @@ -40,8 +40,15 @@ in the workspace run the command at the beginning (and every time you change a p
40 then launch the file you created 40 then launch the file you created
41 41
42 ```bash 42 ```bash
43 - roslaunch aquanaute_gazebo world.launch 43 + roslaunch aquanaute_gazebo aquanaute.launch testing:=false
44 ``` 44 ```
  45 + this parameter can be changed to true to deactivate the plugins from vrx (the world will be changed to an empty world)
  46 +
  47 +on another terminal (inside the ardupilot root) run
  48 + ```bash
  49 + ./Tools/autotest/sim_vehicle.py --vehicle=Rover --frame=sailboat-motor --map --console --location=Xlac
  50 + ```
  51 +
45 ## Author 52 ## Author
46 Ricardo RICO URIBE intern at U2IS in the summer of 2020 53 Ricardo RICO URIBE intern at U2IS in the summer of 2020
47 54
aquanaute_description/launch/description.launch
@@ -5,7 +5,7 @@ @@ -5,7 +5,7 @@
5 5
6 <!-- send fake joint values --> 6 <!-- send fake joint values -->
7 <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"> 7 <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
8 - <param name="use_gui" value="false"/> 8 + <param name="use_gui" value="true"/>
9 </node> 9 </node>
10 10
11 <!-- Combine joint values --> 11 <!-- Combine joint values -->
aquanaute_description/models/meshes/Motor.stl
No preview for this file type
aquanaute_description/models/meshes/Propeller.stl 0 → 100644
No preview for this file type
aquanaute_description/urdf/aquanaute_description.urdf.xacro
@@ -128,24 +128,30 @@ @@ -128,24 +128,30 @@
128 <geometry> 128 <geometry>
129 <mesh filename="package://aquanaute_description/models/meshes/Motor.stl"/> 129 <mesh filename="package://aquanaute_description/models/meshes/Motor.stl"/>
130 </geometry> 130 </geometry>
131 - <material name="purple"/> 131 + <material name="black"/>
132 </visual> 132 </visual>
133 </link> 133 </link>
134 134
135 <link name="propeller_link"> 135 <link name="propeller_link">
  136 + <inertial>
  137 + <origin xyz="-0.034 0 0" rpy="0 0 0"/>
  138 + <mass value="0.233"/>
  139 + <inertia ixx="1.441e-04" ixy="4.406e-09" ixz="0.00" iyy="1.760e-04" iyz="2.373e-06" izz="1.961e-04"/>
  140 + </inertial>
  141 +
136 <collision> 142 <collision>
137 <origin xyz="0 0 0" rpy="0 0 0"/> 143 <origin xyz="0 0 0" rpy="0 0 0"/>
138 <geometry> 144 <geometry>
139 - <cylinder radius="0.05" length="0.04"/> 145 + <mesh filename="package://aquanaute_description/models/meshes/Propeller.stl"/>
140 </geometry> 146 </geometry>
141 </collision> 147 </collision>
142 148
143 <visual> 149 <visual>
144 - <origin rpy="0 0 0" xyz="0 0 0"/> 150 + <origin rpy="0.0 0 0" xyz="0 0 0"/>
145 <geometry> 151 <geometry>
146 - <cylinder radius="0.04" length="0.05"/> 152 + <mesh filename="package://aquanaute_description/models/meshes/Propeller.stl"/>
147 </geometry> 153 </geometry>
148 - <material name="black"/> 154 + <material name="blue"/>
149 </visual> 155 </visual>
150 </link> 156 </link>
151 157
@@ -168,7 +174,7 @@ @@ -168,7 +174,7 @@
168 <geometry> 174 <geometry>
169 <mesh filename="package://aquanaute_description/models/meshes/sensor_frame.stl"/> 175 <mesh filename="package://aquanaute_description/models/meshes/sensor_frame.stl"/>
170 </geometry> 176 </geometry>
171 - <material name="cyan"/> 177 + <material name="white"/>
172 </visual> 178 </visual>
173 </link> 179 </link>
174 180
@@ -206,15 +212,15 @@ @@ -206,15 +212,15 @@
206 <child link="motor_link"/> 212 <child link="motor_link"/>
207 <origin xyz="-0.91 0.0 -0.09" rpy="0 0 0"/> 213 <origin xyz="-0.91 0.0 -0.09" rpy="0 0 0"/>
208 <axis xyz="0 0 1"/> 214 <axis xyz="0 0 1"/>
209 - <limit effort="-1" velocity="-1" lower="-0.9" upper="0.9"/> 215 + <limit effort="1" velocity="1" lower="-0.9" upper="0.9"/>
210 </joint> 216 </joint>
211 217
212 - <joint name="propeller_motor_joint" type="fixed"> 218 + <joint name="propeller_motor_joint" type="revolute">
213 <parent link="motor_link"/> 219 <parent link="motor_link"/>
214 <child link="propeller_link"/> 220 <child link="propeller_link"/>
215 - <origin xyz="-0.5 0 0.42" rpy="0 1.57 0"/>  
216 - <axis xyz="0 0 1"/>  
217 - <limit effort="-1" velocity="-1" lower="-1e+16" upper="1e+16"/> 221 + <origin xyz="-0.265 0 -0.42" rpy="0 0 0"/>
  222 + <axis xyz="1 0 0"/>
  223 + <limit effort="1" velocity="1" lower="-1e+16" upper="1e+16"/>
218 </joint> 224 </joint>
219 225
220 <joint name="sensor_frame_platform_joint" type="fixed"> 226 <joint name="sensor_frame_platform_joint" type="fixed">
@@ -226,6 +232,6 @@ @@ -226,6 +232,6 @@
226 <joint name="imu_platform_joint" type="fixed"> 232 <joint name="imu_platform_joint" type="fixed">
227 <parent link="platform_link"/> 233 <parent link="platform_link"/>
228 <child link="imu_link"/> 234 <child link="imu_link"/>
229 - <origin xyz="0 0 0.1" rpy="0 0 0"/> 235 + <origin xyz="0 0 0.02" rpy="0 0 0"/>
230 </joint> 236 </joint>
231 </robot> 237 </robot>
aquanaute_gazebo/launch/world.launch renamed to aquanaute_gazebo/launch/aquanaute.launch
1 <?xml version="1.0"?> 1 <?xml version="1.0"?>
2 <launch> 2 <launch>
3 <env name="ROSCONSOLE_CONFIG_FILE" value="$(find vrx_gazebo)/config/custom_rosconsole.conf"/> 3 <env name="ROSCONSOLE_CONFIG_FILE" value="$(find vrx_gazebo)/config/custom_rosconsole.conf"/>
4 - <!-- Gazebo world to load -->  
5 - <arg name="world" default="$(find vrx_gazebo)/worlds/ocean.world" /> 4 +
6 <!-- If true, run gazebo GUI --> 5 <!-- If true, run gazebo GUI -->
7 <arg name="gui" default="true" /> 6 <arg name="gui" default="true" />
8 <!-- If true, run gazebo in verbose mode --> 7 <!-- If true, run gazebo in verbose mode -->
@@ -14,10 +13,12 @@ @@ -14,10 +13,12 @@
14 <!-- Start in a default namespace --> 13 <!-- Start in a default namespace -->
15 <arg name="namespace" default="aquanaute"/> 14 <arg name="namespace" default="aquanaute"/>
16 15
  16 + <arg name="testing" default="false"/>
17 <!-- Initial USV location and attitude--> 17 <!-- Initial USV location and attitude-->
18 <arg name="x" default="0" /> 18 <arg name="x" default="0" />
19 <arg name="y" default="0" /> 19 <arg name="y" default="0" />
20 - <arg name="z" default="0.1" /> 20 + <arg name="z" default="0.5" if="$(arg testing)"/>
  21 + <arg name="z" default="0.1" unless="$(arg testing)"/>
21 <arg name="P" default="0" /> 22 <arg name="P" default="0" />
22 <arg name="R" default="0" /> 23 <arg name="R" default="0" />
23 <arg name="Y" default="0" /> 24 <arg name="Y" default="0" />
@@ -26,7 +27,8 @@ @@ -26,7 +27,8 @@
26 27
27 <!-- Start Gazebo with the world file --> 28 <!-- Start Gazebo with the world file -->
28 <include file="$(find gazebo_ros)/launch/empty_world.launch"> 29 <include file="$(find gazebo_ros)/launch/empty_world.launch">
29 - <arg name="world_name" value="$(arg world)"/> 30 + <arg name="world_name" value="$(find aquanaute_gazebo)/worlds/test.world" if="$(arg testing)"/>
  31 + <arg name="world_name" value="$(find vrx_gazebo)/worlds/ocean.world" unless="$(arg testing)"/>
30 <arg name="verbose" value="$(arg verbose)"/> 32 <arg name="verbose" value="$(arg verbose)"/>
31 <arg name="paused" value="$(arg paused)"/> 33 <arg name="paused" value="$(arg paused)"/>
32 <arg name="use_sim_time" value="true"/> 34 <arg name="use_sim_time" value="true"/>
@@ -37,7 +39,8 @@ @@ -37,7 +39,8 @@
37 39
38 <!-- Load robot model --> 40 <!-- Load robot model -->
39 <arg name="urdf" default="$(find aquanaute_gazebo)/urdf/aquanaute_gazebo.urdf.xacro"/> 41 <arg name="urdf" default="$(find aquanaute_gazebo)/urdf/aquanaute_gazebo.urdf.xacro"/>
40 - <param name="$(arg namespace)/robot_description" command="$(find xacro)/xacro '$(arg urdf)'"/> 42 + <param name="$(arg namespace)/robot_description" command="$(find xacro)/xacro '$(arg urdf)'
  43 + testing:=$(arg testing)"/>
41 44
42 <!-- Spawn model in Gazebo, script depending on non_competition_mode --> 45 <!-- Spawn model in Gazebo, script depending on non_competition_mode -->
43 <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x) -y $(arg y) -z $(arg z) 46 <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x) -y $(arg y) -z $(arg z)
aquanaute_gazebo/launch/test.launch deleted
@@ -1,42 +0,0 @@ @@ -1,42 +0,0 @@
1 -<?xml version="1.0"?>  
2 -<launch>  
3 - <!-- Gazebo world to load -->  
4 - <arg name="world" default="$(find aquanaute_gazebo)/worlds/test.world" />  
5 - <!-- If true, run gazebo GUI -->  
6 - <arg name="gui" default="true" />  
7 - <!-- If true, run gazebo in verbose mode -->  
8 - <arg name="verbose" default="true"/>  
9 - <!-- If true, start in paused state -->  
10 - <arg name="paused" default="false"/>  
11 - <!-- Set various other gazebo arguments-->  
12 - <arg name="extra_gazebo_args" default=""/>  
13 - <!-- Start in a default namespace -->  
14 - <arg name="namespace" default="aquanaute"/>  
15 -  
16 - <!-- Initial USV location and attitude-->  
17 - <arg name="x" default="0" />  
18 - <arg name="y" default="0" />  
19 - <arg name="z" default="0.5" />  
20 - <arg name="P" default="0" />  
21 - <arg name="R" default="0" />  
22 - <arg name="Y" default="0" />  
23 -  
24 - <!-- Start Gazebo with the world file -->  
25 - <include file="$(find gazebo_ros)/launch/empty_world.launch">  
26 - <arg name="world_name" value="$(arg world)"/>  
27 - <arg name="verbose" value="$(arg verbose)"/>  
28 - <arg name="paused" value="$(arg paused)"/>  
29 - <arg name="use_sim_time" value="true"/>  
30 - <arg name="gui" value="$(arg gui)" />  
31 - <arg name="enable_ros_network" value="true"/>  
32 - </include>  
33 -  
34 - <!-- Load robot model -->  
35 - <arg name="urdf" default="$(find aquanaute_gazebo)/urdf/aquanaute_gazebo.urdf.xacro"/>  
36 - <param name="$(arg namespace)/robot_description" command="$(find xacro)/xacro '$(arg urdf)'"/>  
37 -  
38 - <!-- Spawn model in Gazebo, script depending on non_competition_mode -->  
39 - <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x) -y $(arg y) -z $(arg z)  
40 - -R $(arg R) -P $(arg P) -Y $(arg Y)  
41 - -urdf -param $(arg namespace)/robot_description -model aquanaute"/>  
42 -</launch>  
aquanaute_gazebo/urdf/aquanaute_gazebo.urdf.xacro
1 <?xml version="1.0"?> 1 <?xml version="1.0"?>
2 <!-- Basic WAM-V with gazebo plugins for dynamics --> 2 <!-- Basic WAM-V with gazebo plugins for dynamics -->
3 <robot xmlns:xacro="http://ros.org/wiki/xacro"> 3 <robot xmlns:xacro="http://ros.org/wiki/xacro">
4 - <!-- Include macros for dynamics plugins  
5 - <xacro:include filename="$(find aquanaute_gazebo)/urdf/macros.xacro" /> --> 4 + <xacro:arg name="testing" default="false" />
  5 + <!-- Include macros for dynamics plugins -->
  6 + <xacro:unless value="$(arg testing)">
  7 + <xacro:include filename="$(find aquanaute_gazebo)/urdf/macros.xacro" />
  8 + </xacro:unless>
  9 +
6 <!-- Defines the base vehicle --> 10 <!-- Defines the base vehicle -->
7 <xacro:include filename="$(find aquanaute_description)/urdf/aquanaute_description.urdf.xacro"/> 11 <xacro:include filename="$(find aquanaute_description)/urdf/aquanaute_description.urdf.xacro"/>
8 12
9 - <!-- Defines the sim plugins--> 13 + <!--Gazebo References-->
10 14
11 <gazebo reference="base_link"> 15 <gazebo reference="base_link">
12 <kp>100000.0</kp> 16 <kp>100000.0</kp>
@@ -49,11 +53,11 @@ @@ -49,11 +53,11 @@
49 </gazebo> 53 </gazebo>
50 54
51 <gazebo reference="propeller_link"> 55 <gazebo reference="propeller_link">
52 - <kp>100000.0</kp>  
53 - <kd>100000.0</kd>  
54 - <mu1>10.0</mu1>  
55 - <mu2>10.0</mu2>  
56 - <material>Gazebo/Black</material> 56 + <kp>1.0</kp>
  57 + <kd>1.0</kd>
  58 + <mu1>1.0</mu1>
  59 + <mu2>1.0</mu2>
  60 + <material>Gazebo/Blue</material>
57 </gazebo> 61 </gazebo>
58 62
59 <gazebo reference="sensor_frame_link"> 63 <gazebo reference="sensor_frame_link">
@@ -64,16 +68,15 @@ @@ -64,16 +68,15 @@
64 <material>Gazebo/White</material> 68 <material>Gazebo/White</material>
65 </gazebo> 69 </gazebo>
66 70
67 - <gazebo>  
68 - <link name="imu_link">  
69 - <sensor name="imu_sensor" type="imu">  
70 - <pose>0 0 0 3.141593 0 0</pose>  
71 - <always_on>1</always_on>  
72 - <update_rate>1000.0</update_rate>  
73 - </sensor>  
74 - </link>  
75 - </gazebo>  
76 - 71 + <gazebo reference="imu_link">
  72 + <sensor name="imu_sensor" type="imu">
  73 + <pose>0 0 0 3.141593 0 0</pose>
  74 + <always_on>1</always_on>
  75 + <update_rate>1000.0</update_rate>
  76 + </sensor>
  77 + <material>Gazebo/Yellow</material>
  78 + </gazebo>
  79 +
77 <!-- * * * GAZEBO Plugins * * * --> 80 <!-- * * * GAZEBO Plugins * * * -->
78 <gazebo> 81 <gazebo>
79 <plugin name="ardupilot_plugin" filename="libArduPilotPlugin.so"> 82 <plugin name="ardupilot_plugin" filename="libArduPilotPlugin.so">
@@ -104,23 +107,25 @@ @@ -104,23 +107,25 @@
104 <cmd_min>-1</cmd_min> 107 <cmd_min>-1</cmd_min>
105 <jointName>aquanaute::motor_platform_joint</jointName> 108 <jointName>aquanaute::motor_platform_joint</jointName>
106 </control> 109 </control>
107 - 110 +
108 <control channel="1"> 111 <control channel="1">
109 - <multiplier>1</multiplier>  
110 - <offset>-0.5</offset>  
111 <type>VELOCITY</type> 112 <type>VELOCITY</type>
  113 + <offset>-0.5</offset>
112 <p_gain>1</p_gain> 114 <p_gain>1</p_gain>
113 <i_gain>0</i_gain> 115 <i_gain>0</i_gain>
114 <d_gain>0</d_gain> 116 <d_gain>0</d_gain>
115 <i_max>0</i_max> 117 <i_max>0</i_max>
116 <i_min>0</i_min> 118 <i_min>0</i_min>
117 - <cmd_max>1</cmd_max>  
118 - <cmd_min>-1</cmd_min> 119 + <cmd_max>10</cmd_max>
  120 + <cmd_min>-10</cmd_min>
119 <jointName>aquanaute::propeller_motor_joint</jointName> 121 <jointName>aquanaute::propeller_motor_joint</jointName>
  122 + <multiplier>70</multiplier>
120 </control> 123 </control>
121 - 124 +
122 </plugin> 125 </plugin>
123 </gazebo> 126 </gazebo>
124 - <!-- Attach hydrodynamics plugin  
125 - <xacro:usv_dynamics_gazebo name="aquanaute_dynamics_plugin"/> --> 127 + <!-- Attach hydrodynamics plugin -->
  128 + <xacro:unless value="$(arg testing)">
  129 + <xacro:usv_dynamics_gazebo name="aquanaute_dynamics_plugin"/>
  130 + </xacro:unless>
126 </robot> 131 </robot>