Commit bb89241c authored by Ricardo Rico Uribe's avatar Ricardo Rico Uribe
Browse files

fixed plugin - error in topic advertisements

parent 4120d958
...@@ -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
## Usage ## Usage
### modify wind and waves ### modify wind and waves
in the ocean.world.xacro you have found in /vrx_gazebo/worlds in the ocean.world.xacro (/vrx_gazebo/worlds) you have
```xml ```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"/>
...@@ -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
then launch the file you created then launch the file you created
```bash ```bash
roslaunch aquanaute_gazebo world.launch 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)
on another terminal (inside the ardupilot root) run
```bash
./Tools/autotest/sim_vehicle.py --vehicle=Rover --frame=sailboat-motor --map --console --location=Xlac
```
## Author ## Author
Ricardo RICO URIBE intern at U2IS in the summer of 2020 Ricardo RICO URIBE intern at U2IS in the summer of 2020
......
...@@ -5,7 +5,7 @@ ...@@ -5,7 +5,7 @@
<!-- send fake joint values --> <!-- send fake joint values -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" value="false"/> <param name="use_gui" value="true"/>
</node> </node>
<!-- Combine joint values --> <!-- Combine joint values -->
......
...@@ -128,24 +128,30 @@ ...@@ -128,24 +128,30 @@
<geometry> <geometry>
<mesh filename="package://aquanaute_description/models/meshes/Motor.stl"/> <mesh filename="package://aquanaute_description/models/meshes/Motor.stl"/>
</geometry> </geometry>
<material name="purple"/> <material name="black"/>
</visual> </visual>
</link> </link>
<link name="propeller_link"> <link name="propeller_link">
<inertial>
<origin xyz="-0.034 0 0" rpy="0 0 0"/>
<mass value="0.233"/>
<inertia ixx="1.441e-04" ixy="4.406e-09" ixz="0.00" iyy="1.760e-04" iyz="2.373e-06" izz="1.961e-04"/>
</inertial>
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<cylinder radius="0.05" length="0.04"/> <mesh filename="package://aquanaute_description/models/meshes/Propeller.stl"/>
</geometry> </geometry>
</collision> </collision>
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<cylinder radius="0.04" length="0.05"/> <mesh filename="package://aquanaute_description/models/meshes/Propeller.stl"/>
</geometry> </geometry>
<material name="black"/> <material name="blue"/>
</visual> </visual>
</link> </link>
...@@ -168,7 +174,7 @@ ...@@ -168,7 +174,7 @@
<geometry> <geometry>
<mesh filename="package://aquanaute_description/models/meshes/sensor_frame.stl"/> <mesh filename="package://aquanaute_description/models/meshes/sensor_frame.stl"/>
</geometry> </geometry>
<material name="cyan"/> <material name="white"/>
</visual> </visual>
</link> </link>
...@@ -206,15 +212,15 @@ ...@@ -206,15 +212,15 @@
<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"/>
</joint> </joint>
<joint name="propeller_motor_joint" type="fixed"> <joint name="propeller_motor_joint" type="revolute">
<parent link="motor_link"/> <parent link="motor_link"/>
<child link="propeller_link"/> <child link="propeller_link"/>
<origin xyz="-0.5 0 0.42" rpy="0 1.57 0"/> <origin xyz="-0.265 0 -0.42" rpy="0 0 0"/>
<axis xyz="0 0 1"/> <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"/>
</joint> </joint>
<joint name="sensor_frame_platform_joint" type="fixed"> <joint name="sensor_frame_platform_joint" type="fixed">
...@@ -226,6 +232,6 @@ ...@@ -226,6 +232,6 @@
<joint name="imu_platform_joint" type="fixed"> <joint name="imu_platform_joint" type="fixed">
<parent link="platform_link"/> <parent link="platform_link"/>
<child link="imu_link"/> <child link="imu_link"/>
<origin xyz="0 0 0.1" rpy="0 0 0"/> <origin xyz="0 0 0.02" rpy="0 0 0"/>
</joint> </joint>
</robot> </robot>
<?xml version="1.0"?> <?xml version="1.0"?>
<launch> <launch>
<env name="ROSCONSOLE_CONFIG_FILE" value="$(find vrx_gazebo)/config/custom_rosconsole.conf"/> <env name="ROSCONSOLE_CONFIG_FILE" value="$(find vrx_gazebo)/config/custom_rosconsole.conf"/>
<!-- Gazebo world to load -->
<arg name="world" default="$(find vrx_gazebo)/worlds/ocean.world" />
<!-- If true, run gazebo GUI --> <!-- If true, run gazebo GUI -->
<arg name="gui" default="true" /> <arg name="gui" default="true" />
<!-- If true, run gazebo in verbose mode --> <!-- If true, run gazebo in verbose mode -->
...@@ -14,10 +13,12 @@ ...@@ -14,10 +13,12 @@
<!-- Start in a default namespace --> <!-- Start in a default namespace -->
<arg name="namespace" default="aquanaute"/> <arg name="namespace" default="aquanaute"/>
<arg name="testing" default="false"/>
<!-- Initial USV location and attitude--> <!-- Initial USV location and attitude-->
<arg name="x" default="0" /> <arg name="x" default="0" />
<arg name="y" default="0" /> <arg name="y" default="0" />
<arg name="z" default="0.1" /> <arg name="z" default="0.5" if="$(arg testing)"/>
<arg name="z" default="0.1" unless="$(arg testing)"/>
<arg name="P" default="0" /> <arg name="P" default="0" />
<arg name="R" default="0" /> <arg name="R" default="0" />
<arg name="Y" default="0" /> <arg name="Y" default="0" />
...@@ -26,7 +27,8 @@ ...@@ -26,7 +27,8 @@
<!-- Start Gazebo with the world file --> <!-- Start Gazebo with the world file -->
<include file="$(find gazebo_ros)/launch/empty_world.launch"> <include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world)"/> <arg name="world_name" value="$(find aquanaute_gazebo)/worlds/test.world" if="$(arg testing)"/>
<arg name="world_name" value="$(find vrx_gazebo)/worlds/ocean.world" unless="$(arg testing)"/>
<arg name="verbose" value="$(arg verbose)"/> <arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/> <arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="true"/> <arg name="use_sim_time" value="true"/>
...@@ -37,7 +39,8 @@ ...@@ -37,7 +39,8 @@
<!-- Load robot model --> <!-- Load robot model -->
<arg name="urdf" default="$(find aquanaute_gazebo)/urdf/aquanaute_gazebo.urdf.xacro"/> <arg name="urdf" default="$(find aquanaute_gazebo)/urdf/aquanaute_gazebo.urdf.xacro"/>
<param name="$(arg namespace)/robot_description" command="$(find xacro)/xacro '$(arg urdf)'"/> <param name="$(arg namespace)/robot_description" command="$(find xacro)/xacro '$(arg urdf)'
testing:=$(arg testing)"/>
<!-- Spawn model in Gazebo, script depending on non_competition_mode --> <!-- Spawn model in Gazebo, script depending on non_competition_mode -->
<node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x) -y $(arg y) -z $(arg z) <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x) -y $(arg y) -z $(arg z)
......
<?xml version="1.0"?>
<launch>
<!-- Gazebo world to load -->
<arg name="world" default="$(find aquanaute_gazebo)/worlds/test.world" />
<!-- If true, run gazebo GUI -->
<arg name="gui" default="true" />
<!-- If true, run gazebo in verbose mode -->
<arg name="verbose" default="true"/>
<!-- If true, start in paused state -->
<arg name="paused" default="false"/>
<!-- Set various other gazebo arguments-->
<arg name="extra_gazebo_args" default=""/>
<!-- Start in a default namespace -->
<arg name="namespace" default="aquanaute"/>
<!-- Initial USV location and attitude-->
<arg name="x" default="0" />
<arg name="y" default="0" />
<arg name="z" default="0.5" />
<arg name="P" default="0" />
<arg name="R" default="0" />
<arg name="Y" default="0" />
<!-- Start Gazebo with the world file -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="$(arg gui)" />
<arg name="enable_ros_network" value="true"/>
</include>
<!-- Load robot model -->
<arg name="urdf" default="$(find aquanaute_gazebo)/urdf/aquanaute_gazebo.urdf.xacro"/>
<param name="$(arg namespace)/robot_description" command="$(find xacro)/xacro '$(arg urdf)'"/>
<!-- Spawn model in Gazebo, script depending on non_competition_mode -->
<node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x) -y $(arg y) -z $(arg z)
-R $(arg R) -P $(arg P) -Y $(arg Y)
-urdf -param $(arg namespace)/robot_description -model aquanaute"/>
</launch>
<?xml version="1.0"?> <?xml version="1.0"?>
<!-- Basic WAM-V with gazebo plugins for dynamics --> <!-- Basic WAM-V with gazebo plugins for dynamics -->
<robot xmlns:xacro="http://ros.org/wiki/xacro"> <robot xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Include macros for dynamics plugins <xacro:arg name="testing" default="false" />
<xacro:include filename="$(find aquanaute_gazebo)/urdf/macros.xacro" /> --> <!-- 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 --> <!-- Defines the base vehicle -->
<xacro:include filename="$(find aquanaute_description)/urdf/aquanaute_description.urdf.xacro"/> <xacro:include filename="$(find aquanaute_description)/urdf/aquanaute_description.urdf.xacro"/>
<!-- Defines the sim plugins--> <!--Gazebo References-->
<gazebo reference="base_link"> <gazebo reference="base_link">
<kp>100000.0</kp> <kp>100000.0</kp>
...@@ -49,11 +53,11 @@ ...@@ -49,11 +53,11 @@
</gazebo> </gazebo>
<gazebo reference="propeller_link"> <gazebo reference="propeller_link">
<kp>100000.0</kp> <kp>1.0</kp>
<kd>100000.0</kd> <kd>1.0</kd>
<mu1>10.0</mu1> <mu1>1.0</mu1>
<mu2>10.0</mu2> <mu2>1.0</mu2>
<material>Gazebo/Black</material> <material>Gazebo/Blue</material>
</gazebo> </gazebo>
<gazebo reference="sensor_frame_link"> <gazebo reference="sensor_frame_link">
...@@ -64,16 +68,15 @@ ...@@ -64,16 +68,15 @@
<material>Gazebo/White</material> <material>Gazebo/White</material>
</gazebo> </gazebo>
<gazebo> <gazebo reference="imu_link">
<link name="imu_link"> <sensor name="imu_sensor" type="imu">
<sensor name="imu_sensor" type="imu"> <pose>0 0 0 3.141593 0 0</pose>
<pose>0 0 0 3.141593 0 0</pose> <always_on>1</always_on>
<always_on>1</always_on> <update_rate>1000.0</update_rate>
<update_rate>1000.0</update_rate> </sensor>
</sensor> <material>Gazebo/Yellow</material>
</link> </gazebo>
</gazebo>
<!-- * * * GAZEBO Plugins * * * --> <!-- * * * GAZEBO Plugins * * * -->
<gazebo> <gazebo>
<plugin name="ardupilot_plugin" filename="libArduPilotPlugin.so"> <plugin name="ardupilot_plugin" filename="libArduPilotPlugin.so">
...@@ -104,23 +107,25 @@ ...@@ -104,23 +107,25 @@
<cmd_min>-1</cmd_min> <cmd_min>-1</cmd_min>
<jointName>aquanaute::motor_platform_joint</jointName> <jointName>aquanaute::motor_platform_joint</jointName>
</control> </control>
<control channel="1"> <control channel="1">
<multiplier>1</multiplier>
<offset>-0.5</offset>
<type>VELOCITY</type> <type>VELOCITY</type>
<offset>-0.5</offset>
<p_gain>1</p_gain> <p_gain>1</p_gain>
<i_gain>0</i_gain> <i_gain>0</i_gain>
<d_gain>0</d_gain> <d_gain>0</d_gain>
<i_max>0</i_max> <i_max>0</i_max>
<i_min>0</i_min> <i_min>0</i_min>
<cmd_max>1</cmd_max> <cmd_max>10</cmd_max>
<cmd_min>-1</cmd_min> <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 -->
<xacro:usv_dynamics_gazebo name="aquanaute_dynamics_plugin"/> --> <xacro:unless value="$(arg testing)">
<xacro:usv_dynamics_gazebo name="aquanaute_dynamics_plugin"/>
</xacro:unless>
</robot> </robot>
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment