Skip to content

Commit a3d1139

Browse files
authored
Merge pull request #44 from Field-Robotics-Lab/ASV2
Surface vehicle (ASV) kinematics code update and example launch
2 parents c036cad + cce711e commit a3d1139

12 files changed

+603
-42
lines changed
Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
<launch>
2+
<arg name="debug" default="0" doc="Sets the DEBUG flag for the uuv_simulator's plugins for verbose mode"/>
3+
4+
<arg name="x" default="0" doc="X coordinate of the initial position to spawn the model (in ENU)"/>
5+
<arg name="y" default="0" doc="Y coordinate of the initial position to spawn the model (in ENU)"/>
6+
<arg name="z" default="0" doc="Z coordinate of the initial position to spawn the model (in ENU)"/>
7+
<arg name="roll" default="0.0" doc="Roll angle of the initial orientation to spawn the model (in ENU)"/>
8+
<arg name="pitch" default="0.0" doc="Pitch angle of the initial orientation to spawn the model (in ENU)"/>
9+
<arg name="yaw" default="0.0" doc="Yaw angle of the initial orientation to spawn the model (in ENU)"/>
10+
11+
<arg name="use_geodetic" default="false"/>
12+
<arg name="latitude" default="0"/>
13+
<arg name="longitude" default="0"/>
14+
<arg name="depth" default="0"/>
15+
16+
<arg name="latitude_ref" default="0"/>
17+
<arg name="longitude_ref" default="0"/>
18+
<arg name="altitude_ref" default="0"/>
19+
20+
<arg name="use_ned_frame" default="true"/>
21+
22+
<!-- Mode to open different robot configurations as set the in file
23+
nomenclature standard for the files in /robots
24+
25+
/robots/<mode>.xacro
26+
-->
27+
<arg name="mode" default="default"/>
28+
29+
<!-- Vehicle's namespace -->
30+
<arg name="namespace" default="glider_asv"/>
31+
32+
<group ns="$(arg namespace)">
33+
<group if="$(arg use_ned_frame)">
34+
<param name="robot_description"
35+
command="$(find xacro)/xacro '$(find glider_hybrid_whoi_description)/robots/glider_asv_$(arg mode)_kinematics.urdf.xacro' --inorder
36+
debug:=$(arg debug)
37+
namespace:=$(arg namespace)
38+
inertial_reference_frame:=world_ned" />
39+
</group>
40+
41+
<group unless="$(arg use_ned_frame)">
42+
<param name="robot_description"
43+
command="$(find xacro)/xacro '$(find glider_hybrid_whoi_description)/robots/glider_asv_$(arg mode)_kinematics.urdf.xacro' --inorder
44+
debug:=$(arg debug)
45+
namespace:=$(arg namespace)
46+
inertial_reference_frame:=world" />
47+
</group>
48+
49+
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
50+
<group if="$(arg use_geodetic)">
51+
<node name="urdf_spawner" pkg="uuv_descriptions" type="spawn_model" respawn="false" output="screen"
52+
args="-urdf -latitude $(arg latitude) -longitude $(arg longitude) -depth $(arg depth) -latitude_ref $(arg latitude_ref) -longitude_ref $(arg longitude_ref) -altitude_ref $(arg altitude_ref) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw) -model $(arg namespace) -param /$(arg namespace)/robot_description"/>
53+
</group>
54+
55+
<group unless="$(arg use_geodetic)">
56+
<node name="urdf_spawner" pkg="uuv_descriptions" type="spawn_model" respawn="false" output="screen"
57+
args="-urdf -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw) -model $(arg namespace) -param /$(arg namespace)/robot_description"/>
58+
</group>
59+
60+
61+
<!-- A joint state publisher plugin already is started with the model, no need to use the default joint state publisher -->
62+
63+
<!-- Publish robot model for ROS -->
64+
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen">
65+
<param name="robot_description" value="/$(arg namespace)/robot_description" />
66+
</node>
67+
</group>
68+
69+
<!-- publish state and tf for in relation to the world frame -->
70+
<include file="$(find uuv_assistants)/launch/message_to_tf.launch">
71+
<arg name="namespace" value="$(arg namespace)"/>
72+
<arg name="world_frame" value="world"/>
73+
<arg name="child_frame_id" value="/$(arg namespace)/base_link"/>
74+
</include>
75+
76+
</launch>
Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
<?xml version="1.0" encoding="utf-8"?>
2+
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
3+
<asset>
4+
<contributor>
5+
<author>Blender User</author>
6+
<authoring_tool>Blender 2.83.3 commit date:2020-07-22, commit time:06:01, hash:353e5bd7493e</authoring_tool>
7+
</contributor>
8+
<created>2020-08-07T07:58:59</created>
9+
<modified>2020-08-07T07:58:59</modified>
10+
<unit name="meter" meter="1"/>
11+
<up_axis>Z_UP</up_axis>
12+
</asset>
13+
<library_effects/>
14+
<library_images/>
15+
<library_geometries>
16+
<geometry id="Cube-mesh" name="Cube">
17+
<mesh>
18+
<source id="Cube-mesh-positions">
19+
<float_array id="Cube-mesh-positions-array" count="72">-0.2254406 -1.525 -0.2748302 -0.3244532 -1.525 0.002065896 -0.2254406 1.525 -0.2748302 -0.3244532 1.525 0.002065896 0.2254406 -1.525 -0.2748302 0.3244532 -1.525 0.002065896 0.2254406 1.525 -0.2748302 0.3244532 1.525 0.002065896 -0.1855943 -0.1876686 1.153577 -0.1855943 -0.1876686 -0.05525326 -0.1855943 -0.2618966 1.153577 -0.1855943 -0.2618966 -0.05525326 -0.2327077 -0.1876686 1.153577 -0.2327077 -0.1876686 -0.05525326 -0.2327077 -0.2618966 1.153577 -0.2327077 -0.2618966 -0.05525326 0.2595886 -0.2066173 0.6515255 0.2595886 -0.2066173 -0.05525326 0.2595886 -0.2429478 0.6515255 0.2595886 -0.2429478 -0.05525326 0.2272799 -0.2066173 0.6515255 0.2272799 -0.2066173 -0.05525326 0.2272799 -0.2429478 0.6515255 0.2272799 -0.2429478 -0.05525326</float_array>
20+
<technique_common>
21+
<accessor source="#Cube-mesh-positions-array" count="24" stride="3">
22+
<param name="X" type="float"/>
23+
<param name="Y" type="float"/>
24+
<param name="Z" type="float"/>
25+
</accessor>
26+
</technique_common>
27+
</source>
28+
<source id="Cube-mesh-normals">
29+
<float_array id="Cube-mesh-normals-array" count="30">-0.9416115 0 -0.3367016 0 1 0 0.9416115 0 -0.3367016 0 -1 0 0 0 -1 0 0 1 -1 0 0 1 0 0 0 1 4.97591e-7 0 -1 4.97591e-7</float_array>
30+
<technique_common>
31+
<accessor source="#Cube-mesh-normals-array" count="10" stride="3">
32+
<param name="X" type="float"/>
33+
<param name="Y" type="float"/>
34+
<param name="Z" type="float"/>
35+
</accessor>
36+
</technique_common>
37+
</source>
38+
<source id="Cube-mesh-map-0">
39+
<float_array id="Cube-mesh-map-0-array" count="216">0.625 0 0.375 0.25 0.375 0 0.625 0.25 0.375 0.5 0.375 0.25 0.625 0.5 0.375 0.75 0.375 0.5 0.625 0.75 0.375 1 0.375 0.75 0.375 0.5 0.125 0.75 0.125 0.5 0.875 0.5 0.625 0.75 0.625 0.5 0.625 0 0.375 0.25 0.375 0 0.625 0.25 0.375 0.5 0.375 0.25 0.625 0.5 0.375 0.75 0.375 0.5 0.625 0.75 0.375 1 0.375 0.75 0.375 0.5 0.125 0.75 0.125 0.5 0.875 0.5 0.625 0.75 0.625 0.5 0.625 0 0.375 0.25 0.375 0 0.625 0.25 0.375 0.5 0.375 0.25 0.625 0.5 0.375 0.75 0.375 0.5 0.625 0.75 0.375 1 0.375 0.75 0.375 0.5 0.125 0.75 0.125 0.5 0.875 0.5 0.625 0.75 0.625 0.5 0.625 0 0.625 0.25 0.375 0.25 0.625 0.25 0.625 0.5 0.375 0.5 0.625 0.5 0.625 0.75 0.375 0.75 0.625 0.75 0.625 1 0.375 1 0.375 0.5 0.375 0.75 0.125 0.75 0.875 0.5 0.875 0.75 0.625 0.75 0.625 0 0.625 0.25 0.375 0.25 0.625 0.25 0.625 0.5 0.375 0.5 0.625 0.5 0.625 0.75 0.375 0.75 0.625 0.75 0.625 1 0.375 1 0.375 0.5 0.375 0.75 0.125 0.75 0.875 0.5 0.875 0.75 0.625 0.75 0.625 0 0.625 0.25 0.375 0.25 0.625 0.25 0.625 0.5 0.375 0.5 0.625 0.5 0.625 0.75 0.375 0.75 0.625 0.75 0.625 1 0.375 1 0.375 0.5 0.375 0.75 0.125 0.75 0.875 0.5 0.875 0.75 0.625 0.75</float_array>
40+
<technique_common>
41+
<accessor source="#Cube-mesh-map-0-array" count="108" stride="2">
42+
<param name="S" type="float"/>
43+
<param name="T" type="float"/>
44+
</accessor>
45+
</technique_common>
46+
</source>
47+
<vertices id="Cube-mesh-vertices">
48+
<input semantic="POSITION" source="#Cube-mesh-positions"/>
49+
</vertices>
50+
<triangles count="36">
51+
<input semantic="VERTEX" source="#Cube-mesh-vertices" offset="0"/>
52+
<input semantic="NORMAL" source="#Cube-mesh-normals" offset="1"/>
53+
<input semantic="TEXCOORD" source="#Cube-mesh-map-0" offset="2" set="0"/>
54+
<p>1 0 0 2 0 1 0 0 2 3 1 3 6 1 4 2 1 5 7 2 6 4 2 7 6 2 8 5 3 9 0 3 10 4 3 11 6 4 12 0 4 13 2 4 14 3 5 15 5 5 16 7 5 17 9 6 18 10 6 19 8 6 20 11 1 21 14 1 22 10 1 23 15 7 24 12 7 25 14 7 26 13 3 27 8 3 28 12 3 29 14 4 30 8 4 31 10 4 32 11 5 33 13 5 34 15 5 35 17 6 36 18 6 37 16 6 38 19 1 39 22 1 40 18 1 41 23 7 42 20 7 43 22 7 44 21 3 45 16 3 46 20 3 47 22 4 48 16 4 49 18 4 50 19 5 51 21 5 52 23 5 53 1 0 54 3 0 55 2 0 56 3 8 57 7 8 58 6 8 59 7 2 60 5 2 61 4 2 62 5 9 63 1 9 64 0 9 65 6 4 66 4 4 67 0 4 68 3 5 69 1 5 70 5 5 71 9 6 72 11 6 73 10 6 74 11 1 75 15 1 76 14 1 77 15 7 78 13 7 79 12 7 80 13 3 81 9 3 82 8 3 83 14 4 84 12 4 85 8 4 86 11 5 87 9 5 88 13 5 89 17 6 90 19 6 91 18 6 92 19 1 93 23 1 94 22 1 95 23 7 96 21 7 97 20 7 98 21 3 99 17 3 100 16 3 101 22 4 102 20 4 103 16 4 104 19 5 105 17 5 106 21 5 107</p>
55+
</triangles>
56+
</mesh>
57+
</geometry>
58+
</library_geometries>
59+
<library_visual_scenes>
60+
<visual_scene id="Scene" name="Scene">
61+
<node id="Collision" name="Collision" type="NODE">
62+
<matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1</matrix>
63+
<instance_geometry url="#Cube-mesh" name="Collision"/>
64+
</node>
65+
</visual_scene>
66+
</library_visual_scenes>
67+
<scene>
68+
<instance_visual_scene url="#Scene"/>
69+
</scene>
70+
</COLLADA>

glider_hybrid_whoi_description/mesh/glider_asv/Wave Glider-Float.dae

Lines changed: 115 additions & 0 deletions
Large diffs are not rendered by default.
Loading
Loading
Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
<?xml version="1.0"?>
2+
<!-- Copyright (c) 2016 The UUV Simulator Authors.
3+
All rights reserved.
4+
5+
Licensed under the Apache License, Version 2.0 (the "License");
6+
you may not use this file except in compliance with the License.
7+
You may obtain a copy of the License at
8+
9+
http://www.apache.org/licenses/LICENSE-2.0
10+
11+
Unless required by applicable law or agreed to in writing, software
12+
distributed under the License is distributed on an "AS IS" BASIS,
13+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
See the License for the specific language governing permissions and
15+
limitations under the License.
16+
-->
17+
<robot name="glider_asv" xmlns:xacro="http://www.ros.org/wiki/xacro" >
18+
<xacro:arg name="debug" default="0"/>
19+
<xacro:arg name="namespace" default="glider_asv"/>
20+
<xacro:arg name="inertial_reference_frame" default="world"/>
21+
<!-- Include the UUV macro file -->
22+
<xacro:include filename="$(find glider_hybrid_whoi_description)/urdf/glider_asv_base_kinematics.xacro"/>
23+
<!-- Create the glider_asv -->
24+
<xacro:glider_asv_base
25+
namespace="$(arg namespace)"
26+
debug="$(arg debug)"
27+
inertial_reference_frame="$(arg inertial_reference_frame)"/>
28+
29+
<!-- Joint state publisher plugin -->
30+
<xacro:default_joint_state_publisher namespace="$(arg namespace)" update_rate="50"/>
31+
32+
</robot>
Lines changed: 128 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,128 @@
1+
<?xml version="1.0"?>
2+
<!-- Copyright (c) 2016 The UUV Simulator Authors.
3+
All rights reserved.
4+
5+
Licensed under the Apache License, Version 2.0 (the "License");
6+
you may not use this file except in compliance with the License.
7+
You may obtain a copy of the License at
8+
9+
http://www.apache.org/licenses/LICENSE-2.0
10+
11+
Unless required by applicable law or agreed to in writing, software
12+
distributed under the License is distributed on an "AS IS" BASIS,
13+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
See the License for the specific language governing permissions and
15+
limitations under the License.
16+
-->
17+
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
18+
<!-- Import macro files -->
19+
<xacro:include filename="$(find uuv_descriptions)/urdf/common.urdf.xacro" />
20+
<xacro:include filename="$(find uuv_sensor_ros_plugins)/urdf/sensor_snippets.xacro"/>
21+
<xacro:include filename="$(find uuv_gazebo_ros_plugins)/urdf/snippets.xacro"/>
22+
23+
<!-- Properties -->
24+
<xacro:property name="mass" value="56.0"/>
25+
<xacro:property name="fluid_density" value="1024.0"/>
26+
<xacro:property name="gravity" value="9.81"/>
27+
<xacro:property name="cob" value="0 0 0"/>
28+
<xacro:property name="cog" value="0 0 0.0054"/>
29+
30+
<xacro:property name="visual_mesh_file" value="file://$(find glider_hybrid_whoi_description)/mesh/glider_asv/Wave Glider-Float.dae" />
31+
<xacro:property name="collision_mesh_file" value="file://$(find glider_hybrid_whoi_description)/mesh/glider_asv/COLLISION-Wave Glider-float.dae" />
32+
33+
<xacro:macro name="glider_asv_base" params="namespace debug inertial_reference_frame">
34+
<!-- Base_link of the model -->
35+
<link name="${namespace}/base_link">
36+
<visual>
37+
<origin xyz="0 0 0" rpy="0 0 ${pi/2}" />
38+
<geometry>
39+
<mesh filename="${visual_mesh_file}" scale="1 1 1" />
40+
</geometry>
41+
</visual>
42+
<collision>
43+
<origin xyz="0 0 0" rpy="0 0 ${pi/2}" />
44+
<geometry>
45+
<mesh filename="${collision_mesh_file}" scale="1 1 1" />
46+
</geometry>
47+
</collision>
48+
<inertial>
49+
<mass value="${mass}"/>
50+
<origin xyz="0 0 0" rpy="0 0 ${pi/2}" />
51+
<inertia ixx="0.3048" ixy="0" ixz="0" iyy="13.976" iyz="0" izz="13.976" />
52+
</inertial>
53+
</link>
54+
55+
<!-- Kinematics Plugin -->
56+
<gazebo>
57+
<plugin name="${namespace}_kinematics_plugin"
58+
filename="libkinematics_ros_plugin.so">
59+
<fluid_deisnty>${fluid_density}</fluid_deisnty>
60+
<writeLog>false</writeLog>
61+
<flow_velocity_topic>/hydrodynamics/current_velocity/${namespace}</flow_velocity_topic>
62+
<use_global_ocean_current>true</use_global_ocean_current>
63+
<espg_projection>26987</espg_projection>
64+
<!-- Kinematics coefficients -->
65+
<surface_vehicle>true</surface_vehicle>
66+
<f_pitch_battpos_cal_m>1.2565</f_pitch_battpos_cal_m>
67+
<f_pitch_battpos_cal_b>-0.055</f_pitch_battpos_cal_b>
68+
<Area>0.0345</Area>
69+
<C_D>0.2534</C_D>
70+
<C_L>0.4160</C_L>
71+
<f_thruster_voltage_v1>1.8953</f_thruster_voltage_v1>
72+
<f_thruster_voltage_v2>-1.995</f_thruster_voltage_v2>
73+
<f_thruster_voltage_v3>1.8701</f_thruster_voltage_v3>
74+
<f_thruster_power_w1>-0.020919</f_thruster_power_w1>
75+
<f_thruster_power_w2>1.4699</f_thruster_power_w2>
76+
<f_thruster_power_w3>0.97895</f_thruster_power_w3>
77+
</plugin>
78+
<plugin name="{namespace}_dave_transient_current_plugin" filename="libdave_transient_current_plugin.so">
79+
<flow_velocity_topic>hydrodynamics/current_velocity/${namespace}</flow_velocity_topic>
80+
<base_link_name>${namespace}/base_link</base_link_name>
81+
<transient_current>
82+
<topic_stratified_database>stratified_current_velocity_database</topic_stratified_database>
83+
<velocity_north>
84+
<noiseAmp>0.3</noiseAmp>
85+
<noiseFreq>0.0</noiseFreq>
86+
</velocity_north>
87+
<velocity_east>
88+
<noiseAmp>0.3</noiseAmp>
89+
<noiseFreq>0.0</noiseFreq>
90+
</velocity_east>
91+
<velocity_down>
92+
<noiseAmp>0.3</noiseAmp>
93+
<noiseFreq>0.0</noiseFreq>
94+
</velocity_down>
95+
</transient_current>
96+
<tide_oscillation>true</tide_oscillation>
97+
</plugin>
98+
</gazebo>
99+
100+
<gazebo>
101+
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
102+
<robotNamespace>/${namespace}</robotNamespace>
103+
<robotParam>/${namespace}/robot_description</robotParam>
104+
105+
<bathymetry>
106+
<interval_s>5.0</interval_s>
107+
</bathymetry>
108+
109+
</plugin>
110+
</gazebo>
111+
112+
<!-- TODO: Parametrize the battery unit -->
113+
<xacro:basic_linear_battery_macro
114+
namespace="${namespace}"
115+
parent_link="${namespace}/base_link"
116+
prefix=""
117+
open_circuit_voltage_constant_coef="3.7"
118+
open_circuit_voltage_linear_coef="-3.1424"
119+
initial_charge="1.1665"
120+
capacity="0.5"
121+
resistance="0.002"
122+
smooth_current_tau="1.9499"
123+
voltage="4.2"
124+
update_rate="2.0"/>
125+
126+
<xacro:include filename="$(find glider_hybrid_whoi_description)/urdf/glider_asv_sensors_kinematics.xacro"/>
127+
</xacro:macro>
128+
</robot>
Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
<?xml version="1.0"?>
2+
<!-- Copyright (c) 2016 The UUV Simulator Authors.
3+
All rights reserved.
4+
5+
Licensed under the Apache License, Version 2.0 (the "License");
6+
you may not use this file except in compliance with the License.
7+
You may obtain a copy of the License at
8+
9+
http://www.apache.org/licenses/LICENSE-2.0
10+
11+
Unless required by applicable law or agreed to in writing, software
12+
distributed under the License is distributed on an "AS IS" BASIS,
13+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
See the License for the specific language governing permissions and
15+
limitations under the License.
16+
-->
17+
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
18+
<!-- Mount a 3D pose sensor -->
19+
<xacro:default_pose_3d_macro
20+
namespace="${namespace}"
21+
parent_link="${namespace}/base_link"
22+
inertial_reference_frame="${inertial_reference_frame}" />
23+
24+
<!-- DVL -->
25+
<xacro:include filename="$(find nps_uw_sensors_gazebo)/urdf/whoi_teledyne_whn_beams.xacro"/>
26+
<xacro:teledyne_whn_macro
27+
name="dvl" namespace="dvl" xyz="0 0 0"
28+
dvl_topic="dvl" ranges_topic="ranges"
29+
robot_link="${namespace}/base_link" joint_xyz="0 0 0" scale="0.25 0.25 0.25" ray_visual="0"/>
30+
31+
<!-- Hector IMU -->
32+
<xacro:include filename="$(find glider_hybrid_whoi_description)/urdf/uw_hector_imu_plugin.xacro"/>
33+
34+
<xacro:switchable_battery_consumer_macro
35+
link_name="${namespace}/imu_link"
36+
battery_link="${namespace}/battery_link"
37+
battery_name="${namespace}/battery"
38+
power_load="20"
39+
topic="${namespace}/imu/state"/>
40+
41+
<!-- Hector GPS -->
42+
<xacro:include filename="$(find glider_hybrid_whoi_description)/urdf/uw_hector_gps_plugin.xacro"/>
43+
44+
45+
</robot>

glider_hybrid_whoi_description/urdf/glider_hybrid_whoi_base_kinematics.xacro

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -158,7 +158,7 @@
158158
<f_thruster_power_w2>1.4699</f_thruster_power_w2>
159159
<f_thruster_power_w3>0.97895</f_thruster_power_w3>
160160
</plugin>
161-
<plugin name="dave_transient_current_plugin" filename="libdave_transient_current_plugin.so">
161+
<plugin name="{namespace}_dave_transient_current_plugin" filename="libdave_transient_current_plugin.so">
162162
<flow_velocity_topic>hydrodynamics/current_velocity/${namespace}</flow_velocity_topic>
163163
<base_link_name>${namespace}/base_link</base_link_name>
164164
<transient_current>

0 commit comments

Comments
 (0)