caishi
/
XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add usv

This commit is contained in:
robin_shaun 2021-02-24 11:10:08 +08:00
parent 7ba9b57abe
commit c2053bc660
661 changed files with 207811 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
README.en.md
View File

@ -10,7 +10,7 @@ XTDrone is a UAV simulation platform based on PX4, ROS and Gazebo. XTDrone suppo
Architecture for single vehicle simulation is shown as the below figure. For more details, see the paper
Xiao, K., Tan, S., Wang, G., An, X., Wang, X., Wang, X.: Xtdrone: A customizable multi-rotor uavs simulation platform. arXiv preprint **[ arXiv:2003.09700](https://arxiv.org/abs/2003.09700)** (2020)
Xiao, K., Tan, S., Wang, G., An, X., Wang, X., Wang, X.: XTDrone: A Customizable Multi-rotor UAVs Simulation Platform. arXiv preprint **[ arXiv:2003.09700](https://arxiv.org/abs/2003.09700)** (2020)
<img src="./image/architecture_1.png" width="640" height="480" />
@ -24,49 +24,53 @@ Developers can quickly verify algorithms with XTDrone, such as:
1. Object Detection and Tracking
<img src="./image/human_tracking.gif" width="640" height="368" />
<img src="./image/human_tracking.gif" width="640" height="360" />
2. Stereo SLAM
<img src="./image/vslam.gif" width="640" height="368" />
<img src="./image/vslam.gif" width="640" height="360" />
3. RGBD-SLAM
<img src="./image/rgbdslam.gif" width="640" height="368" />
<img src="./image/rgbdslam.gif" width="640" height="360" />
4. 2D Laser SLAM
<img src="./image/laser_slam_2d.gif" width="640" height="368" />
<img src="./image/laser_slam_2d.gif" width="640" height="360" />
5. 3D Laser SLAM
<img src="./image/laser_slam_3d.gif" width="640" height="368"/>
<img src="./image/laser_slam_3d.gif" width="640" height="360"/>
6. VIO
<img src="./image/vio.gif" width="640" height="368" />
<img src="./image/vio.gif" width="640" height="360" />
7. Motion Planning
<img src="./image/motion_planning.gif" width="640" height="368" />
<img src="./image/motion_planning.gif" width="640" height="360" />
8. Formation
<img src="./image/formation_1.gif" width="640" height="368" />
<img src="./image/formation_1.gif" width="640" height="360" />
<img src="./image/formation_2.gif" width="640" height="368" />
<img src="./image/formation_2.gif" width="640" height="360" />
9. Fixed wing
<img src="./image/planes.gif" width="640" height="368" />
<img src="./image/planes.gif" width="640" height="360" />
10. VTOLs
<img src="./image/vtols.gif" width="640" height="368" />
<img src="./image/vtols.gif" width="640" height="360" />
11. Self driving
11. UGV
<img src="./image/self_driving.gif" width="640" height="368" />
<img src="./image/ugv.gif" width="640" height="360" />
12. USV
<img src="./image/usv.gif" width="640" height="360" />
#### User manual
@ -77,7 +81,7 @@ Developers can quickly verify algorithms with XTDrone, such as:
- Founders: Kun Xiao, Shaochang Tan
- Adviser: Xiangke Wang
- Developers: Kun Xiao, Shaochang Tan, Guanzheng Wang, Lan Ma, Qipeng Wang, Ruoqiao Guan, Keyan Chen, Gao Chen
- Developers: Kun Xiao, Shaochang Tan, Guanzheng Wang, Lan Ma, Qipeng Wang, Ruoqiao Guan, Xinyu Hu, Keyan Chen, Gao Chen
#### Thanks to Contributers

34
README.md
View File

@ -9,7 +9,7 @@ XTDrone是基于PX4、ROS与Gazebo的无人机仿真平台。支持多旋翼飞
单机仿真架构如下图所示，详见论文
Xiao, K., Tan, S., Wang, G., An, X., Wang, X., Wang, X.: Xtdrone: A customizable multi-rotor uavs simulation platform. arXiv preprint **[ arXiv:2003.09700](https://arxiv.org/abs/2003.09700)** (2020)
Xiao, K., Tan, S., Wang, G., An, X., Wang, X., Wang, X.: XTDrone: A Customizable Multi-rotor UAVs Simulation Platform. arXiv preprint **[ arXiv:2003.09700](https://arxiv.org/abs/2003.09700)** (2020)
<img src="./image/architecture_1.png" width="640" height="480" />
@ -23,50 +23,54 @@ Xiao, K., Ma, L., Tan, S., Cong, Y., Wang, X.: Implementation of UAV Coordinatio
1. 目标检测与追踪
<img src="./image/human_tracking.gif" width="640" height="368" />
<img src="./image/human_tracking.gif" width="640" height="360" />
2. 双目SLAM
<img src="./image/vslam.gif" width="640" height="368" />
<img src="./image/vslam.gif" width="640" height="360" />
3. RGBD-SLAM
<img src="./image/rgbdslam.gif" width="640" height="368" />
<img src="./image/rgbdslam.gif" width="640" height="360" />
4. 2D激光SLAM
<img src="./image/laser_slam_2d.gif" width="640" height="368" />
<img src="./image/laser_slam_2d.gif" width="640" height="360" />
5. 3D激光SLAM
<img src="./image/laser_slam_3d.gif" width="640" height="368"/>
<img src="./image/laser_slam_3d.gif" width="640" height="360"/>
6. 视觉惯性导航
<img src="./image/vio.gif" width="640" height="368" />
<img src="./image/vio.gif" width="640" height="360" />
7. 运动规划
<img src="./image/motion_planning.gif" width="640" height="368" />
<img src="./image/motion_planning.gif" width="640" height="360" />
8. 多机协同
<img src="./image/formation_1.gif" width="640" height="368" />
<img src="./image/formation_1.gif" width="640" height="360" />
<img src="./image/formation_2.gif" width="640" height="368" />
<img src="./image/formation_2.gif" width="640" height="360" />
9. 固定翼
<img src="./image/planes.gif" width="640" height="368" />
<img src="./image/planes.gif" width="640" height="360" />
10. 复合翼
<img src="./image/vtols.gif" width="640" height="368" />
<img src="./image/vtols.gif" width="640" height="360" />
11. 自动驾驶
11. 无人车
<img src="./image/self_driving.gif" width="640" height="368" />
<img src="./image/ugv.gif" width="640" height="360" />
12. 无人船
<img src="./image/usv.gif" width="640" height="360" />
### 教程
@ -77,7 +81,7 @@ Xiao, K., Ma, L., Tan, S., Cong, Y., Wang, X.: Implementation of UAV Coordinatio
- 创立者：肖昆，谭劭昌
- 指导老师：王祥科
- 开发团队：肖昆，谭劭昌，王冠政，马澜，王齐鹏，管若乔，陈科研，陈皋
- 开发团队：肖昆，谭劭昌，王冠政，马澜，王齐鹏，管若乔，胡新雨，陈科研，陈皋
### 加入我们

2
communication/multi_vehicle_communication.sh
View File

@ -1,5 +1,5 @@
#!/bin/bash
iris_num=9
iris_num=6
typhoon_h480_num=0
solo_num=0
plane_num=0

10
communication/multirotor_communication.py
View File

@ -23,6 +23,7 @@ class Communication:
self.vehicle_type = vehicle_type
self.vehicle_id = vehicle_id
self.local_pose = None
self.yaw = 0
self.hover_flag = 0
self.target_motion = PositionTarget()
self.arm_state = False
@ -69,7 +70,7 @@ class Communication:
while not rospy.is_shutdown():
self.target_motion_pub.publish(self.target_motion)
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
if (self.flight_mode is "LAND") and (self.local_pose.position.z < 0.15):
if(self.disarm()):
self.flight_mode = "DISARMED"
@ -86,7 +87,8 @@ class Communication:
rate.sleep()
def local_pose_callback(self, msg):
self.local_pose = msg
self.local_pose = msg.pose
self.yaw = self.q2yaw(msg.pose.orientation)
def mavros_state_callback(self, msg):
self.mavros_state = msg.mode
@ -212,7 +214,7 @@ class Communication:
def hover(self):
self.coordinate_frame = 1
self.motion_type = 0
self.target_motion = self.construct_target(x=self.local_pose.pose.position.x,y=self.local_pose.pose.position.y,z=self.local_pose.pose.position.z)
self.target_motion = self.construct_target(x=self.local_pose.position.x,y=self.local_pose.position.y,z=self.local_pose.position.z,yaw=self.yaw)
def flight_mode_switch(self):
if self.flight_mode == 'HOVER':
@ -227,7 +229,7 @@ class Communication:
return False
def takeoff_detection(self):
if self.local_pose.pose.position.z > 0.3 and self.arm_state:
if self.local_pose.position.z > 0.3 and self.arm_state:
return True
else:
return False

8
communication/vtol_communication.py
View File

@ -22,6 +22,7 @@ class Communication:
self.vehicle_id = vehicle_id
self.imu = None
self.local_pose = None
self.yaw = 0
self.current_state = None
self.target_motion = PositionTarget()
self.global_target = None
@ -83,14 +84,15 @@ class Communication:
odom.pose.pose = response.pose
odom.twist.twist = response.twist
self.odom_groundtruth_pub.publish(odom)
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
if (self.flight_mode is "LAND") and (self.local_pose.position.z < 0.15):
if(self.disarm()):
self.flight_mode = "DISARMED"
rate.sleep()
def local_pose_callback(self, msg):
self.local_pose = msg
self.local_pose = msg.pose
self.yaw = self.q2yaw(msg.pose.orientation)
def q2yaw(self, q):
if isinstance(q, Quaternion):
@ -248,7 +250,7 @@ class Communication:
def hover(self):
self.coordinate_frame = 1
self.motion_type = 0
self.target_motion = self.construct_target(x=self.local_pose.pose.position.x,y=self.local_pose.pose.position.y,z=self.local_pose.pose.position.z,yaw=self.current_heading)
self.target_motion = self.construct_target(x=self.local_pose.position.x,y=self.local_pose.position.y,z=self.local_pose.position.z,yaw=self.current_heading,yaw=self.yaw)
def flight_mode_switch(self):
if self.flight_mode == 'HOVER':

0
image/self_driving.gif → image/ugv.gif
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 886 KiB

After

Width:  |  Height:  |  Size: 886 KiB

BIN
image/usv.gif Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 970 KiB

254
sitl_config/launch/sandisland.launch Normal file
View File

@ -0,0 +1,254 @@
<?xml version="1.0"?>
<launch>
<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/example_course.world" />
<!-- If true, run gazebo GUI -->
<arg name="gui" default="true" />
<!-- If true, run gazebo in verbose mode -->
<arg name="verbose" default="false"/>
<!-- 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="wamv"/>
<!-- Initial USV location and attitude-->
<arg name="x" default="158" />
<arg name="y" default="108" />
<arg name="z" default="0.1" />
<arg name="P" default="0" />
<arg name="R" default="0" />
<arg name="Y" default="-2.76" />
<!-- If true, show non-competition ROS topics (/gazebo/model_states, /vrx/debug/wind/direction, etc.)-->
<arg name="non_competition_mode" default="true"/>
<arg name="enable_ros_network" value="$(arg non_competition_mode)"/>
<env name="VRX_DEBUG" value="$(arg non_competition_mode)"/>
<env unless="$(arg non_competition_mode)" name="GAZEBO_MODEL_PATH" value="$(find vrx_gazebo)/models:$(find wamv_gazebo)/models:$(find wamv_description)/models:$(optenv GAZEBO_MODEL_PATH)"/>
<!-- Allow user specified thruster configurations
H = stern trusters on each hull
T = H with a lateral thruster
X = "holonomic" configuration -->
<arg name="thrust_config" default="H" />
<!-- Do you want to enable sensors? -->
<arg name="camera_enabled" default="false" />
<arg name="gps_enabled" default="false" />
<arg name="imu_enabled" default="false" />
<arg name="lidar_enabled" default="false" />
<arg name="ground_truth_enabled" default="false" />
<!-- 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="$(arg enable_ros_network)"/>
<arg name="extra_gazebo_args" value="$(arg extra_gazebo_args)"/>
</include>
<!-- Load robot model -->
<arg name="urdf" default="$(find wamv_gazebo)/urdf/wamv_gazebo.urdf.xacro"/>
<param name="$(arg namespace)/robot_description"
command="$(find xacro)/xacro &#x002D;&#x002D;inorder '$(arg urdf)'
thruster_config:=$(arg thrust_config)
camera_enabled:=$(arg camera_enabled)
gps_enabled:=$(arg gps_enabled)
imu_enabled:=$(arg imu_enabled)
lidar_enabled:=$(arg lidar_enabled)
ground_truth_enabled:=$(arg ground_truth_enabled)
namespace:=$(arg namespace) "/>
<!-- Spawn model in Gazebo, script depending on non_competition_mode -->
<node name="spawn_model" pkg="gazebo_ros" type="spawn_model" if="$(arg non_competition_mode)"
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 wamv"/>
<node name="spawn_wamv" pkg="vrx_gazebo" type="spawn_wamv.bash" unless="$(arg non_competition_mode)"
args="-x $(arg x) -y $(arg y) -z $(arg z)
-R $(arg R) -P $(arg P) -Y $(arg Y)
--urdf $(arg urdf) --model wamv"/>
<arg name="est" default="ekf2"/>
<!-- iris_0 -->
<group ns="iris_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="0"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:24540@localhost:34580"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="170"/>
<arg name="y" value="111"/>
<arg name="z" value="2"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="3.14"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris"/>
<arg name="mavlink_udp_port" value="18570"/>
<arg name="mavlink_tcp_port" value="4560"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_1 -->
<group ns="iris_1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="1"/>
<arg name="ID_in_group" value="1"/>
<arg name="fcu_url" default="udp://:24541@localhost:34581"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="173"/>
<arg name="y" value="111"/>
<arg name="z" value="2"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="3.14"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris"/>
<arg name="mavlink_udp_port" value="18571"/>
<arg name="mavlink_tcp_port" value="4561"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_2 -->
<group ns="iris_2">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="2"/>
<arg name="ID_in_group" value="2"/>
<arg name="fcu_url" default="udp://:24542@localhost:34582"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="170"/>
<arg name="y" value="114"/>
<arg name="z" value="2"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="3.14"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris"/>
<arg name="mavlink_udp_port" value="18572"/>
<arg name="mavlink_tcp_port" value="4562"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_3 -->
<group ns="iris_3">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="3"/>
<arg name="ID_in_group" value="3"/>
<arg name="fcu_url" default="udp://:24543@localhost:34583"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="173"/>
<arg name="y" value="114"/>
<arg name="z" value="2"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="3.14"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris"/>
<arg name="mavlink_udp_port" value="18573"/>
<arg name="mavlink_tcp_port" value="4563"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_4 -->
<group ns="iris_4">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="4"/>
<arg name="ID_in_group" value="4"/>
<arg name="fcu_url" default="udp://:24544@localhost:34584"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="170"/>
<arg name="y" value="117"/>
<arg name="z" value="2"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="3.14"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris"/>
<arg name="mavlink_udp_port" value="18574"/>
<arg name="mavlink_tcp_port" value="4564"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- iris_5 -->
<group ns="iris_5">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="5"/>
<arg name="ID_in_group" value="5"/>
<arg name="fcu_url" default="udp://:24545@localhost:34585"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="173"/>
<arg name="y" value="117"/>
<arg name="z" value="2"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="3.14"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris"/>
<arg name="mavlink_udp_port" value="18575"/>
<arg name="mavlink_tcp_port" value="4565"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
</launch>

193
sitl_config/usv/Changelog.md Normal file
View File

@ -0,0 +1,193 @@
## VRX 1
1. New hydro params:
* [Pull request 214](https://github.com/osrf/vrx/pull/214)
### VRX 1.3.X
1. Adding camera optical frame:
* [Pull request 223](https://bitbucket.org/osrf/vrx/pull-requests/223)
### VRX 1.2.1
1. Ocean reflection and refraction visual effects:
* [Pull request 165](https://bitbucket.org/osrf/vrx/pull-requests/165)
### VRX 1.2.0
1. Deterministic wind plugin:
* [Pull request 157](https://bitbucket.org/osrf/vrx/pull-requests/157)
1. Waypoint marker visualization:
* [Pull request 162](https://bitbucket.org/osrf/vrx/pull-requests/162)
### VRX 1.1
1. GUI overlay plugin (prototype):
* [Pull request 116](https://bitbucket.org/osrf/vrx/pull-requests/116)
1. Randomized wind speed in the wind plugin:
* [Pull request 86](https://bitbucket.org/osrf/vrx/pull-requests/86)
1. Fix issue with ocean reflecting the laser beams:
* [Pull request 79](https://bitbucket.org/osrf/vrx/pull-requests/79)
1. Adding GPS antenna, collisions and inertia properties:
* [Pull request 90](https://bitbucket.org/osrf/vrx/pull-requests/90)
1. Add sand island basic mesh and texture:
* [Pull request 89](https://bitbucket.org/osrf/vrx/pull-requests/89)
1. Added ground station tents, tables and antenna:
* [Pull request 96](https://bitbucket.org/osrf/vrx/pull-requests/96)
1. Add batteries to WAM-V:
* [Pull request 95](https://bitbucket.org/osrf/vrx/pull-requests/95)
1. Add cpu cases to WAM-V:
* [Pull request 97](https://bitbucket.org/osrf/vrx/pull-requests/97)
1. Add 3d lidar mesh to WAM-V:
* [Pull request 98](https://bitbucket.org/osrf/vrx/pull-requests/98)
1. Support the ability to customize sensor location via YAML:
* [Pull request 99](https://bitbucket.org/osrf/vrx/pull-requests/99)
1. Add camera mesh to WAM-V:
* [Pull request 101](https://bitbucket.org/osrf/vrx/pull-requests/101)
1. Add varying lidar pole length:
* [Pull request 103](https://bitbucket.org/osrf/vrx/pull-requests/103)
1. Support the ability to customize WAM-V thruster and sensor location via YAML:
* [Pull request 104](https://bitbucket.org/osrf/vrx/pull-requests/104)
1. Add ability to playback simulations and add extra_gazebo_args argument to allow for recording:
* [Pull request 121](https://bitbucket.org/osrf/vrx/pull-requests/121)
1. Improved buoyancy model:
* [Pull request 122](https://bitbucket.org/osrf/vrx/pull-requests/122)
### VRX 1.0.1
1. Refactor Docker layout:
* [Pull request 76](https://bitbucket.org/osrf/vrx/pull-requests/76)
1. Convert WAM-V meshes to meters:
* [Pull request 75](https://bitbucket.org/osrf/vrx/pull-requests/75)
* [Pull request 74](https://bitbucket.org/osrf/vrx/pull-requests/74)
### VRX 1.0.0
1. Refactor Docker layout:
* [Pull request 73](https://bitbucket.org/osrf/vrx/pull-requests/73)
1. Transition to Gazebo 9 and ROS Melodic.
* [Pull request 70](https://bitbucket.org/osrf/vrx/pull-requests/70)
1. Scoring plugin for the perception task.
* [Pull request 72](https://bitbucket.org/osrf/vrx/pull-requests/72)
1. Scoring plugin for the wayfinding task.
* [Pull request 69](https://bitbucket.org/osrf/vrx/pull-requests/69)
1. VMRC renamed to VRX.
* [Pull request 68](https://bitbucket.org/osrf/vrx/pull-requests/68)
1. Scoring plugin for the station keeping task.
* [Pull request 67](https://bitbucket.org/osrf/vrx/pull-requests/67)
1. Scoring plugin for the scan and dock task.
* [Pull request 64](https://bitbucket.org/osrf/vrx/pull-requests/64)
* [Pull request 65](https://bitbucket.org/osrf/vrx/pull-requests/65)
* [Pull request 66](https://bitbucket.org/osrf/vrx/pull-requests/66)
1. Simplify URDF.
* [Pull request 62](https://bitbucket.org/osrf/vrx/pull-requests/62)
1. Generic scoring plugin.
* [Pull request 61](https://bitbucket.org/osrf/vrx/pull-requests/61)
1. Scoring plugins for the navigation course task.
* [Pull request 60](https://bitbucket.org/osrf/vrx/pull-requests/60)
1. Improved dynamics of WAM-V.
* [Pull request 59](https://bitbucket.org/osrf/vrx/pull-requests/59)
1. Obstacle course added.
* [Pull request 54](https://bitbucket.org/osrf/vrx/pull-requests/54)
## VMRC 0
### VMRC 0.3.X
1. robotx_gazebo ROS package renamed to vmrc_gazebo.
* [Pull request 44](https://bitbucket.org/osrf/vrx/pull-requests/49)
### VMRC 0.2.0
1. Add a standard WAM-V configuration for VMRC.
* [Pull request 44](https://bitbucket.org/osrf/vrx/pull-requests/44)
1. Add 3D laser to the WAM-V.
* [Pull request 41](https://bitbucket.org/osrf/vrx/pull-requests/41)
1. Add multiple WAM-V propulsion configurations.
* [Pull request 40](https://bitbucket.org/osrf/vrx/pull-requests/40)
1. Add placards and the ability to change shape and color.
* [Pull request 31](https://bitbucket.org/osrf/vrx/pull-requests/31)
1. Add a dock block and a templated mechanism (erb) for creating docks.
* [Pull request 27](https://bitbucket.org/osrf/vrx/pull-requests/27)
1. Add 2016 and 2018 acoustic transit / Entrance gate challenges.
* [Pull request 30](https://bitbucket.org/osrf/vrx/pull-requests/30)
1. Update the mesh of the light buoy.
* [Pull request 28](https://bitbucket.org/osrf/vrx/pull-requests/28)
1. Add colored totem buoys.
* [Pull request 26](https://bitbucket.org/osrf/vrx/pull-requests/26)
1. Improve colors, collisions and inertia properties of the 9504X0 buoys.
* [Pull request 25](https://bitbucket.org/osrf/vrx/pull-requests/25)
1. Simplify the collision model for Sandisland.
* [Pull request 22](https://bitbucket.org/osrf/vrx/pull-requests/22)
1. Various cleanups/refactors.
* [Pull request 3](https://bitbucket.org/osrf/vrx/pull-requests/3)
* [Pull request 9](https://bitbucket.org/osrf/vrx/pull-requests/9)
* [Pull request 14](https://bitbucket.org/osrf/vrx/pull-requests/14)
* [Pull request 17](https://bitbucket.org/osrf/vrx/pull-requests/17)
* [Pull request 18](https://bitbucket.org/osrf/vrx/pull-requests/18)
* [Pull request 21](https://bitbucket.org/osrf/vrx/pull-requests/21)
1. Create wamv_gazebo ROS package.
* [Pull request 4](https://bitbucket.org/osrf/vrx/pull-requests/4)
1. Add "Scan the code" challenge.
* [Pull request 6](https://bitbucket.org/osrf/vrx/pull-requests/6)
1. Fix message generation before building Thrust plugin.
* [Pull request 6](https://bitbucket.org/osrf/vrx/pull-requests/8)
1. Improve the WAM-V model.
* [Pull request 7](https://bitbucket.org/osrf/vrx/pull-requests/7)
1. Initial Docker support.
* [Pull request 5](https://bitbucket.org/osrf/vrx/pull-requests/5)
1. Refactor wind plugin.
* [Pull request 9](https://bitbucket.org/osrf/vrx/pull-requests/9)
1. Add buoys for the obstacle challenge.
* [Pull request 11](https://bitbucket.org/osrf/vrx/pull-requests/11)
1. Add xacro macros and example for a WAM-V with sensors.
* [Pull request 12](https://bitbucket.org/osrf/vrx/pull-requests/12)
1. Refactor thruster plugin to generalize propulsion configuration and enable more flesible use cases.
* [Pull request 34](https://bitbucket.org/osrf/vrx/pull-requests/34)

46
sitl_config/usv/README.md Normal file
View File

@ -0,0 +1,46 @@
# Virtual RobotX (VRX)
This repository is the home to the source code and software documentation for the VRX Simulation and the VRX Challenge. [Challenge documentation](https://github.com/osrf/vrx/wiki/documentation) is available on the project wiki, including links to registration information and documentation of the tasks and technical specifications.
This is an active development project. We are adding and improving things all the time. The project contains a simulation foundation, including an environment similar to the RobotX venue and description of the WAM-V platform. It is intended as a first step for teams that would then extend the tools for their specific development needs.
![VRX](images/sand_island.png)
![Ubuntu CI](https://github.com/osrf/vrx/workflows/Ubuntu%20CI/badge.svg)
## Getting Started
* Watch the [Release 1.2 Highlight Video](https://youtu.be/0Q2ehhmSz0Q)
* The [VRX Wiki](https://github.com/osrf/vrx/wiki) provides documentation and tutorials.
* The instructions assume a basic familiarity with the ROS environment and Gazebo. If these tools are new to you, we recommend starting with the excellent [ROS Tutorials](http://wiki.ros.org/ROS/Tutorials)
* For technical problems, please us the [project issue tracker](https://github.com/osrf/vrx/issues) to describe your problem or request support.
## Reference
If you use the VRX simulation in your work, please cite our summary publication, [Toward Maritime Robotic Simulation in Gazebo](https://wiki.nps.edu/display/BB/Publications?preview=/1173263776/1173263778/PID6131719.pdf):
```
@InProceedings{bingham19toward,
Title = {Toward Maritime Robotic Simulation in Gazebo},
Author = {Brian Bingham and Carlos Aguero and Michael McCarrin and Joseph Klamo and Joshua Malia and Kevin Allen and Tyler Lum and Marshall Rawson and Rumman Waqar},
Booktitle = {Proceedings of MTS/IEEE OCEANS Conference},
Year = {2019},
Address = {Seattle, WA},
Month = {October}
}
```
## Contributing
The simulation tools under active development to support the RobotX teams. We are starting simple with the important fundamental aspects of the robot and environment, but will rely on the community to develop additional functionality around their particular use cases.
If you have any questions about these topics, or would like to work on other aspects, please contribute. You can contact us directly (see below), submit an [issue](https://github.com/osrf/vrx/issues) or, better yet, submit a [pull request](https://github.com/osrf/vrx/pulls/)!
## Contributors
We continue to receive important improvements from the community. We have done our best to document this on our [Contributors Wiki](https://github.com/osrf/vrx/wiki/Contributors).
## Contacts
* Carlos Aguero <caguero@openrobotics.org>
* Brian Bingham <bbingham@nps.edu>

125
sitl_config/usv/docker/Dockerfile Normal file
View File

@ -0,0 +1,125 @@
# Ubuntu 18.04 with nvidia-docker2 beta opengl support.
ARG BASEIMG=ubuntu:bionic
FROM $BASEIMG
# Set ROS distribution
ARG DIST=melodic
# Set Gazebo verison
ARG GAZ=gazebo9
# Tools useful during development.
RUN apt update \
&& apt install -y \
build-essential \
cppcheck \
curl \
cmake \
lsb-release \
gdb \
git \
mercurial \
python3-dbg \
python3-pip \
python3-venv \
ruby \
software-properties-common \
sudo \
vim \
wget \
libeigen3-dev \
pkg-config \
protobuf-compiler \
&& apt clean
RUN export DEBIAN_FRONTEND=noninteractive \
&& apt update \
&& apt install -y \
tzdata \
&& ln -fs /usr/share/zoneinfo/America/Los_Angeles /etc/localtime \
&& dpkg-reconfigure --frontend noninteractive tzdata \
&& apt clean
# Get ROS melodic and Gazebo 9.
COPY docker/keys/gazebo.key /tmp/gazebo.key
COPY docker/keys/ros.key /tmp/ros.key
RUN /bin/sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' \
&& apt-key add /tmp/ros.key \
&& /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" > /etc/apt/sources.list.d/gazebo-stable.list' \
&& apt-key add /tmp/gazebo.key \
&& apt update \
&& apt install -y \
python-rosdep \
qtbase5-dev \
libgles2-mesa-dev \
${GAZ} \
lib${GAZ}-dev \
ros-${DIST}-ros-base \
ros-${DIST}-xacro \
ros-${DIST}-gazebo-ros \
ros-${DIST}-gazebo-plugins \
ros-${DIST}-hector-gazebo-plugins \
ros-${DIST}-joint-state-publisher \
ros-${DIST}-joy \
ros-${DIST}-joy-teleop \
ros-${DIST}-key-teleop \
ros-${DIST}-robot-localization \
ros-${DIST}-robot-state-publisher \
ros-${DIST}-rviz \
ros-${DIST}-teleop-tools \
ros-${DIST}-teleop-twist-keyboard \
ros-${DIST}-velodyne-simulator \
ros-${DIST}-rqt \
ros-${DIST}-rqt-common-plugins \
&& rosdep init \
&& apt clean
RUN rosdep update
# Set USER and GROUP
ARG USER=developer
ARG GROUP=developer
# Add a user with the same user_id as the user outside the container
# Requires a docker build argument `user_id`.
RUN curl -SsL https://github.com/boxboat/fixuid/releases/download/v0.4/fixuid-0.4-linux-amd64.tar.gz | tar -C /usr/local/bin -xzf - && \
chown root:root /usr/local/bin/fixuid && \
chmod 4755 /usr/local/bin/fixuid && \
mkdir -p /etc/fixuid && \
printf "user: $USER\ngroup: $GROUP\n" > /etc/fixuid/config.yml
RUN addgroup --gid 1000 $USER && \
adduser --uid 1000 --ingroup $USER --home /home/$USER --shell /bin/sh --disabled-password --gecos "" $USER
RUN adduser $USER sudo \
&& echo "$USER ALL=NOPASSWD: ALL" >> /etc/sudoers.d/$USER
# Commands below run as the developer user.
USER $USER:$GROUP
# When running a container start in the developer's home folder.
WORKDIR /home/$USER
# Create workspace
RUN mkdir -p vrx_ws/src/vrx
# Copy the VRX repository from the local file system
# We can't use the USER:GROUP variables until Docker adds support to --chown
COPY --chown=developer:developer . vrx_ws/src/vrx/
# Compile the VRX project.
RUN /bin/bash -c ". /opt/ros/${DIST}/setup.bash && cd vrx_ws && catkin_make"
# Source all the needed environment files.
RUN /bin/sh -c 'echo ". /opt/ros/${DIST}/setup.bash" >> ~/.bashrc' \
&& /bin/sh -c 'echo ". /usr/share/gazebo/setup.sh" >> ~/.bashrc' \
&& /bin/sh -c 'echo ". ~/vrx_ws/devel/setup.sh" >> ~/.bashrc'
ENTRYPOINT ["fixuid"]
CMD ["/bin/bash"]
# Customize your image here.
# ...

74
sitl_config/usv/docker/build.bash Executable file
View File

@ -0,0 +1,74 @@
#!/usr/bin/env bash
#
# Copyright (C) 2018 Open Source Robotics Foundation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
#
# Builds a Docker image.
BUILD_BASE=""
BUILD_ROS_GAZ=""
image_name="vrx"
POSITIONAL=()
while [[ $# -gt 0 ]]
do
key="$1"
case $key in
-n|--nvidia)
BUILD_BASE="--build-arg BASEIMG=nvidia/opengl:1.0-glvnd-devel-ubuntu18.04"
image_name="vrx_nvidia"
shift
;;
-k|--kinetic)
BUILD_BASE="--build-arg BASEIMG=ubuntu:xenial"
image_name="vrx_gaz7"
BUILD_ROS_GAZ="--build-arg DIST=kinetic --build-arg GAZ=gazebo7"
shift
;;
-K|--kinetic-nvidia)
BUILD_BASE="--build-arg BASEIMG=nvidia/opengl:1.1-glvnd-devel-ubuntu16.04"
image_name="vrx_nvidia_gaz7"
BUILD_ROS_GAZ="--build-arg DIST=kinetic --build-arg GAZ=gazebo7"
shift
;;
*) # unknown option
POSITIONAL+=("$1")
shift
;;
esac
done
set -- "${POSITIONAL[@]}"
if [ $# -lt 1 ]
then
echo "Usage: $0 [-n --nvidia] <root of vrx repo>"
exit 1
fi
if [ ! -f $1/docker/Dockerfile ]
then
echo "Err: Directory does not contain a Dockerfile to build."
exit 1
fi
image_plus_tag=$image_name:$(export LC_ALL=C; date +%Y_%m_%d_%H%M)
echo ".*" > "${1}"/.dockerignore
docker build --rm -t $image_plus_tag -f "${1}"/docker/Dockerfile "${1}" $BUILD_BASE $BUILD_ROS_GAZ && \
docker tag $image_plus_tag $image_name:latest && \
echo "Built $image_plus_tag and tagged as $image_name:latest"
rm "${1}"/.dockerignore

11
sitl_config/usv/docker/join.bash Executable file
View File

@ -0,0 +1,11 @@
#!/usr/bin/env bash
#
# Typical usage: ./join.bash vrx
#
IMG=$(basename $1)
xhost +
containerid=$(docker ps -aqf "ancestor=${IMG}")
docker exec --privileged -e DISPLAY=${DISPLAY} -e LINES=`tput lines` -it ${containerid} bash
xhost -

31
sitl_config/usv/docker/keys/gazebo.key Normal file
View File

@ -0,0 +1,31 @@
-----BEGIN PGP PUBLIC KEY BLOCK-----
Version: GnuPG v1.4.11 (GNU/Linux)
mQENBFUcKaEBCAD1ajXaWLnow3pZEv44Lypt6s5jAh1rYPN6zpaFZWdkzpwTdYU1
Rpw/0hPzIoiyOPNwCti4E3+dSrv1ogEBu85P2XSy67RnabxF4/z7mPG/++u0EQav
CwfrsN8OpJTtTxk+nKIhVwpAtob+KOLATerTPETrdrKh7qJ/FE8cw/XXbknjwywf
R8uJqaKTu7mWNrTFaS3P5GZF5ss+ztf0EHcyYFMvzEVnSiOGBBL9pw91P1qpggBa
lKL1Ilmf6zZBPihORJ/iTH5qMCAPDdR5BaxxEUHgz+pg+RkLKd2ENEaO+SCDVRhP
yNdkYHpuIslyMHfXrh4y5nHclJ+bNXKXDcudABEBAAG0R09TUkYgUmVwb3NpdG9y
eSAoT1NSRiBSZXBvc2l0b3J5IEdQRyBrZXkpIDxvc3JmYnVpbGRAb3NyZm91bmRh
dGlvbi5vcmc+iQE4BBMBAgAiBQJVHCmhAhsDBgsJCAcDAgYVCAIJCgsEFgIDAQIe
AQIXgAAKCRBnFwWYrySXQ/D4CACVnSdHT/1dEkOrYkCnaFLNBrG2tJdBrbIZOxKy
+xV0yGniqsQFAxLESoy+ygaiKdTnAFlA24ozoNY8ur+oKMFt6CrUY01ItTq/WMA1
iper0TO935SpDzNIPjPnD2WUSXShISWP0bFg64g0aAD1S7Yg/v7/eOmMSoeMav0T
h8KOo6yhJuhgGp3lHKAKLppH94b77d8JYqGeP03Gv6gcaqNojyKccdXrKTugZui5
+7V/cOJTo9XqzXjkpfwp24jR8FlKI7EWqCVqtRAXHeqRgo3OaKmuoKLcJ4/8BjSU
+ppmJtEstSaL+qw49P/GQHwUkCHlx1mV5dSdVFLBPreli1ChuQENBFUcKaEBCAC7
ZgTdYubw1sU/4A6+NvW/poBfh2DDOeh3uHJc0y235JFjr+tC1AwouaxLOUm8FE9k
7qzwnyXbeklmXAHxw6wXZdE4PEYA/sgBYhTQy+s4PHlI6TGhwgcROkJKlW4Lld+W
IJ/fzW93DXyhEkV3AAhkrVcOLOgCPdpK5EXxJ3p6dCOKC5Vjyz1PxTNcRaLpp9w6
J0hLIXmmoCN4aoYSXWtL/C9J+B5Cr+HHgrmFsGNrHmmVv1gMXLcVzw5p3Z4d8SuT
g9a1CemSE5bFIoOHKEQRwv/CGpoviAr+T3za3dPFTcSMOoJuYvoheTJ6fhf2sj74
bp2Fwi4L7am/asfa7xWVABEBAAGJAR8EGAECAAkFAlUcKaECGwwACgkQZxcFmK8k
l0OX9Af+IrzUChXf6H0nZZY77gcjwFgVChRX1RLzHTTHum4WNKGP9Sw1aGdHpmdt
LhypQImxdT2yhCPEyB8EQxhgPHjqZ6UUMeYMw5rAvrcb3/ercy5pG7O8Z+Bea6hu
TAXquJ1tsFessZwMS3RUXp/gtZCHbESR7PeBlZJWBWxG/lOmX7Z4fa88dWRU0Pl/
nfns7v6eb57HXbf0teCitRRsJwCMhYbHj2m1slZHMjhEc6kv2bgPmAFb04bcyEAP
BAo3BKu2XUVqE1t7Q2EfsItL/0FpfDY6zGKM6NIi+C40CsRl4W0o6egUhiDqsMYX
9Su5aZdCoxMhzy5QxS3sXcpNAWH2gw==
=YM5F
-----END PGP PUBLIC KEY BLOCK-----

30
sitl_config/usv/docker/keys/ros.key Normal file
View File

@ -0,0 +1,30 @@
-----BEGIN PGP PUBLIC KEY BLOCK-----
Version: GnuPG v1
mQINBFzvJpYBEADY8l1YvO7iYW5gUESyzsTGnMvVUmlV3XarBaJz9bGRmgPXh7jc
VFrQhE0L/HV7LOfoLI9H2GWYyHBqN5ERBlcA8XxG3ZvX7t9nAZPQT2Xxe3GT3tro
u5oCR+SyHN9xPnUwDuqUSvJ2eqMYb9B/Hph3OmtjG30jSNq9kOF5bBTk1hOTGPH4
K/AY0jzT6OpHfXU6ytlFsI47ZKsnTUhipGsKucQ1CXlyirndZ3V3k70YaooZ55rG
aIoAWlx2H0J7sAHmqS29N9jV9mo135d+d+TdLBXI0PXtiHzE9IPaX+ctdSUrPnp+
TwR99lxglpIG6hLuvOMAaxiqFBB/Jf3XJ8OBakfS6nHrWH2WqQxRbiITl0irkQoz
pwNEF2Bv0+Jvs1UFEdVGz5a8xexQHst/RmKrtHLct3iOCvBNqoAQRbvWvBhPjO/p
V5cYeUljZ5wpHyFkaEViClaVWqa6PIsyLqmyjsruPCWlURLsQoQxABcL8bwxX7UT
hM6CtH6tGlYZ85RIzRifIm2oudzV5l+8oRgFr9yVcwyOFT6JCioqkwldW52P1pk/
/SnuexC6LYqqDuHUs5NnokzzpfS6QaWfTY5P5tz4KHJfsjDIktly3mKVfY0fSPVV
okdGpcUzvz2hq1fqjxB6MlB/1vtk0bImfcsoxBmF7H+4E9ZN1sX/tSb0KQARAQAB
tCZPcGVuIFJvYm90aWNzIDxpbmZvQG9zcmZvdW5kYXRpb24ub3JnPokCVAQTAQoA
PhYhBMHPbjHmut6IaLFytPQu1vurF8ZUBQJc7yaWAhsDBQkDwmcABQsJCAcCBhUK
CQgLAgQWAgMBAh4BAheAAAoJEPQu1vurF8ZUkhIP/RbZY1ErvCEUy8iLJm9aSpLQ
nDZl5xILOxyZlzpg+Ml5bb0EkQDr92foCgcvLeANKARNCaGLyNIWkuyDovPV0xZJ
rEy0kgBrDNb3++NmdI/+GA92pkedMXXioQvqdsxUagXAIB/sNGByJEhs37F05AnF
vZbjUhceq3xTlvAMcrBWrgB4NwBivZY6IgLvl/CRQpVYwANShIQdbvHvZSxRonWh
NXr6v/Wcf8rsp7g2VqJ2N2AcWT84aa9BLQ3Oe/SgrNx4QEhA1y7rc3oaqPVu5ZXO
K+4O14JrpbEZ3Xs9YEjrcOuEDEpYktA8qqUDTdFyZrxb9S6BquUKrA6jZgT913kj
J4e7YAZobC4rH0w4u0PrqDgYOkXA9Mo7L601/7ZaDJob80UcK+Z12ZSw73IgBix6
DiJVfXuWkk5PM2zsFn6UOQXUNlZlDAOj5NC01V0fJ8P0v6GO9YOSSQx0j5UtkUbR
fp/4W7uCPFvwAatWEHJhlM3sQNiMNStJFegr56xQu1a/cbJH7GdbseMhG/f0BaKQ
qXCI3ffB5y5AOLc9Hw7PYiTFQsuY1ePRhE+J9mejgWRZxkjAH/FlAubqXkDgterC
h+sLkzGf+my2IbsMCuc+3aeNMJ5Ej/vlXefCH/MpPWAHCqpQhe2DET/jRSaM53US
AHNx8kw4MPUkxExgI7Sd
=4Ofr
-----END PGP PUBLIC KEY BLOCK-----

107
sitl_config/usv/docker/run.bash Executable file
View File

@ -0,0 +1,107 @@
#!/usr/bin/env bash
#
# Copyright (C) 2018 Open Source Robotics Foundation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
#
# Runs a docker container with the image created by build.bash
# Requires:
# docker
# nvidia-docker
# an X server
# Recommended:
# A joystick mounted to /dev/input/js0 or /dev/input/js1
RUNTIME="runc"
POSITIONAL=()
while [[ $# -gt 0 ]]
do
key="$1"
case $key in
-n|--nvidia)
RUNTIME="nvidia"
shift
;;
*) # unknown option
POSITIONAL+=("$1")
shift
;;
esac
done
set -- "${POSITIONAL[@]}"
if [ $# -lt 1 ]
then
echo "Usage: $0 [-n --nvidia] <docker image> [<dir with workspace> ...]"
exit 1
fi
IMG=$1
ARGS=("$@")
WORKSPACES=("${ARGS[@]:1}")
# Make sure processes in the container can connect to the x server
# Necessary so gazebo can create a context for OpenGL rendering (even headless)
XAUTH=/tmp/.docker.xauth
if [ ! -f $XAUTH ]
then
xauth_list=$(xauth nlist :0 | sed -e 's/^..../ffff/')
if [ ! -z "$xauth_list" ]
then
echo $xauth_list | xauth -f $XAUTH nmerge -
else
touch $XAUTH
fi
chmod a+r $XAUTH
fi
VRX_PATH="$( cd "$( dirname "${BASH_SOURCE[0]}" )/../" >/dev/null 2>&1 && pwd )"
DOCKER_OPTS=
# Example: Bind mount a local repository on the host machine:
#DOCKER_OPTS="--mount type=bind,source=${VRX_PATH},target=/home/developer/vrx_ws/src/vrx"
# Share your vim settings.
# VIMRC=~/.vimrc
# if [ -f $VIMRC ]
# then
# DOCKER_OPTS="$DOCKER_OPTS -v $VIMRC:/home/developer/.vimrc:ro"
# fi
USERID=$(id -u)
GROUPID=$(id -g)
docker run -it \
-e DISPLAY \
-e QT_X11_NO_MITSHM=1 \
-e XAUTHORITY=$XAUTH \
-v "$XAUTH:$XAUTH" \
-v "/tmp/.X11-unix:/tmp/.X11-unix" \
-v "/etc/localtime:/etc/localtime:ro" \
-v "/dev/input:/dev/input" \
--privileged \
--rm \
--runtime=$RUNTIME \
--security-opt seccomp=unconfined \
-u $USERID:$GROUPID \
$DOCKER_OPTS \
$IMG

BIN
sitl_config/usv/images/sand_island.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 337 KiB

162
sitl_config/usv/tools/code_check.sh Executable file
View File

@ -0,0 +1,162 @@
#!/bin/sh
#
# Copyright (C) 2016 Open Source Robotics Foundation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Jenkins will pass -xml, in which case we want to generate XML output
xmlout=0
if test "$1" = "-xmldir" -a -n "$2"; then
xmlout=1
xmldir=$2
mkdir -p $xmldir
rm -rf $xmldir/*.xml
# Assuming that Jenkins called, the `build` directory is sibling to src dir
builddir=../build
else
# This is a heuristic guess; not everyone puts the `build` dir in the src dir
builddir=./build
fi
# Identify cppcheck version
CPPCHECK_VERSION=`cppcheck --version | sed -e 's@Cppcheck @@'`
CPPCHECK_LT_157=`echo "$CPPCHECK_VERSION 1.57" | \
awk '{if ($1 < $2) print 1; else print 0}'`
QUICK_CHECK=0
if test "$1" = "--quick"
then
QUICK_CHECK=1
QUICK_SOURCE=$2
hg_root=`hg root`
if [ "$?" -ne "0" ] ; then
echo This is not an hg repository
exit
fi
cd $hg_root
hg log -r $QUICK_SOURCE > /dev/null
if [ "$?" -ne "0" ] ; then
echo $QUICK_SOURCE is not a valid changeset hash
exit
fi
CHECK_FILES=""
while read line; do
for f in $line; do
CHECK_FILES="$CHECK_FILES `echo $f | grep '\.[ch][ch]*$' | grep -v '^deps'`"
done
done
CPPCHECK_FILES="$CHECK_FILES"
CPPLINT_FILES="$CHECK_FILES"
QUICK_TMP=`mktemp -t asdfXXXXXXXXXX`
else
CHECK_DIRS=" \
./usv_gazebo_plugins/src ./usv_gazebo_plugins/include \
./vrx_gazebo/src ./vrx_gazebo/include \
./wave_gazebo_plugins/src ./wave_gazebo_plugins/include \
"
if [ $CPPCHECK_LT_157 -eq 1 ]; then
# cppcheck is older than 1.57, so don't check header files (issue #907)
CPPCHECK_FILES=`find $CHECK_DIRS -name "*.cc"`
else
CPPCHECK_FILES=`find $CHECK_DIRS -name "*.cc" -o -name "*.hh"`
fi
CPPLINT_FILES=`\
find $CHECK_DIRS -name "*.cc" -o -name "*.hh" -o -name "*.c" -o -name "*.h"\
| grep -v -e NetUtils -e Helpers -e example/build`
fi
SUPPRESS=/tmp/cpp_check.suppress
touch $SUPPRESS
# The follow suppression is useful when checking for missing includes.
# It's disable for now because checking for missing includes is very
# time consuming. See CPPCHECK_CMD3.
#echo "missingIncludeSystem" >> $SUPPRESS
#cppcheck
CPPCHECK_BASE="cppcheck -q --suppressions-list=$SUPPRESS --inline-suppr"
if [ $CPPCHECK_LT_157 -eq 0 ]; then
# use --language argument if 1.57 or greater (issue #907)
CPPCHECK_BASE="$CPPCHECK_BASE --language=c++"
fi
CPPCHECK_INCLUDES="-I . -I $builddir -I test -I include"
CPPCHECK_RULES="-DIGNITION_TRANSPORT_VISIBLE"
CPPCHECK_CMD1A="-j 4 --enable=style,performance,portability"
CPPCHECK_CMD1B="$CPPCHECK_RULES $CPPCHECK_FILES"
CPPCHECK_CMD1="$CPPCHECK_CMD1A $CPPCHECK_CMD1B -I include"
CPPCHECK_CMD2="--enable=unusedFunction $CPPCHECK_FILES"
# Checking for missing includes is very time consuming. This is disabled
# for now
CPPCHECK_CMD3="-j 4 --enable=missingInclude $CPPCHECK_FILES"\
" $CPPCHECK_INCLUDES"
#CPPCHECK_CMD3=""
if [ $xmlout -eq 1 ]; then
# Performance, style, portability, and information
($CPPCHECK_BASE --xml $CPPCHECK_CMD1) 2> $xmldir/cppcheck.xml
# Check the configuration
($CPPCHECK_BASE --xml $CPPCHECK_CMD3) 2> $xmldir/cppcheck-configuration.xml
elif [ $QUICK_CHECK -eq 1 ]; then
for f in $CHECK_FILES; do
prefix=`basename $f | sed -e 's@\..*$@@'`
ext=`echo $f | sed -e 's@^.*\.@@'`
tmp2="$QUICK_TMP"."$ext"
tmp2base=`basename "$QUICK_TMP"`
hg cat -r $QUICK_SOURCE $hg_root/$f > $tmp2
# Fix suppressions for tmp files
sed -i -e "s@$f@$tmp2@" $SUPPRESS
# Skip cppcheck for header files if cppcheck is old
DO_CPPCHECK=0
if [ $ext = 'cc' ]; then
DO_CPPCHECK=1
elif [ $CPPCHECK_LT_157 -eq 0 ]; then
DO_CPPCHECK=1
fi
if [ $DO_CPPCHECK -eq 1 ]; then
$CPPCHECK_BASE $CPPCHECK_CMD1A $CPPCHECK_RULES $tmp2 2>&1 \
| sed -e "s@$tmp2@$f@g" \
| grep -v 'use --check-config for details' \
| grep -v 'Include file: .*not found'
fi
# Undo changes to suppression file
sed -i -e "s@$tmp2@$f@" $SUPPRESS
python $hg_root/tools/cpplint.py $tmp2 2>&1 \
| sed -e "s@$tmp2@$f@g" -e "s@$tmp2base@$prefix@g" \
| grep -v 'Total errors found: 0'
rm $tmp2
done
rm $QUICK_TMP
else
# Performance, style, portability, and information
$CPPCHECK_BASE $CPPCHECK_CMD1 2>&1
# Check the configuration
$CPPCHECK_BASE $CPPCHECK_CMD3 2>&1
fi
# cpplint
if [ $xmlout -eq 1 ]; then
(echo $CPPLINT_FILES | xargs python tools/cpplint.py 2>&1) \
| python tools/cpplint_to_cppcheckxml.py 2> $xmldir/cpplint.xml
elif [ $QUICK_CHECK -eq 0 ]; then
echo $CPPLINT_FILES | xargs python tools/cpplint.py 2>&1
fi

5622
sitl_config/usv/tools/cpplint.py vendored Normal file
View File

File diff suppressed because it is too large Load Diff

70
sitl_config/usv/tools/cpplint_to_cppcheckxml.py Normal file
View File

@ -0,0 +1,70 @@
#!/usr/bin/env python
#
# Copyright (C) 2016 Open Source Robotics Foundation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Convert output from Google's cpplint.py to the cppcheck XML format for
# consumption by the Jenkins cppcheck plugin.
# Reads from stdin and writes to stderr (to mimic cppcheck)
import sys
import re
import xml.sax.saxutils
def cpplint_score_to_cppcheck_severity(score):
# I'm making this up
if score == 1:
return 'style'
elif score == 2:
return 'style'
elif score == 3:
return 'warning'
elif score == 4:
return 'warning'
elif score == 5:
return 'error'
def parse():
# TODO: do this properly, using the xml module.
# Write header
sys.stderr.write('''<?xml version="1.0" encoding="UTF-8"?>\n''')
sys.stderr.write('''<results>\n''')
# Do line-by-line conversion
r = re.compile('([^:]*):([0-9]*): ([^\[]*)\[([^\]]*)\] \[([0-9]*)\].*')
for l in sys.stdin.readlines():
m = r.match(l.strip())
if not m:
continue
g = m.groups()
if len(g) != 5:
continue
fname, lineno, rawmsg, label, score = g
# Protect Jenkins from bad XML, which makes it barf
msg = xml.sax.saxutils.escape(rawmsg)
severity = cpplint_score_to_cppcheck_severity(int(score))
sys.stderr.write('''<error file="%s" line="%s" id="%s" severity="%s" msg="%s"/>\n'''%(fname, lineno, label, severity, msg))
# Write footer
sys.stderr.write('''</results>\n''')
if __name__ == '__main__':
parse()

258
sitl_config/usv/usv_gazebo_plugins/CHANGELOG.rst Normal file
View File

@ -0,0 +1,258 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package usv_gazebo_plugins
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.3.0 (2019-12-26)
------------------
* Make code_check happy.
* Mod to make use of maxCmd and update to .hgignore
* Contributors: Brian Bingham <briansbingham@gmail.com>, Carlos Aguero
1.2.6 (2019-10-04)
------------------
1.2.5 (2019-09-19)
------------------
* merging default into branch
* Changes for code checker
* Contributors: Brian Bingham <briansbingham@gmail.com>
1.2.4 (2019-09-12)
------------------
1.2.3 (2019-09-12)
------------------
* Minor maintenance updates.
* Fix style error.
* Adding a default value for the length_n plugin parameter
* Contributors: Brian Bingham <briansbingham@gmail.com>, Carlos Aguero, Carlos Aguero <caguero@osrfoundation.org>
1.2.2 (2019-09-06)
------------------
1.2.1 (2019-09-05)
------------------
* updated comments
* minor cleanup + env flag + disable z
* Namespace tweaks.
* Style changes.
* gazebo 7 bug fix
* removed from wamv
* gazebo 7 compatibility
* force vectors are correct; scaling added
* Contributors: Carlos Aguero, Rumman Waqar <rumman.waqar05@gmail.com>
1.2.0 (2019-08-19)
------------------
* Deterministic wind.
* Add v3d plugin - this publishes a vecotr based on the world frame velocity in Gazebo
Update gps configuration to add gazebo gps and v3d plugins to standard configuration
* Add plugin for ROS interface to gazebo GPS sensor.
* added cylinder placeholder
* incremental
* added plate and sphere models
* functional for cubes
* added force visual plugin
* Contributors: Carlos Aguero, Jonathan Wheare <jonathan.wheare@flinders.edu.au>, MarshallRawson, Rumman Waqar <rumman.waqar05@gmail.com>
1.1.2 (2019-07-10)
------------------
* usv_gazebo_wind_plugin.hh changes
* Contributors: Brian Bingham <briansbingham@gmail.com>, Carlos Agüero <cen.aguero@gmail.com>, Rumman Waqar <rumman.waqar05@gmail.com>
1.1.1 (2019-07-03)
------------------
* Reinterpret the wind 'gain' parameter. Set defaults to zero
* updated style for buoyancy plugin
* Contributors: Brian Bingham <briansbingham@gmail.com>, Rumman Waqar <rumman.waqar05@gmail.com>
1.1.0 (2019-07-01)
------------------
* Refactor SpinPropeller to be more clear and match style of RotateEngine
* Publish joint state for chassis engine joint to fix tf tree issue
* addressed Brian's comments
* Connecting wave model to buoyancy plugin
* buoyancy now uses wave height
* capitalized brief sentences
* gazebo 7 vector3[] operator lhv error fixed try 2
* gazebo 7 vector3[] operator lhv error fixed
* gazebo <= 8 fixes for Mass and AngularVel
* complex objects + xacro cleanup
* force applied in correct place
* centroid + volume complete
* moved shape volume into its own file and renamed classes
* tiny refactor
* polyhedron class finished and tests passed
* implemented polyhedron based cube + cylinder submerged volume and cov
* rotation bug fix
* basic volume for box, sphere and cylinder
* removed old volume data structure + created skeleton for volume calc
* removed old volume data structure + created skeleton for volume calc
* added move constructor for buoyancy object and remove copy constructor
* added links and buoyancy object to plugin
* minor cleanup
* added pose for buoyancy element
* parsed buoyancy shape now saved as unique_ptr
* cleaned up parsing
* updated buoyancy xacro + parsing
* Setup buoyancy test build system + test world
* merging with default - need to check wind
* added wind capabilities
* Removing gazebo::msg::Param references and cleaning up for gazebo version < 8 compatibility.
* Removed gazebo messaging, introduces redundancy in model.sdf for ocean. USV and buoyancy plugins only get wave parameters once instead of every update.
* Move link error message.
* Another Nitpick fix (== nullptr) => !
* Nitpick fix (== nullptr) => !
* Clarify required and optional parameters, and remove unused confusing default declarations
* Put required parameters together and make it obvious which are required
* Retune PID for engineJoint with lower P gain, for more realistic behavior
* Add <enableAngle> bool parameter that controls if angle is adjustable or not
* C++ Code style fixes
* Add documentation about maxAngle and angleTopic
* updated documentation
* Attempt to fix build issue with .GetAngle().Radian()
* Attempt to fix build issue with Position() -> GetAngle() for old gazebo version
* Implement PID controller for engine joint to set joint angle
* changed sdf sytax for passing models to be effected by wind and addressed styling
* Attempt to fix build issues SetAngle->SetPosition
* Attempt to fix build issue with different setAngle setPosition implementation based on Gazebo version
* Implement turnable thruster joint
* Basic implementation of angle adjustable thrusters, still need to test, add joints, and change visuals
* merging with default
* fix build issue for gz <8
* merged. expanded xacro capabilities
* Rewrite implementation of setting windDirection
* documenting wind direction
* changing the interface from timePeriod to frequency
* cleaning up the includes order and white spaces
* cleanup
* adding ROS API to probe for wind speed
* enabling the user to input only the angle for wind direction
* increment
* documented
* incremental(basic testing passed)
* changed wind plugin(untested
* Initial testing of random seed with print statements
* Added wavegauge plugin to visualize physical wave height. Setup example with buoy world. Implemented simplified wave height calculation in WavefieldSampler for regularly spaced grid (steepness=1=0).
* verifying with examples
* toward buoy examples
* merging default into named branch
* removed currentVarVel from member variable list and fixed indentation for directives
* made gzmsg more efficient
* Implemented changed after PR is reviewed - V1
Remove Ros dependency (regarding time)
fixed typoes
fixed wrong comments
Exposed seed value to user
Updated purpose of SDF params in the header file
lines are now shorted than 80 chars
added comments around explaining the calculations done
* made wind speed randomized
* merging default to update the feature branch
* Remove more trailing whitespace
Redundant codepath in usv_gazwebo_dynamics_plugin removed. Euler values now derived identically between gazebo 7 and 9.
* Fix trailing whitespace
* Fix line breaks
* Alter patch to use .Ign method to convert between gazebo::math and Ignition::math types
* Add support for Kinetic/Gazebo-7
The ignition types are mostly kept, with code transforming from the methods deprecated in gazebo-8
* Changing license text
* Adding two packages from asv_wave_sim as a part of VRC
* Issue #23: Coordinate the physics and visualization of the wave field
1. Use the asv_wave_sim_gazebo_plugins package for wave field visualisation and depth calculation.
2. Update the buoyancy and dynamics plugins for buoyancy calculations.
3. Update sdf and xacro for models that require buoyancy.
4. Replace the ocean model with ocean_waves in the sandisland world.
* Contributors: Brian Bingham <briansbingham@gmail.com>, Carlos Aguero, Carlos Aguero <caguero@osrfoundation.org>, Carlos Agüero <cen.aguero@gmail.com>, Jonathan Wheare <jonathan.wheare@flinders.edu.au>, MarshallRawson, Rhys Mainwaring <rhys.mainwaring@me.com>, Rumman Waqar <rumman.waqar05@gmail.com>, Tyler Lum <tylergwlum@gmail.com>, Youssef Khaky <youssefkhaky@hotmail.com>, YoussefKhaky <youssefkhaky@hotmail.com.com>
1.0.1 (2019-03-01)
------------------
1.0.0 (2019-02-28)
------------------
* Porting to Gazebo 9
* Rename vmrc to vrx.
* More progress.
* Changed from buoyancy calculation method
* Fixing error where buoyancy force could be applied in the negative direction (downward)
* Add dependency on usv_msgs by usv_gazebo_pinger_plugin. This forces the message to be built before the plugin is compiled.
* Set the sensor WAM-V as the default model
* Fix the doxygen generation
* Update variable names and comments to be compliant with the Gazebo style guide.
* Add the pinger plugin to the wamv_gazebo package.
The wamv_gazebo_sensors.urdf file has been modified to add support for the pinger plugin.
* removing static tags so vessel is freee to move
* Contributors: Brian Bingham <briansbingham@gmail.com>, Carlos Aguero, Carlos Aguero <caguero@osrfoundation.org>, Jonathan Wheare <jonathan.wheare@flinders.edu.au>
0.3.2 (2018-10-08)
------------------
* Include jrivero as maintainer of the ROS packages
* Include headers in the installation of usv_gazebo_plugins
* Contributors: Jose Luis Rivero <jrivero@osrfoundation.org>
0.3.1 (2018-10-05)
------------------
* Decleare eigen as dependency for usv_gazebo_plugins
* modifying grid spacing
* Contributors: Brian Bingham <briansbingham@gmail.com>, Jose Luis Rivero <jrivero@osrfoundation.org>
0.3.0 (2018-09-28)
------------------
* vrx metapackage and spring cleaning.
* adding publication of forces/moments
* trying to get wamv to be static using a fixed joint
* Adding publication from dynamics plugin for wave height at USV CG for Josh's thesis work
* Tweak
* Changelog and minor tweaks.
* Remove extra dependency.
* Merged in generalize-thruster-desc (pull request #34)
Generalize thruster desc
Approved-by: Brian Bingham <briansbingham@gmail.com>
Approved-by: Carlos Agüero <cen.aguero@gmail.com>
* merging changes from PR branch into development branch
* resolving merge conflict
* Adding bits to repond to PR comments
* adding examples for T and X thruster configurations - accessible as args to sandisland.launch. Prototype - too much redundancy in the various urdf.xacro file hierarchy, but functional.
* Tweaks.
* Tabs -> spaces
* Initial style pass
* props now spinning, removed old method of thrust implementation, removed custome UsvDrive message
* working prototype - next remove old method
* prior to splitting thruster into its own header
* increment - builds, but need to go home
* catching up with default
* increment, pushing to work from home
* first steps towards new structure
* Drop log level to DEBUG for imformation unimportant to user
* Minor style changes in the gazebo_ros_color plugin.
* Tweak
* Move log message to DEBUG.
* adding a bit more doxygen, including link to Theory of Operation document
* Tweaks.
* adding doxygen comments
* Doxygen and cleaning up
* Rename buoyLinks to buoyancyLinks and remove debug output.
* More style.
* More tweaks.
* Initial style changes.
* Merge from default.
* Apply Gazebo style.
* Move some ROS_INFO messages to ROS_DEBUG and remove ros::init().
* More tweaks.
* Tweaks
* Tweaks
* Initial work
* Publish joint_states from thrust plugin
* Tweak
* Refactor wind plugin.
* Split the wamv xacro file.
* Generate messages before building the Thrust plugin.
* More modular model with spinning propellers.
* Merge from default
* Add message_generation.
* Backed out changeset 8023d94fc0e1
* Add light buoy challenge
* Remove unsused buoyancy plugin (already in gazebo)
* Boostrap usv_gazebo_plugins
* Move gazebo plugins to usv_gazebo_plugins
* Contributors: Brian Bingham <briansbingham@gmail.com>, Carlos Aguero, Carlos Agüero <caguero@osrfoundation.org>, Kevin Allen <kallen@osrfoundation.org>

129
sitl_config/usv/usv_gazebo_plugins/CMakeLists.txt Normal file
View File

@ -0,0 +1,129 @@
cmake_minimum_required(VERSION 2.8.3)
project(usv_gazebo_plugins)
# Set policy for CMake 3.1+. Use OLD policy to let FindBoost.cmake, dependency
# of gazebo, use quoted variables in if()
if(POLICY CMP0054)
cmake_policy(SET CMP0054 OLD)
endif()
find_package(catkin REQUIRED COMPONENTS gazebo_dev roscpp message_generation xacro wave_gazebo_plugins usv_msgs)
find_package(Eigen3 REQUIRED)
###################################
## catkin specific configuration ##
###################################
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS message_runtime gazebo_dev roscpp wave_gazebo_plugins
)
# Plugins require c++11
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
include_directories( include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
link_directories(
${catkin_LIBRARY_DIRS}
)
## Declare a C++ library
add_library(buoyancy_gazebo_plugin
src/buoyancy_gazebo_plugin.cc
src/shape_volume.cc
src/polyhedron_volume.cc
)
target_link_libraries(buoyancy_gazebo_plugin
${catkin_LIBRARIES}
${GAZEBO_LIBRARIES}
${Eigen_LIBRARIES}
)
install(TARGETS buoyancy_gazebo_plugin
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
## Declare a C++ library
add_library(usv_gazebo_dynamics_plugin
src/usv_gazebo_dynamics_plugin.cc
)
target_link_libraries(usv_gazebo_dynamics_plugin
${catkin_LIBRARIES}
${Eigen_LIBRARIES}
)
install(TARGETS usv_gazebo_dynamics_plugin
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
## Declare a C++ library
add_library(usv_gazebo_thrust_plugin
src/usv_gazebo_thrust_plugin.cc
)
target_link_libraries(usv_gazebo_thrust_plugin
${catkin_LIBRARIES}
${Eigen_LIBRARIES}
)
install(TARGETS usv_gazebo_thrust_plugin
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
## Declare a C++ library
add_library(usv_gazebo_wind_plugin
src/usv_gazebo_wind_plugin.cc
)
target_link_libraries(usv_gazebo_wind_plugin
${catkin_LIBRARIES}
${Eigen_LIBRARIES}
)
install(TARGETS usv_gazebo_wind_plugin
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
## Declare a C++ library
add_library(usv_gazebo_acoustic_pinger_plugin
src/acoustic_pinger_plugin.cc
)
add_dependencies(usv_gazebo_acoustic_pinger_plugin usv_msgs_generate_messages_cpp)
target_link_libraries(usv_gazebo_acoustic_pinger_plugin
${catkin_LIBRARIES}
${Eigen_LIBRARIES}
)
install(TARGETS usv_gazebo_acoustic_pinger_plugin
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
set(XACRO_INORDER)
if(DEFINED ENV{ROS_DISTRO})
if($ENV{ROS_DISTRO} STREQUAL "kinetic")
set(XACRO_INORDER INORDER)
endif()
endif()
# Generate demo world files from xacro and install
xacro_add_files(
worlds/buoyancy_plugin_demo.world.xacro
${XACRO_INORDER} INSTALL DESTINATION worlds
)
install(DIRECTORY worlds/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/worlds)
if(CATKIN_ENABLE_TESTING)
# buoyancy plugin test
catkin_add_gtest(buoyancy_plugin_test test/buoyancy_test.cc)
target_link_libraries(buoyancy_plugin_test buoyancy_gazebo_plugin)
endif()
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})

122
sitl_config/usv/usv_gazebo_plugins/include/usv_gazebo_plugins/acoustic_pinger_plugin.hh Normal file
View File

@ -0,0 +1,122 @@
/*
* Copyright (C) 2020 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef VRX_GAZEBO_ACOUSTIC_PINGER_PLUGIN_HH_
#define VRX_GAZEBO_ACOUSTIC_PINGER_PLUGIN_HH_
#include <geometry_msgs/Vector3.h>
#include <ros/ros.h>
#include <memory>
#include <mutex>
#include <string>
#include <gazebo/common/Events.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/common/Time.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/sensors/GaussianNoiseModel.hh>
#include <ignition/math/Vector3.hh>
namespace gazebo
{
/// \brief Implements a simulated range and bearing pinger localisation system
///
/// Implements a range and bearing pinger system. This assumes that the pinger
/// localisation has a mechanism for estimating the range and bearing
/// of the pinger. Pinger estimates are published using a custom message to the
/// ROS system along with a standard header. This should allow the tf library
/// to transform the sensor reading between frames.
///
/// Accepts the following SDF parameters:
/// <robotNamespace> - Set the namespace of the robot. Used to setup the ROS
/// nodehandle.
/// <frameId> - Tf frame of the sensor message. Used as part of the sensor
/// message publication.
/// <topicName> - Name of the topic that the sensor message will be published on
/// <setPositionTopicName> - Name of the topic that is used to set the position
/// of the simulated pinger sensor.
/// <position> - Position of the simulated pinger. Defaults to origin.
/// <updateRate> - Rate of simulated sensor messages.
/// <rangeNoise> - Noise model for the range to the simulated pinger.
/// <bearingNoise> - Noise model for the bearing to the simulated pinger.
/// <elevationNoise> - Noise model for the elevation to the simulated pinger.
class AcousticPinger : public ModelPlugin
{
/// \brief Constructor.
public: AcousticPinger();
/// \brief Destructor.
public: virtual ~AcousticPinger();
// Documentation inherited.
public: void Load(physics::ModelPtr _parent,
sdf::ElementPtr _sdf);
/// \brief Callback used by gazebo to update the plugin.
protected: virtual void Update();
/// \brief Callback function called when receiving a new pinger position
/// via the pinger subscription callback.
/// \param[in] _pos New pinger position.
public: void PingerPositionCallback(
const geometry_msgs::Vector3ConstPtr &_pos);
// ROS integration
/// \brief Nodehandle used to integrate with the ROS system.
private: std::unique_ptr<ros::NodeHandle> rosNodeHandle;
/// \brief Subscribes to the topic that set the pinger position.
public: ros::Subscriber setPositionSub;
/// \brief Publisher used to send sensor messages generated by the plugin.
private: ros::Publisher rangeBearingPub;
/// \brief Mutex to protect the position vector.
public: std::mutex mutex;
/// \brief Pointer to model object.
private: physics::ModelPtr model;
/// \brief Vector storing the position of the pinger.
private: ignition::math::Vector3d position;
// Variables that contain parameters of sensor simulation.
/// \brief String holding the frame id of the sensor.
private: std::string frameId;
/// \brief Sensor update rate.
private: float updateRate;
/// \brief Variable used to track time of last update. This is used to
/// produce data at the correct rate.
private: common::Time lastUpdateTime;
/// \brief Pointer used to connect gazebo callback to plugins update function.
private: event::ConnectionPtr updateConnection;
// From Brian Bingham's rangebearing_gazebo_plugin.
/// \brief rangeNoise - Gazebo noise object for range.
private: gazebo::sensors::NoisePtr rangeNoise = nullptr;
/// \brief Gazebo noise object for bearing angle.
private: gazebo::sensors::NoisePtr bearingNoise = nullptr;
/// \brief Gazebo noise object for elevation angle.
private: gazebo::sensors::NoisePtr elevationNoise = nullptr;
};
}
#endif

175
sitl_config/usv/usv_gazebo_plugins/include/usv_gazebo_plugins/buoyancy_gazebo_plugin.hh Normal file
View File

@ -0,0 +1,175 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef USV_GAZEBO_PLUGINS_BUOYANCY_GAZEBO_PLUGIN_HH_
#define USV_GAZEBO_PLUGINS_BUOYANCY_GAZEBO_PLUGIN_HH_
#include <map>
#include <string>
#include <vector>
#include <gazebo/common/common.hh>
#include <gazebo/common/Event.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Vector3.hh>
#include "usv_gazebo_plugins/shape_volume.hh"
#include "wave_gazebo_plugins/Wavefield.hh"
#include "wave_gazebo_plugins/WavefieldEntity.hh"
#include "wave_gazebo_plugins/WavefieldModelPlugin.hh"
namespace gazebo
{
namespace buoyancy
{
/// \brief A class for storing buoyancy object properties
class BuoyancyObject
{
/// \brief Default constructor
public: BuoyancyObject();
/// \brief Default move constructor
public: BuoyancyObject(BuoyancyObject&& obj) noexcept; // NOLINT
/// \brief No copy constructor
public: BuoyancyObject(BuoyancyObject& obj) = delete;
/// \brief Load buoyancy object from SDF
/// @param model model associated with buoyancy object
/// @param elem sdf for buoyancy element
public: void Load(const physics::ModelPtr model,
const sdf::ElementPtr elem);
/// \brief Display string for buoyancy object
public: std::string Disp();
/// \brief Associated link ID
public: int linkId;
/// \brief Associated link name
public: std::string linkName;
/// \brief Pose of buoyancy relative to link
public: ignition::math::Pose3d pose;
/// \brief Object mass (from inertial elem)
public:double mass;
/// \brief Buoyancy object's shape properties
public: ::buoyancy::ShapeVolumePtr shape;
};
} // end of buoyancy namespace
/// \brief This plugin simulates buoyancy of an object in fluid.
/// <wave_model>: Name of the wave model object (optional)
///
/// <fluid_density>: Sets the density of the fluid that surrounds the
/// buoyant object [kg/m^3].
/// This parameter is optional (default value 997 kg/m^3).
///
/// <fluid_level>: The height of the fluid/air interface [m].
/// This parameter is optional (default value 0 m).
///
/// <linear_drag>: Linear drag coefficent [N/(m/s)].
/// Translational drag implement as linear function
/// of velocity.
/// This parameter is optional.
///
/// <angular_drag>: Angular drag coefficent [(Nm)/(rad/s)].
/// Rotational drag implemented as linear function
/// of velocity.
/// This parameter is optional.
///
/// <buoyancy>: Describes the volume properties
/// For example:
///
/// <buoyancy name="buoyancy1">
/// <link_name>link</link_name>
/// <geometry>
/// ...
/// </geometry>
/// </buoyancy>
///
/// <link>: Name of associated link element
///
/// <geometry>: Geometry element specifying buoyancy object's
/// volume properties.
/// Supported shapes: box, sphere, cylinder
class BuoyancyPlugin : public ModelPlugin
{
/// \brief Constructor.
public: BuoyancyPlugin();
// Documentation inherited.
public: virtual void Load(physics::ModelPtr _model,
sdf::ElementPtr _sdf);
// Documentation inherited.
public: virtual void Init();
/// \brief Callback for World Update events.
protected: virtual void OnUpdate();
/// \brief Connection to World Update events.
protected: event::ConnectionPtr updateConnection;
/// \brief The density of the fluid in which the object is submerged in
/// kg/m^3. Defaults to 1000, the fluid density of water at 15 Celsius.
protected: double fluidDensity;
/// \brief The height of the fluid/air interface [m]. Defaults to 0.
protected: double fluidLevel;
/// \brief Linear drag coefficient. Defaults to 0.
protected: double linearDrag;
/// \brief Angular drag coefficient. Defaults to 0.
protected: double angularDrag;
/// \brief List of buoyancy objects for model
protected: std::vector<buoyancy::BuoyancyObject> buoyancyObjects;
/// \brief Map of <link ID, link pointer>
protected: std::map<int, gazebo::physics::LinkPtr> linkMap;
/// \brief Pointer to base model
protected: physics::ModelPtr model;
/// \brief Pointer to the Gazebo world
/// Retrieved when the model is loaded.
protected: physics::WorldPtr world;
/// \brief The name of the wave model
protected: std::string waveModelName;
/// \brief Map of water height at each link from previous timestep
protected: std::map<gazebo::physics::LinkPtr, double> linkHeights;
/// \brief Map of water velocity at each link
protected: std::map<gazebo::physics::LinkPtr, double> linkHeightDots;
/// \brief Previous update time
protected: double lastSimTime;
/// \brief The wave parameters.
protected: std::shared_ptr<const asv::WaveParameters> waveParams;
};
}
#endif

141
sitl_config/usv/usv_gazebo_plugins/include/usv_gazebo_plugins/polyhedron_volume.hh Normal file
View File

@ -0,0 +1,141 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#pragma once
#include <cassert>
#include <vector>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Quaternion.hh>
namespace buoyancy
{
/// \brief Represents a plane as a normal and offset
struct Plane
{
/// \brief Initializes plane at z=0
Plane();
/// \brief Vector3 normal to plane
ignition::math::Vector3d normal;
/// \brief Offset w.r.t. normal
float offset;
};
/// \brief Represents output volume with centroid
struct Volume
{
Volume();
/// \brief Overloads += for volume object
Volume& operator+=(const Volume& rhs);
/// \brief Submerged volume of shape
double volume;
/// \brief Vector3 representing volume centroid
ignition::math::Vector3d centroid;
};
/// \brief Submerged volume calculation using polyhedron
/// based on: Exact Buoyancy for Polyhedra by Eric Catto
class Polyhedron
{
/// \brief Store vertex index for a triangular face
public: struct Face
{
Face() = default;
Face(int i1, int i2, int i3);
/// \brief Index of vertices
int i1, i2, i3;
};
/// \brief Generate a cube polyhedron centered at origin
/// @param x: length of cube
/// @param y: width of cube
/// @param z: height of cube
/// @return Polyhedron object
public: static Polyhedron makeCube(double x,
double y,
double z);
/// \brief Generate a cylinder polyhedron centered at origin
/// @param r: radius of cylinder
/// @param l: length of cylinder
/// @param n: number of segments
/// @return Polyhedron object
public: static Polyhedron makeCylinder(double r,
double l,
int n);
/// \brief Compute full volume and center of buoyancy of the polyhedron
/// @return Volume object with volume and centroid
public: Volume ComputeFullVolume();
/// \brief Compute submerge volume and center of buoyancy of a polyhedron
/// @param x: our position
/// @param q: our orientation (quaternions)
/// @param plane: water surface defined as a plane
/// @return Volume object with volume and centroid (relative to world)
public: Volume SubmergedVolume(const ignition::math::Vector3d &x,
const ignition::math::Quaterniond &q,
Plane &plane);
/// \brief Computes volume and centroid of tetrahedron
/// tetrahedron formed by triangle + arbitrary point
/// @param v1: point on triangle
/// @param v2: point on triangle
/// @param v3: point on triangle
/// @param p: arbitrary point
/// @return Volume object with volume and centroid
private: static Volume tetrahedronVolume(const ignition::math::Vector3d& v1,
const ignition::math::Vector3d& v2,
const ignition::math::Vector3d& v3,
const ignition::math::Vector3d& p =
ignition::math::Vector3d({0., 0., 0.}));
/// \brief Clips a partially submerged triangle
/// @param v1: point on triangle
/// @param v2: point on triangle
/// @param v3: point on triangle
/// @param d1: distance of point v1 to the splitting plane
/// @param d2: distance of point v2 to the splitting plane
/// @param d3: distance of point v3 to the splitting plane
/// @return Volume object for clipped tetrahedron
private: static Volume clipTriangle(const ignition::math::Vector3d& v1,
const ignition::math::Vector3d& v2,
const ignition::math::Vector3d& v3,
double d1,
double d2,
double d3,
const ignition::math::Vector3d& p =
ignition::math::Vector3d({0., 0., 0.}));
/// \brief Object vertices
private: std::vector<ignition::math::Vector3d> vertices;
/// \brief Object faces
private: std::vector<Face> faces;
/// \brief Values below this are zeroed out
private: const double EPSILON = 1e-6;
}; // class Polyhedron
}

165
sitl_config/usv/usv_gazebo_plugins/include/usv_gazebo_plugins/shape_volume.hh Normal file
View File

@ -0,0 +1,165 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#pragma once
#include <cmath>
#include <exception>
#include <memory>
#include <string>
#include <ignition/math/Pose3.hh>
#include <sdf/sdf.hh>
#include "usv_gazebo_plugins/polyhedron_volume.hh"
namespace buoyancy
{
/// \brief Type of geometry shape
enum class ShapeType
{
None,
Box,
Sphere,
Cylinder
};
/// \brief Parent shape object for volume objects
struct ShapeVolume
{
/// \brief Default destructor
virtual ~ShapeVolume() = default;
/// \brief Factory method for shape. Parses a shape object from sdf data
/// \param sdf geometry SDF element
static std::unique_ptr<ShapeVolume> makeShape(const sdf::ElementPtr sdf);
/// \brief Display string for shape object
virtual std::string Display();
/// \brief Calculates volume + centroid of submerged shape
/// if the shape is out of water returns Volume{}
/// @param pose: world pose of volume
/// @param fluidLevel: height of fluid
/// @return volume object with volume + centroid (relative to world)
virtual Volume CalculateVolume(const ignition::math::Pose3d& pose,
double fluidLevel) = 0;
/// \brief Type of shape
ShapeType type;
/// \brief Full volume of object
double volume;
/// \brief Average length of object
/// estimate used for drag torque calculation
double averageLength;
};
typedef std::unique_ptr<ShapeVolume> ShapeVolumePtr;
/// \brief Box shape volume
struct BoxVolume : public ShapeVolume
{
/// \brief Default constructor
/// @param x: length
/// @param y: width
/// @param z: height
explicit BoxVolume(double x, double y, double z);
/// \brief Display string for box shape
std::string Display() override;
// Documentation inherited.
Volume CalculateVolume(const ignition::math::Pose3d& pose,
double fluidLevel) override;
/// \brief Length
double x;
/// \brief Width
double y;
/// \brief Height
double z;
private:
/// \brief Polyhedron defining a box
Polyhedron polyhedron;
};
/// \brief Cylinder shape volume
struct CylinderVolume : public ShapeVolume
{
/// \brief Default constructor
/// @param r: radius
/// @param l: length
explicit CylinderVolume(double r, double l);
/// \brief Display string for cylinder shape
std::string Display() override;
// Documentation inherited.
Volume CalculateVolume(const ignition::math::Pose3d& pose,
double fluidLevel) override;
/// \brief Radius of cylinder
double r;
/// \brief Height of cylinder
double h;
private:
/// \brief Polyhedron defining a cylinder
Polyhedron polyhedron;
};
/// \brief Sphere shape volume
struct SphereVolume : public ShapeVolume
{
/// \brief Default constructor
/// @param r: radius
explicit SphereVolume(double r);
/// \brief Display string for sphere shape
std::string Display() override;
// Documentation inherited.
Volume CalculateVolume(const ignition::math::Pose3d& pose,
double fluidLevel) override;
/// \brief Radius of sphere
double r;
};
/// \brief Custom exception for parsing errors
struct ParseException : public std::exception
{
ParseException(const char* shape, const char* message)
: output_("")
{
std::stringstream ss;
ss << "Parse error for <" << shape << ">: " << message;
// cppcheck-suppress useInitializationList
this->output_ = ss.str();
}
const char* what() const throw()
{
return this->output_.c_str();
}
private: std::string output_;
};
}

217
sitl_config/usv/usv_gazebo_plugins/include/usv_gazebo_plugins/usv_gazebo_dynamics_plugin.hh Normal file
View File

@ -0,0 +1,217 @@
/*
* Copyright (C) 2017 Brian Bingham
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef USV_GAZEBO_PLUGINS_DYNAMICS_PLUGIN_HH_
#define USV_GAZEBO_PLUGINS_DYNAMICS_PLUGIN_HH_
#include <Eigen/Core>
#include <string>
#include <vector>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>
#include <gazebo/physics/physics.hh>
#include <sdf/sdf.hh>
#include "wave_gazebo_plugins/Wavefield.hh"
#include "wave_gazebo_plugins/WavefieldEntity.hh"
#include "wave_gazebo_plugins/WavefieldModelPlugin.hh"
namespace gazebo
{
/// \brief Plugin class to implement hydrodynamics and wave response.
/// This plugin accepts the following SDF parameters:
///
/// <bodyName>: Name of base link for receiving pose and and applying forces.
/// <boatArea>: Horizontal surface area [m^2]. Default value is 0.48.
/// <boatLength>: Boat length [m]. Default value is 1.35.
/// <boatWidth>: Boat width [m]. Default value is 1.
/// <waterDensity>: Water density [kg/m^3]. Default value is 997.7735.
/// <waterLevel>: Water height [m]. Default value is 0.5.
/// <xDotU>: Added mass coeff, surge.
/// <yDotV>: Added mass coeff, sway.
/// <zDotW>: Added mass coeff, heave.
/// <kDotP>: Added mass coeff, roll.
/// <mDotQ>: Added mass coeff, pitch.
/// <nDotR>: Added mass coeff, yaw.
/// <xU>: Linear drag coeff surge.
/// <xUU>: Quadratic drag coeff surge.
/// <yV>: Linear drag coeff sway.
/// <yVV>: Quadratic drag coeff sway
/// <zW>: Linear drag coeff heave.
/// <zWW>: Quadratic drag coeff heave.
/// <kP>: Linear drag coeff roll.
/// <kPP>: Quadratic drag coeff roll.
/// <mQ>: Linear drag coeff pitch.
/// <mQQ>: Quadratic drag coeff pitch.
/// <nR>: Linear drag coeff yaw.
/// <nRR>: Quadratic drag coeff yaw.
/// <wave_n>: Number of waves to generate wave field.
/// <wave_amp<N>>: Amplitude for each component [m].
/// <wave_period<N>>: Period for each compenent [s].
/// <wave_direction<N>>: Direction of motion for each component ENU [rad].
class UsvDynamicsPlugin : public ModelPlugin
{
/// \brief Constructor.
public: UsvDynamicsPlugin();
/// \brief Destructor.
public: virtual ~UsvDynamicsPlugin() = default;
// Documentation inherited.
public: virtual void Load(physics::ModelPtr _model,
sdf::ElementPtr _sdf);
/// \brief Callback for Gazebo simulation engine.
protected: virtual void Update();
/// \brief Convenience function for getting SDF parameters.
/// \param[in] _sdfPtr Pointer to an SDF element to parse.
/// \param[in] _paramName The name of the element to parse.
/// \param[in] _defaultVal The default value returned if the element
/// does not exist.
/// \return The value parsed.
private: double SdfParamDouble(sdf::ElementPtr _sdfPtr,
const std::string &_paramName,
const double _defaultVal) const;
/// \brief Convenience function for calculating the area of circle segment
/// \param[in] R Radius of circle
/// \param[in] h Height of the chord line
/// \return The area
private: double CircleSegment(double R, double h);
/// \brief Pointer to the Gazebo world, retrieved when the model is loaded.
private: physics::WorldPtr world;
/// \brief Pointer to model link in gazebo,
/// optionally specified by the bodyName parameter.
/// The states are taken from this link and forces applied to this link.
private: physics::LinkPtr link;
/// \brief Simulation time of the last update.
private: common::Time prevUpdateTime;
/// \brief Linear velocity from previous time step,
/// for estimating acceleration.
private: ignition::math::Vector3d prevLinVel;
/// \brief Angular velocity from previous time step,
/// for estimating acceleration.
private: ignition::math::Vector3d prevAngVel;
/// \brief Values to set via Plugin Parameters.
/// Plugin Parameter: Added mass in surge, X_\dot{u}.
private: double paramXdotU;
/// \brief Plugin Parameter: Added mass in sway, Y_\dot{v}.
private: double paramYdotV;
/// \brief Plugin Parameter: Added mass in heave, Z_\dot{w}.
private: double paramZdotW;
/// \brief Plugin Parameter: Added mass in roll, K_\dot{p}.
private: double paramKdotP;
/// \brief Plugin Parameter: Added mass in pitch, M_\dot{q}.
private: double paramMdotQ;
/// \brief Plugin Parameter: Added mass in yaw, N_\dot{r}.
private: double paramNdotR;
/// \brief Plugin Parameter: Linear drag in surge.
private: double paramXu;
/// \brief Plugin Parameter: Quadratic drag in surge.
private: double paramXuu;
/// \brief Plugin Parameter: Linear drag in sway.
private: double paramYv;
/// \brief Plugin Parameter: Quadratic drag in sway.
private: double paramYvv;
/// \brief Plugin Parameter: Linear drag in heave.
private: double paramZw;
/// \brief Plugin Parameter: Quadratic drag in heave.
private: double paramZww;
/// \brief Plugin Parameter: Linear drag in roll.
private: double paramKp;
/// \brief Plugin Parameter: Quadratic drag in roll.
private: double paramKpp;
/// \brief Plugin Parameter: Linear drag in pitch.
private: double paramMq;
/// \brief Plugin Parameter: Quadratic drag in pitch.
private: double paramMqq;
/// \brief Plugin Parameter: Linear drag in yaw.
private: double paramNr;
/// \brief Plugin Parameter: Quadratic drag in yaw.
private: double paramNrr;
/// \brief Water height [m].
private: double waterLevel;
/// \brief Water density [kg/m^3].
private: double waterDensity;
/// \brief Vessel length [m].
private: double paramBoatLength;
/// \brief Vessel width [m].
private: double paramBoatWidth;
/// \brief Demi-hull radius [m].
private: double paramHullRadius;
/// \brief Length discretization, i.e., "N"
private: int paramLengthN;
/// \brief Added mass matrix, 6x6.
private: Eigen::MatrixXd Ma;
/// \brief The name of the wave model
protected: std::string waveModelName;
// /// \brief Wave parameters.
// private: int paramWaveN;
// /// \brief Wave amplitude values for N components.
// private: std::vector<float> paramWaveAmps;
// /// \brief Wave period values for N components.
// private: std::vector<float> paramWavePeriods;
// /// \brief Wave direction values for N components.
// private: std::vector<std::vector<float>> paramWaveDirections;
/// \brief Pointer to the update event connection.
private: event::ConnectionPtr updateConnection;
/// \brief The wave parameters.
private: std::shared_ptr<const asv::WaveParameters> waveParams;
};
}
#endif

281
sitl_config/usv/usv_gazebo_plugins/include/usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh Normal file
View File

@ -0,0 +1,281 @@
/*
* Copyright (C) 2017 Brian Bingham
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef USV_GAZEBO_PLUGINS_THRUST_HH
#define USV_GAZEBO_PLUGINS_THRUST_HH
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <sensor_msgs/JointState.h>
#include <memory>
#include <mutex>
#include <string>
#include <vector>
#include <gazebo/common/CommonTypes.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/common/Time.hh>
#include <gazebo/physics/physics.hh>
#include <sdf/sdf.hh>
namespace gazebo
{
// Foward declaration of UsvThrust class
class UsvThrust;
/// \brief Thruster class
class Thruster
{
/// \brief Constructor
/// \param[in] _parent Pointer to an SDF element to parse.
public: explicit Thruster(UsvThrust *_parent);
/// \brief Callback for new thrust commands.
/// \param[in] _msg The thrust message to process.
public: void OnThrustCmd(const std_msgs::Float32::ConstPtr &_msg);
/// \brief Callback for new thrust angle commands.
/// \param[in] _msg The thrust angle message to process.
public: void OnThrustAngle(const std_msgs::Float32::ConstPtr &_msg);
/// \brief Maximum abs val of incoming command.
public: double maxCmd;
/// \brief Max forward force in Newtons.
public: double maxForceFwd;
/// \brief Max reverse force in Newtons.
public: double maxForceRev;
/// \brief Maximum abs val of angle
public: double maxAngle;
/// \brief Link where thrust force is applied.
public: physics::LinkPtr link;
/// \brief Thruster mapping (0=linear; 1=GLF, nonlinear).
public: int mappingType;
/// \brief Topic name for incoming ROS thruster commands.
public: std::string cmdTopic;
/// \brief Subscription to thruster commands.
public: ros::Subscriber cmdSub;
/// \brief If true, thruster will have adjustable angle.
/// If false, thruster will have constant angle.
public: bool enableAngle;
/// \brief Topic name for incoming ROS thruster angle commands.
public: std::string angleTopic;
/// \brief Subscription to thruster commands.
public: ros::Subscriber angleSub;
/// \brief Current, most recent command.
public: double currCmd;
/// \brief Most recent desired angle.
public: double desiredAngle;
/// \brief Last time received a command via ROS topic.
public: common::Time lastCmdTime;
/// \brief Last time of update
public: common::Time lastAngleUpdateTime;
/// \brief Joint controlling the propeller.
public: physics::JointPtr propJoint;
/// \brief Joint controlling the engine.
public: physics::JointPtr engineJoint;
/// \brief PID for engine joint angle
public: common::PID engineJointPID;
/// \brief Plugin parent pointer - for accessing world, etc.
protected: UsvThrust *plugin;
};
/// \brief A plugin to simulate a propulsion system under water.
/// This plugin accepts the following SDF parameters.
/// See https://github.com/bsb808/robotx_docs/blob/master/theoryofoperation/theory_of_operation.pdf
/// for more information.
///
/// <robotNamespace>: Namespace prefix for USV.
/// <cmdTimeout>: Timeout, after which thrust is set to zero [s].
///
/// Multiple thrusters are declared by including <thruster> SDF elements,
/// where each thruster includes the following SDF parameters specific to
/// the individual thruster
/// Required elements:
/// <linkName>: Name of the link on which to apply thrust forces.
/// <propJointName>: The name of the propeller joint.
/// <engineJointName>: The name of the engine joint.
/// <cmdTopic>: The ROS topic to control this thruster,
/// typically within [-1.0 , 1.0]
/// <angleTopic>: The ROS topic to control the angle of this thruster,
/// will be clipped to stay within [-maxAngle, maxAngle]
/// <enableAngle>: If true, thruster will have adjustable angle.
/// If false, thruster will have constant angle.
/// Optional elements:
/// <mappingType>: Thruster mapping (0=linear; 1=GLF, nonlinear),
/// default is 0
/// <maxCmd>:Maximum (abs val) of thrust commands,
/// defualt is 1.0
/// <maxForceFwd>: Maximum forward force [N].
/// default is 250.0 N
/// <maxForceRev>: Maximum reverse force [N].
/// default is -100.0 N
/// <maxAngle>: Absolute value of maximum thruster angle [radians].
/// default is pi/2
///
/// Here is an example:
///
/// <plugin name="example" filename="libusv_gazebo_thrust_plugin.so">
/// <!-- General plugin parameters -->
/// <cmdTimeout>1.0</cmdTimeout>
///
/// <!-- Two thrusters -->
/// <thruster>
/// <linkName>left_propeller_link</linkName>
/// <propJointName>left_engine_propeller_joint</propJointName>
/// <engineJointName>left_chasis_engine_joint</engineJointName>
/// <cmdTopic>left_thrust_cmd</cmdTopic>
/// <angleTopic>left_thrust_angle</angleTopic>
/// <enableAngle>false</enableAngle>
/// <mappingType>1</mappingType>
/// <maxCmd>1.0</maxCmd>
/// <maxForceFwd>250.0</maxForceFwd>
/// <maxForceRev>-100.0</maxForceRev>
/// <maxAngle>1.57</maxAngle>
/// </thruster>
/// <thruster>
/// <linkName>right_propeller_link</linkName>
/// <propJointName>right_engine_propeller_joint</propJointName>
/// <engineJointName>right_chasis_engine_joint</engineJointName>
/// <cmdTopic>right_thrust_cmd</cmdTopic>
/// <angleTopic>right_thrust_angle</angleTopic>
/// <enableAngle>false</enableAngle>
/// <mappingType>1</mappingType>
/// <maxCmd>1.0</maxCmd>
/// <maxForceFwd>250.0</maxForceFwd>
/// <maxForceRev>-100.0</maxForceRev>
/// <maxAngle>1.57</maxAngle>
/// </thruster>
/// </plugin>
class UsvThrust : public ModelPlugin
{
/// \brief Constructor.
public: UsvThrust() = default;
/// \brief Destructor.
public: virtual ~UsvThrust() = default;
// Documentation inherited.
public: virtual void Load(physics::ModelPtr _parent,
sdf::ElementPtr _sdf);
/// \brief Callback executed at every physics update.
protected: virtual void Update();
/// \brief Convenience function for getting SDF parameters.
/// \param[in] _sdfPtr Pointer to an SDF element to parse.
/// \param[in] _paramName The name of the element to parse.
/// \param[in] _defaultVal The default value returned if the element
/// does not exist.
/// \return The value parsed.
private: double SdfParamDouble(sdf::ElementPtr _sdfPtr,
const std::string &_paramName,
const double _defaultVal) const;
/// \brief Takes ROS Drive commands and scales them by max thrust.
/// \param[in] _cmd ROS drive command.
/// \return Value scaled and saturated.
private: double ScaleThrustCmd(const double _cmd,
const double _max_cmd,
const double _max_pos,
const double _max_neg) const;
/// \brief Generalized logistic function (GLF) used for non-linear
/// thruster model.
/// \param[in] _x Independent variable (input) of GLF.
/// \param[in] _A Lower asymptote.
/// \param[in] _K Upper asymptote.
/// \param[in] _B Growth rate
/// \param[in] _v Affects near which asymptote max. growth occurs.
/// \param[in] _C Typically near 1.0.
/// \param[in] _M Offset to input.
/// \return
private: double Glf(const double _x,
const float _A,
const float _K,
const float _B,
const float _v,
const float _C,
const float _M) const;
/// \brief Uses GLF function to map thrust command to thruster force
/// in Newtons.
/// \param[in] _cmd Thrust command {-1.0,1.0}.
/// \return Thrust force [N].
private: double GlfThrustCmd(const double _cmd,
const double _maxPos,
const double _maxNeg) const;
/// \brief Rotate engine using engine joint PID
/// \param[in] _i Index of thruster whose engine will be rotated
/// \param[in] _stepTime common::Time since last rotation
private: void RotateEngine(size_t _i,
common::Time _stepTime);
/// \brief Spin a propeller based on its current command
/// \param[in] _i Index of thruster whose propeller will be spun
private: void SpinPropeller(size_t _i);
/// \brief A mutex to protect member variables accessed during
/// OnThustCmd() and Update().
public: std::mutex mutex;
/// \brief The ROS node handler used for communications.
private: std::unique_ptr<ros::NodeHandle> rosnode;
/// \brief Pointer to the Gazebo world, retrieved when the model is loaded.
public: physics::WorldPtr world;
/// \brief Pointer to Gazebo parent model, retrieved when the model is
/// loaded.
private: physics::ModelPtr model;
/// \brief Timeout for receiving Drive commands [s].
private: double cmdTimeout;
/// \brief Vector of thruster instances
private: std::vector<Thruster> thrusters;
/// \brief Pointer to the update event connection.
private: event::ConnectionPtr updateConnection;
/// \brief For publishing to /joint_state with propeller state.
private: ros::Publisher jointStatePub;
/// \brief The propeller message state.
private: sensor_msgs::JointState jointStateMsg;
};
}
#endif

163
sitl_config/usv/usv_gazebo_plugins/include/usv_gazebo_plugins/usv_gazebo_wind_plugin.hh Normal file
View File

@ -0,0 +1,163 @@
/*
* Copyright (C) 2017 Brian Bingham
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef USV_GAZEBO_PLUGINS_WIND_HH_
#define USV_GAZEBO_PLUGINS_WIND_HH_
#include <ros/ros.h>
#include <memory>
#include <random>
#include <string>
#include <vector>
#include <gazebo/common/CommonTypes.hh>
#include <gazebo/common/Plugin.hh>
#include <ignition/math/Vector3.hh>
#include <gazebo/physics/physics.hh>
#include <sdf/sdf.hh>
namespace gazebo
{
/// \brief A plugin that simulates a simple wind model. It accepts the
/// following parameters:
///
/// <wind_objs>: Block of objects (models) to be effected by the wind.
/// <wind_obj>: A wind object. NOTE: may include as many objects as you like
/// <name>: Name of the model (object) that will be effected by the wind
/// <link_name>: Link on that model which will feel the force of the wind
/// (limited to ONE per model).
/// <coeff_vector>: Coefficient vector of the particluar wind object.
///
/// <wind_direction>: Wind direction vector. Wind direction is specified as
/// the positive direction of the wind velocity vector in the horizontal plane
/// in degrees using the ENU coordinate convention
///
/// <wind_mean_velocity>: The wind average velocity.
///
/// <var_wind_gain_constants>: Variable wind speed gain constant.
///
/// <var_wind_time_constants>: Variable wind speed time constant.
///
/// <random_seed>: Set the seed for wind speed randomization.
///
/// <update_rate>: Publishing rate of the wind topic. If set to 0, it will not
/// publish, if set to a -1 it will publish every simulation iteration.
/// "Station-keeping control of an unmanned surface vehicle exposed to
/// current and wind disturbances".
///
/// <topic_wind_speed>: The debug topic to advertise the wind speed.
///
/// <topic_wind_direction>: The debug topic to advertise the wind direction.
class UsvWindPlugin : public WorldPlugin
{
struct WindObj
{
/// \Bool to show weather the model and link pointers have been set
// cppcheck-suppress unusedStructMember
bool init = false;
/// \name of model as it will be looked by in the world
std::string modelName;
/// \model Pointer to the model
physics::ModelPtr model;
/// \Name of the link on that model
std::string linkName;
/// \brief Pointer to model link in gazebo,
/// optionally specified by the bodyName parameter,
/// The states are taken from this link and forces applied to this link.
physics::LinkPtr link;
/// \brief Wind force coefficients.
ignition::math::Vector3d windCoeff;
};
/// \brief Constructor.
public: UsvWindPlugin();
/// \brief Destructor.
public: virtual ~UsvWindPlugin() = default;
// Documentation inherited.
public: virtual void Load(physics::WorldPtr _parent,
sdf::ElementPtr _sdf);
/// \brief Callback executed at every physics update.
protected: virtual void Update();
/// \brief vector of simple objects effected by the wind
private: std::vector<UsvWindPlugin::WindObj> windObjs;
/// \brief Bool to keep track if ALL of the windObjs have been initialized
private: bool windObjsInit = false;
/// \brief Bool to keep track if ALL of the windObjs have been initialized
private: unsigned int windObjsInitCount = 0;
/// \brief Pointer to the Gazebo world
private: physics::WorldPtr world;
/// \brief Wind velocity unit vector in Gazebo coordinates [m/s].
private: ignition::math::Vector3d windDirection;
/// \brief Average wind velocity.
private: double windMeanVelocity;
/// \brief User specified gain constant.
private: double gainConstant;
/// \brief Calculated filter gain constant.
private: double filterGain;
/// \brief Time constant.
private: double timeConstant;
/// \brief Previous time.
private: double previousTime;
/// \brief Variable velocity component.
private: double varVel;
/// \brief ROS node handle.
private: std::unique_ptr<ros::NodeHandle> rosNode;
/// \brief Publisher for wind speed.
private: ros::Publisher windSpeedPub;
/// \brief Publisher for wind direction.
private: ros::Publisher windDirectionPub;
/// \brief Topic where the wind speed is published.
private: std::string topicWindSpeed = "/vrx/debug/wind/speed";
/// \brief Topic where the wind direction is published.
private: std::string topicWindDirection = "/vrx/debug/wind/direction";
/// \brief Last time wind speed and direction was published.
private: double lastPublishTime = 0;
/// \brief Update rate buffer for wind speed and direction.
private: double updateRate;
/// \brief Pointer to the update event connection.
private: event::ConnectionPtr updateConnection;
/// \breif Bool debug set by environment var VRX_DEBUG
private: bool debug = true;
/// \def Random generator.
private: std::unique_ptr<std::mt19937> randGenerator;
};
}
#endif

18
sitl_config/usv/usv_gazebo_plugins/launch/buoyancy_plugin_demo.launch Normal file
View File

@ -0,0 +1,18 @@
<?xml version="1.0"?>
<launch>
<!-- Gazebo world to load -->
<arg name="world" default="$(find usv_gazebo_plugins)/worlds/buoyancy_plugin_demo.world" />
<!-- If true, run gazebo GUI -->
<arg name="gui" default="true" />
<!-- If true, run gazebo in verbose mode -->
<arg name="verbose" default="true"/>
<!-- 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="true"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="$(arg gui)" />
</include>
</launch>

26
sitl_config/usv/usv_gazebo_plugins/package.xml Normal file
View File

@ -0,0 +1,26 @@
<?xml version="1.0"?>
<package format="2">
<name>usv_gazebo_plugins</name>
<version>1.3.0</version>
<description>
Gazebo plugins for simulating Unmanned Surface Vehicles
Originaly copied from https://github.com/bsb808/usv_gazebo_plugins
</description>
<maintainer email="caguero@osrfoundation.org">Carlos Aguero</maintainer>
<maintainer email="jriveroo@osrfoundation.org">Jose Luis Rivero</maintainer>
<license>Apache 2.0</license>
<url type="website">http://wiki.ros.org/usv_gazebo_plugins</url>
<url type="bugtracker">http://github.com/osrf/vrx/issues</url>
<author email="briansbingham@gmail.com">Brian Bingham</author>
<author email="caguero@osrfoundation.org">Carlos Aguero</author>
<buildtool_depend>catkin</buildtool_depend>
<depend version_gte="2.5.17">gazebo_ros</depend>
<depend>gazebo_dev</depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
<depend>eigen</depend>
<depend>xacro</depend>
<depend>wave_gazebo_plugins</depend>
<depend>usv_msgs</depend>
<exec_depend>message_runtime</exec_depend>
</package>

313
sitl_config/usv/usv_gazebo_plugins/src/acoustic_pinger_plugin.cc Normal file
View File

@ -0,0 +1,313 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Open Source Robotics Foundation
* nor the names of its contributors may be
* used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/**
* \author Dave Coleman
* \desc Example ROS plugin for Gazebo
* \Original plugin from gazebo_plugins package
* \(https://github.com/ros-simulation/gazebo_ros_pkgs),
* \modified to implement a simulated pinger sensor for Maritime ASVs
* \Modifications by Jonathan Wheare.
*
*/
#include <usv_msgs/RangeBearing.h>
#include <cmath>
#include <ignition/math/Pose3.hh>
#include "usv_gazebo_plugins/acoustic_pinger_plugin.hh"
using namespace gazebo;
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(AcousticPinger);
//////////////////////////////////////////////////
AcousticPinger::AcousticPinger()
{
}
//////////////////////////////////////////////////
AcousticPinger::~AcousticPinger()
{
}
//////////////////////////////////////////////////
void AcousticPinger::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
// Store pointer to model for later use.
this->model = _parent;
// From gazebo_ros_color plugin.
GZ_ASSERT(_parent != nullptr, "Received NULL model pointer");
// Make sure the ROS node for Gazebo has already been initialised.
if (!ros::isInitialized())
{
ROS_FATAL_STREAM_NAMED("usv_gazebo_acoustic_pinger_plugin", "A ROS node for"
" Gazebo hasn't been initialised, unable to load plugin. Load the Gazebo "
"system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
// From gazebo_ros_color plugin
// Load namespace from SDF if available. Otherwise, use the model name.
std::string modelName = _parent->GetName();
auto delim = modelName.find(":");
if (delim != std::string::npos)
modelName = modelName.substr(0, delim);
// Initialise the namespace.
std::string ns = modelName;
if (_sdf->HasElement("robotNamespace"))
ns = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
{
ROS_DEBUG_NAMED("usv_gazebo_acoustic_pinger_plugin",
"missing <robotNamespace>, defaulting to %s", ns.c_str());
}
// Set the frame_id. Defaults to "pinger".
this->frameId = "pinger";
if (_sdf->HasElement("frameId"))
this->frameId = _sdf->GetElement("frameId")->Get<std::string>();
// Load topic from SDF if available.
std::string topicName = "/pinger/range_bearing";
if (_sdf->HasElement("topicName"))
topicName = _sdf->GetElement("topicName")->Get<std::string>();
else
{
ROS_INFO_NAMED("usv_gazebo_acoustic_pinger_plugin",
"missing <topicName>, defaulting to %s", topicName.c_str());
}
// Set the topic to be used to publish the sensor message.
std::string setPositionTopicName = "/pinger/set_pinger_position";
if (_sdf->HasElement("setPositionTopicName"))
{
setPositionTopicName =
_sdf->GetElement("setPositionTopicName")->Get<std::string>();
}
else
{
ROS_INFO_NAMED("usv_gazebo_acoustic_pinger_plugin",
"missing <setPositionTopicName>, defaulting to %s", topicName.c_str());
}
// Initialise pinger position. Defaults to origin.
this->position = ignition::math::Vector3d::Zero;
if (_sdf->HasElement("position"))
this->position = _sdf->Get<ignition::math::Vector3d>("position");
// Initialise update rate. Default to 1 reading per second.
this->updateRate = 1.0f;
if (_sdf->HasElement("updateRate"))
this->updateRate = _sdf->Get<float>("updateRate");
// From Brian Bingham's rangebearing_gazebo_plugin.
// Noise setup and parse SDF.
if (_sdf->HasElement("rangeNoise"))
{
sdf::ElementPtr rangeNoiseElem = _sdf->GetElement("rangeNoise");
// Note - it is hardcoded into the NoiseFactory.cc that the SDF
// element be "noise".
if (rangeNoiseElem->HasElement("noise"))
{
this->rangeNoise = sensors::NoiseFactory::NewNoiseModel(
rangeNoiseElem->GetElement("noise"));
}
else
{
ROS_WARN("usv_gazebo_acoustic_pinger_plugin: "
"The rangeNoise SDF element must contain noise tag");
}
}
else
{
ROS_INFO("usv_gazebo_acoustic_pinger_plugin: "
"No rangeNoise tag found, no noise added to measurements");
}
// Load the noise model from the SDF file.
if (_sdf->HasElement("bearingNoise"))
{
sdf::ElementPtr bearingNoiseElem = _sdf->GetElement("bearingNoise");
// Note - it is hardcoded into the NoiseFactory.cc that the SDF
// element be "noise".
if (bearingNoiseElem->HasElement("noise"))
{
this->bearingNoise = sensors::NoiseFactory::NewNoiseModel(
bearingNoiseElem->GetElement("noise"));
}
else
{
ROS_WARN("usv_gazebo_acoustic_pinger_plugin: "
"The bearingNoise SDF element must contain noise tag");
}
}
else
{
ROS_INFO("usv_gazebo_acoustic_pinger_plugin: "
"No bearingNoise tag found, no noise added to measurements");
}
if (_sdf->HasElement("elevationNoise"))
{
sdf::ElementPtr elevationNoiseElem = _sdf->GetElement("elevationNoise");
// Note - it is hardcoded into the NoiseFactory.cc that the SDF
// element be "noise".
if (elevationNoiseElem->HasElement("noise"))
{
this->elevationNoise = sensors::NoiseFactory::NewNoiseModel(
elevationNoiseElem->GetElement("noise"));
}
else
{
ROS_WARN("usv_gazebo_acoustic_pinger_plugin: "
"The elevationNoise SDF element must contain noise tag");
}
}
else
{
ROS_INFO("usv_gazebo_acoustic_pinger_plugin: "
"No elevationNoise tag found, no noise added to measurements");
}
// initialise the ros handle.
this->rosNodeHandle.reset(new ros::NodeHandle(ns));
// setup the publisher.
this->rangeBearingPub =
this->rosNodeHandle->advertise<usv_msgs::RangeBearing>(
std::string(topicName), 1);
this->setPositionSub = this->rosNodeHandle->subscribe(
setPositionTopicName, 1, &AcousticPinger::PingerPositionCallback, this);
// Initialise the time with world time.
#if GAZEBO_MAJOR_VERSION >= 8
this->lastUpdateTime = this->model->GetWorld()->SimTime();
#else
this->lastUpdateTime = this->model->GetWorld()->GetSimTime();
#endif
// connect the update function to the world update event.
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&AcousticPinger::Update, this));
}
//////////////////////////////////////////////////
void AcousticPinger::Update()
{
// Test to see if it's time to generate a sensor reading.
#if GAZEBO_MAJOR_VERSION >= 8
if ((this->model->GetWorld()->SimTime() - this->lastUpdateTime) >
(1.0f / this->updateRate))
#else
if ((this->model->GetWorld()->GetSimTime() - this->lastUpdateTime) >
(1.0f / this->updateRate))
#endif
{
// lock the thread to protect this->position vector.
std::lock_guard<std::mutex> lock(this->mutex);
#if GAZEBO_MAJOR_VERSION >= 8
this->lastUpdateTime = this->model->GetWorld()->SimTime();
// Find the pose of the model.
ignition::math::Pose3d modelPose = this->model->WorldPose();
#else
this->lastUpdateTime = this->model->GetWorld()->GetSimTime();
// Find the pose of the model.
ignition::math::Pose3d modelPose = this->model->GetWorldPose().Ign();
#endif
// Direction vector to the pinger from the USV.
ignition::math::Vector3d direction = this->position - modelPose.Pos();
// Sensor reading is in the sensor frame. Rotate the direction vector into
// the frame of reference of the sensor.
ignition::math::Vector3d directionSensorFrame =
modelPose.Rot().RotateVectorReverse(direction);
// Generate a 2d vector for elevation calculation.
ignition::math::Vector3d directionSensorFrame2d =
ignition::math::Vector3d(
directionSensorFrame.X(), directionSensorFrame.Y(), 0);
// bearing is calculated by finding the world frame direction vector
// and transforming into the pose of the sensor. Bearing is calculated
// using the atan2 function of the x and y components of the transformed
// vector. The elevation is calculated from the length of the 2D only
// and the z component of the sensor frame vector.
double bearing = atan2(directionSensorFrame.Y(), directionSensorFrame.X());
double range = directionSensorFrame.Length();
double elevation =
atan2(directionSensorFrame.Z(), directionSensorFrame2d.Length());
// Apply noise to each measurement.
// From Brian Binghams rangebearing_gazebo_plugin.
if (this->rangeNoise != nullptr)
range = this->rangeNoise->Apply(range);
if (this->bearingNoise != nullptr)
bearing = this->bearingNoise->Apply(bearing);
if (this->elevationNoise != nullptr)
elevation = this->elevationNoise->Apply(elevation);
// Publish a ROS message.
usv_msgs::RangeBearing msg;
// generate ROS header. Sequence number is automatically populated.
msg.header.stamp = ros::Time(this->lastUpdateTime.sec,
this->lastUpdateTime.nsec);
// frame_id is neccesary for finding the tf transform. The frame_id is
// specified in the sdf file.
msg.header.frame_id = this->frameId;
// Fill out the members of the message.
msg.range = range;
msg.bearing = bearing;
msg.elevation = elevation;
// publish range and bearing message.
this->rangeBearingPub.publish(msg);
}
}
//////////////////////////////////////////////////
void AcousticPinger::PingerPositionCallback(
const geometry_msgs::Vector3ConstPtr &_pos)
{
// Mutex added to prevent simulataneous reads and writes of mutex.
std::lock_guard<std::mutex> lock(this->mutex);
this->position = ignition::math::Vector3d(_pos->x, _pos->y, _pos->z);
}

316
sitl_config/usv/usv_gazebo_plugins/src/buoyancy_gazebo_plugin.cc Normal file
View File

@ -0,0 +1,316 @@
/*
* Copyright (C) 2015 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <functional>
#include <string>
#include <gazebo/common/Assert.hh>
#include <gazebo/common/Events.hh>
#include "usv_gazebo_plugins/buoyancy_gazebo_plugin.hh"
using namespace asv;
using namespace gazebo;
using namespace gazebo::buoyancy;
using namespace ::buoyancy;
//////////////////////////////////////////////////
BuoyancyObject::BuoyancyObject()
: linkId(-1),
linkName(""),
pose(0, 0, 0, 0, 0, 0),
mass(0.0),
shape(nullptr)
{
}
///////////////////////////////////////////////////
BuoyancyObject::BuoyancyObject(BuoyancyObject &&obj) noexcept // NOLINT
: linkId(obj.linkId),
linkName(obj.linkName),
pose(obj.pose),
mass(obj.mass),
shape(std::move(obj.shape))
{
}
///////////////////////////////////////////////////
void BuoyancyObject::Load(const physics::ModelPtr model,
const sdf::ElementPtr elem)
{
// parse link
if (elem->HasElement("link_name"))
{
this->linkName = elem->GetElement("link_name")->Get<std::string>();
physics::LinkPtr link = model->GetLink(linkName);
if (!link)
{
throw ParseException("link_name", "invalid link name");
}
this->linkId = link->GetId();
}
else
{
throw ParseException("link_name", "missing element");
}
// parse pose (optional)
if (elem->HasElement("pose"))
{
this->pose = elem->GetElement("pose")->Get<ignition::math::Pose3d>();
}
// parse geometry
if (elem->HasElement("geometry"))
{
sdf::ElementPtr geometry = elem->GetElement("geometry");
try
{
this->shape = std::move(ShapeVolume::makeShape(geometry));
}
catch (...)
{
throw;
}
}
else
{
throw ParseException("geometry", "missing element");
}
}
//////////////////////////////////////////////////
std::string BuoyancyObject::Disp() {
std::stringstream ss;
ss << "Buoyancy object\n"
<< "\tlink: " << linkName << "[" << linkId << "]\n"
<< "\tpose: " << pose << '\n'
<< "\tgeometry " << shape->Display() << '\n'
<< "\tmass " << mass;
return ss.str();
}
/////////////////////////////////////////////////
BuoyancyPlugin::BuoyancyPlugin()
: fluidDensity(997),
fluidLevel(0.0),
linearDrag(0.0),
angularDrag(0.0),
waveModelName(""),
lastSimTime(0.0)
{
}
/////////////////////////////////////////////////
void BuoyancyPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
GZ_ASSERT(_model != nullptr, "Received NULL model pointer");
GZ_ASSERT(_sdf != nullptr, "Received NULL SDF pointer");
// Capture the model and world pointers.
this->model = _model;
this->world = this->model->GetWorld();
// Capture the wave model
if (_sdf->HasElement("wave_model"))
{
this->waveModelName = _sdf->Get<std::string>("wave_model");
}
this->waveParams = nullptr;
if (_sdf->HasElement("fluid_density"))
{
this->fluidDensity = _sdf->Get<double>("fluid_density");
}
if (_sdf->HasElement("fluid_level"))
{
this->fluidLevel = _sdf->Get<double>("fluid_level");
}
if (_sdf->HasElement("linear_drag"))
{
this->linearDrag = _sdf->Get<double>("linear_drag");
}
if (_sdf->HasElement("angular_drag"))
{
this->angularDrag = _sdf->Get<double>("angular_drag");
}
if (_sdf->HasElement("buoyancy"))
{
gzmsg << "Found buoyancy element(s), looking at each element..."
<< std::endl;
for (sdf::ElementPtr buoyancyElem = _sdf->GetElement("buoyancy");
buoyancyElem;
buoyancyElem = buoyancyElem->GetNextElement("buoyancy")) {
try
{
BuoyancyObject buoyObj = BuoyancyObject();
buoyObj.Load(_model, buoyancyElem);
// add link to linkMap if it is not in the map
if (this->linkMap.find(buoyObj.linkId) == this->linkMap.end())
{
this->linkMap[buoyObj.linkId] = _model->GetLink(buoyObj.linkName);
// initialize link height
if (!this->waveModelName.empty())
{
this->linkHeights[linkMap[buoyObj.linkId]] = this->fluidLevel;
}
else
{
this->linkHeights[linkMap[buoyObj.linkId]] = 0;
}
// initialize link height velocity
this->linkHeightDots[linkMap[buoyObj.linkId]] = 0;
}
// get mass
#if GAZEBO_MAJOR_VERSION >= 8
buoyObj.mass = this->linkMap[buoyObj.linkId]->GetInertial()->Mass();
#else
buoyObj.mass =
this->linkMap[buoyObj.linkId]->GetInertial()->GetMass();
#endif
// add buoyancy object to list and display stats
gzmsg << buoyObj.Disp() << std::endl;
buoyancyObjects.push_back(std::move(buoyObj));
}
catch (const std::exception& e)
{
gzwarn << e.what() << std::endl;
}
}
}
// Initialize sim time memory
#if GAZEBO_MAJOR_VERSION >= 8
this->lastSimTime = this->world->SimTime().Double();
#else
this->lastSimTime = this->world->GetSimTime().Double();
#endif
}
/////////////////////////////////////////////////
void BuoyancyPlugin::Init()
{
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&BuoyancyPlugin::OnUpdate, this));
}
/////////////////////////////////////////////////
void BuoyancyPlugin::OnUpdate()
{
// update wave height if wave model specified
if (!this->waveModelName.empty())
{
// If we haven't yet, retrieve the wave parameters from ocean model plugin.
if (!this->waveParams)
{
gzmsg << "usv_gazebo_dynamics_plugin: waveParams is null. "
<< "Trying to get wave parameters from ocean model" << std::endl;
this->waveParams = WavefieldModelPlugin::GetWaveParams(
this->world, this->waveModelName);
}
#if GAZEBO_MAJOR_VERSION >= 8
double simTime = this->world->SimTime().Double();
#else
double simTime = this->world->GetSimTime().Double();
#endif
double dt = simTime - this->lastSimTime;
this->lastSimTime = simTime;
// get wave height for each link
for (auto& link : this->linkMap) {
auto linkPtr = link.second;
#if GAZEBO_MAJOR_VERSION >= 8
ignition::math::Pose3d linkFrame = linkPtr->WorldPose();
#else
ignition::math::Pose3d linkFrame = linkPtr->GetWorldPose().Ign();
#endif
// Compute the wave displacement at the centre of the link frame.
// Wave field height at the link, relative to the mean water level.
double waveHeight = WavefieldSampler::ComputeDepthSimply(
*waveParams, linkFrame.Pos(), simTime);
this->linkHeightDots[linkPtr] =
(waveHeight - this->linkHeights[linkPtr]) / dt;
this->linkHeights[linkPtr] = waveHeight;
}
}
for (auto& buoyancyObj : this->buoyancyObjects)
{
auto link = this->linkMap[buoyancyObj.linkId];
#if GAZEBO_MAJOR_VERSION >= 8
ignition::math::Pose3d linkFrame = link->WorldPose();
#else
ignition::math::Pose3d linkFrame = link->GetWorldPose().Ign();
#endif
linkFrame = linkFrame * buoyancyObj.pose;
auto submergedVolume = buoyancyObj.shape->CalculateVolume(linkFrame,
this->linkHeights[link] + this->fluidLevel);
GZ_ASSERT(submergedVolume.volume >= 0,
"Non-positive volume found in volume properties!");
// calculate buoyancy and drag forces
if (submergedVolume.volume > 1e-6)
{
// By Archimedes' principle,
// buoyancy = -(mass*gravity)*fluid_density/object_density
// object_density = mass/volume, so the mass term cancels.
ignition::math::Vector3d buoyancy = -this->fluidDensity
* submergedVolume.volume * model->GetWorld()->Gravity();
#if GAZEBO_MAJOR_VERSION >= 8
ignition::math::Vector3d linVel = link->WorldLinearVel();
ignition::math::Vector3d angVel = link->RelativeAngularVel();
#else
ignition::math::Vector3d linVel= link->GetWorldLinearVel().Ign();
ignition::math::Vector3d angVel = link->GetRelativeAngularVel().Ign();
#endif
// partial mass = total_mass * submerged_vol / total_vol
float partialMass = buoyancyObj.mass * submergedVolume.volume
/ buoyancyObj.shape->volume;
// drag (based on Exact Buoyancy for Polyhedra by Eric Catto)
// linear drag
ignition::math::Vector3d relVel =
ignition::math::Vector3d(0, 0, this->linkHeightDots[link]) - linVel;
ignition::math::Vector3d dragForce = linearDrag * partialMass * relVel;
buoyancy += dragForce;
if (buoyancy.Z() < 0.0)
{
buoyancy.Z() = 0.0;
}
// apply force
link->AddForceAtWorldPosition(buoyancy, submergedVolume.centroid);
// drag torque
double averageLength2 = ::pow(buoyancyObj.shape->averageLength, 2);
ignition::math::Vector3d dragTorque = (-partialMass * angularDrag
* averageLength2) * angVel;
link->AddRelativeTorque(dragTorque);
}
}
}
GZ_REGISTER_MODEL_PLUGIN(BuoyancyPlugin)

289
sitl_config/usv/usv_gazebo_plugins/src/polyhedron_volume.cc Normal file
View File

@ -0,0 +1,289 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include "usv_gazebo_plugins/polyhedron_volume.hh"
using namespace buoyancy;
using Face = Polyhedron::Face;
//////////////////////////////////////////////////////
Plane::Plane()
: normal(0., 0., 1.), offset(0.)
{
}
//////////////////////////////////////////////////////
Polyhedron::Face::Face(int _i1, int _i2, int _i3)
: i1(_i1), i2(_i2), i3(_i3)
{
}
//////////////////////////////////////////////////////
Volume::Volume()
: volume(0.0), centroid(ignition::math::Vector3d({0, 0, 0}))
{
}
//////////////////////////////////////////////////////
Volume& Volume::operator+=(const Volume &rhs)
{
this->volume += rhs.volume;
this->centroid += rhs.centroid;
return *this;
}
//////////////////////////////////////////////////////
Polyhedron Polyhedron::makeCube(double x, double y, double z)
{
Polyhedron cube;
// generate vertices
for (int i = 0; i < 2; ++i)
{
for (int j = 0; j < 2; ++j)
{
for (int k = 0; k < 2; ++k)
{
cube.vertices.emplace_back(
ignition::math::Vector3d(i * x - x / 2.0,
j * y - y / 2.0, k * z - z / 2.0));
}
}
}
// generate faces
cube.faces.emplace_back(Face(0, 6, 4));
cube.faces.emplace_back(Face(0, 2, 6));
cube.faces.emplace_back(Face(0, 3, 2));
cube.faces.emplace_back(Face(0, 1, 3));
cube.faces.emplace_back(Face(2, 7, 6));
cube.faces.emplace_back(Face(2, 3, 7));
cube.faces.emplace_back(Face(4, 6, 7));
cube.faces.emplace_back(Face(4, 7, 5));
cube.faces.emplace_back(Face(0, 4, 5));
cube.faces.emplace_back(Face(0, 5, 1));
cube.faces.emplace_back(Face(1, 5, 7));
cube.faces.emplace_back(Face(1, 7, 3));
return cube;
}
//////////////////////////////////////////////////////
Polyhedron Polyhedron::makeCylinder(double r, double l, int n)
{
assert(n > 4);
buoyancy::Polyhedron cylinder;
// generate all vertices
double angle_step = 2.0 * M_PI / n;
double l_2 = l / 2.0;
cylinder.vertices.resize(2*n+2);
cylinder.vertices[0] = ignition::math::Vector3d{0, 0, -l_2};
for (int i = 1; i <= n; ++i)
{
double x = r * ::sin(angle_step * (i-1));
double y = r * ::cos(angle_step * (i-1));
// bottom plate
cylinder.vertices[i] = ignition::math::Vector3d{x, y, -l_2};
// top plate
cylinder.vertices[i+n] = ignition::math::Vector3d{x, y, l_2};
}
cylinder.vertices[2*n+1] = ignition::math::Vector3d{0, 0, l_2};
// generate all faces
for (int i = 1; i <= n; ++i)
{ // bottom plate
cylinder.faces.emplace_back(Face(0, i, (i%n)+1));
}
for (int i = 1; i <= n; ++i)
{ // walls
cylinder.faces.emplace_back(Face(i+1, i, n+i));
cylinder.faces.emplace_back(Face((i%n)+n, (i%n)+n+1, (i%n)+1));
}
for (int i = 1; i <= n; ++i)
{ // top plate
cylinder.faces.emplace_back(Face(i+n, 2*n+1, (i%n)+n+1));
}
assert(cylinder.vertices.size() == 2 * n + 2);
assert(cylinder.faces.size() == 4 * n);
return cylinder;
}
//////////////////////////////////////////////////////
Volume Polyhedron::tetrahedronVolume(const ignition::math::Vector3d &v1,
const ignition::math::Vector3d &v2, const ignition::math::Vector3d &v3,
const ignition::math::Vector3d &p)
{
ignition::math::Vector3d a = v2 - v1;
ignition::math::Vector3d b = v3 - v1;
ignition::math::Vector3d r = p - v1;
Volume output;
output.volume = (1/6.) * (b.Cross(a)).Dot(r);
output.centroid = 0.25 * output.volume * (v1 + v2 + v3 + p);
return output;
}
//////////////////////////////////////////////////////
Volume Polyhedron::ComputeFullVolume()
{
Volume output;
// Compute the contribution of each triangle face
for (const auto& face : faces)
{
ignition::math::Vector3d v1 = vertices[face.i1];
ignition::math::Vector3d v2 = vertices[face.i2];
ignition::math::Vector3d v3 = vertices[face.i3];
output += tetrahedronVolume(v1, v2, v3);
}
return output;
}
//////////////////////////////////////////////////////
Volume Polyhedron::clipTriangle(const ignition::math::Vector3d &v1,
const ignition::math::Vector3d &v2,
const ignition::math::Vector3d &v3, double d1, double d2, double d3,
const ignition::math::Vector3d &p)
{
assert(d1 * d2 < 0);
Volume output;
// calculate the intersection point from a to b
ignition::math::Vector3d ab = v1 + (d1/(d1 - d2))*(v2 - v1);
if (d1 < 0)
{
// b to c crosses the clipping plane
if (d3 < 0)
{
// Case B - a quadrilateral or two triangles
// Calculate intersection point from b to c.
ignition::math::Vector3d bc = v2 + (d2/(d2 - d3))*(v3 - v2);
output += tetrahedronVolume(ab, bc, v1, p);
output += tetrahedronVolume(bc, v3, v1, p);
}
else
{
// Case A - a single triangle.
ignition::math::Vector3d ac = v1 + (d1/(d1 - d3))*(v3 - v1);
output += tetrahedronVolume(ab, ac, v1, p);
}
}
else
{
if (d3 < 0)
{
// Case B
ignition::math::Vector3d ac = v1 + (d1/(d1 - d3))*(v3 - v1);
output += tetrahedronVolume(ab, v2, v3, p);
output += tetrahedronVolume(ab, v3, ac, p);
}
else
{
// Case A
ignition::math::Vector3d bc = v2 + (d2/(d2 - d3))*(v3 - v2);
output += tetrahedronVolume(ab, v2, bc, p);
}
}
return output;
}
//////////////////////////////////////////////////////
Volume Polyhedron::SubmergedVolume(const ignition::math::Vector3d &x,
const ignition::math::Quaterniond &q, Plane &plane)
{
// transform the plane into the polyhedron frame
auto qt = q.Inverse();
auto normal = qt.RotateVector(plane.normal);
double offset = plane.offset - plane.normal.Dot(x);
// compute vertex heights relative to surface
std::vector<double> ds(vertices.size());
int numSubmerged = 0;
int sampleVert = 0;
for (size_t i = 0; i < vertices.size(); ++i)
{
ds[i] = normal.Dot(vertices[i]) - offset;
if (ds[i] < -EPSILON)
{
++numSubmerged;
sampleVert = i;
}
}
// if no submerged vertices return
if (numSubmerged == 0)
{
return Volume{};
}
// Find a point on the water surface. Project a submerged point to
// get improved accuracy. This point serves as the point of origin for
// computing all the tetrahedron volumes. Since this point is on the
// surface, all of the surface faces get zero volume tetrahedrons.
// This way the surface polygon does not need to be considered.
ignition::math::Vector3d p = vertices[sampleVert] - ds[sampleVert] * normal;
// compute the contribution of each triangle
Volume output;
for (const auto& face : faces)
{
ignition::math::Vector3d v1 = vertices[face.i1];
ignition::math::Vector3d v2 = vertices[face.i2];
ignition::math::Vector3d v3 = vertices[face.i3];
double d1 = ds[face.i1];
double d2 = ds[face.i2];
double d3 = ds[face.i3];
if (d1 * d2 < 0)
{ // v1-v2 crosses the plane
output += clipTriangle(v1, v2, v3, d1, d2, d3, p);
}
else if (d1 * d3 < 0)
{ // v1-v3 crosses the plane
output += clipTriangle(v3, v1, v2, d3, d1, d2, p);
}
else if (d2 * d3 < 0)
{ // v2-v3 crosses the plane
output += clipTriangle(v2, v3, v1, d2, d3, d1, p);
}
else if (d1 < 0 || d2 < 0 || d3 < 0)
{ // fully submerged
output += tetrahedronVolume(v1, v2, v3, p);
}
}
// small submerged slivers may have rounding error leading to
// a zero or negative volume. If so, then return a result of zero.
if (output.volume <= EPSILON)
{
return Volume{};
}
// normalize the centroid by the total volume
output.centroid *= 1.0 / output.volume;
// transform the centroid into world coordinates
output.centroid = x + q.RotateVector(output.centroid);
// if centroid is very small make it zero
output.centroid.X() = ::fabs(output.centroid[0]) < EPSILON ?
0 : output.centroid.X();
output.centroid.Y() = ::fabs(output.centroid[1]) < EPSILON ?
0 : output.centroid.Y();
output.centroid.Z() = ::fabs(output.centroid[2]) < EPSILON ?
0 : output.centroid.Z();
return output;
}

228
sitl_config/usv/usv_gazebo_plugins/src/shape_volume.cc Normal file
View File

@ -0,0 +1,228 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include "usv_gazebo_plugins/shape_volume.hh"
using namespace buoyancy;
/////////////////////////////////////////////////
ShapeVolumePtr ShapeVolume::makeShape(const sdf::ElementPtr sdf)
{
double epsilon = 1e-20;
ShapeVolume* shape = nullptr;
if (sdf->HasElement("box"))
{
auto boxElem = sdf->GetElement("box");
if (boxElem->HasElement("size"))
{
ignition::math::Vector3d dim = boxElem->GetElement("size")
->Get<ignition::math::Vector3d>();
if (dim[0] > epsilon && dim[1] > epsilon && dim[2] > epsilon)
{
shape = dynamic_cast<ShapeVolume*>(
new BoxVolume(dim[0], dim[1], dim[2]));
}
else
{
throw ParseException("box", "incorrect dimensions");
}
}
else
{
throw ParseException("box", "missing <size> element");
}
}
else if (sdf->HasElement("sphere"))
{
auto sphereElem = sdf->GetElement("sphere");
if (sphereElem->HasElement("radius"))
{
auto r = sphereElem->GetElement("radius")->Get<double>();
if (r > epsilon)
{
shape = dynamic_cast<ShapeVolume*>(new SphereVolume(r));
}
else
{
throw ParseException("sphere", "incorrect dimensions");
}
}
else
{
throw ParseException("sphere", "missing <radius> element");
}
}
else if (sdf->HasElement("cylinder"))
{
auto cylinderElem = sdf->GetElement("cylinder");
if (cylinderElem->HasElement("radius") &&
cylinderElem->HasElement("length"))
{
auto r = cylinderElem->GetElement("radius")->Get<double>();
auto l = cylinderElem->GetElement("length")->Get<double>();
if (r > epsilon || l > epsilon)
{
shape = dynamic_cast<ShapeVolume*>(new CylinderVolume(r, l));
}
else
{
throw ParseException("cylinder", "incorrect dimensions");
}
}
else
{
throw ParseException("cylinder", "missing <radius> or <length> element");
}
}
else
{
throw ParseException(
"geometry", "missing <box>, <cylinder> or <sphere> element");
}
return std::unique_ptr<ShapeVolume>(shape);
}
/////////////////////////////////////////////////
std::string ShapeVolume::Display()
{
switch (type)
{
case ShapeType::None:
return "None";
case ShapeType::Box:
return "Box";
case ShapeType::Cylinder:
return "Cylinder";
case ShapeType::Sphere:
return "Sphere";
}
}
//////////////////////////////////////////////////
BoxVolume::BoxVolume(double x, double y, double z)
: x(x),
y(y),
z(z),
polyhedron(Polyhedron::makeCube(x, y, z))
{
type = ShapeType::Box;
volume = x * y * z;
averageLength = (x + y + z) / 3.0;
}
//////////////////////////////////////////////////
std::string BoxVolume::Display()
{
std::stringstream ss;
ss << ShapeVolume::Display() << ":" << x << "," << y << "," << z;
return ss.str();
}
//////////////////////////////////////////////////
Volume BoxVolume::CalculateVolume(const ignition::math::Pose3d &pose,
double fluidLevel)
{
Plane waterSurface;
waterSurface.offset = fluidLevel;
return this->polyhedron.SubmergedVolume(pose.Pos(), pose.Rot(), waterSurface);
}
/////////////////////////////////////////////////
CylinderVolume::CylinderVolume(double r, double h)
: r(r),
h(h),
polyhedron(Polyhedron::makeCylinder(r, h, 20))
{
type = ShapeType::Cylinder;
volume = M_PI * r * r * h;
averageLength = (2 * r + h) / 2.0;
}
/////////////////////////////////////////////////
std::string CylinderVolume::Display()
{
std::stringstream ss;
ss << ShapeVolume::Display() << ":" << r << "," << h;
return ss.str();
}
/////////////////////////////////////////////////
Volume CylinderVolume::CalculateVolume(const ignition::math::Pose3d &pose,
double fluidLevel)
{
Plane waterSurface;
waterSurface.offset = fluidLevel;
return this->polyhedron.SubmergedVolume(pose.Pos(), pose.Rot(), waterSurface);
}
//////////////////////////////////////////////////
SphereVolume::SphereVolume(double r)
: r(r)
{
type = ShapeType::Sphere;
volume = 4./3. * M_PI * r * r * r;
averageLength = 2 * r;
}
//////////////////////////////////////////////////
std::string SphereVolume::Display()
{
std::stringstream ss;
ss << ShapeVolume::Display() << ":" << r;
return ss.str();
}
//////////////////////////////////////////////////
Volume SphereVolume::CalculateVolume(const ignition::math::Pose3d &pose,
double fluidLevel)
{
Volume output{};
// Location of bottom of object relative to the fluid surface - assumes
// origin is at cog of the object.
double h = fluidLevel - (pose.Pos().Z() - r);
if (h <= 0) {
return output;
}
// limits of integration
double intLimitLower = -r;
double intLimitUpper = (-r + h) > r ? r : (-r + h);
// volume = integral of (R^2 - z^2) dz * pi
output.volume = (pow(r, 2) * (intLimitUpper - intLimitLower)
- (pow(intLimitUpper, 3)/3.0 - pow(intLimitLower, 3)/3.0)) * M_PI;
output.centroid.X() = pose.Pos().X();
output.centroid.Y() = pose.Pos().Y();
if (output.volume > 0) {
// centroid is always centered to object in X and Y plane
output.centroid.X() = pose.Pos().X();
output.centroid.Y() = pose.Pos().Y();
// z_bar = (integral of (z(R)^2 - z^3) dz) * pi / volume
output.centroid.Z() = (pow(r, 2) / 2.0 *
(pow(intLimitUpper, 2) - pow(intLimitLower, 2)) -
(pow(intLimitUpper, 4) - pow(intLimitLower, 4))/ 4.0)
* M_PI / output.volume;
// convert centroid.z to global frame
output.centroid.Z() = pose.Pos().Z() + output.centroid.Z();
}
return output;
}

360
sitl_config/usv/usv_gazebo_plugins/src/usv_gazebo_dynamics_plugin.cc Normal file
View File

@ -0,0 +1,360 @@
/*
* Copyright (C) 2017 Brian Bingham
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <ros/ros.h>
#include <tf2/LinearMath/Transform.h>
#include <cmath>
#include <functional>
#include <sstream>
#include <algorithm>
#include <ignition/math/Pose3.hh>
#include "usv_gazebo_plugins/usv_gazebo_dynamics_plugin.hh"
#define GRAVITY 9.815
using namespace asv;
using namespace gazebo;
//////////////////////////////////////////////////
UsvDynamicsPlugin::UsvDynamicsPlugin()
{
}
//////////////////////////////////////////////////
double UsvDynamicsPlugin::SdfParamDouble(sdf::ElementPtr _sdfPtr,
const std::string &_paramName, const double _defaultVal) const
{
if (!_sdfPtr->HasElement(_paramName))
{
ROS_INFO_STREAM("Parameter <" << _paramName << "> not found: "
"Using default value of <" << _defaultVal << ">.");
return _defaultVal;
}
double val = _sdfPtr->Get<double>(_paramName);
ROS_DEBUG_STREAM("Parameter found - setting <" << _paramName <<
"> to <" << val << ">.");
return val;
}
//////////////////////////////////////////////////
void UsvDynamicsPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
ROS_DEBUG("Loading usv_gazebo_dynamics_plugin");
this->world = _model->GetWorld();
// Get parameters from SDF
std::string linkName;
if (!_sdf->HasElement("bodyName") ||
!_sdf->GetElement("bodyName")->GetValue())
{
this->link = _model->GetLink();
linkName = this->link->GetName();
ROS_INFO_STREAM("Did not find SDF parameter bodyName");
}
else
{
linkName = _sdf->GetElement("bodyName")->Get<std::string>();
this->link = _model->GetLink(linkName);
ROS_DEBUG_STREAM("Found SDF parameter bodyName as <" << linkName<< ">");
}
if (!this->link)
{
ROS_FATAL("usv_gazebo_dynamics_plugin error: bodyName: %s does not exist\n",
linkName.c_str());
return;
}
else
{
ROS_DEBUG_STREAM("USV Dynamics Model Link Name = " << linkName);
}
this->waterLevel = this->SdfParamDouble(_sdf, "waterLevel" , 0.5);
this->waterDensity = this->SdfParamDouble(_sdf, "waterDensity", 997.7735);
this->paramXdotU = this->SdfParamDouble(_sdf, "xDotU" , 5);
this->paramYdotV = this->SdfParamDouble(_sdf, "yDotV" , 5);
this->paramZdotW = this->SdfParamDouble(_sdf, "zDotW" , 0.1);
this->paramKdotP = this->SdfParamDouble(_sdf, "kDotP" , 0.1);
this->paramMdotQ = this->SdfParamDouble(_sdf, "mDotQ" , 0.1);
this->paramNdotR = this->SdfParamDouble(_sdf, "nDotR" , 1);
this->paramXu = this->SdfParamDouble(_sdf, "xU" , 20);
this->paramXuu = this->SdfParamDouble(_sdf, "xUU" , 0);
this->paramYv = this->SdfParamDouble(_sdf, "yV" , 20);
this->paramYvv = this->SdfParamDouble(_sdf, "yVV" , 0);
this->paramZw = this->SdfParamDouble(_sdf, "zW" , 20);
this->paramZww = this->SdfParamDouble(_sdf, "zWW" , 0);
this->paramKp = this->SdfParamDouble(_sdf, "kP" , 20);
this->paramKpp = this->SdfParamDouble(_sdf, "kPP" , 0);
this->paramMq = this->SdfParamDouble(_sdf, "mQ" , 20);
this->paramMqq = this->SdfParamDouble(_sdf, "mQQ" , 0);
this->paramNr = this->SdfParamDouble(_sdf, "nR" , 20);
this->paramNrr = this->SdfParamDouble(_sdf, "nRR" , 0);
this->paramHullRadius = this->SdfParamDouble(_sdf, "hullRadius" , 0.213);
this->paramBoatWidth = this->SdfParamDouble(_sdf, "boatWidth" , 1.0);
this->paramBoatLength = this->SdfParamDouble(_sdf, "boatLength" , 1.35);
if (!_sdf->HasElement("length_n"))
{
int defaultVal = 2;
ROS_INFO_STREAM("Parameter <length_n> not found: "
"Using default value of <" << defaultVal << ">.");
this->paramLengthN = defaultVal;
}
else
{
this->paramLengthN = _sdf->GetElement("length_n")->Get<int>();
}
// Wave model
if (_sdf->HasElement("wave_model"))
{
this->waveModelName = _sdf->Get<std::string>("wave_model");
}
this->waveParams = nullptr;
// Get inertia and mass of vessel
#if GAZEBO_MAJOR_VERSION >= 8
const ignition::math::Vector3d kInertia =
this->link->GetInertial()->PrincipalMoments();
const double kMass = this->link->GetInertial()->Mass();
#else
const ignition::math::Vector3d kInertia =
this->link->GetInertial()->GetPrincipalMoments().Ign();
const double kMass = this->link->GetInertial()->GetMass();
#endif
// Report some of the pertinent parameters for verification
ROS_DEBUG("USV Dynamics Parameters: From URDF XACRO model definition");
ROS_DEBUG_STREAM("Vessel Mass (rigid-body): " << kMass);
ROS_DEBUG_STREAM("Vessel Inertia Vector (rigid-body): X:" << kInertia[0] <<
" Y:" << kInertia[1] << " Z:" << kInertia[2]);
// Initialize time and odometry position
#if GAZEBO_MAJOR_VERSION >= 8
this->prevUpdateTime = this->world->SimTime();
#else
this->prevUpdateTime = this->world->GetSimTime();
#endif
// Listen to the update event broadcastes every physics iteration.
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&UsvDynamicsPlugin::Update, this));
// Initialize Added Mass Matrix
this->Ma = Eigen::MatrixXd(6, 6);
this->Ma <<
this->paramXdotU, 0, 0, 0, 0, 0,
0, this->paramYdotV, 0, 0, 0, 0,
0, 0, this->paramZdotW, 0, 0, 0,
0, 0, 0, this->paramKdotP, 0, 0,
0, 0, 0, 0, this->paramMdotQ, 0,
0, 0, 0, 0, 0, this->paramNdotR;
}
double UsvDynamicsPlugin::CircleSegment(double R, double h)
{
return R*R*acos( (R-h)/R ) - (R-h)*sqrt(2*R*h-h*h);
}
//////////////////////////////////////////////////
void UsvDynamicsPlugin::Update()
{
// If we haven't yet, retrieve the wave parameters from ocean model plugin.
if (waveParams == nullptr)
{
gzmsg << "usv_gazebo_dynamics_plugin: waveParams is null. "
<< " Trying to get wave parameters from ocean model" << std::endl;
this->waveParams = WavefieldModelPlugin::GetWaveParams(
this->world, this->waveModelName);
}
#if GAZEBO_MAJOR_VERSION >= 8
const common::Time kTimeNow = this->world->SimTime();
#else
const common::Time kTimeNow = this->world->GetSimTime();
#endif
double dt = (kTimeNow - this->prevUpdateTime).Double();
this->prevUpdateTime = kTimeNow;
// Get Pose/Orientation from Gazebo (if no state subscriber is active)
#if GAZEBO_MAJOR_VERSION >= 8
const ignition::math::Pose3d kPose = this->link->WorldPose();
#else
const ignition::math::Pose3d kPose = this->link->GetWorldPose().Ign();
#endif
const ignition::math::Vector3d kEuler = kPose.Rot().Euler();
// Get body-centered linear and angular rates
#if GAZEBO_MAJOR_VERSION >= 8
const ignition::math::Vector3d kVelLinearBody =
this->link->RelativeLinearVel();
#else
const ignition::math::Vector3d kVelLinearBody =
this->link->GetRelativeLinearVel().Ign();
#endif
ROS_DEBUG_STREAM_THROTTLE(0.5, "Vel linear: " << kVelLinearBody);
#if GAZEBO_MAJOR_VERSION >= 8
const ignition::math::Vector3d kVelAngularBody =
this->link->RelativeAngularVel();
#else
const ignition::math::Vector3d kVelAngularBody =
this->link->GetRelativeAngularVel().Ign();
#endif
ROS_DEBUG_STREAM_THROTTLE(0.5, "Vel angular: " << kVelAngularBody);
// Estimate the linear and angular accelerations.
// Note the the GetRelativeLinearAccel() and AngularAccel() functions
// appear to be unreliable
const ignition::math::Vector3d kAccelLinearBody =
(kVelLinearBody - this->prevLinVel) / dt;
this->prevLinVel = kVelLinearBody;
ROS_DEBUG_STREAM_THROTTLE(0.5, "Accel linear: " << kAccelLinearBody);
const ignition::math::Vector3d kAccelAngularBody =
(kVelAngularBody - this->prevAngVel) / dt;
this->prevAngVel = kVelAngularBody;
ROS_DEBUG_STREAM_THROTTLE(0.5, "Accel angular: " << kAccelAngularBody);
// Create state and derivative of state (accelerations)
Eigen::VectorXd stateDot = Eigen::VectorXd(6);
Eigen::VectorXd state = Eigen::VectorXd(6);
Eigen::MatrixXd Cmat = Eigen::MatrixXd::Zero(6, 6);
Eigen::MatrixXd Dmat = Eigen::MatrixXd::Zero(6, 6);
stateDot << kAccelLinearBody.X(), kAccelLinearBody.Y(), kAccelLinearBody.Z(),
kAccelAngularBody.X(), kAccelAngularBody.Y(), kAccelAngularBody.Z();
state << kVelLinearBody.X(), kVelLinearBody.Y(), kVelLinearBody.Z(),
kVelAngularBody.X(), kVelAngularBody.Y(), kVelAngularBody.Z();
// Added Mass
const Eigen::VectorXd kAmassVec = -1.0 * this->Ma * stateDot;
ROS_DEBUG_STREAM_THROTTLE(1.0, "stateDot: \n" << stateDot);
ROS_DEBUG_STREAM_THROTTLE(1.0, "amassVec :\n" << kAmassVec);
// Coriolis - added mass components
Cmat(0, 5) = this->paramYdotV * kVelLinearBody.Y();
Cmat(1, 5) = this->paramXdotU * kVelLinearBody.X();
Cmat(5, 0) = this->paramYdotV * kVelLinearBody.Y();
Cmat(5, 1) = this->paramXdotU * kVelLinearBody.X();
// Drag
Dmat(0, 0) = this->paramXu + this->paramXuu * std::abs(kVelLinearBody.X());
Dmat(1, 1) = this->paramYv + this->paramYvv * std::abs(kVelLinearBody.Y());
Dmat(2, 2) = this->paramZw + this->paramZww * std::abs(kVelLinearBody.Z());
Dmat(3, 3) = this->paramKp + this->paramKpp * std::abs(kVelAngularBody.X());
Dmat(4, 4) = this->paramMq + this->paramMqq * std::abs(kVelAngularBody.Y());
Dmat(5, 5) = this->paramNr + this->paramNrr * std::abs(kVelAngularBody.Z());
ROS_DEBUG_STREAM_THROTTLE(1.0, "Dmat :\n" << Dmat);
const Eigen::VectorXd kDvec = -1.0 * Dmat * state;
ROS_DEBUG_STREAM_THROTTLE(1.0, "Dvec :\n" << kDvec);
// Vehicle frame transform
tf2::Quaternion vq = tf2::Quaternion();
tf2::Matrix3x3 m;
m.setEulerYPR(kEuler.Z(), kEuler.Y(), kEuler.X());
m.getRotation(vq);
tf2::Transform xformV = tf2::Transform(vq);
// Sum all forces - in body frame
const Eigen::VectorXd kForceSum = kAmassVec + kDvec;
// Forces in fixed frame
ROS_DEBUG_STREAM_THROTTLE(1.0, "forceSum :\n" << kForceSum);
// Add dynamic forces/torques to link at CG
this->link->AddRelativeForce(
ignition::math::Vector3d(kForceSum(0), kForceSum(1), kForceSum(2)));
this->link->AddRelativeTorque(
ignition::math::Vector3d(kForceSum(3), kForceSum(4), kForceSum(5)));
// Loop over boat grid points
// Grid point location in boat frame - might be able to precalculate these?
tf2::Vector3 bpnt(0, 0, 0);
// Grid point location in world frame
tf2::Vector3 bpntW(0, 0, 0);
// For each hull
for (int ii = 0; ii < 2; ii++)
{
// Grid point in boat frame
bpnt.setY((ii*2.0-1.0)*this->paramBoatWidth/2.0);
// For each length segment
for (int jj = 1; jj <= this->paramLengthN; jj++)
{
bpnt.setX(((jj - 0.5) / (static_cast<float>(this->paramLengthN)) - 0.5) *
this->paramBoatLength);
// Transform from vessel to water/world frame
bpntW = xformV * bpnt;
// Debug
ROS_DEBUG_STREAM_THROTTLE(1.0, "[" << ii << "," << jj <<
"] grid points" << bpnt.x() << "," << bpnt.y() << "," << bpnt.z());
ROS_DEBUG_STREAM_THROTTLE(1.0, "v frame euler " << kEuler);
ROS_DEBUG_STREAM_THROTTLE(1.0, "in water frame" << bpntW.x() << "," <<
bpntW.y() << "," << bpntW.z());
// Vertical location of boat grid point in world frame
const float kDdz = kPose.Pos().Z() + bpntW.z();
ROS_DEBUG_STREAM("Z, pose: " << kPose.Pos().Z() << ", bpnt: "
<< bpntW.z() << ", dd: " << kDdz);
// Find vertical displacement of wave field
// World location of grid point
ignition::math::Vector3d X;
X.X() = kPose.Pos().X() + bpntW.x();
X.Y() = kPose.Pos().Y() + bpntW.y();
// Compute the depth at the grid point.
double simTime = kTimeNow.Double();
// double depth = WavefieldSampler::ComputeDepthDirectly(
// *waveParams, X, simTime);
double depth = 0.0;
if (waveParams)
{
depth = WavefieldSampler::ComputeDepthSimply(*waveParams, X, simTime);
}
// Vertical wave displacement.
double dz = depth + X.Z();
// Total z location of boat grid point relative to water surface
double deltaZ = (this->waterLevel + dz) - kDdz;
deltaZ = std::max(deltaZ, 0.0); // enforce only upward buoy force
deltaZ = std::min(deltaZ, this->paramHullRadius);
// Buoyancy force at grid point
const float kBuoyForce = CircleSegment(this->paramHullRadius, deltaZ) *
this->paramBoatLength/(static_cast<float>(this->paramLengthN)) *
GRAVITY * this->waterDensity;
ROS_DEBUG_STREAM("buoyForce: " << kBuoyForce);
// Apply force at grid point
// From web, Appears that position is in the link frame
// and force is in world frame
this->link->AddForceAtRelativePosition(
ignition::math::Vector3d(0, 0, kBuoyForce),
ignition::math::Vector3d(bpnt.x(), bpnt.y(), bpnt.z()));
}
}
}
GZ_REGISTER_MODEL_PLUGIN(UsvDynamicsPlugin);

456
sitl_config/usv/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc Normal file
View File

@ -0,0 +1,456 @@
/*
* Copyright (C) 2017 Brian Bingham
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <boost/algorithm/clamp.hpp>
#include <ros/time.h>
#include <cmath>
#include <functional>
#include "usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh"
using namespace gazebo;
//////////////////////////////////////////////////
Thruster::Thruster(UsvThrust *_parent)
{
// Initialize fields
this->plugin = _parent;
this->engineJointPID.Init(300, 0.0, 20);
this->currCmd = 0.0;
this->desiredAngle = 0.0;
#if GAZEBO_MAJOR_VERSION >= 8
this->lastCmdTime = this->plugin->world->SimTime();
#else
this->lastCmdTime = this->plugin->world->GetSimTime();
#endif
}
//////////////////////////////////////////////////
void Thruster::OnThrustCmd(const std_msgs::Float32::ConstPtr &_msg)
{
// When we get a new thrust command!
ROS_DEBUG_STREAM("New thrust command! " << _msg->data);
std::lock_guard<std::mutex> lock(this->plugin->mutex);
#if GAZEBO_MAJOR_VERSION >= 8
this->lastCmdTime = this->plugin->world->SimTime();
#else
this->lastCmdTime = this->plugin->world->GetSimTime();
#endif
this->currCmd = _msg->data;
}
//////////////////////////////////////////////////
void Thruster::OnThrustAngle(const std_msgs::Float32::ConstPtr &_msg)
{
// When we get a new thrust angle!
ROS_DEBUG_STREAM("New thrust angle! " << _msg->data);
std::lock_guard<std::mutex> lock(this->plugin->mutex);
this->desiredAngle = boost::algorithm::clamp(_msg->data, -this->maxAngle,
this->maxAngle);
}
//////////////////////////////////////////////////
double UsvThrust::SdfParamDouble(sdf::ElementPtr _sdfPtr,
const std::string &_paramName, const double _defaultVal) const
{
if (!_sdfPtr->HasElement(_paramName))
{
ROS_INFO_STREAM("Parameter <" << _paramName << "> not found: "
"Using default value of <" << _defaultVal << ">.");
return _defaultVal;
}
double val = _sdfPtr->Get<double>(_paramName);
ROS_DEBUG_STREAM("Parameter found - setting <" << _paramName <<
"> to <" << val << ">.");
return val;
}
//////////////////////////////////////////////////
void UsvThrust::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
ROS_DEBUG("Loading usv_gazebo_thrust_plugin");
this->model = _parent;
this->world = this->model->GetWorld();
// Get parameters from SDF
std::string nodeNamespace = "";
if (_sdf->HasElement("robotNamespace"))
{
nodeNamespace = _sdf->Get<std::string>("robotNamespace") + "/";
ROS_INFO_STREAM("Thruster namespace <" << nodeNamespace << ">");
}
this->cmdTimeout = this->SdfParamDouble(_sdf, "cmdTimeout", 1.0);
ROS_DEBUG_STREAM("Loading thrusters from SDF");
// For each thruster
int thrusterCounter = 0;
if (_sdf->HasElement("thruster"))
{
sdf::ElementPtr thrusterSDF = _sdf->GetElement("thruster");
while (thrusterSDF)
{
// Instatiate
Thruster thruster(this);
ROS_DEBUG_STREAM("Thruster #" << thrusterCounter);
// REQUIRED PARAMETERS
// Find link by name in SDF
if (thrusterSDF->HasElement("linkName"))
{
std::string linkName = thrusterSDF->Get<std::string>("linkName");
thruster.link = this->model->GetLink(linkName);
if (!thruster.link)
{
ROS_ERROR_STREAM("Could not find a link by the name <" << linkName
<< "> in the model!");
}
else
{
ROS_DEBUG_STREAM("Thruster added to link <" << linkName << ">");
}
}
else
{
ROS_ERROR_STREAM("Please specify a link name for each thruster!");
}
// Parse out propellor joint name
if (thrusterSDF->HasElement("propJointName"))
{
std::string propName =
thrusterSDF->GetElement("propJointName")->Get<std::string>();
thruster.propJoint = this->model->GetJoint(propName);
if (!thruster.propJoint)
{
ROS_ERROR_STREAM("Could not find a propellor joint by the name of <"
<< propName << "> in the model!");
}
else
{
ROS_DEBUG_STREAM("Propellor joint <" << propName <<
"> added to thruster");
}
}
else
{
ROS_ERROR_STREAM("No propJointName SDF parameter for thruster #"
<< thrusterCounter);
}
// Parse out engine joint name
if (thrusterSDF->HasElement("engineJointName"))
{
std::string engineName =
thrusterSDF->GetElement("engineJointName")->Get<std::string>();
thruster.engineJoint = this->model->GetJoint(engineName);
if (!thruster.engineJoint)
{
ROS_ERROR_STREAM("Could not find a engine joint by the name of <" <<
engineName << "> in the model!");
}
else
{
ROS_DEBUG_STREAM("Engine joint <" << engineName <<
"> added to thruster");
}
}
else
{
ROS_ERROR_STREAM("No engineJointName SDF parameter for thruster #"
<< thrusterCounter);
}
// Parse for cmd subscription topic
if (thrusterSDF->HasElement("cmdTopic"))
{
thruster.cmdTopic = thrusterSDF->Get<std::string>("cmdTopic");
}
else
{
ROS_ERROR_STREAM("Please specify a cmdTopic (for ROS subscription) "
"for each thruster!");
}
// Parse for angle subscription topic
if (thrusterSDF->HasElement("angleTopic"))
{
thruster.angleTopic = thrusterSDF->Get<std::string>("angleTopic");
}
else
{
ROS_ERROR_STREAM("Please specify a angleTopic (for ROS subscription) "
"for each thruster!");
}
// Parse for enableAngle bool
if (thrusterSDF->HasElement("enableAngle"))
{
thruster.enableAngle = thrusterSDF->Get<bool>("enableAngle");
}
else
{
ROS_ERROR_STREAM("Please specify for each thruster if it should enable "
"angle adjustment (for ROS subscription)!");
}
// OPTIONAL PARAMETERS
// Parse individual thruster SDF parameters
if (thrusterSDF->HasElement("mappingType"))
{
thruster.mappingType = thrusterSDF->Get<int>("mappingType");
ROS_DEBUG_STREAM("Parameter found - setting <mappingType> to <" <<
thruster.mappingType << ">.");
}
else
{
thruster.mappingType = 0;
ROS_INFO_STREAM("Parameter <mappingType> not found: "
"Using default value of <" << thruster.mappingType << ">.");
}
thruster.maxCmd = this->SdfParamDouble(thrusterSDF, "maxCmd", 1.0);
thruster.maxForceFwd =
this->SdfParamDouble(thrusterSDF, "maxForceFwd", 250.0);
thruster.maxForceRev =
this->SdfParamDouble(thrusterSDF, "maxForceRev", -100.0);
thruster.maxAngle = this->SdfParamDouble(thrusterSDF, "maxAngle",
M_PI / 2);
// Push to vector and increment
this->thrusters.push_back(thruster);
thrusterSDF = thrusterSDF->GetNextElement("thruster");
thrusterCounter++;
}
}
else
{
ROS_WARN_STREAM("No 'thruster' tags in description - how will you move?");
}
ROS_DEBUG_STREAM("Found " << thrusterCounter << " thrusters");
// Initialize the ROS node and subscribe to cmd_drive
this->rosnode.reset(new ros::NodeHandle(nodeNamespace));
// Advertise joint state publisher to view engines and propellers in rviz
// TODO: consider throttling joint_state pub for performance
// (every OnUpdate may be too frequent).
this->jointStatePub =
this->rosnode->advertise<sensor_msgs::JointState>("joint_states", 1);
this->jointStateMsg.name.resize(2 * thrusters.size());
this->jointStateMsg.position.resize(2 * thrusters.size());
this->jointStateMsg.velocity.resize(2 * thrusters.size());
this->jointStateMsg.effort.resize(2 * thrusters.size());
for (size_t i = 0; i < this->thrusters.size(); ++i)
{
// Prefill the joint state message with the engine and propeller joint.
this->jointStateMsg.name[2 * i] = this->thrusters[i].engineJoint->GetName();
this->jointStateMsg.name[2 * i + 1] =
this->thrusters[i].propJoint->GetName();
// Subscribe to commands for each thruster.
this->thrusters[i].cmdSub = this->rosnode->subscribe(
this->thrusters[i].cmdTopic, 1, &Thruster::OnThrustCmd,
&this->thrusters[i]);
// Subscribe to angles for each thruster.
if (this->thrusters[i].enableAngle)
{
this->thrusters[i].angleSub = this->rosnode->subscribe(
this->thrusters[i].angleTopic, 1, &Thruster::OnThrustAngle,
&this->thrusters[i]);
}
}
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&UsvThrust::Update, this));
}
//////////////////////////////////////////////////
double UsvThrust::ScaleThrustCmd(const double _cmd, const double _maxCmd,
const double _maxPos, const double _maxNeg) const
{
double val = 0.0;
if (_cmd >= 0.0)
{
val = _cmd / _maxCmd * _maxPos;
val = std::min(val, _maxPos);
}
else
{
double absMaxNeg = std::abs(_maxNeg);
val = _cmd / _maxCmd * absMaxNeg;
val = std::max(val, -1.0 * absMaxNeg);
}
return val;
}
//////////////////////////////////////////////////
double UsvThrust::Glf(const double _x, const float _A, const float _K,
const float _B, const float _v, const float _C, const float _M) const
{
return _A + (_K - _A) / (pow(_C + exp(-_B * (_x - _M)), 1.0 / _v));
}
//////////////////////////////////////////////////
double UsvThrust::GlfThrustCmd(const double _cmd,
const double _maxPos,
const double _maxNeg) const
{
double val = 0.0;
if (_cmd > 0.01)
{
val = this->Glf(_cmd, 0.01f, 59.82f, 5.0f, 0.38f, 0.56f, 0.28f);
val = std::min(val, _maxPos);
}
else if (_cmd < 0.01)
{
val = this->Glf(_cmd, -199.13f, -0.09f, 8.84f, 5.34f, 0.99f, -0.57f);
val = std::max(val, _maxNeg);
}
return val;
}
//////////////////////////////////////////////////
void UsvThrust::Update()
{
#if GAZEBO_MAJOR_VERSION >= 8
common::Time now = this->world->SimTime();
#else
common::Time now = this->world->GetSimTime();
#endif
for (size_t i = 0; i < this->thrusters.size(); ++i)
{
{
std::lock_guard<std::mutex> lock(this->mutex);
// Enforce command timeout
double dtc = (now - this->thrusters[i].lastCmdTime).Double();
if (dtc > this->cmdTimeout && this->cmdTimeout > 0.0)
{
this->thrusters[i].currCmd = 0.0;
ROS_DEBUG_STREAM_THROTTLE(1.0, "[" << i << "] Cmd Timeout");
}
// Adjust thruster engine joint angle with PID
this->RotateEngine(i, now - this->thrusters[i].lastAngleUpdateTime);
// Apply the thrust mapping
ignition::math::Vector3d tforcev(0, 0, 0);
switch (this->thrusters[i].mappingType)
{
case 0:
tforcev.X() = this->ScaleThrustCmd(this->thrusters[i].currCmd/
this->thrusters[i].maxCmd,
this->thrusters[i].maxCmd,
this->thrusters[i].maxForceFwd,
this->thrusters[i].maxForceRev);
break;
case 1:
tforcev.X() = this->GlfThrustCmd(this->thrusters[i].currCmd/
this->thrusters[i].maxCmd,
this->thrusters[i].maxForceFwd,
this->thrusters[i].maxForceRev);
break;
default:
ROS_FATAL_STREAM("Cannot use mappingType=" <<
this->thrusters[i].mappingType);
break;
}
// Apply force for each thruster
this->thrusters[i].link->AddLinkForce(tforcev);
// Spin the propellers
this->SpinPropeller(i);
}
}
// Publish the propeller joint state
this->jointStateMsg.header.stamp = ros::Time::now();
this->jointStatePub.publish(this->jointStateMsg);
}
//////////////////////////////////////////////////
void UsvThrust::RotateEngine(size_t _i, common::Time _stepTime)
{
// Calculate angleError for PID calculation
double desiredAngle = this->thrusters[_i].desiredAngle;
#if GAZEBO_MAJOR_VERSION >= 8
double currAngle = this->thrusters[_i].engineJoint->Position(0);
#else
double currAngle = this->thrusters[_i].engineJoint->GetAngle(0).Radian();
#endif
double angleError = currAngle - desiredAngle;
double effort = this->thrusters[_i].engineJointPID.Update(angleError,
_stepTime);
this->thrusters[_i].engineJoint->SetForce(0, effort);
// Set position, velocity, and effort of joint from gazebo
#if GAZEBO_MAJOR_VERSION >= 8
ignition::math::Angle position =
this->thrusters[_i].engineJoint->Position(0);
#else
gazebo::math::Angle position = this->thrusters[_i].engineJoint->GetAngle(0);
#endif
position.Normalize();
this->jointStateMsg.position[2 * _i] = position.Radian();
this->jointStateMsg.velocity[2 * _i] =
this->thrusters[_i].engineJoint->GetVelocity(0);
this->jointStateMsg.effort[2 * _i] = effort;
// Store last update time
this->thrusters[_i].lastAngleUpdateTime += _stepTime;
}
//////////////////////////////////////////////////
void UsvThrust::SpinPropeller(size_t _i)
{
const double kMinInput = 0.1;
const double kMaxEffort = 2.0;
double effort = 0.0;
physics::JointPtr propeller = this->thrusters[_i].propJoint;
// Calculate effort on propeller joint
if (std::abs(this->thrusters[_i].currCmd/
this->thrusters[_i].maxCmd) > kMinInput)
effort = (this->thrusters[_i].currCmd /
this->thrusters[_i].maxCmd) * kMaxEffort;
propeller->SetForce(0, effort);
// Set position, velocity, and effort of joint from gazebo
#if GAZEBO_MAJOR_VERSION >= 8
ignition::math::Angle position = propeller->Position(0);
#else
gazebo::math::Angle position = propeller->GetAngle(0);
#endif
position.Normalize();
this->jointStateMsg.position[2 * _i + 1] = position.Radian();
this->jointStateMsg.velocity[2 * _i + 1] = propeller->GetVelocity(0);
this->jointStateMsg.effort[2 * _i + 1] = effort;
}
GZ_REGISTER_MODEL_PLUGIN(UsvThrust);

303
sitl_config/usv/usv_gazebo_plugins/src/usv_gazebo_wind_plugin.cc Normal file
View File

@ -0,0 +1,303 @@
/*
* Copyright (C) 2018 Brian Bingham
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <std_msgs/Float64.h>
#include <functional>
#include <gazebo/common/Console.hh>
#include "usv_gazebo_plugins/usv_gazebo_wind_plugin.hh"
using namespace gazebo;
//////////////////////////////////////////////////
UsvWindPlugin::UsvWindPlugin()
{
}
//////////////////////////////////////////////////
void UsvWindPlugin::Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
{
if (char* env_dbg = std::getenv("VRX_DEBUG"))
{
gzdbg << std::string(env_dbg) <<std::endl;
if (std::string(env_dbg) == "false")
this->debug = false;
}
else
{
gzwarn << "VRX_DEBUG environment variable not set, defaulting to true"
<< std::endl;
}
this->world = _parent;
// Retrieve models' parameters from SDF
if (!_sdf->HasElement("wind_obj") ||
!_sdf->GetElement("wind_obj"))
{
gzerr << "Did not find SDF parameter wind_obj" << std::endl;
}
else
{
sdf::ElementPtr windObjSDF = _sdf->GetElement("wind_obj");
while (windObjSDF)
{
UsvWindPlugin::WindObj obj;
if (!windObjSDF->HasElement("name") ||
!windObjSDF->GetElement("name")->GetValue())
{
gzerr << ("Did not find SDF parameter name") << std::endl;
}
else
{
obj.modelName = windObjSDF->GetElement("name")->Get<std::string>();
}
if (!windObjSDF->HasElement("link_name") ||
!windObjSDF->GetElement("link_name")->GetValue())
{
gzerr << ("Did not find SDF parameter link_name") << std::endl;
}
else
{
obj.linkName = windObjSDF->GetElement("link_name")->Get<std::string>();
}
if (!windObjSDF->HasElement("coeff_vector") ||
!windObjSDF->GetElement("coeff_vector")->GetValue())
{
gzerr << ("Did not find SDF parameter coeff_vector") << std::endl;
}
else
{
obj.windCoeff = windObjSDF->GetElement("coeff_vector")->
Get<ignition::math::Vector3d>();
}
this->windObjs.push_back(obj);
gzdbg << obj.modelName << " loaded" << std::endl;
windObjSDF = windObjSDF->GetNextElement("wind_obj");
}
}
if (_sdf->HasElement("wind_direction"))
{
double windAngle = _sdf->GetElement("wind_direction")->Get<double>();
this->windDirection.X(cos(windAngle * M_PI / 180));
this->windDirection.Y(sin(windAngle * M_PI / 180));
this->windDirection.Z(0);
}
gzmsg << "Wind direction unit vector = " << this->windDirection << std::endl;
if (_sdf->HasElement("wind_mean_velocity"))
{
this->windMeanVelocity =
_sdf->GetElement("wind_mean_velocity")->Get<double>();
}
gzmsg << "Wind mean velocity = " << this->windMeanVelocity << std::endl;
if (_sdf->HasElement("var_wind_gain_constants"))
{
this->gainConstant =
_sdf->GetElement("var_wind_gain_constants")->Get<double>();
}
gzmsg << "var wind gain constants = " << this->gainConstant << std::endl;
if (_sdf->HasElement("var_wind_time_constants"))
{
this->timeConstant =
_sdf->GetElement("var_wind_time_constants")->Get<double>();
}
gzmsg << "var wind time constants = " << this->timeConstant << std::endl;
if (_sdf->HasElement("update_rate"))
{
this->updateRate =
_sdf->GetElement("update_rate")->Get<double>();
}
gzmsg << "update rate = " << this->updateRate << std::endl;
if (_sdf->HasElement("topic_wind_speed"))
{
this->topicWindSpeed =
_sdf->GetElement("topic_wind_speed")->Get<std::string>();
}
gzmsg << "topic wind speed = " << this->topicWindSpeed << std::endl;
if (_sdf->HasElement("topic_wind_direction"))
{
this->topicWindDirection =
_sdf->GetElement("topic_wind_direction")->Get<std::string>();
}
gzmsg << "topic wind direction = " << this->topicWindDirection << std::endl;
// Setting the seed for the random generator.
unsigned int seed = std::random_device {}();
if (_sdf->HasElement("random_seed") &&
_sdf->GetElement("random_seed")->Get<unsigned int>() != 0)
{
seed = _sdf->GetElement("random_seed")->Get<unsigned int>();
}
gzmsg << "Random seed value = " << seed << std::endl;
this->randGenerator.reset(new std::mt19937(seed));
// Calculate filter constant
this->filterGain = this->gainConstant*sqrt(2.0*this->timeConstant);
gzmsg << "Var wind filter gain = " << this->filterGain << std::endl;
// Initialize previous time and previous velocity
#if GAZEBO_MAJOR_VERSION >= 8
this->previousTime = this->world->SimTime().Double();
#else
this->previousTime = this->world->GetSimTime().Double();
#endif
this->varVel = 0;
// Initialize ROS transport.
this->rosNode.reset(new ros::NodeHandle());
this->windSpeedPub =
this->rosNode->advertise<std_msgs::Float64>(this->topicWindSpeed, 100);
this->windDirectionPub = this->rosNode->advertise<std_msgs::Float64>(
this->topicWindDirection, 100);
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&UsvWindPlugin::Update, this));
}
//////////////////////////////////////////////////
void UsvWindPlugin::Update()
{
// Look for the missing models if not all of them have been initialized
if (!this->windObjsInit)
{
for (auto& i : this->windObjs)
{
#if GAZEBO_MAJOR_VERSION >= 8
if ((!i.init) && (this->world->ModelByName(i.modelName)))
{
#else
if ((!i.init) && (this->world->GetModel(i.modelName)))
{
#endif
gzdbg << i.modelName << " initialized" << std::endl;
++this->windObjsInitCount;
i.init = true;
#if GAZEBO_MAJOR_VERSION >= 8
i.model = this->world->ModelByName(i.modelName);
#else
i.model = this->world->GetModel(i.modelName);
#endif
i.link = i.model->GetLink(i.linkName);
if (!i.link)
{
gzdbg << i.modelName << "'s link name: " << i.linkName
<< " is invalid" << std::endl;
}
}
}
if (windObjsInitCount == windObjs.size())
this->windObjsInit = true;
}
#if GAZEBO_MAJOR_VERSION >= 8
double currentTime = this->world->SimTime().Double();
#else
double currentTime = this->world->GetSimTime().Double();
#endif
double dT= currentTime - this->previousTime;
std::normal_distribution<double> dist(0, 1);
double randomDist = dist(*this->randGenerator);
// Current variable wind velocity
this->varVel += 1.0/this->timeConstant*
(-1.0*this->varVel+this->filterGain/sqrt(dT)*randomDist)*dT;
// Current wind velocity
double velocity = this->varVel + this->windMeanVelocity;
for (auto& windObj : this->windObjs)
{
// Apply the forces of the wind to all wind objects only if they have been
// initialized
if (!windObj.init || !windObj.link)
continue;
// Transform wind from world coordinates to body coordinates
#if GAZEBO_MAJOR_VERSION >= 8
ignition::math::Vector3d relativeWind =
windObj.link->WorldPose().Rot().Inverse().RotateVector(
this->windDirection*velocity);
#else
ignition::math::Vector3d relativeWind =
windObj.link->GetWorldPose().rot.Ign().Inverse().RotateVector(
this->windDirection*velocity);
#endif
// Calculate apparent wind
#if GAZEBO_MAJOR_VERSION >= 8
ignition::math::Vector3d apparentWind =
relativeWind - windObj.link->RelativeLinearVel();
#else
ignition::math::Vector3d apparentWind = relativeWind
- windObj.link->GetRelativeLinearVel().Ign();
#endif
// gzdbg << "Relative wind: " << relativeWind << std::endl;
// gzdbg << "Apparent wind: " << apparentWind << std::endl;
// Calculate wind force - body coordinates
ignition::math::Vector3d windForce(
windObj.windCoeff.X() * relativeWind.X() * abs(relativeWind.X()),
windObj.windCoeff.Y() * relativeWind.Y() * abs(relativeWind.Y()),
-2.0 * windObj.windCoeff.Z() * relativeWind.X() * relativeWind.Y());
// Add forces/torques to link at CG
windObj.link->AddRelativeForce(
ignition::math::Vector3d(windForce.X(), windForce.Y(), 0.0));
windObj.link->AddRelativeTorque(
ignition::math::Vector3d(0.0, 0.0, windForce.Z()));
}
// Moving the previous time one step forward.
this->previousTime = currentTime;
double publishingBuffer = 1/this->updateRate;
if (this->updateRate >= 0)
{
publishingBuffer = 1/this->updateRate;
}
else
{
publishingBuffer = -1;
}
// Publishing the wind speed and direction
if ((currentTime - this->lastPublishTime > publishingBuffer) && this->debug)
{
std_msgs::Float64 windSpeedMsg;
std_msgs::Float64 windDirectionMsg;
windSpeedMsg.data = velocity;
windDirectionMsg.data =
atan2(this->windDirection[1], this->windDirection[0]) * 180 / M_PI;
this->windSpeedPub.publish(windSpeedMsg);
this->windDirectionPub.publish(windDirectionMsg);
this->lastPublishTime = currentTime;
}
}
GZ_REGISTER_WORLD_PLUGIN(UsvWindPlugin);

345
sitl_config/usv/usv_gazebo_plugins/test/buoyancy_test.cc Normal file
View File

@ -0,0 +1,345 @@
#include <gtest/gtest.h>
#include "usv_gazebo_plugins/polyhedron_volume.hh"
#include "usv_gazebo_plugins/shape_volume.hh"
using namespace buoyancy;
/////////////////////////////////////////////////
TEST(PolyhedronTest, CubeTotalVolume)
{
auto cube = Polyhedron::makeCube(2,2,2);
auto volume = cube.ComputeFullVolume();
EXPECT_FLOAT_EQ(volume.volume, 8.0);
EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Z(), 0.0);
}
/////////////////////////////////////////////////
TEST(PolyhedronTest, CylinderTotalVolume)
{
auto cylinder = Polyhedron::makeCylinder(0.5,2,100);
auto volume = cylinder.ComputeFullVolume();
EXPECT_NEAR(volume.volume, 1.57, 0.01);
EXPECT_NEAR(volume.centroid.X(), 0.0, 1e-10);
EXPECT_NEAR(volume.centroid.Y(), 0.0, 1e-10);
EXPECT_NEAR(volume.centroid.Z(), 0.0, 1e-10);
}
///////////////////////////////////////////////////
TEST(PolyhedronTest, CubeNotSubmerged)
{
auto cube = Polyhedron::makeCube(1,1,1);
// water surface at z = 0
buoyancy::Plane waterSurface;
auto volume = cube.SubmergedVolume(ignition::math::Vector3d{0,0,2},
ignition::math::Quaterniond{1,0,0,0},
waterSurface);
EXPECT_FLOAT_EQ(volume.volume, 0.0);
EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Z(), 0.0);
}
///////////////////////////////////////////////////
TEST(PolyhedronTest, CylinderNotSubmerged)
{
auto cylinder = Polyhedron::makeCylinder(0.5,2,10);
// water surface at z = 0
buoyancy::Plane waterSurface;
auto volume = cylinder.SubmergedVolume(ignition::math::Vector3d{0,0,2},
ignition::math::Quaterniond{1,0,0,0},
waterSurface);
EXPECT_FLOAT_EQ(volume.volume, 0.0);
EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Z(), 0.0);
}
///////////////////////////////////////////////////
TEST(PolyhedronTest, CubeSubmerged)
{
auto cube = Polyhedron::makeCube(1,1,1);
// water surface at z = 0
buoyancy::Plane waterSurface;
// half of the cube is submerged
// -----
// -------| |--------
// -----
auto volume = cube.SubmergedVolume(ignition::math::Vector3d{0,0,0},
ignition::math::Quaterniond{1,0,0,0},
waterSurface);
EXPECT_FLOAT_EQ(volume.volume, 0.5);
EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Z(), -0.25);
// cube is fully submerged
// -------------------
// -----
// | |
// -----
volume = cube.SubmergedVolume(ignition::math::Vector3d{0,0,-2},
ignition::math::Quaterniond{1,0,0,0},
waterSurface);
EXPECT_FLOAT_EQ(volume.volume, 1.0);
EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Z(), -2.0);
// cube is slightly submerged at a 45 degree angle in roll
// /\
// / \
// -----\ /---------
// \/
volume = cube.SubmergedVolume(ignition::math::Vector3d{0,0,0.25},
ignition::math::Quaterniond{0.9238795, 0.3826834, 0, 0},
waterSurface);
EXPECT_NEAR(volume.volume, 0.21, 0.01);
EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
EXPECT_NEAR(volume.centroid.Z(), -0.15, 0.01);
}
///////////////////////////////////////////////////
TEST(PolyhedronTest, CylinderSubmerged)
{
auto cylinder = Polyhedron::makeCylinder(0.5, 2, 100);
// water surface at z = 0
buoyancy::Plane waterSurface;
// half of the cylinder is submerged
// ---
// -------| |--------
// ---
auto volume = cylinder.SubmergedVolume(ignition::math::Vector3d{0,0,0.0},
ignition::math::Quaterniond{1,0,0,0},
waterSurface);
EXPECT_NEAR(volume.volume, 0.785, 0.001);
EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Z(), -0.5);
// cylinder is fully submerged
// -------------------
// ---
// | |
// ---
volume = cylinder.SubmergedVolume(ignition::math::Vector3d{0,0,-4},
ignition::math::Quaterniond{1,0,0,0},
waterSurface);
EXPECT_NEAR(volume.volume, 1.57, 0.01);
EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Z(), -4.0);
// cube is half submerged at a 45 degree angle in roll
// --------
// ----| |------
// --------
volume = cylinder.SubmergedVolume(ignition::math::Vector3d{0,0,0},
ignition::math::Quaterniond{0.707,0.707,0,0},
waterSurface);
EXPECT_NEAR(volume.volume, 0.785, 0.001);
EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
EXPECT_NEAR(volume.centroid.Z(), -0.21, 0.01);
}
/////////////////////////////////////////////////
sdf::ElementPtr generateGeometryElem(const std::string& str)
{
std::ostringstream modelStr;
modelStr << "<sdf version='" << SDF_VERSION << "'>"
<< "<model name ='model'>"
<< "<link name ='link'>"
<< " <visual name ='vis'>"
<< " <geometry>"
<< str
<< " </geometry>"
<< " </visual>"
<< "</link>"
<< "</model>"
<< "</sdf>";
sdf::SDF sdf;
sdf.SetFromString(modelStr.str());
return sdf.Root()->GetElement("model")->GetElement("link")->GetElement("visual")->GetElement("geometry");
}
/////////////////////////////////////////////////
TEST(ShapeVolumeTest, CubeNoRotation)
{
sdf::ElementPtr boxElem = generateGeometryElem("<box><size>2 2 2</size></box>");
ShapeVolumePtr box = std::move(ShapeVolume::makeShape(boxElem));
const double fluidLevel = 0;
const std::vector<std::vector<double>> poses = {{0,0,1.0},
{0,0,0.5},
{0,0,0.0},
{0,0,-0.5},
{0,0,-1.},
{0,0,-1.5}};
const std::vector<std::vector<double>> expectedResult = {{0,0,0,0},
{2,0,0,-0.25},
{4,0,0,-0.5},
{6,0,0,-0.75},
{8,0,0,-1.0},
{8,0,0,-1.5}};
ignition::math::Pose3d pose;
for(size_t i=0; i<poses.size(); i++)
{
pose.Pos().X() = poses[i][0];
pose.Pos().Y() = poses[i][1];
pose.Pos().Z() = poses[i][2];
auto vol = box->CalculateVolume(pose, fluidLevel);
EXPECT_FLOAT_EQ(vol.volume, expectedResult[i][0]);
EXPECT_FLOAT_EQ(vol.centroid.X(), expectedResult[i][1]);
EXPECT_FLOAT_EQ(vol.centroid.Y(), expectedResult[i][2]);
EXPECT_FLOAT_EQ(vol.centroid.Z(), expectedResult[i][3]);
}
}
/////////////////////////////////////////////////
TEST(ShapeVolumeTest, CubeRotation)
{
sdf::ElementPtr boxElem = generateGeometryElem("<box><size>2 2 2</size></box>");
ShapeVolumePtr box = std::move(ShapeVolume::makeShape(boxElem));
const double fluidLevel = 0;
const std::vector<std::vector<double>> poses = {{0,0,1.5},
{0,0,1.0},
{0,0,0.5},
{0,0,0.0},
{0,0,-0.5},
{0,0,-1.},
{0,0,-1.5}};
const std::vector<std::vector<double>> expectedResult = {{0,0,0,0},
{0.343,0,0,-0.138},
{1.672,0,0,-0.305},
{4.000,0,0,-0.471},
{6.328,0,0,-0.713},
{7.657,0,0,-1.051},
{8.000,0,0,-1.500}};
ignition::math::Pose3d pose;
pose.Rot() = {0.9238795, 0.3826834, 0, 0};
for(size_t i=0; i<poses.size(); i++)
{
pose.Pos().X() = poses[i][0];
pose.Pos().Y() = poses[i][1];
pose.Pos().Z() = poses[i][2];
auto vol = box->CalculateVolume(pose, fluidLevel);
EXPECT_NEAR(vol.volume, expectedResult[i][0], 0.001);
EXPECT_FLOAT_EQ(vol.centroid.X(), expectedResult[i][1]);
EXPECT_FLOAT_EQ(vol.centroid.Y(), expectedResult[i][2]);
EXPECT_NEAR(vol.centroid.Z(), expectedResult[i][3], 0.001);
}
}
/////////////////////////////////////////////////
TEST(ShapeVolumeTest, CylinderNoRotation)
{
sdf::ElementPtr cylinderElem = generateGeometryElem("<cylinder><radius>0.5</radius><length>2</length></cylinder>");
ShapeVolumePtr cylinder = std::move(ShapeVolume::makeShape(cylinderElem));
const double fluidLevel = 0;
const std::vector<std::vector<double>> poses = {{0,0,1.0},
{0,0,0.5},
{0,0,0.0},
{0,0,-0.5},
{0,0,-1.},
{0,0,-1.5}};
// note since we are approximating final volume is 1.55 instead of 1.57
// (increase segments for precision)
const std::vector<std::vector<double>> expectedResult = {{0,0,0,0},
{0.39,0,0,-0.25},
{0.77,0,0,-0.5},
{1.16,0,0,-0.75},
{1.55,0,0,-1.0},
{1.55,0,0,-1.5}};
ignition::math::Pose3d pose;
for(size_t i=0; i<poses.size(); i++)
{
pose.Pos().X() = poses[i][0];
pose.Pos().Y() = poses[i][1];
pose.Pos().Z() = poses[i][2];
auto vol = cylinder->CalculateVolume(pose, fluidLevel);
EXPECT_NEAR(vol.volume, expectedResult[i][0], 0.01);
EXPECT_FLOAT_EQ(vol.centroid.X(), expectedResult[i][1]);
EXPECT_FLOAT_EQ(vol.centroid.Y(), expectedResult[i][2]);
EXPECT_FLOAT_EQ(vol.centroid.Z(), expectedResult[i][3]);
}
}
/////////////////////////////////////////////////
TEST(ShapeVolumeTest, CylinderRotation)
{
sdf::ElementPtr cylinderElem = generateGeometryElem("<cylinder><radius>0.5</radius><length>2</length></cylinder>");
ShapeVolumePtr cylinder = std::move(ShapeVolume::makeShape(cylinderElem));
const double fluidLevel = 0;
const std::vector<std::vector<double>> poses = {{0,0,0.5},
{0,0,0.0},
{0,0,-0.5}};
// note since we are approximating final volume is 1.55 instead of 1.57
// (increase segments for precision)
const std::vector<std::vector<double>> expectedResult = {{0,0,0,0},
{0.77,0,0,-0.21},
{1.55,0,0,-0.5},
{1.55,0,0,-1.0}};
ignition::math::Pose3d pose;
pose.Rot() = {0.707, 0.707, 0, 0};
for(size_t i=0; i<poses.size(); i++)
{
pose.Pos().X() = poses[i][0];
pose.Pos().Y() = poses[i][1];
pose.Pos().Z() = poses[i][2];
auto vol = cylinder->CalculateVolume(pose, fluidLevel);
EXPECT_NEAR(vol.volume, expectedResult[i][0], 0.01);
EXPECT_FLOAT_EQ(vol.centroid.X(), expectedResult[i][1]);
EXPECT_FLOAT_EQ(vol.centroid.Y(), expectedResult[i][2]);
EXPECT_NEAR(vol.centroid.Z(), expectedResult[i][3], 0.01);
}
}
/////////////////////////////////////////////////
TEST(ShapeVolumeTest, Sphere) {
sdf::ElementPtr sphereElem = generateGeometryElem("<sphere><radius>0.5</radius></sphere>");
ShapeVolumePtr sphere = std::move(ShapeVolume::makeShape(sphereElem));
const double fluidLevel = 0;
const std::vector<std::vector<double>> poses = {{0, 0, 0.5},
{0, 0, 0.25},
{0, 0, 0.0},
{0, 0, -0.25},
{0, 0, -0.5},
{0, 0, -1.0}};
const std::vector<std::vector<double>> expectedResult = {{0, 0, 0, 0},
{0.082, 0, 0, -0.088},
{0.262, 0, 0, -0.188},
{0.442, 0, 0, -0.313},
{0.523, 0, 0, -0.500},
{0.523, 0, 0, -1.000}};
ignition::math::Pose3d pose;
pose.Rot() = {0.9238795, 0.3826834, 0, 0}; // rotation has no impact
for(size_t i=0; i<poses.size(); i++)
{
pose.Pos().X() = poses[i][0];
pose.Pos().Y() = poses[i][1];
pose.Pos().Z() = poses[i][2];
auto vol = sphere->CalculateVolume(pose, fluidLevel);
EXPECT_NEAR(vol.volume, expectedResult[i][0], 0.001);
EXPECT_FLOAT_EQ(vol.centroid.X(), expectedResult[i][1]);
EXPECT_FLOAT_EQ(vol.centroid.Y(), expectedResult[i][2]);
EXPECT_NEAR(vol.centroid.Z(), expectedResult[i][3], 0.001);
}
}
/////////////////////////////////////////////////
/// Main
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

271
sitl_config/usv/usv_gazebo_plugins/worlds/buoyancy_plugin_demo.world.xacro Normal file
View File

@ -0,0 +1,271 @@
<?xml version="1.0" ?>
<!-- World containing buoyancy test models -->
<sdf version="1.6" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="basic_shape" params="name static *pose *shape *inertial">
<model name="${name}">
<xacro:insert_block name="pose" />
<static>${static}</static>
<link name="link">
<visual name="visual">
<geometry>
<xacro:insert_block name="shape" />
</geometry>
</visual>
<collision name="collision">
<geometry>
<xacro:insert_block name="shape" />
</geometry>
</collision>
<xacro:insert_block name="inertial" />
</link>
<plugin name="BuoyancyPlugin" filename="libbuoyancy_gazebo_plugin.so">
<fluid_density>1000</fluid_density>
<fluid_level>0.0</fluid_level>
<linear_drag>10.0</linear_drag>
<angular_drag>4.0</angular_drag>
<buoyancy name="buoyancy">
<link_name>link</link_name>
<geometry>
<xacro:insert_block name="shape" />
</geometry>
</buoyancy>
</plugin>
</model>
</xacro:macro>
<!-- calculates inertial components of a box with mass and dimensions (x, y, z) -->
<xacro:macro name="box_inertial" params="mass x y z">
<inertial>
<mass>${mass}</mass>
<inertia>
<ixx>${0.083 * mass * (y*y+z*z)}</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>${0.083 * mass * (x*x+z*z)}</iyy>
<iyz>0</iyz>
<izz>${0.083 * mass * (x*x+y*y)}</izz>
</inertia>
</inertial>
</xacro:macro>
<xacro:macro name="sphere_inertial" params="mass r">
<inertial>
<mass>${mass}</mass>
<inertia>
<ixx>${0.4 * mass * r * r}</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>${0.4 * mass * r * r}</iyy>
<iyz>0</iyz>
<izz>${0.4 * mass * r * r}</izz>
</inertia>
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertial" params="mass r l">
<inertial>
<mass>${mass}</mass>
<inertia>
<ixx>${0.0833333 * mass * (3 * r * r + l * l)}</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>${0.0833333 * mass * (3 * r * r + l * l)}</iyy>
<iyz>0</iyz>
<izz>${0.5 * mass * r * r}</izz>
</inertia>
</inertial>
</xacro:macro>
<world name='buoyancy_test'>
<scene>
<grid>false</grid>
<origin_visual>false</origin_visual>
<ambient>0.6 0.6 0.6 1</ambient>
<background>0.8 0.8 0.8 1</background>
</scene>
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose frame=''>4.84693 -4.51466 1.77033 0 0.275643 2.35619</pose>
<view_controller>orbit</view_controller>
</camera>
</gui>
<!-- water level for visual reference at z = 0-->
<model name="plane">
<static>true</static>
<pose>0.8 0.8 0 0 0 0</pose>
<link name="link">
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>2 2</size>
</plane>
</geometry>
<material>
<ambient>0.0 0.0 0.6 1</ambient>
</material>
</visual>
</link>
</model>
<!-- test_1: basic shape drop test | no rotation -->
<!-- density ~ 400 kg/m^3 (wood) | mass = 2kg | volume = 0.005m^3-->
<xacro:basic_shape name="1_box" static="false">
<pose>0.1 0 0.5 0 0 0</pose>
<box>
<size>0.171 0.171 0.171</size>
</box>
<xacro:box_inertial mass="2" x="0.171" y="0.171" z="0.171"/>
</xacro:basic_shape>
<xacro:basic_shape name="1_sphere" static="false">
<pose>0.4 0 0.5 0 0 0</pose>
<sphere>
<radius>0.106</radius>
</sphere>
<xacro:sphere_inertial mass="2" r="0.106"/>
</xacro:basic_shape>
<xacro:basic_shape name="1_cylinder" static="false">
<pose>0.7 0 0.5 0 0 0</pose>
<cylinder>
<radius>0.1</radius>
<length>0.159</length>
</cylinder>
<xacro:cylinder_inertial mass="2" r="0.1" l="0.159" />
</xacro:basic_shape>
<!-- test_2: basic shape drop test | rotation -->
<xacro:basic_shape name="2_box" static="false">
<pose>0.1 0.5 0.5 1.0 0 0</pose>
<box>
<size>0.171 0.171 0.171</size>
</box>
<xacro:box_inertial mass="2" x="0.171" y="0.171" z="0.171"/>
</xacro:basic_shape>
<xacro:basic_shape name="2_sphere" static="false">
<pose>0.4 0.5 0.5 1.0 0 0</pose>
<sphere>
<radius>0.106</radius>
</sphere>
<xacro:sphere_inertial mass="2" r="0.106"/>
</xacro:basic_shape>
<xacro:basic_shape name="2_cylinder" static="false">
<pose>0.7 0.5 0.5 1.0 0 0</pose>
<cylinder>
<radius>0.1</radius>
<length>0.159</length>
</cylinder>
<xacro:cylinder_inertial mass="2" r="0.1" l="0.159" />
</xacro:basic_shape>
<!-- test_3: basic shape float test | rotation -->
<xacro:basic_shape name="3_box" static="false">
<pose>0.1 1.0 -0.5 1.0 0 0</pose>
<box>
<size>0.171 0.171 0.171</size>
</box>
<xacro:box_inertial mass="2" x="0.171" y="0.171" z="0.171"/>
</xacro:basic_shape>
<xacro:basic_shape name="3_sphere" static="false">
<pose>0.4 1.0 -0.5 1.0 0 0</pose>
<sphere>
<radius>0.106</radius>
</sphere>
<xacro:sphere_inertial mass="2" r="0.106"/>
</xacro:basic_shape>
<xacro:basic_shape name="3_cylinder" static="false">
<pose>0.7 1.0 -0.5 1.0 0 0</pose>
<cylinder>
<radius>0.1</radius>
<length>0.159</length>
</cylinder>
<xacro:cylinder_inertial mass="2" r="0.1" l="0.159" />
</xacro:basic_shape>
<!-- test_4: complex object -->
<model name="4_complex">
<pose>0.1 1.5 0.5 0 0 0</pose>
<link name="link_cylinder">
<visual name="visual">
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.3</length>
</cylinder>
</geometry>
</visual>
<collision name="collision">
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.3</length>
</cylinder>
</geometry>
</collision>
<xacro:cylinder_inertial mass="5" r="0.1" l="0.3" />
</link>
<link name="link_box">
<pose>0 0.2 0.15 0 0 0</pose>
<visual name="visual">
<geometry>
<box>
<size>0.171 0.3 0.171</size>
</box>
</geometry>
</visual>
<collision name="collision">
<geometry>
<box>
<size>0.171 0.3 0.171</size>
</box>
</geometry>
</collision>
<xacro:box_inertial mass="10" x="0.171" y="0.3" z="0.171"/>
</link>
<joint name="joint" type="fixed">
<parent>link_box</parent>
<child>link_cylinder</child>
</joint>
<plugin name="BuoyancyPlugin" filename="libbuoyancy_gazebo_plugin.so">
<fluid_density>1000</fluid_density>
<fluid_level>0.0</fluid_level>
<linear_drag>5.0</linear_drag>
<angular_drag>2.0</angular_drag>
<buoyancy name="buoyancy_cylinder">
<link_name>link_cylinder</link_name>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.3</length>
</cylinder>
</geometry>
</buoyancy>
<buoyancy name="buoyancy_box">
<link_name>link_box</link_name>
<geometry>
<box>
<size>0.171 0.3 0.171</size>
</box>
</geometry>
</buoyancy>
</plugin>
</model>
</world>
</sdf>

26
sitl_config/usv/usv_msgs/CMakeLists.txt Normal file
View File

@ -0,0 +1,26 @@
cmake_minimum_required(VERSION 2.8.3)
project(usv_msgs)
find_package(catkin REQUIRED COMPONENTS
message_generation
message_runtime
std_msgs
)
add_message_files(
FILES
RangeBearing.msg
)
generate_messages(
DEPENDENCIES
std_msgs
)
catkin_package(
CATKIN_DEPENDS message_generation message_runtime std_msgs
)
include_directories(
${catkin_INCLUDE_DIRS}
)

5
sitl_config/usv/usv_msgs/msg/RangeBearing.msg Normal file
View File

@ -0,0 +1,5 @@
std_msgs/Header header
float32 range
float32 bearing
float32 elevation

22
sitl_config/usv/usv_msgs/package.xml Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0"?>
<package format="2">
<name>usv_msgs</name>
<version>0.0.1</version>
<description>The usv_msgs package. Contains message definitions to support
the development of USV simulations for the Virtual Maritime RobotX
Competition. </description>
<maintainer email="jonathan.wheare@flinders.edu.au">Jonathan Wheare</maintainer>
<license>Apache 2.0</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_runtime</build_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>message_generation</exec_depend>
<exec_depend>std_msgs</exec_depend>
<export>
</export>
</package>

672
sitl_config/usv/vrx_gazebo/CHANGELOG.rst Normal file
View File

@ -0,0 +1,672 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package vrx_gazebo
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.3.0 (2019-12-26)
------------------
* Fix pose frame in scan_and_dock1.world
* make marker id more intelligible
* documenting new yaw parameter
* adding direction indicator to visual markers for stationkeeping and wayfinding
* Fix perception plugin object types.
* Fix offset in 2016 dynamic dock.
* Center all placards in the dock bays.
* Update expected type in perception plugin for surmark objects.
* Set initial and ready states to 10 seconds in dock tasks.
* Merged in worlds_refactor (pull request #213)
Refactor VRX worlds location
Approved-by: Coline Ramee <coline.ramee@gatech.edu>
Approved-by: Brian Bingham <briansbingham@gmail.com>
* Make the exit gate a blue-red gate.
* Refactor VRX worlds location.
* Updated perception score according to the 1.4 version of the tasks document.
* dealing with fail and style tweaks
* adding a modified nav course world to speed up testing
* changing scoring method to be consistent with documentation
* tracking down more bad dock worlds
* disables color checker in example models and by default in dock.xacro
* prohibiting p3d sensor in compliance check
* update old worlds
* changing task names in standalone
* initializing error total
* adding worlds and models used in Phase2 for Phase 3 practice
* adding xacro parameter to specify task name in plugin
* adding commad line arg to keydrive launch
* remapping for new namespace
* Add sail and rudder link to wamv base, as well as turn post to wind direction arrow
* Extra path to be added to GAZEBO_MODEL_PATH.
* Set GAZEBO_MODEL_PATH while playing back.
* adding practice worlds for stationkeeping and wayfinding
* Contributors: Brian Bingham <briansbingham@gmail.com>, Carlos Aguero, Carlos Agüero <cen.aguero@gmail.com>, Tyler Lum <tylergwlum@gmail.com>, m1chaelm
1.2.6 (2019-10-04)
------------------
* new worlds with only one dock
* Scan and dock worlds - models are aleady present
* Merged in stationkeeping_wind (pull request #187)
Default Wind for Tasks
Approved-by: Brian Bingham <briansbingham@gmail.com>
* Adding stdout messages when to help with debugging color sequence checking service
* Adding navigation course models and worlds.
* Adding practice perception worlds.
* adding dock worlds and associated models to support dress rehearsal practice
* style
* Functionality to turn off color checker in the scan_dock_scoring_plugin
* restore rms_error topic name
* merging default
* Merged in issue_164 (pull request #188)
Fixing dock scoring plugin for Gazebo 9
Approved-by: Brian Bingham <briansbingham@gmail.com>
* tweak to generator to allow calling same macro more than once
* towards permutations
* adding empty directory
* Remove dock_2018_dynamic/model.sdf
* fix meanError equation
* switching from RMS error to mean pose error
* new heading error formula
* adding model_name paramter to dock.xacro for flexibility in generating automated tasks
* state diagram
* images for wiki
* Tweak
* Double activation zone.
* Style.
* removing activation zone viz from world.xacro
* adding gzmsgs
* merging with issue_164 and moving dock closer to WAMV
* increment
* Moving dock closer to WAMV
* Functional
* temporary
* working on brute force docking solution
* easier setup, but not working?
* adding insert blocks to generate debug markers for wayfinding and stationkeeping tasks
* Tweak
* Fixing dock scoring plugin for Gazebo 9.
* Move lock_guard.
* code check whitespace
* reinit when getting new color seq.
* adding default wind to dock, scan-dock and wayfinding tasks
* connecting usv_wind_plugin to stationkeeping world
* Model default is off.
* adding params to dock.xacro for task/trial generation
* adds light buoys
* changes to debugging messages to help find issues with docking
* More Python3 tweaks.
* Remove .keys().
* Easy docking example for testing
* adding wamv_locked argument to vrx.launch
* adding fog and ambient elements to sandisland xacro
* messing with plugins
* simple macro for inserting a model - useful for task generation
* a bit of a hack, but allows us repeated use of same macro
* Contributors: Brian Bingham <briansbingham@gmail.com>, Carlos Aguero, Carlos Agüero <cen.aguero@gmail.com>, m1chaelm
1.2.5 (2019-09-19)
------------------
* Remove <announce_symbol> parameter.
* moving static docks to waterline
* Use __future_\_ import
* Adding running_duration parameter to the nav_challenge.xacro.
* Tweak buoyany primative so that 4x4 dock element floats at water line instead of above
* Adding parameters to dock.xacro to for auto. generation of tasks
* adding params to dock xacro for the benefit of automatically generating tasks
* Changes for code checker
* Contributors: Brian Bingham <briansbingham@gmail.com>, Carlos Aguero, Carlos Aguero <caguero@openrobotics.org>
1.2.4 (2019-09-12)
------------------
* Add protobuf-dev as a buildtool depend for vrx_gazebo
* Tweaks: Adding missing line breaks and adjusting perspective file for new topic namespace
* Contributors: Brian Bingham <briansbingham@gmail.com>, Tully Foote <tfoote@osrfoundation.org>
1.2.3 (2019-09-12)
------------------
* Minor maintenance updates.
* Contributors: Carlos Aguero
1.2.2 (2019-09-06)
------------------
1.2.1 (2019-09-05)
------------------
* Removing other instances of <placard_color_shape>.
* Just a couple comments to support task tutorial for changing the shape and color
* Style
* Tweak
* Remove extra joint in colored totems.
* Tweak
* Wrap the entire fog element inside a xacro block.
* merged master
* updated waypoint plugin
* merged
* new waypoint marker class + updated station keeping
* Add hardcoded robot namespace to thrusters
* Namespace tweaks.
* Style
* Remove oyaml use
* Comment out missing files, as a workaround
* namespace fix
* gazebo 7 compatibility
* force vectors are correct; scaling added
* merged with master
* handle gazebo 7
* included for ignition stuff
* removed whitespace at the end of line
* added marker for station keeping
* added waypoints for wayfinding task
* Contributors: Brian Bingham <briansbingham@gmail.com>, Carlos Aguero, Carlos Aguero <caguero@openrobotics.org>, Rumman Waqar <rumman.waqar05@gmail.com>, Tyler Lum <tylergwlum@gmail.com>
1.2.0 (2019-08-19)
------------------
* Merged default into topic_namespace_generation
* Modify teleop examples to use namespaces
* Add a namespace parameter to the launch file.
* Merged default into topic_namespace_generation
* now installs the custom protobuf message types
* Adding dependency on wave_gazebo.
* Merged in floating_docks (pull request #146)
Floating docks
Approved-by: Carlos Agüero <cen.aguero@gmail.com>
* fixed triangle for dynamic
* dock 016 dynamic base name fixed
* Tweak default duration.
* fixed triangle position
* incremental
* uperception scoring: updated ymal files and fixed end condition bug when not looping
* lowered gaines for waves
* styling and documentation
* perception plugin refactored to be more fllexible
* Fully functional solution, with urdf file modified when calling spawn_wamv.bash and giving proper model dirs to wamv_gazebo and wamv_description
* Fully functional solution, with model.config errors only when non_competition_mode:=false
* tracking dock model sdf's
* static dynamic refactor complete
* files renamed
* halfway through dock name refactor
* static dock preformance improved
* incremental
* scan_dock now dynamic dock
* added static dock counter parts
* adding static dock option
* merged with default
* Merged default into Add-Option-To-Hide-Gazebo-Topics
* Tweak in wait_until_gzserver_is_up().
* Merged in issue_92 (pull request #157)
Fix issue #92
Approved-by: Tyler Lum <tylergwlum@gmail.com>
* Camera control plugin.
* Clean launch file
* Improve sandisland.launch clarity
* Improve spawn_wamv.bash comments clarity
* Change argument from competition_mode to non_competition_mode to fix publishing errors
* Add competition_mode argument
* Concatenate original Gazebo Model Path to new value
* Rm fake vrx_gazebo/models and make it work, needs testing
* Change model names to No Model
* Use if and unless in launch file for roslaunch spawn
* Deterministic wind.
* Merged default into topic_namespace_generation
* Fix namepsaces to work with robotNamespace parameter
* Merge changes with default branch
* Merged default into gps_plugin
* Add v3d plugin - this publishes a vecotr based on the world frame velocity in Gazebo
Update gps configuration to add gazebo gps and v3d plugins to standard configuration
* Add plugin for ROS interface to gazebo GPS sensor.
* Merged default into gps_plugin
* added more params to dock xacro
* fix nav_challenge
* resolved merge conflicts
* dock xacro fixed
* incremental
* added scoring plugin to the nav_challenge xacro for world generation
* Remove redundant sleep
* Update script to run rosrun xacro to make final urdf, seems to work well
* Add arg parsing for spawn_wamv, does not work with urdf.xacros yet
* Add wait for gzserver, might need testing
* Spawn wamv with bash script, still needs improvement
* Add fake models in vrx_gazebo, wamv_description, wamv_gazebo to avoid GAZEBO_MODEL_PATH errors
* Merged default into Add-Option-To-Hide-Gazebo-Topics
* Merged in wamv-lock-at-run-time (pull request #152)
Wamv lock at run time
Approved-by: Carlos Agüero <cen.aguero@gmail.com>
* Fix spacing in yaml
* Tweaks.
* Fix nav_challenge_deep
* Add nav_challenge scoring plugin and gates
* Update xacros/dock.xacro and xacros/scan_and_dock.xacro to match original (add missing parameters)
* Add verbose and paused option for improved playback
* Merged in ocean-wave-xacro (pull request #150)
Ocean wave xacro
Approved-by: Marshall Rawson <marshallrawson@osrfoundation.org>
* cleaning
* wamv-locked by plugin permanetely
* added spinning out detection
* Merged default into ocean-wave-xacro
* incremental
* incremental
* Change big shallow and deep world yaml to individual tasks
* nodes now shutdown and use safe_yaml
* VRX_DEBUG now linked to enable_ros_network
* fixed ambient light and fog
* print to rospy.loginfo
* fixed xacro insert block ordering issue
* includes the yaml files in repository
* functional?
* Change to default to true, as it should
* Add enable_ros_network argument
* incremental
* added plate and sphere models
* functional for cubes
* incremental
* made taskMsgPub and taskMsg protected
* Now published taskMsg in OnFinished
* added time stamp to OnFinsihed
* moved exit to scoring plugin.
* removed old debug msg
* Fix build issue by resolving typo with ROS_ERROR msg
* styling
* functional, does not shutdown gzclient
* functional
* approximate 4x4 dock block as a sphere
* merge
* Merged in compliance-refactor (pull request #139)
Compliance refactor
Approved-by: Brian Bingham <briansbingham@gmail.com>
Approved-by: Tyler Lum <tylergwlum@gmail.com>
* fix print
* styling
* added overall compliance error
* Removed old print statments, Added yaml file confirmation ROSINFO message
* fixed dock inertial issues
* fixed error message
* example_sensor_config.yaml edited online with Bitbucket
* merge
* functional
* merge
* Contributors: Carlos Aguero, Carlos Agüero <cen.aguero@gmail.com>, Jonathan Wheare <jonathan.wheare@flinders.edu.au>, Marshall Rawson <marshallrawson@osrfoundation.org>, MarshallRawson, MarshallRawson <marshallrawson@osrfoundation.org>, Tyler Lum <tylergwlum@gmail.com>
1.1.2 (2019-07-10)
------------------
* Merged in world-gen-bug-fix (pull request #145)
world gen bug fix
Approved-by: Tyler Lum <tylergwlum@gmail.com>
* Workaround to fix compile errors on Kinetic
The version of ign-math2 present in Ubuntu Xenial (2.2.3) lacks
of some features (Zero or Length) implemented starting on 2.3.x.
This change add some preprocessors defines to workaround the
problem. A more elegant solution would be ideal.
* World generator now imbeds the coordinate with axies specified by the yaml file for easy post gen sorting
* added a space parameter to the bounding boxes
* Contributors: Jose Luis Rivero <jrivero@osrfoundation.org>, MarshallRawson, MarshallRawson <marshallrawson@osrfoundation.org>, Tyler Lum <tylergwlum@gmail.com>
1.1.1 (2019-07-03)
------------------
* Reinterpret the wind 'gain' parameter. Set defaults to zero
* Add replaces cluase to vrx_gazebo
* Contributors: Brian Bingham <briansbingham@gmail.com>, Jose Luis Rivero <jrivero@osrfoundation.org>
1.1.0 (2019-07-01)
------------------
* Merged in issue#94-buoyancy (pull request #122)
Issue#94 buoyancy
Approved-by: Carlos Agüero <cen.aguero@gmail.com>
* changing buoy buoyancy to sphere, adding feature to generator
* Merge from default.
* Merged in rename_scan_dock (pull request #133)
renaming "dock" and "scan and dock" files to match new task names
Approved-by: Brian Bingham <briansbingham@gmail.com>
* renaming files to match new task names
* updating default values in example
* now interpreting time of spawned objects relative to start of running state
* removed overidden OnFinished method
* clean up: removed overriden methods that made no alterations from parent class
* added a gzmsg where missing to overriden methods
* added a gzmsg to default implementations of OnReady, OnRunning, OnFinished
* New score policy.
* Update scores.
* Light buoy with 2 seconds off.
* Now impliments Enviornment variable instad of debug sdf parameters
* Incremental
* ready for detailed lidar spec input
* updated markers + polyform models for wave buoyancy
* functional. no recording
* added wind to navigation task
* Add extra_gazebo_args to all launch files and remove recording arg
* Light buoy should now be synced with scoring and visual plugin through the definition in scan_and_dock_b.launch
* updated vrx model buoyancy plugin
* Add playback.launch to play back recorded log files
* Add recording functionality to sandisland, and add extra_gazebo_args to optionally choose record path
* Incremental
* no longer supported for gz7 or older
* clunky version - but visuals and placards stay with dock for 2018
* working version with dock buoyancy, but need to attach placards
* first cut - dock elements work, but to build a full dock need to add joints between elements
* changing perception transition
* attempt build gz <=7 issue
* attempt fix build issue
* incremental
* Added allowences for post_Y and moved wamv_imu, wamv_gps default locations to be within compliance
* attempt fix gz 7 compatability issue
* functionsal. needs cleaning
* initializing sampleCount to 0 and change to int
* added wind capabilities
* Merged in add-wind-support-for-yaml-world-gen (pull request #115)
added support for wind in yaml world gen and updates wiki
Approved-by: Carlos Agüero <cen.aguero@gmail.com>
* Style
* testing side by side scaling
* latest case
* case 2
* case 0
* Testing scalability of new implementation - updated hgignore vmrc->vrx
* code styling
* styling
* styling
* incremental
* build
* merge
* removed unused header
* incremental
* finished rename
* added support for wind in yaml world gen and updates wiki
* added support for default wamv effects on vrx.launch and sandisland.launch
* incremental
* Reshow instructions after some speed change updates (match with twist_teleop_keyboard)
* Remove extra diffdrive yaml file
* Implement new getch function to fix output issues
* Remove set_thrust_angle parameter
* Reverse angles when teleoperation.
* merge
* incremental
* incremental
* incremental
* styling fixes
* made more user friendly
* Now builds. Currently, the MOC in CMake requires the header and source file to be in the same directory.
* fixed ros issues
* merging default
* Add new .yaml file for joy teleop to publish thrust angles
* Add settable max_angle parameter upon usv_keydrive launch startup
* Add ability to change thrust angle speed
* Add key2thrust_angle.py node that allows for h and ; to control thruster angle
* Merge from default.
* Merge from default, conflicts and style.
* Merge default
* fixed builf issues
* Merged in remove-README (pull request #111)
removed README.txt from yaml_world_genreeration and created wiki page instead
Approved-by: Carlos Agüero <cen.aguero@gmail.com>
* Add style checker.
* removed README.txt from yaml_world_genreeration and created wiki page instead
* finish rename
* fix build issue
* incremental
* fix build issue
* renamed xacro
* updates xacro
* fix build issues
* incremental
* Merged in Issue#90_YAML_world_genreation (pull request #102)
Issue#90 YAML world generation
Approved-by: Carlos Agüero <cen.aguero@gmail.com>
* No markdown
* Use markdown
* Fix typos.
* Added thruster compliance
* added more flexibility to permitted parameters
* fixed math error
* Partially fix compile issues in code with Task msg, still issue with FormatTime and duration
* Attempt to fix build issue by adding Qt5IncludeDirs and spreading out find_packages
* added roslaunch params
* styling
* filled out the SensorCompliance. It is formatted by the sensors_compliance files
* Attempt to implement basic GUI overlay to show VRX Task Info. Stuck on build issues with Qt5
* added white spaceing to make more readable
* edited launch file
* incremental
* merge for api update
* merged with Issue#97-yaml-thruster-configuration for api update
* fixed launch file
* changed sdf sytax for passing models to be effected by wind and addressed styling
* Changing name of ocean model in sandisland test
* adding pdf for pr
* Remove unneeded images and add documentation
* adding to docs and allowing for both PMS and CWR wavefield models
* merged. expanded xacro capabilities
* Add back unused functions in utils.py for future compliance tests
* Move gazebo thruster config tags to new function
* Update python files using flake8, all files pass
* Update Changelog and remove available_sensors param
* Remove unnecessary files
* Add generate_wamv launch and bin files
* Clean create_xacro_file() function and add comments
* Remove old sensor and thruster config files
* Remove unused utils.py functions
* Remove unneeded files and improve clarity with documentation
* Added support for any parameter to be evaluated as lambda vs string. updated README.
* fixed functional evaluation bug
* testing wave fields
* Added support for ** xacro inserts. used as normal parameters, but prfaced with /**. (this is to help with the wind and ave plugins in the future.
* Added wind xacro (utilizes xacro inserts). NOTE: wind plugin only applies force to one link per model
* Working implementation of generate_wamv, which takes both sensor and yaml files
* adding exponential increase in wave field and LaTeX doc^C
* CMakeLists improvement and spacing
* changelog update
* added more to README.txt, added scene_macro and sandisland2 to give more confiuration flexibilty to the worlds. NOTE: time SDF is being written into the world file correctly(I think), but gazebo appears to not change anything under the scene tab in the gui.
* Make thruster config with yaml work without affecting use of sensor yaml config, still need to clean up
* Move engine.xacro to thrusters directory to allow for different types of thrusters
* more README stuff
* Merge
* increment
* Copy similar sensor yaml files for thrusters, needs to be adjusted, particularly utils.py
* increment
* merging default into branch
* README incremental
* added more comments
* Added Quick Start Instructions
* added README for filling out the YAML file
* fixed for real this time
* fixed build problem
* Merged in yaml_sensor_configuration (pull request #99)
Yaml sensor configuration
Approved-by: Carlos Agüero <cen.aguero@gmail.com>
* added README
* updated README
* Merged in ykhky/vrx/Issue#49-collision-detection (pull request #94)
Issue#49 collision detection
Approved-by: Carlos Agüero <cen.aguero@gmail.com>
* calling on collision
* spelling correcting
* move variables to correct section in header file
* OnCollision virtual + documenting stuff+ renaming variables
* remove extra bracket
* logging collisions and timestamps
* spacing
* removing world name hard code
* remove cout + adding buffer to nav task
* formatting + exposing collision buffer
* Doc format
* counter + cleanup
* frequency of collision reporting reduced to 1/3 Hz
* added collision detection node
* restored sensors params to sandisland.launch
* Added wavegauge plugin to visualize physical wave height. Setup example with buoy world. Implemented simplified wave height calculation in WavefieldSampler for regularly spaced grid (steepness=1=0).
* removed directory xacro checking and variance features
* incremental
* incremental
* Added sequence override option in YAML
* verifying with examples
* increased flexibility of compliance.py
* fixed xacro parsing bug
* Added support for sequence breakout specified in yaml file
* Added xacros for feild elements
* toward buoy examples
* Added launch file
* Script will now be installed, added coordinate generation
* merge
* incremental
* merge
* now auto-generates the world.xacro(may need to be changed to devel) file in src
* incremental
* merge, added launch file
* incremental
* merge
* made branch
* fixed build issue for real this time
* fixed build issue
* commited setup.py, removed unrelated files from vrx_gazebo_python
* scripts will now be installed
* updated readme, changed operation procedure, still not installed
* fixed styling problems with flake8, updated readme
* incremental
* Add mono_camera mesh and .sdf .config files with correct collision and inertia
* changed directory, added launch file support
* incremental, now supports macros with no parameters
* made boiler plate usage more flexible
H: Enter commit message. Lines beginning with 'HG:' are removed.
* incremental
* Add sensor_post_arm.dae
* Break sensor_post.dae into two files, then fix model
* Add fixed joint and position arm relative to post
* Add sensor post mesh with correct collision and inertia
* merging default into named branch
* incremental
* Added readme
* moved script. Improved File System
* Flip the ground stations and spread the posts.
* added chairs
* Adding chairs.
* Change cpu case collision box from 1 box to 2 boxes
* incremental
* Add CPU cases only in VRX configuration + remove redundant pose info
* removed pose 0 tags from models
* Tweak indentation.
* documentation, incremental
* incremental
* incremental
* fixed battery/model.sdf
* Add 3D Lidar mesh and put it on WAM-V
* Fix formatting (tab->spaces, etc.)
* Fix .sdf file
* Add CPU case model to WAM-V
* review commented implemented
* finished ground station without chairs
* added table
* added tent and antenna model
* incremental
* incremental
* Added Batteries to vrx_gazebo/models(sdf format) and macro(urdf format) to place on wamv
* Updated texture with a flat area in the beach to place the tents in the future.
* Tweaks.
* Using WAM-V yaw in setting where objects are moved during perception task
* Minor tweak.
* moving station keeping goal closer to wam-v spawn point
* turning wind off to better test - tweaking waypoints in wayfinding task example
* Tweaking positions and adding post and navigation course.
* Restoring cameras and laser visuals and creating demo.launch
* Sandisland texture, sensor meshes and extra objects.
* Restore generate_xxx
* Tweak CMakeLists.txt
* Run the plugin at 1Hz sim time.
* Use sim time to update the light buoy plugin.
* Fix placard symbols.
* Deterministic sequence in light buoy plugin
* Use a ROS subscription for changing the color sequence.
* Modify velodyne configuration to set intensity filtering
Alter ocean laser retro to be filtered by the lidar sensor
* Remove more trailing whitespace
Redundant codepath in usv_gazwebo_dynamics_plugin removed. Euler values now derived identically between gazebo 7 and 9.
* Fix trailing whitespace
* Use auto keyword
* Fix ign method for staionkeeping_scoing_plugin
* Alter patch to use .Ign method to convert between gazebo::math and Ignition::math types
* Fix indention
* Add support for Kinetic/Gazebo-7
The ignition types are mostly kept, with code transforming from the methods deprecated in gazebo-8
* adding a rqt config file for a perspective task tutorial
* Issue #23: Coordinate the physics and visualization of the wave field
1. Use the asv_wave_sim_gazebo_plugins package for wave field visualisation and depth calculation.
2. Update the buoyancy and dynamics plugins for buoyancy calculations.
3. Update sdf and xacro for models that require buoyancy.
4. Replace the ocean model with ocean_waves in the sandisland world.
* Red placards and rearrange a bit the sensors.
* Port to VRX code using Gazebo9.
* Contributors: Brian Bingham <briansbingham@gmail.com>, Carlos Aguero, Carlos Aguero <caguero@osrfoundation.org>, Carlos Agüero <caguero@osrfoundation.org>, Carlos Agüero <cen.aguero@gmail.com>, Jonathan Wheare <jonathan.wheare@flinders.edu.au>, MarshallRawson, Rhys Mainwaring <rhys.mainwaring@me.com>, Rumman Waqar <rumman.waqar05@gmail.com>, Tyler Lum <tylergwlum@gmail.com>, Youssef Khaky <youssefkhaky@hotmail.com>, m1chaelm
1.0.1 (2019-03-01)
------------------
1.0.0 (2019-02-28)
------------------
* Merge from default.
* tweak the example
* addressing missing documentation and simplifying by removing start_index parameter
* Removing leftovers
* Tweaks
* Style changes.
* Merge from default.
* Merged in symbols_dock_part3 (pull request #66)
Scan and dock scoring plugin - Part3
Approved-by: Brian Bingham <briansbingham@gmail.com>
* syncing with default
* Change to use real-time pose for error calculation
* Simplifying by removing some of the timing bits that appear to be specific to the ARIAC Population plugin
* Renaming internal
* Rename part 2 of 2
* Renaming part 1
* Adding scoring and running a quick test
* Functional plugin prototype
* Merge from default.
* Two variants of the scan and dock.
* Remove unused code.
* updating topic names so they match tasks
* tweak
* now publishing waypoints as a latched GeoPath message
* fix function name PublishWaypoints
* only start scoring when in running state
* fixing task names
* Re-basing poplulation plugin to scoring_plugin and adding ROS functionality. Incomplete, but going home to work from there
* tweak a comment
* tweak
* Granting extra points for docking.
* Tweaks
* PR feedback
* Wrong merges.
* Merge from default.
* Merged in wayfinding-task (pull request #69)
Wayfinding task
Approved-by: Brian Bingham <briansbingham@gmail.com>
* remove pointless latch in waypoints topic
* fix timer
* publish at 1 Hz, latch waypoints topic, tweaks
* Merge from default.
* Tweaks.
* Merge from default.
* Prototype of population plugin - only for a single object at a time. Moves it back to original position when done
* Updates to PopulationPlugin
* Prototype - using PopulationPlugin straight from ARIAC source
* Remove warnings.
* More vrx updates.
* Merge from symbols_dock_part2
* More vrx tweaks.
* Merge from default.
* More updates.
* Porting to Gazebo 9
* Custom tweaks
* Updating the station keeping task.
* More leftovers.
* Rename vmrc to vrx.
* Contributors: Brian Bingham <briansbingham@gmail.com>, Carlos Aguero, Carlos Aguero <caguero@osrfoundation.org>, Carlos Agüero <cen.aguero@gmail.com>, Michael McCarrin <mrmccarr@nps.edu>, m1chaelm
0.3.2 (2018-10-08)
------------------
* Include jrivero as maintainer of the ROS packages
* Contributors: Jose Luis Rivero <jrivero@osrfoundation.org>
0.3.1 (2018-10-05)
------------------
0.3.0 (2018-09-28)
------------------
* Tweak
* Rename robotx_gazebo to vrx_gazebo and remove metapackage.
* Contributors: Carlos Agüero <caguero@osrfoundation.org>

349
sitl_config/usv/vrx_gazebo/CMakeLists.txt Normal file
View File

@ -0,0 +1,349 @@
cmake_minimum_required(VERSION 2.8.3)
project(vrx_gazebo)
# We need erb to process the .world.erb files.
find_program(ERB_EXE_PATH erb)
if(NOT ERB_EXE_PATH)
message(FATAL_ERROR "Could not find the `erb` tool. Try `sudo apt-get install ruby`")
endif()
# For Qt
set(CMAKE_AUTOMOC ON)
set(CMAKE_AUTOUIC ON)
# Set policy for CMake 3.1+. Use OLD policy to let FindBoost.cmake, dependency
# of gazebo, use quoted variables in if()
if(POLICY CMP0054)
cmake_policy(SET CMP0054 OLD)
endif()
# Use OLD policy to be able to use *_generate_services_cpp before they exist,
# without error.
if(POLICY CMP0046)
cmake_policy(SET CMP0046 OLD)
endif()
find_package(Qt5Widgets REQUIRED)
find_package(Qt5Core REQUIRED)
find_package(Protobuf REQUIRED)
find_package(gazebo REQUIRED)
find_package(catkin REQUIRED COMPONENTS
gazebo_dev
geographic_msgs
message_generation
roscpp
rospy
std_msgs
wamv_gazebo
wave_gazebo
xacro
)
########################
## Message generation ##
########################
add_message_files(
FILES
Task.msg
Contact.msg
)
add_service_files(
FILES
ColorSequence.srv
)
# Python scripts setup
catkin_python_setup()
generate_messages(
DEPENDENCIES
std_msgs
)
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS wamv_gazebo wave_gazebo xacro gazebo_dev geographic_msgs std_msgs message_runtime
LIBRARIES scoring_plugin
)
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
include_directories(include ${catkin_INCLUDE_DIRS}
${Qt5Core_INCLUDE_DIRS}
${QT_USE_FILE}
${GAZEBO_INCLUDE_DIRS}
${CMAKE_CURRENT_BINARY_DIR}/msgs
)
link_directories(${GAZEBO_LIBRARY_DIRS} ${CMAKE_CURRENT_BINARY_DIR}/msgs)
add_subdirectory(msgs)
add_definitions(${QT_DEFINITIONS})
# Library for displaying waypoint markers
add_library(waypoint_markers src/waypoint_markers.cc)
target_link_libraries(waypoint_markers ${catkin_LIBRARIES})
add_dependencies(waypoint_markers ${catkin_EXPORTED_TARGETS})
install(TARGETS waypoint_markers
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# Plugin for setting color of light buoy.
add_library(light_buoy_plugin src/light_buoy_plugin.cc)
target_link_libraries(light_buoy_plugin
${catkin_LIBRARIES}
light_buoy_colors_msgs
)
add_dependencies(light_buoy_plugin ${catkin_EXPORTED_TARGETS}
light_buoy_colors_msgs
)
install(TARGETS light_buoy_plugin
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# Plugin for setting color and shape of a placard.
add_library(placard_plugin src/placard_plugin.cc)
target_link_libraries(placard_plugin
${catkin_LIBRARIES}
dock_placard_msgs
)
add_dependencies(placard_plugin
${catkin_EXPORTED_TARGETS}
dock_placard_msgs
)
install(TARGETS placard_plugin
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# A generic scoring plugin.
add_library(scoring_plugin src/scoring_plugin.cc)
target_link_libraries(scoring_plugin
${catkin_LIBRARIES}
)
add_dependencies(scoring_plugin
${catkin_EXPORTED_TARGETS}
vrx_gazebo_generate_messages_cpp
vrx_gazebo_generate_services_cpp
)
install(TARGETS scoring_plugin
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# Plugin for scoring the navigation challenge task.
add_library(navigation_scoring_plugin src/navigation_scoring_plugin.cc)
target_link_libraries(navigation_scoring_plugin
${catkin_LIBRARIES}
scoring_plugin)
add_dependencies(navigation_scoring_plugin ${catkin_EXPORTED_TARGETS})
install(TARGETS navigation_scoring_plugin
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# Plugin for scoring the station keeping challenge task.
add_library(stationkeeping_scoring_plugin src/stationkeeping_scoring_plugin.cc)
target_link_libraries(stationkeeping_scoring_plugin
${catkin_LIBRARIES}
scoring_plugin
waypoint_markers)
add_dependencies(stationkeeping_scoring_plugin ${catkin_EXPORTED_TARGETS})
install(TARGETS stationkeeping_scoring_plugin
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# Plugin for scoring the waypoint navigation challenge task.
add_library(wayfinding_scoring_plugin src/wayfinding_scoring_plugin.cc)
target_link_libraries(wayfinding_scoring_plugin
${catkin_LIBRARIES}
scoring_plugin
waypoint_markers)
add_dependencies(wayfinding_scoring_plugin ${catkin_EXPORTED_TARGETS})
install(TARGETS wayfinding_scoring_plugin
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# Plugin for scoring the scan and dock task.
add_library(scan_dock_scoring_plugin src/scan_dock_scoring_plugin.cc)
target_link_libraries(scan_dock_scoring_plugin
${catkin_LIBRARIES}
scoring_plugin
light_buoy_colors_msgs
dock_placard_msgs
)
add_dependencies(scan_dock_scoring_plugin
${catkin_EXPORTED_TARGETS}
light_buoy_colors_msgs
dock_placard_msgs
)
install(TARGETS scan_dock_scoring_plugin
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
if(${GAZEBO_MAJOR_VERSION} GREATER 7)
# Plugin for Task Info GUI Overlay
add_library(gui_task_widget SHARED ${headers_MOC}
src/gui_task_widget.cc
)
target_link_libraries(gui_task_widget
${catkin_LIBRARIES}
${GAZEBO_LIBRARIES}
${PROTOBUF_LIBRARIES}
${QT_LIBRARIES}
)
qt5_use_modules(gui_task_widget Widgets)
add_dependencies(gui_task_widget ${catkin_EXPORTED_TARGETS})
install(TARGETS gui_task_widget
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
endif()
# Plugin for scoring the perception task.
add_library(perception_scoring_plugin src/perception_scoring_plugin.cc)
target_link_libraries(perception_scoring_plugin
${catkin_LIBRARIES}
scoring_plugin)
add_dependencies(perception_scoring_plugin ${catkin_EXPORTED_TARGETS})
install(TARGETS perception_scoring_plugin
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# Dock base files that need to be processed with erb
set(dock_base_erb_files
models/dock_2016_base/model.sdf.erb
models/dock_2018_base/model.sdf.erb
models/dock_2016_base_dynamic/model.sdf.erb
models/dock_2018_base_dynamic/model.sdf.erb
)
# Dock files that need to be processed with erb
set(dock_erb_files
models/dock_2016/model.sdf.erb
models/dock_2018/model.sdf.erb
models/dock_2016_dynamic/model.sdf.erb
models/dock_2018_dynamic/model.sdf.erb
)
# Process the dock base erb files
foreach(_erb ${dock_base_erb_files})
string(REGEX REPLACE ".sdf.erb" ".sdf" _model ${_erb})
set(_model ${CMAKE_CURRENT_SOURCE_DIR}/${_model})
add_custom_command(OUTPUT ${_model}
COMMAND ${ERB_EXE_PATH} ${_erb} > ${_model}
DEPENDS
${CMAKE_CURRENT_SOURCE_DIR}/${_erb}
${CMAKE_CURRENT_SOURCE_DIR}/dock_generator.erb
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR})
list(APPEND dock_base_files ${_model})
endforeach()
add_custom_target(dock_base_erb_generation ALL DEPENDS ${dock_base_files})
# Process the dock erb files
foreach(_erb ${dock_erb_files})
string(REGEX REPLACE ".sdf.erb" ".sdf" _model ${_erb})
set(_model ${CMAKE_CURRENT_SOURCE_DIR}/${_model})
add_custom_command(OUTPUT ${_model}
COMMAND ${ERB_EXE_PATH} ${_erb} > ${_model}
DEPENDS
${CMAKE_CURRENT_SOURCE_DIR}/${_erb}
${CMAKE_CURRENT_SOURCE_DIR}/dock_generator.erb
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR})
list(APPEND dock_files ${_model})
endforeach()
add_custom_target(dock_erb_generation ALL
DEPENDS
${dock_files}
dock_base_erb_generation
)
set(XACRO_INORDER)
if(DEFINED ENV{ROS_DISTRO})
if($ENV{ROS_DISTRO} STREQUAL "kinetic")
set(XACRO_INORDER INORDER)
endif()
endif()
# Generate world files from xacro and install
xacro_add_files(
worlds/example_course.world.xacro
worlds/navigation_task.world.xacro
worlds/perception_task.world.xacro
worlds/sandisland.world.xacro
worlds/dock.world.xacro
worlds/scan_and_dock.world.xacro
worlds/stationkeeping_task.world.xacro
worlds/wayfinding_task.world.xacro
worlds/wind_test.world.xacro
worlds/ocean.world.xacro
${XACRO_INORDER} INSTALL DESTINATION worlds
)
# Generate obstacle course
add_custom_command(
OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/models/robotx_2018_qualifying_avoid_obstacles_buoys/model.sdf
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/scripts/generate_avoid_obstacles_buoys
COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/scripts/generate_avoid_obstacles_buoys --seed 1337 --a3 6 --a5 7 --a7 7 > ${CMAKE_CURRENT_SOURCE_DIR}/models/robotx_2018_qualifying_avoid_obstacles_buoys/model.sdf
)
add_custom_target(${PROJECT_NAME}_generate_obstacle_course_buoys ALL DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/models/robotx_2018_qualifying_avoid_obstacles_buoys/model.sdf)
# Install all the config files
install(DIRECTORY config/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config)
# Install all the world files
install(DIRECTORY worlds/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/worlds)
# Install all the model files
install(DIRECTORY models/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/models)
# Install all the launch files
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
install(DIRECTORY include/
DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.hh"
)
install(PROGRAMS scripts/spawn_wamv.bash
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_rostest_gtest(sandisland_test
test/sandisland.test
test/sandisland.cc)
target_link_libraries(sandisland_test ${catkin_LIBRARIES})
endif()
# Python Scripts
catkin_install_python(PROGRAMS
nodes/twist2thrust.py
nodes/key2thrust_angle.py
scripts/generate_worlds
scripts/generate_wamv
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

46
sitl_config/usv/vrx_gazebo/README.md Normal file
View File

@ -0,0 +1,46 @@
# vrx_gazebo
## Description
This package provides gazebo models, plugins, and examples for simulating the [RobotX challenge](https://www.robotx.org/index.php/2014-01-05-21-55-32/2016-rules-requirements) within Gazebo using ROS.
## Usage
To launch gazebo with an example course layout and WAM-V platform, run:
```roslaunch vrx_gazebo sandisland.launch```
## Course models
The following models are used in the RobotX challenge and will be included in this package.
| Task Element | Product | Status | Model |
|:-----------------------------|:-------------------------|:-------|:-------|
| Light Buoy | Custom | ADDED | robotx_light_buoy |
| Obstacle - Small | PolyForm A-3 black | ADDED | polyform_a3 |
| Obstacle - Medium | PolyForm A-5 black | ADDED | polyform_a5 |
| Obstacle - Large | PolyForm A-7 black | ADDED | polyform_a7 |
| Red Can buoy | Sur-Mark Can Buoy 950410 | ADDED | surmark950410 |
| Green Can buoy | Sur-Mark Can Buoy 950400 | ADDED | surmark950400 |
| White Can buoy | Sur-Mark Can Buoy 46104 | ADDED | surmark46104 |
| Green Totem | 46104 w/ Green Cover | ADDED | green_totem |
| Yellow Totem | 46104 w/ Yellow Cover | ADDED | yellow_totem |
| Blue Totem | 46104 w/ Blue Cover | ADDED | blue_totem |
| Red Totem | 46104 w/ Red Cover | ADDED | red_totem |
| Black Totem | 46104 w/ Black Cover | ADDED | black_totem |
| Dock Material | JetDock C000000008 | ADDED | dock_block |
| 2016 Dock | Custom Assembly | ADDED | robotx_dock_2016 |
| 2018 Dock | Custom Assembly | ADDED | robotx_dock_2018 |
| Blue Circle Symbol | Custom | ADDED | symbol_circle |
| Blue Cruciform Symbol | Custom | ADDED | symbol_cross |
| Blue Triangle Symbol | Custom | ADDED | symbol_triangle |
| Green Circle Symbol | Custom | ADDED | symbol_circle |
| Green Cruciform Symbol | Custom | ADDED | symbol_cross |
| Green Triangle Symbol | Custom | ADDED | symbol_triangle |
| Red Circle Symbol | Custom | ADDED | symbol_circle |
| Red Cruciform Symbol | Custom | ADDED | symbol_cross |
| Red Triangle Symbol | Custom | ADDED | symbol_triangle |
| 2016 Pinger Transit (quals) | Custom Group of objects | ADDED | robotx_2016_qualifying_pinger_transit |
| 2016 Pinger Transit (finals) | Custom Group of objects | ADDED | robotx_2016_finals_pinger_transit |
| 2018 Entrance/Exit Gate | Custom Group of objects | ADDED | robotx_2018_entrance_gate |
| White placard | Custom | ADDED | placard |
*= More detailed model needed

7
sitl_config/usv/vrx_gazebo/config/custom_rosconsole.conf Normal file
View File

@ -0,0 +1,7 @@
#
# You can define your own by e.g. copying this file and setting
# ROSCONSOLE_CONFIG_FILE (in your environment) to point to the new file
#
log4j.logger.ros=INFO
log4j.logger.ros.vrx_gazebo=INFO
log4j.logger.ros.roscpp.superdebug=WARN

37
sitl_config/usv/vrx_gazebo/config/diffdrive.yaml Normal file
View File

@ -0,0 +1,37 @@
teleop:
move:
type: topic
message_type: geometry_msgs/Twist
topic_name: cmd_vel
axis_mappings:
-
axis: 1
target: linear.x
scale: 1.0
-
axis: 3
target: angular.z
scale: 1.0
joy_priority:
type: action
action_name: joy_priority_action
buttons: [9]
turn_left_thruster:
type: topic
message_type: std_msgs/Float32
topic_name: left_thrust_angle
axis_mappings:
- axis: 2
target: data
scale: -1.0
turn_right_thruster:
type: topic
message_type: std_msgs/Float32
topic_name: right_thrust_angle
axis_mappings:
- axis: 2
target: data
scale: -1.0

174
sitl_config/usv/vrx_gazebo/config/front_stereo.perspective Normal file
View File

@ -0,0 +1,174 @@
{
"keys": {},
"groups": {
"pluginmanager": {
"keys": {
"running-plugins": {
"type": "repr",
"repr": "{u'rqt_image_view/ImageView': [2, 1]}"
}
},
"groups": {
"plugin__rqt_image_view__ImageView__2": {
"keys": {},
"groups": {
"dock_widget__ImageViewWidget": {
"keys": {
"dockable": {
"type": "repr",
"repr": "True"
},
"parent": {
"type": "repr",
"repr": "None"
},
"dock_widget_title": {
"type": "repr",
"repr": "u'Image View (2)'"
}
},
"groups": {}
},
"plugin": {
"keys": {
"max_range": {
"type": "repr",
"repr": "10.0"
},
"rotate": {
"type": "repr",
"repr": "0"
},
"smooth_image": {
"type": "repr",
"repr": "False"
},
"mouse_pub_topic": {
"type": "repr",
"repr": "u'/wamv/sensors/cameras/front_right_camera/image_raw_mouse_left'"
},
"num_gridlines": {
"type": "repr",
"repr": "0"
},
"toolbar_hidden": {
"type": "repr",
"repr": "False"
},
"zoom1": {
"type": "repr",
"repr": "False"
},
"dynamic_range": {
"type": "repr",
"repr": "False"
},
"topic": {
"type": "repr",
"repr": "u'/wamv/sensors/cameras/front_right_camera/image_raw'"
},
"publish_click_location": {
"type": "repr",
"repr": "False"
}
},
"groups": {}
}
}
},
"plugin__rqt_image_view__ImageView__1": {
"keys": {},
"groups": {
"dock_widget__ImageViewWidget": {
"keys": {
"dockable": {
"type": "repr",
"repr": "True"
},
"parent": {
"type": "repr",
"repr": "None"
},
"dock_widget_title": {
"type": "repr",
"repr": "u'Image View'"
}
},
"groups": {}
},
"plugin": {
"keys": {
"max_range": {
"type": "repr",
"repr": "10.0"
},
"rotate": {
"type": "repr",
"repr": "0"
},
"smooth_image": {
"type": "repr",
"repr": "False"
},
"mouse_pub_topic": {
"type": "repr",
"repr": "u'/wamv/sensors/cameras/front_left_camera/image_raw_mouse_left'"
},
"num_gridlines": {
"type": "repr",
"repr": "0"
},
"toolbar_hidden": {
"type": "repr",
"repr": "False"
},
"zoom1": {
"type": "repr",
"repr": "False"
},
"dynamic_range": {
"type": "repr",
"repr": "False"
},
"topic": {
"type": "repr",
"repr": "u'/wamv/sensors/cameras/front_left_camera/image_raw'"
},
"publish_click_location": {
"type": "repr",
"repr": "False"
}
},
"groups": {}
}
}
}
}
},
"mainwindow": {
"keys": {
"geometry": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00020000000000640000006400000372000002800000006400000089000003720000028000000000000000000780')",
"pretty-print": " d d r d r "
},
"state": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd00000001000000030000030f000001cefc0100000002fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d0061006700650056006900650077005700690064006700650074010000000000000178000000bf00fffffffb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0032005f005f0049006d0061006700650056006900650077005700690064006700650074010000017e00000191000000d300ffffff0000030f0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
"pretty-print": " Zrqt_image_view__ImageView__1__ImageViewWidget Zrqt_image_view__ImageView__2__ImageViewWidget 6MinimizedDockWidgetsToolbar "
}
},
"groups": {
"toolbar_areas": {
"keys": {
"MinimizedDockWidgetsToolbar": {
"type": "repr",
"repr": "8"
}
},
"groups": {}
}
}
}
}
}

1
sitl_config/usv/vrx_gazebo/config/pinger.yaml Normal file
View File

@ -0,0 +1 @@
position_1: [ 65.0,-47.5,-2]

25
sitl_config/usv/vrx_gazebo/config/wamv_config/example_sensor_config.yaml Normal file
View File

@ -0,0 +1,25 @@
wamv_camera:
- name: front_left_camera
x: 0.75
y: 0.1
P: ${radians(15)}
- name: front_right_camera
x: 0.75
y: -0.1
P: ${radians(15)}
- name: far_left_camera
x: 0.75
y: 0.3
P: ${radians(15)}
wamv_gps:
- name: gps_wamv
x: -0.85
wamv_imu:
- name: imu_wamv
y: -0.2
wamv_p3d:
- name: p3d_wamv
lidar:
- name: lidar_wamv
type: 16_beam
P: ${radians(8)}

7
sitl_config/usv/vrx_gazebo/config/wamv_config/example_thruster_config.yaml Normal file
View File

@ -0,0 +1,7 @@
engine:
- prefix: "left"
position: "-2.373776 1.027135 0.318237"
orientation: "0.0 0.0 0.0"
- prefix: "right"
position: "-2.373776 -1.027135 0.318237"
orientation: "0.0 0.0 0.0"

8
sitl_config/usv/vrx_gazebo/config/wamv_config/sensor_compliance/bounding_boxes.yaml Normal file
View File

@ -0,0 +1,8 @@
sensor_compliance_for:
pose: '0.8 0 1.8 0 0 0'
size: '1 1 1'
capacity: '-1'
sensor_compliance_aft:
pose: '-0.9 0 1.8 0 0 0'
size: '0.5 1 1'
capacity: '-1'

53
sitl_config/usv/vrx_gazebo/config/wamv_config/sensor_compliance/numeric.yaml Normal file
View File

@ -0,0 +1,53 @@
wamv_camera:
num: 3
allowed_params:
x
y
z
R
P
Y
name
post_Y
wamv_gps:
num: 1
allowed_params:
x
y
z
R
P
Y
name
post_Y
wamv_imu:
num: 1
allowed_params:
x
y
z
R
P
Y
name
post_Y
lidar:
num: 2
allowed_params:
x
y
z
R
P
Y
name
post_Y
type
wamv_p3d:
num: 0
allowed_params:
name

20
sitl_config/usv/vrx_gazebo/config/wamv_config/thruster_compliance/bounding_boxes.yaml Normal file
View File

@ -0,0 +1,20 @@
thruster_compliance_port_aft:
pose: '-2.25 1 0 0 0 0'
size: '1 1 1.2'
capacity: '1'
thruster_compliance_star_aft:
pose: '-2.25 -1 0 0 0 0'
size: '1 1 1.2'
capacity: '1'
thruster_compliance_port_for:
pose: '1 1 0 0 0 0'
size: '1 1 1.2'
capacity: '1'
thruster_compliance_star_for:
pose: '1 -1 0 0 0 0'
size: '1 1 1.2'
capacity: '1'
thruster_compliance_middle:
pose: 0.25 0 0 0 0 0
size: '2.5 1 1.2'
capacity: '1'

6
sitl_config/usv/vrx_gazebo/config/wamv_config/thruster_compliance/numeric.yaml Normal file
View File

@ -0,0 +1,6 @@
engine:
num: 4
allowed_params:
prefix
position
orientation

134
sitl_config/usv/vrx_gazebo/config/wave_motion_plot.perspective Normal file
View File

@ -0,0 +1,134 @@
{
"keys": {},
"groups": {
"pluginmanager": {
"keys": {
"running-plugins": {
"type": "repr",
"repr": "{u'rqt_plot/Plot': [2, 1]}"
}
},
"groups": {
"plugin__rqt_plot__Plot__1": {
"keys": {},
"groups": {
"dock_widget__DataPlotWidget": {
"keys": {
"dockable": {
"type": "repr",
"repr": "True"
},
"parent": {
"type": "repr",
"repr": "None"
},
"dock_widget_title": {
"type": "repr",
"repr": "u'MatPlot'"
}
},
"groups": {}
},
"plugin": {
"keys": {
"autoscroll": {
"type": "repr",
"repr": "True"
},
"plot_type": {
"type": "repr",
"repr": "1"
},
"topics": {
"type": "repr",
"repr": "u'/gazebo/model_states/pose[6]/position/z'"
},
"y_limits": {
"type": "repr",
"repr": "[-0.35689860792936967, 0.16984086058445746]"
},
"x_limits": {
"type": "repr",
"repr": "[1184.6169135408588, 1194.2]"
}
},
"groups": {}
}
}
},
"plugin__rqt_plot__Plot__2": {
"keys": {},
"groups": {
"dock_widget__DataPlotWidget": {
"keys": {
"dockable": {
"type": "repr",
"repr": "True"
},
"parent": {
"type": "repr",
"repr": "None"
},
"dock_widget_title": {
"type": "repr",
"repr": "u'MatPlot (2)'"
}
},
"groups": {}
},
"plugin": {
"keys": {
"autoscroll": {
"type": "repr",
"repr": "True"
},
"plot_type": {
"type": "repr",
"repr": "1"
},
"topics": {
"type": "repr",
"repr": "[u'/wamv_rpy/x', u'/wamv_rpy/y']"
},
"y_limits": {
"type": "repr",
"repr": "[-0.03314272443981232, 0.02756334328054815]"
},
"x_limits": {
"type": "repr",
"repr": "[156.56528262177736, 161.82799999999997]"
}
},
"groups": {}
}
}
}
}
},
"mainwindow": {
"keys": {
"geometry": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb000200000000080e0000002900000c58000003500000080e0000004e00000c580000035000000001000000000780')",
"pretty-print": " ) X P N X P "
},
"state": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd00000001000000030000044b000002dafc0100000003fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d00610067006500560069006500770057006900640067006500740100000000000002080000000000000000fb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0031005f005f00440061007400610050006c006f00740057006900640067006500740100000000000001f70000011300fffffffb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0032005f005f00440061007400610050006c006f007400570069006400670065007401000001fd0000024e0000011300ffffff0000044b0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
"pretty-print": " Zrqt_image_view__ImageView__1__ImageViewWidget Brqt_plot__Plot__1__DataPlotWidget Brqt_plot__Plot__2__DataPlotWidget 6MinimizedDockWidgetsToolbar "
}
},
"groups": {
"toolbar_areas": {
"keys": {
"MinimizedDockWidgetsToolbar": {
"type": "repr",
"repr": "8"
}
},
"groups": {}
}
}
}
}
}

110
sitl_config/usv/vrx_gazebo/dock_generator.erb Normal file
View File

@ -0,0 +1,110 @@
<!--
This is how this dock should look like:
<%- static_dock = layout.pop %>
<% layout.each do |row| -%>
<%= row -%>
<% end %>
-->
<%- link_counter = 0 -%>
<%- row_counter = 0 -%>
<%- x_counter = 0 -%>
<%- delta_x = 2 -%>
<%- delta_y = 2 -%>
<%- i = 0 -%>
<%- if static_dock == true -%>
<static>true</static>
<%- end -%>
<%- layout.each do |row| %>
<!-- Row <%=row_counter %> -->
<%- y_counter = 0 -%>
<%- for j in 0..row.size - 1 -%>
<%- c = row[j] -%>
<%- if c == 'X' -%>
<include>
<name>dock_block_<%= i %>_<%= j %></name>
<pose><%= x_counter %> <%= y_counter %> 0.25 0 0 0</pose>
<%- if static_dock != true -%>
<uri>model://dock_block_4x4_dynamic</uri>
<%- else -%>
<uri>model://dock_block_4x4</uri>
<%- end -%>
</include>
<%- link_counter += 1 -%>
<%- end -%>
<%- y_counter += delta_x -%>
<%- end -%>
<%- i += 1 -%>
<%- x_counter += delta_y -%>
<%- row_counter += 1 -%>
<%- end %>
<%- if static_dock != true -%>
<!-- Add joints across rows -->
<%- joint_limit = 0.1 -%>
<%- x_counter = 0 -%>
<%- delta_x = 2 -%>
<%- delta_y = 2 -%>
<%- i = 0 -%>
<%- layout.each do |row| %>
<!-- Row <%= i%> -->
<%- y_counter = delta_y -%>
<%- for j in 1..row.size - 1 -%>
<%- c = row[j-1] -%>
<%- d = row[j] -%>
<%- if (c == 'X') and (d == 'X') -%>
<joint name="<%= i%>_<%= j-1%>_to_<%= i%>_<%= j%>" type="revolute">
<pose> 0 <%= delta_y/2.0 %> 0 0 0 0</pose>
<parent>dock_block_<%= i %>_<%= j %>::link</parent>
<child>dock_block_<%= i %>_<%= j-1 %>::link</child>
<axis>
<limit>
<lower>-<%= joint_limit %></lower>
<upper><%= joint_limit %></upper>
</limit>
<xyz>1 0 0</xyz>
</axis>
</joint>
<%- end -%>
<%- y_counter += delta_x -%>
<%- end -%>
<%- i += 1 -%>
<%- x_counter += delta_y -%>
<%- end %>
<!-- Add joints across columns -->
<%- y_counter = 0 -%>
<%- delta_x = 2 -%>
<%- delta_y = 2 -%>
<%- j = 0 -%>
<%- for j in 0..layout[0].length - 1 %>
<!-- Column <%= j%> -->
<%- x_counter = delta_x -%>
<%- for i in 1..layout.length - 1 -%>
<%- c = layout[i-1][j] -%>
<%- d = layout[i][j] -%>
<%- if (c == 'X') and (d == 'X') -%>
<joint name="<%= i%>_<%= j%>_to_<%= i-1%>_<%= j%>" type="revolute">
<pose> <%= delta_x/2.0 %> 0 0 0 0 0</pose>
<parent>dock_block_<%= i %>_<%= j %>::link</parent>
<child>dock_block_<%= i-1 %>_<%= j %>::link</child>
<axis>
<limit>
<lower>-<%= joint_limit %></lower>
<upper><%= joint_limit %></upper>
</limit>
<xyz>0 1 0</xyz>
</axis>
</joint>
<%- end -%>
<%- x_counter += delta_x -%>
<%- end -%>
<%- j += 1 -%>
<%- y_counter += delta_y -%>
<%- end %>
<%- end %>

98
sitl_config/usv/vrx_gazebo/docs/Commands.tex Executable file
View File

@ -0,0 +1,98 @@
% List of new commands
% bbing 06-11-98
%\newcommand{\spacing}[1]{\renewcommand{\baselinestretch}{#1} \normalsize}
% Degree symbol within text
\newcommand{\degrees}{$^{\circ}$}
\newcommand{\CRlb}{Cram\'{e}r Rao }
\newcommand{\btab}{\begin{tabular}}
\newcommand{\etab}{\end{tabular}}
\newcommand{\bfig}{\begin{figure}}
\newcommand{\efig}{\end{figure}}
\newcommand{\beqn}{\begin{equation}}
\newcommand{\eeqn}{\end{equation}}
\newcommand{\bdm}{\begin{displaymath}}
\newcommand{\edm}{\end{displaymath}}
\newcommand{\bearray}{\begin{eqnarray}}
\newcommand{\eearray}{\end{eqnarray}}
\newcommand{\Bearray}{\begin{eqnarray}}
\newcommand{\Eearray}{\end{eqnarray}}
\newcommand{\bgat}{\begin{gather}}
\newcommand{\egat}{\end{gather}}
\newcommand{\Htwo}{$\mathcal{H}_2$\ }
\newcommand{\Hinf}{$\mathcal{H}_{\infty}$\ }
%\newcommand{\V}[1]{\mathbf{#1}}
\newcommand{\plusminus}{{\textstyle \frac{+}{-}}}
\newcommand{\TM}{$^{TM}$}
% Definitions %from homero!
%
%\newcommand{\mtitle}[1]{\begin{center} {\huge \bf{#1}}\end{center}}
\newcommand{\lb}{\linebreak}
\newcommand{\xaxis}{\mbox{$x$-axis}}
\newcommand{\yaxis}{\mbox{$y$-axis}}
\newcommand{\zaxis}{\mbox{$z$-axis}}
\newcommand{\bc}{\begin{center}}
\newcommand{\ec}{\end{center}}
%\newcommand{\be}{\begin{equation}}
%\newcommand{\ee}{\end{equation}}
\newcommand{\bd}{\begin{displaymath}}
\newcommand{\ed}{\end{displaymath}}
\newcommand{\bi}{\begin{itemize}}
\newcommand{\ei}{\end{itemize}}
%\newcommand{\bt}{\begin{tabular}}
%\newcommand{\et}{\end{tabular}}
\newcommand{\ba}{\begin{array}}
\newcommand{\ea}{\end{array}}
\newcommand{\baa}[2]{\begin{array}[t]{cc}
\left[#1\right.\!\!&\!\!\left.#2\right]\end{array}}
\newcommand{\bea}{\begin{eqnarray}}
\newcommand{\eea}{\end{eqnarray}}
\newcommand{\bsea}{\begin{subeqnarray}}
\newcommand{\esea}{\end{subeqnarray}}
\newcommand{\degr}{\mbox{$^{\circ}$}}
\newcommand{\jw}{j\omega}
\newcommand{\dw}{\mbox{\rm d}\omega}
\newcommand{\dx}[1]{\,\mbox{\rm d}#1}
\newcommand{\til}{^{\sim}}
\newcommand{\abcd}[4]{\left[\begin{array}{c|c}#1&#2\\ \hline #3&#4
\end{array}\right]}
\newcommand{\etal}{{\em et al.}}
\newcommand{\etc}{{\em etc.}}
\newcommand{\eg}{{\em e.g., }}
\newcommand{\ie}{{\em i.e., }}
\newcommand{\eqnref}[1]{Equation~(\ref{#1})}
\newcommand{\eqrefa}[1]{Eq'n~(\ref{#1})}
\newcommand{\eqrefb}[1]{(\ref{#1})}
\newcommand{\expec}[1]{\left\langle #1 \right\rangle}
\newcommand{\pder}[2]{\frac{\partial #1}{\partial #2}}
\newcommand{\half}{{\textstyle \frac{1}{2}}}
\newcommand{\msp}{\mbox{\hspace{.5in}}}
\newcommand{\ub}[2]{\underbrace{#1}_{#2}}
\newcommand{\eqn}[1]{Eq.~\ref{eq:#1}}
%
%\newenvironment{proof}{\par\noindent{\bf Proof: }}{\hfill $\Box$}
%\newenvironment{remark}{{\bf Remark: }}{}
%
\def\sinc{\mathop{\rm sinc}\nolimits}
%
\newcommand{\vdashes}[2]
{{\vfuzz #1 \vbox to 0pt{\vskip -#2
\vbox{\xleaders\vbox{\vskip 1pt\hfuzz 1pt\hbox to 0pt{\vrule height 4pt
depth 0pt}\vskip 1pt}\vskip #1}}}}
\newcommand{\bmat}[1]{\left[ \begin{array}{#1}}
\newcommand{\emat}{\end{array} \right]}
%\newcommand{\bvec}{\left( \begin{array}{c}}
\newcommand{\evec}{\end{array} \right)}

43
sitl_config/usv/vrx_gazebo/docs/Makefile Normal file
View File

@ -0,0 +1,43 @@
#LATEX = latex
LATEX = pdflatex
BIBTEX = bibtex
DVIPS = dvips
PS2PDF = ps2pdf
VIEWPDF = okular
BASE_NAME = wave_generation # theory_of_operation
all: $(BASE_NAME).pdf
#byu-sow.pdf: byu-sow.tex
# $(LATEX) $< && $(LATEX) $<
%.pdf: %.tex $(PNG_FILES) %.bbl
$(LATEX) $< && $(LATEX) $< && $(LATEX) $<
$(BASE_NAME).bbl $(BASE_NAME).blg: $(BASE_NAME).bib $(BASE_NAME).aux
$(BIBTEX) $(BASE_NAME)
$(BASE_NAME).aux: $(BASE_NAME).bib
$(LATEX) $(BASE_NAME).tex
$(BASE_NAME).bib: $(BASE_NAME).tex
$(LATEX) $(BASE_NAME).tex
$(BASE_NAME).dvi: $(BASE_NAME).tex
$(LATEX) $< && $(LATEX) $<
$(BASE_NAME).ps: $(BASE_NAME).dvi
$(DVIPS) -P pdf -f -t a4 $(BASE_NAME).dvi > $@
display: $(BASE_NAME).pdf
$(VIEWPDF) $(BASE_NAME).pdf &
%.eps: %.dia
cd `dirname $<` && dia `basename $<` -t eps
%.pdf: %.eps
epstopdf $<
clean:
rm -rf *.ps *.dvi *.log *.aux *.blg *.toc missfont.log *.bbl doxygen html latex *.out

144
sitl_config/usv/vrx_gazebo/docs/defs.tex Executable file
View File

@ -0,0 +1,144 @@
% Stolen from Austratlian Center for Field Robotics
% Thanks Alex!
% bbing 24.02.03
% references to figurs and equations
\newcommand{\fref}[1] {Figure~\ref{#1}}
\newcommand{\eref}[1] {~(\ref{#1})}
% general global definitions
\newcommand{\Def}{\ {\buildrel \triangle\over =}\ }
%\newcommand{\beq} {\begin{equation}}
%\newcommand{\eeq} {\end{equation}}
%\newcommand{\beqn} {\begin{eqnarray}}
%\newcommand{\eeqn} {\end{eqnarray}}
%\newcommand{\E}[1] {\mbox{$ {\rm E} \{ #1 \}$ }}
\newcommand{\Es}[2] {\mbox{$ {\rm E}^{#1} \{ #2 \}$ }}
\newcommand{\Set}[1] {\mbox{$ \{ #1 \} $ }}
\newcommand{\Cal}[1] {\mbox{$ {\cal #1 } $}}
\newcommand{\PR}[1] {\mbox{$ P(#1) $}}
\newcommand{\Pri}[2] {\mbox{$ P_{#1}(#2) $}}
\newcommand{\PRi}[2] {\mbox{$ P_{#1}(#2) $}}
\newcommand{\Pris}[3] {\mbox{$ P_{#1}^{#2}(#3) $}}
\newcommand{\like}[1] {\mbox{$ \Lambda(\bf #1) $}}
\newcommand{\likei}[2] {\mbox{$ \Lambda_{#2}(\bf #1) $}}
\newcommand{\LL}[1] {\mbox{$ l(#1) $}} % loglikelihood
\newcommand{\LLi}[2] {\mbox{$ l_{#1}(#2) $}} %loglikelihood
\newcommand{\EN}[1] {\mbox{$ H(#1) $}} % entropy
\newcommand{\ENi}[2] {\mbox{$ H_{#1}(#2) $}} % entropy
\newcommand{\mEN}[1] {\mbox{$ \overline{H}(#1) $}} % mean entropy
\newcommand{\MI}[1] {\mbox{$ I(#1) $}} %mutual information
\newcommand{\est}[1] {\mbox{$\hat{\bf #1}$}}
\newcommand{\estk}[2] {\mbox{$\hat{\bf #1}(#2)$}}
\newcommand{\D}[1] {\mbox{${\rm d} {#1}$}}
\newcommand{\mean}[1] {\mbox{$\overline{ #1}$}}
\newcommand{\Det}[1] {\mbox{$\mid {#1} \mid $}}
\newcommand{\One} {\mbox{${\bf 1}$}}
\newcommand{\Zero} {\mbox{${\bf 0}$}}
\newcommand{\grad}[1] {\mbox{${\bf\nabla} #1$}}
\newcommand{\J}[3] {\mbox{${\bf\nabla}{\bf #1}_{\bf #2}(#3)$}}
\newcommand{\Jt}[3] {\mbox{${\bf\nabla}^T{\bf #1}_{\bf #2}(#3)$}}
\newcommand{\pdf}{{\it pdf\ }}
\newcommand{\dxt}[2] {\mbox{$\dot{\bf #1}( #2)$}}
% defining different types of vectors
% first those with no time subscripts
\newcommand{\V}[1] {\mbox{${\bf #1}$}}
\newcommand{\Vt}[1] {\mbox{${\bf #1}^T$}}
\newcommand{\Vin}[1] {\mbox{${\bf #1}^{-1}$}}
\newcommand{\Vgin}[1] {\mbox{${\bf #1}^{\dagger}$}}
\newcommand{\Vi}[2] {\mbox{${\bf #1}_{#2}$}}
\newcommand{\Vs}[2] {\mbox{${\bf #1}^{#2}$}}
\newcommand{\Vis}[3] {\mbox{${\bf #1}_{#2}^{#3}$}}
\newcommand{\Vit}[2] {\mbox{${\bf #1}_{#2}^T$}}
\newcommand{\Vini}[2] {\mbox{${\bf #1}_{#2}^{-1}$}}
\newcommand{\Vgini}[2] {\mbox{${\bf #1}^{\dagger}$}}
% next those with time subscript k (very common)
\newcommand{\Vk}[1] {\mbox{${\bf #1}(k)$}}
\newcommand{\Vkt}[1] {\mbox{${\bf #1}^T(k)$}}
\newcommand{\Vkin}[1] {\mbox{${\bf #1}^{-1}(k)$}}
\newcommand{\Vkgin}[1] {\mbox{${\bf #1}^{\dagger}(k)$}}
\newcommand{\Vki}[2] {\mbox{${\bf #1}_{#2}(k)$}}
\newcommand{\Vks}[2] {\mbox{${\bf #1}^{#2}(k)$}}
\newcommand{\Vkis}[3] {\mbox{${\bf #1}_{#2}^{#3}(k)$}}
\newcommand{\Vkit}[2] {\mbox{${\bf #1}_{#2}^T(k)$}}
\newcommand{\Vkini}[2] {\mbox{${\bf #1}_{#2}^{-1}(k)$}}
\newcommand{\Vkgini}[2] {\mbox{${\bf #1}^{\dagger}_{#2}(k)$}}
\newcommand{\tVk}[1] {\mbox{$\tilde{\bf #1}(k)$}}
\newcommand{\tVki}[2] {\mbox{$\tilde{\bf #1}_{#2}(k)$}}
% now those with general purpose time subscripts
%\newcommand{\Vec}[2] {\mbox{${\bf #1}(#2)$}}
\newcommand{\Vect}[2] {\mbox{${\bf #1}^T(#2)$}}
\newcommand{\Vecin}[2] {\mbox{${\bf #1}^{-1}(#2)$}}
\newcommand{\Veci}[3] {\mbox{${\bf #1}_{#2}(#3)$}}
\newcommand{\Vecit}[3] {\mbox{${\bf #1}_{#2}^T(#3)$}}
\newcommand{\Vecini}[3] {\mbox{${\bf #1}_{#2}^{-1}(#3)$}}
\newcommand{\Vecgin}[2] {\mbox{${\bf #1}^{\dagger}(#2)$}}
\newcommand{\Vecgini}[3] {\mbox{${\bf #1}^{\dagger}_{#2}(#3)$}}
% special symbols used very commonly
% state estimates of different sorts
\newcommand{\x}[2] {\mbox{$\hat{\bf x}( #1 \mid #2)$}}
\newcommand{\ix}[3] {\mbox{$\hat{\bf x}_{#1}( #2 \mid #3)$}}
\newcommand{\tx}[2] {\mbox{$\tilde{\bf x}( #1 \mid #2 )$}}
\newcommand{\txi}[3] {\mbox{$\tilde{\bf x}_{#1}( #2 \mid #3 )$}}
\newcommand{\z}[2] {\mbox{$\hat{\bf z}( #1 \mid #2)$}}
\newcommand{\tz}[2] {\mbox{$\tilde{\bf z}( #1 \mid #2)$}}
\newcommand{\tzt}[2] {\mbox{$\tilde{\bf z}^T( #1 \mid #2)$}}
\newcommand{\zi}[3] {\mbox{$\hat{\bf z}_{#1}( #2 \mid #3)$}}
\newcommand{\di}[3] {\mbox{$\hat{\bf \delta}_{#1}( #2 \mid #3)$}}
% variances of different sorts
\newcommand{\var}[2] {\mbox{${\bf P}( #1 \mid #2)$}}
\newcommand{\varin}[2] {\mbox{${\bf P}^{-1}( #1 \mid #2)$}}
\newcommand{\tvar}[2] {\mbox{$\tilde{\bf P}( #1 \mid #2)$}}
\newcommand{\tvarin}[2] {\mbox{$\tilde{\bf P}^{-1}( #1 \mid #2)$}}
\newcommand{\vari}[3] {\mbox{${\bf P}_{#1}( #2 \mid #3)$}}
\newcommand{\varini}[3] {\mbox{${\bf P}^{-1}_{#1}( #2 \mid #3)$}}
\newcommand{\tvari}[3] {\mbox{$\tilde{\bf P}_{#1}( #2 \mid #3)$}}
\newcommand{\tvarini}[3] {\mbox{$\tilde{\bf P}^{-1}_{#1}( #2 \mid #3)$}}
% information states and variances
\newcommand{\y}[2] {\mbox{$\hat{\bf y}( #1 \mid #2)$}}
\newcommand{\ty}[2] {\mbox{$\tilde{\bf y}( #1 \mid #2)$}}
\newcommand{\yi}[3] {\mbox{$\hat{\bf y}_{#1}( #2 \mid #3)$}}
\newcommand{\tyi}[3] {\mbox{$\tilde{\bf y}_{#1}( #2 \mid #3)$}}
\newcommand{\Y}[2] {\mbox{${\bf Y}( #1 \mid #2)$}}
\newcommand{\Yin}[2] {\mbox{${\bf Y}^{-1}( #1 \mid #2)$}}
\newcommand{\tY}[2] {\mbox{$\tilde{\bf Y}( #1 \mid #2)$}}
\newcommand{\Yi}[3] {\mbox{${\bf Y}_{#1}( #2 \mid #3)$}}
\newcommand{\Yini}[3] {\mbox{${\bf Y}_{#1}^{-1}( #2 \mid #3)$}}
\newcommand{\tYi}[3] {\mbox{$\tilde{\bf Y}_{#1}( #2 \mid #3)$}}
\newcommand{\info}[1] {\mbox{${\bf i}( #1)$}}
\newcommand{\Info}[1] {\mbox{${\bf I}( #1)$}}
\newcommand{\infoi}[2] {\mbox{${\bf i}_{#1}( #2)$}}
\newcommand{\infois}[3] {\mbox{${\bf i}_{#1}^{#2}(#3)$}}
\newcommand{\Infoi}[2] {\mbox{${\bf I}_{#1}( #2)$}}
\newcommand{\tInfoi}[2] {\mbox{$\tilde{\bf I}_{#1}( #2)$}}
\newcommand{\Infoini}[2] {\mbox{${\bf I}^{\dagger}_{#1}( #2)$}}
\newcommand{\Prop}[2] {\mbox{${\bf L}( #1 \mid #2)$}}
\newcommand{\Propi}[3] {\mbox{${\bf L}_{#1}( #2 \mid #3)$}}
\newcommand{\Z}[2] {\mbox{${\cal Z}^{#1}_{#2}$}}
% spurious ones
\newcommand{\maybe}{\ {\buildrel ?\over =}\ } %chapter 4
\newcommand{\svd} {\mbox{$\dagger$}} % chapter 4
\newcommand{\dnoise} {\mbox{$\delta d$}} % chapter 6 and 7
\newcommand{\unoise} {\mbox{$\delta u$}} % chapter 6
\newcommand{\ns}[1] {\mbox{$ #1$}} % chapter 6
\newcommand{\vs} {\vspace{0.17in}}
\newcommand{\svs} {\vspace{0.17cm}}
\newcommand{\vsf} {\vspace{0.4in}}
\newcommand{\vsff} {\vspace{1in}}
\newcommand{\veqns} {\vspace{-0.15in}}
\newcommand{\veqn} {\vspace{-0.12in}}
\newcommand{\sveqn} {\vspace{-0.06in}}
%\newcommand{\bc}{\begin{center}}
%\newcommand{\ec}{\end{center}}
%\newcommand{\bi}{\begin{itemize}}
%\newcommand{\ei}{\end{itemize}}
%\newcommand{\be}{\begin{enumerate}}
%\newcommand{\ee}{\end{enumerate}}
\newcommand{\Quote}{\parbox[t]{12.5cm}}
\newtheorem{example}{Example}

BIN
sitl_config/usv/vrx_gazebo/docs/gz_wavefield_test_mr.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 282 KiB

BIN
sitl_config/usv/vrx_gazebo/docs/images/dock_ex.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 804 KiB

BIN
sitl_config/usv/vrx_gazebo/docs/images/docking_ex.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 406 KiB

14592
sitl_config/usv/vrx_gazebo/docs/images/docking_ex.svg Normal file
View File

File diff suppressed because it is too large Load Diff
Side by Side

After

Width:  |  Height:  |  Size: 1.1 MiB

215
sitl_config/usv/vrx_gazebo/docs/images/docking_states.graphml Normal file
View File

@ -0,0 +1,215 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<graphml xmlns="http://graphml.graphdrawing.org/xmlns" xmlns:java="http://www.yworks.com/xml/yfiles-common/1.0/java" xmlns:sys="http://www.yworks.com/xml/yfiles-common/markup/primitives/2.0" xmlns:x="http://www.yworks.com/xml/yfiles-common/markup/2.0" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:y="http://www.yworks.com/xml/graphml" xmlns:yed="http://www.yworks.com/xml/yed/3" xsi:schemaLocation="http://graphml.graphdrawing.org/xmlns http://www.yworks.com/xml/schema/graphml/1.1/ygraphml.xsd">
<!--Created by yEd 3.19-->
<key attr.name="Description" attr.type="string" for="graph" id="d0"/>
<key for="port" id="d1" yfiles.type="portgraphics"/>
<key for="port" id="d2" yfiles.type="portgeometry"/>
<key for="port" id="d3" yfiles.type="portuserdata"/>
<key attr.name="url" attr.type="string" for="node" id="d4"/>
<key attr.name="description" attr.type="string" for="node" id="d5"/>
<key for="node" id="d6" yfiles.type="nodegraphics"/>
<key for="graphml" id="d7" yfiles.type="resources"/>
<key attr.name="url" attr.type="string" for="edge" id="d8"/>
<key attr.name="description" attr.type="string" for="edge" id="d9"/>
<key for="edge" id="d10" yfiles.type="edgegraphics"/>
<graph edgedefault="directed" id="G">
<data key="d0"/>
<node id="n0">
<data key="d5"/>
<data key="d6">
<y:ShapeNode>
<y:Geometry height="87.0" width="154.0" x="429.0" y="110.0"/>
<y:Fill color="#FFCC00" transparent="false"/>
<y:BorderStyle color="#000000" raised="false" type="line" width="1.0"/>
<y:NodeLabel alignment="center" autoSizePolicy="content" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" height="17.96875" horizontalTextPosition="center" iconTextGap="4" modelName="custom" textColor="#000000" verticalTextPosition="bottom" visible="true" width="73.515625" x="40.2421875" xml:space="preserve" y="34.515625">UNDOCKED<y:LabelModel><y:SmartNodeLabelModel distance="4.0"/></y:LabelModel><y:ModelParameter><y:SmartNodeLabelModelParameter labelRatioX="0.0" labelRatioY="0.0" nodeRatioX="0.0" nodeRatioY="0.0" offsetX="0.0" offsetY="0.0" upX="0.0" upY="-1.0"/></y:ModelParameter></y:NodeLabel>
<y:Shape type="roundrectangle"/>
</y:ShapeNode>
</data>
</node>
<node id="n1">
<data key="d5"/>
<data key="d6">
<y:ShapeNode>
<y:Geometry height="87.0" width="154.0" x="429.0" y="290.0"/>
<y:Fill color="#FFCC00" transparent="false"/>
<y:BorderStyle color="#000000" raised="false" type="line" width="1.0"/>
<y:NodeLabel alignment="center" autoSizePolicy="content" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" height="31.9375" horizontalTextPosition="center" iconTextGap="4" modelName="custom" textColor="#000000" verticalTextPosition="bottom" visible="true" width="71.798828125" x="41.1005859375" xml:space="preserve" y="27.53125">DOCKING
Start Timer<y:LabelModel><y:SmartNodeLabelModel distance="4.0"/></y:LabelModel><y:ModelParameter><y:SmartNodeLabelModelParameter labelRatioX="0.0" labelRatioY="0.0" nodeRatioX="0.0" nodeRatioY="0.0" offsetX="0.0" offsetY="0.0" upX="0.0" upY="-1.0"/></y:ModelParameter></y:NodeLabel>
<y:Shape type="roundrectangle"/>
</y:ShapeNode>
</data>
</node>
<node id="n2">
<data key="d5"/>
<data key="d6">
<y:ShapeNode>
<y:Geometry height="18.0" width="18.0" x="429.0" y="49.875"/>
<y:Fill color="#000000" transparent="false"/>
<y:BorderStyle color="#000000" raised="false" type="line" width="1.0"/>
<y:NodeLabel alignment="center" autoSizePolicy="content" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" hasText="false" height="4.0" horizontalTextPosition="center" iconTextGap="4" modelName="custom" textColor="#000000" verticalTextPosition="bottom" visible="true" width="4.0" x="7.0" y="7.0">
<y:LabelModel>
<y:SmartNodeLabelModel distance="4.0"/>
</y:LabelModel>
<y:ModelParameter>
<y:SmartNodeLabelModelParameter labelRatioX="0.0" labelRatioY="0.0" nodeRatioX="0.0" nodeRatioY="0.0" offsetX="0.0" offsetY="0.0" upX="0.0" upY="-1.0"/>
</y:ModelParameter>
</y:NodeLabel>
<y:Shape type="ellipse"/>
</y:ShapeNode>
</data>
</node>
<node id="n3">
<data key="d5"/>
<data key="d6">
<y:ShapeNode>
<y:Geometry height="87.0" width="154.0" x="429.0" y="470.0"/>
<y:Fill color="#FFCC00" transparent="false"/>
<y:BorderStyle color="#000000" raised="false" type="line" width="1.0"/>
<y:NodeLabel alignment="center" autoSizePolicy="content" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" height="17.96875" horizontalTextPosition="center" iconTextGap="4" modelName="custom" textColor="#000000" verticalTextPosition="bottom" visible="true" width="55.755859375" x="49.1220703125" xml:space="preserve" y="34.515625">DOCKED<y:LabelModel><y:SmartNodeLabelModel distance="4.0"/></y:LabelModel><y:ModelParameter><y:SmartNodeLabelModelParameter labelRatioX="0.0" labelRatioY="0.0" nodeRatioX="0.0" nodeRatioY="0.0" offsetX="0.0" offsetY="0.0" upX="0.0" upY="-1.0"/></y:ModelParameter></y:NodeLabel>
<y:Shape type="roundrectangle"/>
</y:ShapeNode>
</data>
</node>
<node id="n4">
<data key="d5"/>
<data key="d6">
<y:ShapeNode>
<y:Geometry height="87.0" width="154.0" x="429.0" y="650.0"/>
<y:Fill color="#FFCC00" transparent="false"/>
<y:BorderStyle color="#000000" raised="false" type="line" width="1.0"/>
<y:NodeLabel alignment="center" autoSizePolicy="content" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" height="31.9375" horizontalTextPosition="center" iconTextGap="4" modelName="custom" textColor="#000000" verticalTextPosition="bottom" visible="true" width="94.10546875" x="29.947265625" xml:space="preserve" y="27.53125">SUCCESS
Evaluate Score<y:LabelModel><y:SmartNodeLabelModel distance="4.0"/></y:LabelModel><y:ModelParameter><y:SmartNodeLabelModelParameter labelRatioX="0.0" labelRatioY="0.0" nodeRatioX="0.0" nodeRatioY="0.0" offsetX="0.0" offsetY="0.0" upX="0.0" upY="-1.0"/></y:ModelParameter></y:NodeLabel>
<y:Shape type="roundrectangle"/>
</y:ShapeNode>
</data>
</node>
<node id="n5">
<data key="d5"/>
<data key="d6">
<y:ShapeNode>
<y:Geometry height="18.0" width="18.0" x="497.0" y="781.75"/>
<y:Fill color="#000000" transparent="false"/>
<y:BorderStyle color="#000000" raised="false" type="line" width="1.0"/>
<y:NodeLabel alignment="center" autoSizePolicy="content" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" hasText="false" height="4.0" horizontalTextPosition="center" iconTextGap="4" modelName="custom" textColor="#000000" verticalTextPosition="bottom" visible="true" width="4.0" x="7.0" y="7.0">
<y:LabelModel>
<y:SmartNodeLabelModel distance="4.0"/>
</y:LabelModel>
<y:ModelParameter>
<y:SmartNodeLabelModelParameter labelRatioX="0.0" labelRatioY="0.0" nodeRatioX="0.0" nodeRatioY="0.0" offsetX="0.0" offsetY="0.0" upX="0.0" upY="-1.0"/>
</y:ModelParameter>
</y:NodeLabel>
<y:Shape type="ellipse"/>
</y:ShapeNode>
</data>
</node>
<node id="n6">
<data key="d5"/>
<data key="d6">
<y:ShapeNode>
<y:Geometry height="30.0" width="30.0" x="491.0" y="775.75"/>
<y:Fill hasColor="false" transparent="false"/>
<y:BorderStyle color="#000000" raised="false" type="line" width="1.0"/>
<y:NodeLabel alignment="center" autoSizePolicy="content" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" hasText="false" height="4.0" horizontalTextPosition="center" iconTextGap="4" modelName="custom" textColor="#000000" verticalTextPosition="bottom" visible="true" width="4.0" x="13.0" y="13.0">
<y:LabelModel>
<y:SmartNodeLabelModel distance="4.0"/>
</y:LabelModel>
<y:ModelParameter>
<y:SmartNodeLabelModelParameter labelRatioX="0.0" labelRatioY="0.0" nodeRatioX="0.0" nodeRatioY="0.0" offsetX="0.0" offsetY="0.0" upX="0.0" upY="-1.0"/>
</y:ModelParameter>
</y:NodeLabel>
<y:Shape type="ellipse"/>
</y:ShapeNode>
</data>
</node>
<edge id="e0" source="n0" target="n1">
<data key="d9"/>
<data key="d10">
<y:ArcEdge>
<y:Path sx="0.0" sy="0.0" tx="0.0" ty="0.0">
<y:Point x="551.0" y="243.5"/>
</y:Path>
<y:LineStyle color="#000000" type="line" width="1.0"/>
<y:Arrows source="none" target="standard"/>
<y:EdgeLabel alignment="center" configuration="AutoFlippingLabel" distance="2.0" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" height="31.9375" horizontalTextPosition="center" iconTextGap="4" modelName="custom" preferredPlacement="anywhere" ratio="0.5" textColor="#000000" verticalTextPosition="bottom" visible="true" width="258.994140625" x="21.78598269437782" xml:space="preserve" y="26.989942213749316">InternalActivationZone::onActivationEvent
msg == 1<y:LabelModel><y:SmartEdgeLabelModel autoRotationEnabled="false" defaultAngle="0.0" defaultDistance="10.0"/></y:LabelModel><y:ModelParameter><y:SmartEdgeLabelModelParameter angle="6.283185307179586" distance="126.07754482948533" distanceToCenter="true" position="left" ratio="59.06145474898199" segment="-2"/></y:ModelParameter><y:PreferredPlacementDescriptor angle="0.0" angleOffsetOnRightSide="0" angleReference="absolute" angleRotationOnRightSide="co" distance="-1.0" frozen="true" placement="anywhere" side="anywhere" sideReference="relative_to_edge_flow"/></y:EdgeLabel>
<y:Arc height="45.0" ratio="1.0" type="fixedRatio"/>
</y:ArcEdge>
</data>
</edge>
<edge id="e1" source="n1" target="n0">
<data key="d9"/>
<data key="d10">
<y:ArcEdge>
<y:Path sx="0.0" sy="0.0" tx="0.0" ty="0.0">
<y:Point x="461.0" y="243.5"/>
</y:Path>
<y:LineStyle color="#000000" type="line" width="1.0"/>
<y:Arrows source="none" target="standard"/>
<y:EdgeLabel alignment="center" configuration="AutoFlippingLabel" distance="2.0" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" height="59.875" horizontalTextPosition="center" iconTextGap="4" modelName="custom" preferredPlacement="anywhere" ratio="0.5" textColor="#000000" verticalTextPosition="bottom" visible="true" width="285.103515625" x="-310.32702883695595" xml:space="preserve" y="-78.88056721374943">InternalActivationZone::onActivationEvent
msg == 0
AND
Timer.ElapsedTime &lt; minDockTime (typ. 10 s)<y:LabelModel><y:SmartEdgeLabelModel autoRotationEnabled="false" defaultAngle="0.0" defaultDistance="10.0"/></y:LabelModel><y:ModelParameter><y:SmartEdgeLabelModelParameter angle="6.283185307179586" distance="140.32304549651582" distanceToCenter="true" position="left" ratio="-67.44810576259334" segment="-1"/></y:ModelParameter><y:PreferredPlacementDescriptor angle="0.0" angleOffsetOnRightSide="0" angleReference="absolute" angleRotationOnRightSide="co" distance="-1.0" frozen="true" placement="anywhere" side="anywhere" sideReference="relative_to_edge_flow"/></y:EdgeLabel>
<y:Arc height="45.0" ratio="1.0" type="fixedRatio"/>
</y:ArcEdge>
</data>
</edge>
<edge id="e2" source="n2" target="n0">
<data key="d9"/>
<data key="d10">
<y:ArcEdge>
<y:Path sx="0.0" sy="0.0" tx="0.0" ty="0.0">
<y:Point x="495.65625" y="89.1875"/>
</y:Path>
<y:LineStyle color="#000000" type="line" width="1.0"/>
<y:Arrows source="none" target="standard"/>
<y:Arc height="29.131052017211914" ratio="1.0" type="fixedRatio"/>
</y:ArcEdge>
</data>
</edge>
<edge id="e3" source="n1" target="n3">
<data key="d9"/>
<data key="d10">
<y:ArcEdge>
<y:Path sx="0.0" sy="0.0" tx="0.0" ty="0.0">
<y:Point x="551.0" y="423.5"/>
</y:Path>
<y:LineStyle color="#000000" type="line" width="1.0"/>
<y:Arrows source="none" target="standard"/>
<y:EdgeLabel alignment="center" configuration="AutoFlippingLabel" distance="2.0" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" height="17.96875" horizontalTextPosition="center" iconTextGap="4" modelName="custom" preferredPlacement="anywhere" ratio="0.5" textColor="#000000" verticalTextPosition="bottom" visible="true" width="295.158203125" x="24.207857694377708" xml:space="preserve" y="32.98212971374943">Timer.ElapsedTime &gt;= minDockTime (typ. 10 s)<y:LabelModel><y:SmartEdgeLabelModel autoRotationEnabled="false" defaultAngle="0.0" defaultDistance="10.0"/></y:LabelModel><y:ModelParameter><y:SmartEdgeLabelModelParameter angle="6.283185307179586" distance="144.86051584048352" distanceToCenter="true" position="left" ratio="67.34364090626835" segment="-2"/></y:ModelParameter><y:PreferredPlacementDescriptor angle="0.0" angleOffsetOnRightSide="0" angleReference="absolute" angleRotationOnRightSide="co" distance="-1.0" frozen="true" placement="anywhere" side="anywhere" sideReference="relative_to_edge_flow"/></y:EdgeLabel>
<y:Arc height="45.0" ratio="1.0" type="fixedRatio"/>
</y:ArcEdge>
</data>
</edge>
<edge id="e4" source="n3" target="n4">
<data key="d9"/>
<data key="d10">
<y:ArcEdge>
<y:Path sx="0.0" sy="0.0" tx="0.0" ty="0.0">
<y:Point x="551.0" y="603.5"/>
</y:Path>
<y:LineStyle color="#000000" type="line" width="1.0"/>
<y:Arrows source="none" target="standard"/>
<y:EdgeLabel alignment="center" configuration="AutoFlippingLabel" distance="2.0" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" height="31.9375" horizontalTextPosition="center" iconTextGap="4" modelName="custom" preferredPlacement="anywhere" ratio="0.5" textColor="#000000" verticalTextPosition="bottom" visible="true" width="262.533203125" x="23.266451444377935" xml:space="preserve" y="31.489942213749373">ExternalActivationZone::onActivationEvent
msg == 1<y:LabelModel><y:SmartEdgeLabelModel autoRotationEnabled="false" defaultAngle="0.0" defaultDistance="10.0"/></y:LabelModel><y:ModelParameter><y:SmartEdgeLabelModelParameter angle="6.283185307179586" distance="126.97197202048523" distanceToCenter="true" position="left" ratio="64.53982129385655" segment="-2"/></y:ModelParameter><y:PreferredPlacementDescriptor angle="0.0" angleOffsetOnRightSide="0" angleReference="absolute" angleRotationOnRightSide="co" distance="-1.0" frozen="true" placement="anywhere" side="anywhere" sideReference="relative_to_edge_flow"/></y:EdgeLabel>
<y:Arc height="45.0" ratio="1.0" type="fixedRatio"/>
</y:ArcEdge>
</data>
</edge>
<edge id="e5" source="n4" target="n6">
<data key="d9"/>
<data key="d10">
<y:ArcEdge>
<y:Path sx="0.0" sy="0.0" tx="0.0" ty="0.0">
<y:Point x="530.3125" y="742.125"/>
</y:Path>
<y:LineStyle color="#000000" type="line" width="1.0"/>
<y:Arrows source="none" target="standard"/>
<y:Arc height="24.3125" ratio="1.0" type="fixedRatio"/>
</y:ArcEdge>
</data>
</edge>
</graph>
<data key="d7">
<y:Resources/>
</data>
</graphml>

BIN
sitl_config/usv/vrx_gazebo/docs/images/docking_states.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 24 KiB

BIN
sitl_config/usv/vrx_gazebo/docs/images/sarda_tcurve.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 43 KiB

BIN
sitl_config/usv/vrx_gazebo/docs/images/wamv_glf.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 149 KiB

BIN
sitl_config/usv/vrx_gazebo/docs/images/wamv_glf_annote.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 218 KiB

78
sitl_config/usv/vrx_gazebo/docs/math_bbing.sty Executable file
View File

@ -0,0 +1,78 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% math_bbing.sty
%
% Brian Bingham's LaTeX Math Style File
%
% HISTORY
% 20.03.2007 bbing Created from rme's style file
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\ProvidesPackage{math_bbing}
\usepackage{ifthen}
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{cancel}
\newcommand{\ifempty}[3]% Macro tests when calling param is empty
{\ifcat_#1_ %
#2 \else % Action when calling parameter empty
#3 \fi}% % Action when calling parameter not empty
%% Probability Utilities
% normal cov: syntax \normalcov[optional]{u}{P}
\providecommand{\normalcov}[3][]{
\ifempty{#1}{\ensuremath{\mathcal{N}\bigl({#2},{#3}\bigr)}} % N(u,P)
{\ensuremath{\mathcal{N}\bigl({#1};{#2},{#3}\bigr)}} % N(x;u,P)
}
% normal info: syntax \normalinfo[optional]{u}{P}
\providecommand{\normalinfo}[3][]{
\ifempty{#1}{\ensuremath{\mathcal{N}^{-1}\bigl({#2},{#3}\bigr)}} % N^-1(u,P)
{\ensuremath{\mathcal{N}^{-1}\bigl({#1};{#2},{#3}\bigr)}}% N^-1(x;u,P)
}
% pdf
%\providecommand{\pdf}[2][]{
% \ifempty{#1}{\ensuremath{p\bigl({#2}\bigr)}} % p(x)
% {\ensuremath{p\bigl({#1}\bigl\vert{#2}\bigr)}} % p(x|y)
%}
% expectation
\providecommand{\E}[1]{\ensuremath{\mathsf{E}\bigl[#1\bigr]}}
%% Vectors and Matricies
% bold vector
\providecommand{\bvec}[1]{\ensuremath{\boldsymbol{\mathrm{#1}}}}
% homogenous vector
\providecommand{\hvec}[1]{\ensuremath{\underline{\boldsymbol{\mathrm{#1}}}}}
% mean vector
\providecommand{\mvec}[1]{\ensuremath{\overline{\boldsymbol{\mathrm{#1}}}}}
% matrix
\providecommand{\mat}[1]{\ensuremath{\mathrm{#1}}}
% skew-symmetric matrix
\providecommand{\skewsym}[1]{\ensuremath{[{#1}]_{\times}}}
% identity matrix
\providecommand{\eye}[1]{\ensuremath{\mat{I}_{#1\times#1}}}
% zeros matrix
\providecommand{\zeros}[2][]{%
\ifempty{#1}{\ensuremath{0_{#2\times#2}}} %0_{nxn}
{\ensuremath{0_{#1\times#2}}} %0_{mxn}
}
% absolute
\providecommand{\abs}[1]{\ensuremath{\lvert{#1}\rvert}}
% norm
\providecommand{\norm}[1]{\ensuremath{\left\lVert{#1}\right\rVert}}
% left sub/sup script
\providecommand{\leftexp}[2]{\ensuremath{{\vphantom{#2}}{#1}{#2}}}
\providecommand{\cov}[1]{\ensuremath{\operatorname{cov}({#1})}}
% derivative
\providecommand{\deriv}[2]{\ensuremath{\frac{\mathrm{d}#1}{\mathrm{d}#2}}}
% partial derivative
\providecommand{\pderiv}[2]{\ensuremath{\frac{\partial#1}{\partial#2}}}
% differential
\providecommand{\dt}[1][]{
\ifempty{#1}{\ensuremath{\mathrm{dt}}} %dt
{\ensuremath{\mathrm{d}#1}} %d#1
}

BIN
sitl_config/usv/vrx_gazebo/docs/pm_ex.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 179 KiB

45
sitl_config/usv/vrx_gazebo/docs/pm_sampling.py Normal file
View File

@ -0,0 +1,45 @@
def pm(omega,omega_p):
'''
Return spectrum value as a function of freq and peak freq
'''
alpha = 0.0081
beta = 0.74
g = 9.81
return alpha*g**2/(omega**5)*exp(-(5.0/4.0)*(omega_p/omega)**4)
figure(1)
clf()
Tps = [pi]#2,4,6]
for Tp in Tps:
omega_p = 2*pi/Tp
omegas = linspace(omega_p*.25,omega_p*4,100)
s = []
for o in omegas:
s.append(pm(o,omega_p))
plot(omegas,s, label='$\\bar{T}=T_p$ = %.2f s, $\omega_p$=%.2f rad/s'%(Tp,omega_p))
scale = 1.5
omegas = [omega_p/scale, omega_p, omega_p*scale]
delos = [omega_p*(1-1/scale), omega_p*(scale-1/scale)/2.0, omega_p*(scale-1)]
s = []
a = []
for o,d in zip(omegas,delos):
s.append(pm(o,omega_p))
a.append(sqrt(2*pm(o,omega_p)*d))
print (2*pi/omega_p)
print (2*pi/array(omegas))
print (a)
plot(omegas,s,'o',label='Samples for s = %.2f'%scale)
legend()
grid(True)
xlabel('$\omega \,\, [rad/s]$')
ylabel('$S(\omega) \,\, [m^2/(rad/s)]$')
title('P-M Spectrum')
show()

66
sitl_config/usv/vrx_gazebo/docs/refs.bib Normal file
View File

@ -0,0 +1,66 @@
% This file was created with JabRef 2.10.
% Encoding: UTF-8
@Misc{gpugems_waves,
Title = {{GPU} Gems: Chapter 1. Effective Water Simulation from Physical Models},
Author = {Mark Finch},
HowPublished = {\url{https://developer.nvidia.com/gpugems/GPUGems/gpugems_ch01.html}},
Note = {Accessed: 2019-0604},
Owner = {bsb},
Timestamp = {2019.06.05}
}
@Book{fossen11handbook,
Title = {Handbook of Marine Craft Hydrodynamics and Motion Control},
Author = {Thor I. Fossen},
Publisher = {Wiley},
Year = {2011},
Owner = {bsb},
Timestamp = {2016.12.13}
}
@Book{fossen94guidance,
Title = {Guidance and Control of Ocean Vehicles},
Author = {Thor I. Fossen},
Publisher = {Wiley},
Year = {1994},
Owner = {bsb},
Timestamp = {2016.12.13}
}
@InProceedings{frechot06realistic,
Title = {Realistic simulation of ocean surface using wave spectra},
Author = {Jocelyn Fréchot},
Booktitle = {Proceedings of the First
International Conference on Computer Graphics Theory and Applications (GRAPP 2006)},
Year = {2006},
Owner = {bsb},
Timestamp = {2019.06.06}
}
@Article{sarda17station,
Title = {Station-keeping control of an unmanned surface vehicle exposed to current and wind disturbances},
Author = {Edoardo I. Sarda and Huajin Qu and Ivan R. Bertaska and Karl D. von Ellenrieder},
Journal = {Ocean Engineering},
Year = {2016},
Pages = {305 - 324},
Volume = {127},
Doi = {https://doi.org/10.1016/j.oceaneng.2016.09.037},
ISSN = {0029-8018},
Keywords = {Station-keeping, Nonlinear control, Wind feedforward, Unmanned surface vehicles},
Url = {http://www.sciencedirect.com/science/article/pii/S0029801816304206}
}
@InProceedings{tessendorf99simulating,
Title = {Simulating ocean water},
Author = {Jerry Tessendorf and Copyright C and Jerry Tessendorf},
Year = {1999}
}

217
sitl_config/usv/vrx_gazebo/docs/theory_of_operation.tex Normal file
View File

@ -0,0 +1,217 @@
\documentclass[11pt]{article}
%\usepackage[firstpage]{draftwatermark}
\usepackage{times}
\usepackage{pdfpages}
\usepackage{fullpage}
\usepackage{url}
\usepackage{hyperref}
\usepackage{fancyhdr}
\usepackage{graphicx}
\usepackage{tabularx}
\usepackage{enumitem}
\usepackage{indentfirst}
\usepackage{subcaption}
\usepackage{units}
\usepackage{bm}
\usepackage{./math_bbing}
% Highlighting
\usepackage{color,soul}
\DeclareRobustCommand{\hlr}[1]{{\sethlcolor{red}\hl{#1}}}
\DeclareRobustCommand{\hlg}[1]{{\sethlcolor{green}\hl{#1}}}
\DeclareRobustCommand{\hlb}[1]{{\sethlcolor{blue}\hl{#1}}}
\DeclareRobustCommand{\hly}[1]{{\sethlcolor{yellow}\hl{#1}}}
\setcounter{secnumdepth}{4}
\graphicspath{{images/}}
\pagestyle{fancy}
% Conditional for notes
\newif\ifnotes
\notesfalse
\newcommand{\doctitle}{USV Plugins, Theory of Operation}
\newcommand{\docnumber}{\doctitle}
\addtolength{\headheight}{2em}
\addtolength{\headsep}{1.5em}
\lhead{\docnumber}
\rhead{}
\newcommand{\capt}[1]{\caption{\small \em #1}}
\cfoot{\small Brian Bingham \today \\ \thepage}
\renewcommand{\footrulewidth}{0.4pt}
\newenvironment{xitemize}{\begin{itemize}\addtolength{\itemsep}{-0.75em}}{\end{itemize}}
\newenvironment{tasklist}{\begin{enumerate}[label=\textbf{\thesubsubsection-\arabic*},ref=\thesubsubsection-\arabic*,leftmargin=*]}{\end{enumerate}}
\newcommand\todo[1]{{\bf TODO: #1}}
\setcounter{tocdepth}{2}
\setcounter{secnumdepth}{4}
\makeatletter
\newcommand*{\compress}{\@minipagetrue}
\makeatother
%\renewcommand{\chaptername}{Volume}
%\renewcommand{\thesection}{\Roman{section}}
%\renewcommand{\thesubsection}{\Roman{section}-\Alph{subsection}}
\begin{document}
\input{./Commands}
\input{./defs}
\newpage
% Title Page
\setcounter{page}{1}
\begin{center}
{\huge \doctitle}
\end{center}
\section{Overview}
The rigid body dynamics implemented in Gazebo are augmented with environmental forces to simulate an unmanned surface vessel via a set of Gazebo plugins. These plugins simulate the effects of
\begin{itemize}
\item Dynamics
\begin{itemize}
\item Maneuvering - added mass, drag, etc.
\item Wave field - motion of the water surface
\end{itemize}
\item Thrust - vehicle propulsion
\item Wind - windage.
\end{itemize}
This document is a high-level description of the implementation, but not a full documentation of the models and techniques used. The code itself is the ultimate documentation; comments have been included in the source to allow for users to adapt and extend these simple techniques for their own purposes.
\section{Maneuvering Model and Waves}
The influence of both maneuvering and wave forces on the motion of the USV is implemented as a single model plugin for Gazebo. Currently there is only one implementation, but other implementations, with varying fidelity, could be developed in the future.
\subsection{usv gazebo dynamics plugin}
\subsubsection{Maneuvering Model}
The plugin implements a portion of the nonlinear maneuvering equations from \cite{fossen11handbook},
\beqn
\underbrace{\bm{M}_{RB}\dot{\bm{\nu}}+\bm{C}_{RB}(\bm{\nu})\bm{\nu}}_\text{rigid-body forces} +
\underbrace{\bm{M}_A\dot{\bm{\nu}}_r + \bm{C}_A(\bm{\nu}_r)\bm{\nu}_r +
\bm{D}(\bm{\nu}_r)\bm{\nu}_r}_\text{hydrodynamic forces}
= \bm{\tau}+\bm{\tau}_{wind}+\bm{\tau}_{waves}
\label{e:fossenmodel}
\eeqn
where the state vector $\bm{\nu}=[u,v,r]^T$ includes the velocities $u$, $v$ and $r$ are in the surge, sway and yaw directions respectively and $\bm{\nu}_r$ is the velocity vector relative to an irrotational water current $\bm{\nu}_c$, i.e., $\bm{\nu}=\bm{\nu}_r+\bm{\nu}_c$.
The \emph{rigid-body forces} components are simulated via the Gazebo physics engine, while the \emph{hydrodynamic forces} are determined via the plugin.
The six DOF maneuvering model is specified by the following matrices. The added mass matrix expressed as
\beqn
\bm{M}_{A}= \left[
\begin{array}{cccccc}
-X_{\dot{u}} & 0 & 0 &0 &0 &0 \\
0 & -Y_{\dot{v}} & 0 &0 &0 &0 \\
0 & 0 &0.1 &0 &0 &0 \\
0 &0 &0 &0.1 &0 &0 \\
0 &0 &0 &0 &0.1 &0 \\
0 &0 &0 &0 & 0 & -N_{\dot{r}}
\end{array} \right].
\eeqn
The Coriolis-centripetal matrix for the added mass expressed as
\beqn
\bm{C}_{A}(\bm{\nu}_r)= \left[
\begin{array}{cccccc}
0 &0 &0 &0 & 0 & Y_{\dot{v}}v_r+Y_{\dot{r}}r \\
0 &0 &0 &0 & 0 & -X_{\dot{u}}u_r\\
0 &0 &0 &0 &0 &0 \\
0 &0 &0 &0 &0 &0 \\
0 &0 &0 &0 &0 &0 \\
0 &0 &0 & -Y_{\dot{v}}v_r - Y_{\dot{r}}r& X_{\dot{u}}u_r & 0
\end{array} \right].
\eeqn.
It is worth noting that $\bm{C}_A$ includes the nonlinear Munk moment (see \cite{fossen11handbook} p.121). Following \cite{fossen11handbook} the SNAME notation for the hydrodynamic derivatives. Currently these terms are neglected in the model for simplicity and because of the challenges in estimating and verifying the pertinent parameters.
The linear and quadratic drag terms
\beqn
\bm{D}(\bm{\nu}_r)= \left[
\begin{array}{cccccc}
X_u + X_{u|u|}|u| & 0 & 0 &0 &0 &0\\
0 & Y_v + Y_{v|v|}|v| &0 &0 &0 &0\\
0 &0 &Z_w &0 &0 &0 \\
0 &0 &0 &K_p &0 &0 \\
0 &0 &0 &0 &M_q &0 \\
0 &0 &0 &0 &0 & N_r+N_{r|r|}|r|
\end{array} \right].
\eeqn
which neglects coupleing between sway and yaw.
\subsubsection{Wave Forcing}
Gerstner waves with three components \cite{tessendorf99simulating}. Each of the component waves is user-specified by an amplitude, period and direction. Deep water dispersion is used in simulating the wave behaviors.
To determine the influence of the wave field on the vessel, the vessel footprint is decomposed into a simple grid, with points at each corner of the vessel. The vertical displacement is calculated for each of these points. Using the vessel's position and attitude, the buoyancy force at each location is determined and then applied at that grid point.
\section{Thrust}
The external force and torque from the vessel propulsion is implemented in a standalone plugin to allow for independent extension to higher-fidelity thrust configurations and models.
\subsection{usv gazebo thrust plugin}
The plugin subscribes the \verb+cmd_drive+ topic to receive messages of type UsvDrive (defined in the \verb+usv_msgs+ package). These message specify left and right thrust commands where the commands are scaled from $\{-1.0-1.0\}$.
To emulate the thruster behavior, the commands are mapped to a thrust force applied to the model. Two possible mappings are currently available. Users select which mapping using the \verb+mappingType+ SDF tag.
The axial force is applied at a point vertically separated from the CG as specified by the \texttt{thrustOffsetZ} parameter. This results in coupling between the forward thrust and the vehicle pitch.
\subsubsection{0: Linear thruster map}
The command values (-1.0 to 1.0) are scaled linearly to the \texttt{maxForceFwd} and \texttt{maxForceRev} SDF parameters. A total forward thrust is caculated as the sum. A total torque is calculated assuming the two thrust forces are applied at opposite ends of the \texttt{boatWidth} parameter.
\subsubsection{1: GLF thruster map}
In this mode two generalized logistic functions (GLFs) are used to convert commands to thrust force. One set of GLF parameters are used for positive commands (0 to 1.0) and a second set of parameters are used for negative commands (-1 to 0). The form of the GLF used is
\begin{equation}
T = A + \frac{K-A}{\left(C+\exp(-B(x-M))\right)^{1/\nu}}
\end{equation}
where $T$ is the thrust force in Newtons, $x$ is the command and the remaining variables are the GLF parameters. To identify the GLF parameters, the data from
\cite{sarda17station} was used---Figure\ref{f:sarda}. The Python script for accomplishing this is include in the \verb+usv_gazebo_plugins+ repository in the \verb+thrust_curve_fit+ directory.
\begin{figure}[h]
\centering
\includegraphics[width=0.6\textwidth]{images/sarda_tcurve.png}
\caption{Empirical thrust performance data from \cite{sarda17station}.}
\label{f:sarda}
\end{figure}
The data was fit with two separate GLF functions using the \verb+scipy.optimize.fmin+ optimization routine to minimize the squared error. The results are shown in Figure\ref{f:fit}.
\begin{figure}[h]
\centering
\includegraphics[width=0.6\textwidth]{images/wamv_glf_annote.png}
\caption{GLF fit of empirical data.}
\label{f:fit}
\end{figure}
\section{Wind}
The influence of wind on the motion of the USV is implemented as a standalone model plugin for Gazebo. Currently there is only one implementation, but other implementations, with varying fidelity, could be developed in the future.
\subsection{usv gazebo wind plugin}
The wind forces (x and y) and moment (yaw) are predicted following the models presented by Fossen~\cite{fossen94guidance}.
The wind velocity on the vessel ($V_w$) is considered to be a constant velocity and direction. If desired, this could be extended to include a parameterized wind spectrum the distribution of wind velocities over time, e.g., average wind velocity, gusts, etc. For the current implementation the constant wind velocity is specified as a three element vector which specifies the wind speed the world-frame x, y and z coordinates with units of \unitfrac[]{m}{s}. The z component is ignored.
The resulting forces and moments on the vessel are determined based on the user-specified force/moment coefficients and the relative wind velocity. Within the plugin, the relative (or apparent) wind velocity vector $V_R$. The forces/moment are calculated as
\begin{eqnarray}
X_{wind} &=& (C_X) V_{R_x} |V_{R_x}| \\
Y_{wind} &=& (C_Y) V_{R_y} |V_{R_y}| \\
N_{wind} &=& -2.0 (C_N) V_{R_x} V_{R_y} \\
\end{eqnarray}
where $C_X$, $C_Y$ and $C_N$ are specified as the three element \verb+wind_coeff_vector+. Approximate values for these coefficients are given in \cite{sarda17station} which can then be tuned to give reasonable response.
%\newpage
%\setcounter{page}{1}
\bibliographystyle{ieeetr}
\bibliography{refs}
\end{document}

BIN
sitl_config/usv/vrx_gazebo/docs/wave_generation.pdf Normal file
View File

Binary file not shown.

203
sitl_config/usv/vrx_gazebo/docs/wave_generation.tex Normal file
View File

@ -0,0 +1,203 @@
\documentclass[11pt]{article}
%\usepackage[firstpage]{draftwatermark}
\usepackage{times}
\usepackage{pdfpages}
\usepackage{fullpage}
\usepackage{url}
\usepackage{hyperref}
\usepackage{fancyhdr}
\usepackage{graphicx}
\usepackage{tabularx}
\usepackage{enumitem}
\setlist{nosep} % or \setlist{noitemsep} to leave space around whole list
\usepackage{indentfirst}
\usepackage{subcaption}
\usepackage{units}
\usepackage{bm}
\usepackage{algorithm}
\usepackage{algorithmic}
\usepackage{./math_bbing}
% Highlighting
\usepackage{color,soul}
\DeclareRobustCommand{\hlr}[1]{{\sethlcolor{red}\hl{#1}}}
\DeclareRobustCommand{\hlg}[1]{{\sethlcolor{green}\hl{#1}}}
\DeclareRobustCommand{\hlb}[1]{{\sethlcolor{blue}\hl{#1}}}
\DeclareRobustCommand{\hly}[1]{{\sethlcolor{yellow}\hl{#1}}}
\setcounter{secnumdepth}{4}
\graphicspath{{images/}}
\pagestyle{fancy}
% Conditional for notes
\newif\ifnotes
\notesfalse
\newcommand{\doctitle}{Wave Generation}
\newcommand{\docnumber}{\doctitle}
\addtolength{\headheight}{2em}
\addtolength{\headsep}{1.5em}
\lhead{\docnumber}
\rhead{}
\newcommand{\capt}[1]{\caption{\small \em #1}}
\cfoot{\small Brian Bingham \today \\ \thepage}
\renewcommand{\footrulewidth}{0.4pt}
\newenvironment{xitemize}{\begin{itemize}\addtolength{\itemsep}{-0.75em}}{\end{itemize}}
\newenvironment{tasklist}{\begin{enumerate}[label=\textbf{\thesubsubsection-\arabic*},ref=\thesubsubsection-\arabic*,leftmargin=*]}{\end{enumerate}}
\newcommand\todo[1]{{\bf TODO: #1}}
\setcounter{tocdepth}{2}
\setcounter{secnumdepth}{4}
\makeatletter
\newcommand*{\compress}{\@minipagetrue}
\makeatother
%\renewcommand{\chaptername}{Volume}
%\renewcommand{\thesection}{\Roman{section}}
%\renewcommand{\thesubsection}{\Roman{section}-\Alph{subsection}}
\begin{document}
\input{./Commands}
\input{./defs}
\newpage
% Title Page
\setcounter{page}{1}
\begin{center}
{\huge \doctitle}
\end{center}
\section{Gerstner Wave Model}
Using \cite{tessendorf99simulating}, \cite{gpugems_waves} and \cite{frechot06realistic} the wave field is generated by summation of Gersnter waves at a nominal location ($\Vi{x_o} = \{x_o, y_o\}$) to generate a wave height $z$ at the location $\V{x} = \{x, y\}$
\begin{eqnarray}
\V{x} & = & \Vi{x}{o} - \sum_{i=1}^{N} q_i A_i \sin(\Vi{k}{i} \cdot \Vi{x}{o} - \omega_i t + \phi_i) \\
z & = & \sum_{i=1}^N A_i \cos(\Vi{k}{i} \cdot \Vi{x}{o} - \omega_i t + \phi_i)
\label{e:gerstner}
\end{eqnarray}
where for each component wave $q_i$ is the steepness, $A_i$ is the amplitude, $\Vi{k}{i}$ is the wavevector, $\omega_i$ is the angular frequency and $\phi_i$ is the phase. The wavevector is a horizontal vector in the direction of travel of the wave with the magnitude equal to the wavenumber, $k=2\pi/\lambda$, where $\lambda$ is the wavelength. The wavenumber and angular frequency are related through the deep water dispersion relation
\begin{equation}
\omega^2 = gk.
\end{equation}
\subsection{Startup Transient}
To address large forces due to transients at the start of the simulation, we modulate the wave height using a user-specified time constant $\tau$ so that the actual wave height is
\begin{equation}
z = (1 - e^{-t/\tau}) \sum_{i=1}^N A_i \cos(\Vi{k}{i} \cdot \Vi{x}{o} - \omega_i t + \phi_i)
\end{equation}
\section{Wavefield Models}
The properties of the component waves (\ref{e:gerstner}) are derived from a user-specified wavefield model. We currently support the following two wavefield models:
\begin{enumerate}
\item Pierson-Moskowitz wave spectrum sampling (PMS)
\item Constant wavelength-amplitude ratio (CWR) as described in \cite{gpugems_waves}
\end{enumerate}
We use a common set of user-specified parameters for the wavefield model.
\subsection{User-Specified Wavefield Model Parameters}
Because we use a common set of wavefield model parameters to specify two possible models, the interpretation of some the parameters changes with the chosen model.
\begin{description}
\item[number ($N$)]: Same for PMS and CWR. The number of component waves. Currently the visual implementation in GernstnerWaves.vert is limited to no more than three, so $N \leq 3$.
\item[steepness ($q$)]: Same for PMS and CWR. Steepness factor for Gernstner Waves - see Tessendorf, Simulating Ocean Waves.
\item[period ($\bar{T}$)]:
\begin{itemize}
\item PMS: Specifies the peak of the P-M spectra.
\item CWR: Median period, which specifies the median wavelength via the deep water dispersion relationship.
\end{itemize}
\item[amplitude ($\bar{A}$)]:
\begin{itemize}
\item CWR: Not used. Amplitude for each component is determiend by P-M spectrum.
\item PMS: The median amplitude. The ratio of median amplitude and median wavelength is held constant.
\end{itemize}
\item[scale ($s$)]:
\begin{itemize}
\item PMS: A sampling scale factor specifying the multiplier on the angular frequency to sample successive wave components. Typically 1.5.
\item CWR: A scale factor specifying the wavelength/wavenumber spacing between successive wave components. Typically 2.0.
\end{itemize}
\item[gain ($\gamma$)]:
\begin{itemize}
\item PMS: Multiplier applied to the amplitude components.
\item CWR: Not used
\end{itemize}
\item[direction ($\bar{\V{d}}=\{d_x,d_y\}=\angle{\theta_d}$)]: Same for PMS and CWR. Mean direction of wave field, expressed as 2D unit vector
\item[angle ($a$)]: Same for PMS and CWR. Incremental angle relative to specified mean direction for generating individual component waves.
\item[tau ($\tau$)]: Same for PMS and CWR. Time constant for gradualy increasing wave field.
\end{description}
\subsection{Pierson-Moskowitz wave spectrum sampling (PMS)}
The Pierson-Moskowitz spectrum can be expressed as
\begin{equation}
S_{PM}(\omega)=\frac{\alpha g^2}{\omega^5} \exp{\left[ -\frac{5}{4} \left(\frac{\omega_p}{\omega}\right)^4 \right]}
\end{equation}
where $\alpha=8.1(10^{-3})$ and $\omega_p$ is the peak angular frequency. We specify a geometric sampling where we use individual frequencies
\begin{equation}
\omega_i = s^{i-(N-1)/2}\omega_p
\end{equation}
where the user sets th e scale value $s$ and the number of samples $N$.
\begin{figure}[hbt!]
\centerline{
{\includegraphics[width=0.8\textwidth]{pm_ex.png}}}
\caption{P-M spectrum example.}
\label{f:pm}
\end{figure}
The amplitude is determined from the spectrum by
\[
a_i^2 = 2 S(\omega_i) \Delta_i
\]
where $\Delta_i$ is the frequency spacing between samples. For $N=3$ this spacing is
\begin{eqnarray}
i=0 & : & \Delta_0 = \omega_p(1-1/s) \\
i=1 & : & \Delta_1 = \omega_p \frac{ s - 1/s}{s} \\
i=2 & : & \Delta_2 = \omega_p (s-1).
\end{eqnarray}
We also include a gain value ($\gamma$) as a multiplier of the amplitude so that
\[
A_i = \gamma a_i = \gamma \sqrt{ 2 S(\omega_i) \Delta_i}
\]
\subsection{Constant Wavelength-Amplitude Ratio (CWR) Wavefield Model}
This model for generating the component waves is described in \cite{gpugems_waves}. The general idea is that the user specifies the median wavelength and median amplitude. The ratio of wavelength/amplitude is held constant, so longer wavelength/period waves tend to dominate the wave field. The median wavelength is specified indirectly via the median period, which is then used to calculate the median angular frequency and then the median wavenumber via the dispersion relationship.
\begin{algorithm}
\caption{Constant Wavelength-Amplitude Ratio (CWR) Wavefield Model}
\begin{algorithmic}
\STATE $\bar{\omega} = \frac{2 \pi}{\bar{T}}$ (Median angular freq.)
\STATE $\bar{k}= \frac{\bar{\omega}^2}{g}$ (Median wavenumber/lenght, via deep water dispersion)
\STATE $i = 0$
\FOR{$i<N$}
\STATE $n=i - N/2$
\STATE $A_i = \bar{A} (s^n)$
\STATE $k_i = \frac{\bar{k}}{s^n}$
\STATE $\omega_i = \sqrt{g k_i} = \bar{\omega}/ \sqrt{s^n}$
\STATE $T_i = \bar{T} \sqrt{s^n}$
\STATE $\Vi{d}{i} = \angle{(\theta_d + (n)(a))}$
\STATE $\Vi{k}{i} = k_i \Vi{d}{i}$
\STATE $q_i = \min{\{1.0, q/(A_i k_i N)\}}$
\STATE $\phi_i = 0$
\STATE $i++$
\ENDFOR
\end{algorithmic}
\end{algorithm}
\newpage
%\setcounter{page}{1}
\bibliographystyle{ieeetr}
\bibliography{refs}
\end{document}

BIN
sitl_config/usv/vrx_gazebo/docs/wavefield_envelope.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 127 KiB

17
sitl_config/usv/vrx_gazebo/docs/wavefield_envelope.py Normal file
View File

@ -0,0 +1,17 @@
# Period values
T = [1, 1, 2, 4, 6, 8, 8, 1];
# Gain values
G = [0, 1, 1, 0.5, 0.375, 0.375, 0, 0];
figure(1)
clf()
plot(T,G,'r-o')
fill(T,G,'g')
xlabel('P-M Peak Period ($T_p$) [s]')
ylabel('Gain ($\gamma$) [m/m]')
grid(True)
xlim([0, 9])
ylim([0, 1.1])
title('P-M Wavefield Model Envelope')
show()

12
sitl_config/usv/vrx_gazebo/docs/wavefield_envelope.py~ Normal file
View File

@ -0,0 +1,12 @@
# Period values
T = [1, 1, 2, 4, 6, 8, 8];
# Gain values
G = [0, 1, 1, 0.5, 0.375, 0.375, 0];
figure(1)
clf()
plot(T,G,'-o')
xlabel('P-M Peak Period ($T_p$) [s]')
ylabel('Gain ($\gamma$) [m/m]')
grid(True)

184
sitl_config/usv/vrx_gazebo/include/vrx_gazebo/light_buoy_plugin.hh Normal file
View File

@ -0,0 +1,184 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef VRX_GAZEBO_LIGHT_BUOY_PLUGIN_HH_
#define VRX_GAZEBO_LIGHT_BUOY_PLUGIN_HH_
#include <ros/ros.h>
#include <std_msgs/ColorRGBA.h>
#include <std_msgs/Empty.h>
#include <light_buoy_colors.pb.h>
#include <array>
#include <cstdint>
#include <mutex>
#include <string>
#include <utility>
#include <vector>
#include <gazebo/gazebo.hh>
#include <sdf/sdf.hh>
namespace gazebo
{
typedef const boost::shared_ptr<
const light_buoy_colors_msgs::msgs::LightBuoyColors>
ConstLightBuoyColorsPtr;
}
/// \brief Visual plugin for changing the color of some visual elements using
/// ROS messages. This plugin accepts the following SDF parameters:
///
/// <color_1>: The first color of the sequence (RED, GREEN, BLUE, YELLOW).
/// <color_2>: The second color of the sequence (RED, GREEN, BLUE, YELLOW).
/// <color_3>: The third color of the sequence (RED, GREEN, BLUE, YELLOW).
/// <shuffle>: True if the topic for shuffling the sequence is enabled.
/// <robot_namespace>: The ROS namespace for this node. If not present,
/// the model name without any "::"" will be used.
/// E.g.: The plugin under a visual named
/// "model1::my_submodel::link::visual" will use "model1"
/// as namespace unless a value is specified.
/// <ros_shuffle_topic>: The ROS topic used to request color changes.
/// <gz_colors_topic>: The gazebo topic used to request specific color changes.
// defaults to /vrx/light_buoy/new_pattern
/// <visuals>: The collection of visuals that change in color. It accepts N
/// elements of <visual> elements.
///
/// Here's an example:
/// <plugin name="light_buoy_plugin" filename="liblight_buoy_plugin.so">
/// <color_1>RED</color_1>
/// <color_2>GREEN</color_2>
/// <color_3>BLUE</color_3>
/// <visuals>
/// <visual>robotx_light_buoy::base_link::panel_1</visual>
/// <visual>robotx_light_buoy::base_link::panel_2</visual>
/// <visual>robotx_light_buoy::base_link::panel_3</visual>
/// </visuals>
/// <shuffle>true</shuffle>
/// <robot_namespace>vrx</robot_namespace>
/// <ros_shuffle_topic>light_buoy/shuffle</ros_shuffle_topic>
/// </plugin>
class LightBuoyPlugin : public gazebo::VisualPlugin
{
// \brief Constructor
public: LightBuoyPlugin();
// Documentation inherited.
public: void Load(gazebo::rendering::VisualPtr _parent,
sdf::ElementPtr _sdf);
/// \brief Creates a std_msgs::ColorRGBA message from 4 doubles.
/// \param[in] _r Red.
/// \param[in] _g Green.
/// \param[in] _b Blue.
/// \param[in] _a Alpha.
/// \return The ColorRGBA message.
private: static std_msgs::ColorRGBA CreateColor(const double _r,
const double _g,
const double _b,
const double _a);
/// \brief Return the index of the color from its string.
/// \param[in] _color The color
/// \return The index in kColors.
private: static uint8_t IndexFromColor(const std::string &_color);
/// \brief Initialize all color sequences.
private: void InitializeAllPatterns();
/// \brief Parse all SDF parameters.
/// \param[in] _sdf SDF elements.
private: bool ParseSDF(sdf::ElementPtr _sdf);
/// \brief ROS callback for generating a new color pattern.
/// \param[in] _msg Not used.
private: void ChangePattern(const std_msgs::Empty::ConstPtr &_msg);
/// \brief Gazebo callback for changing light to a specific color pattern.
/// \param[in] _msg New color sequence.
private: void ChangePatternTo(gazebo::ConstLightBuoyColorsPtr &_msg);
/// \brief Display the next color in the sequence, or start over if at the end
private: void Update();
/// \def Colors_t
/// \brief A pair of RGBA color and its name as a string.
private: using Colors_t = std::pair<std_msgs::ColorRGBA, std::string>;
/// \def Pattern_t
/// \brief The current pattern to display, pattern[3] and pattern[4]
/// are always OFF.
private: using Pattern_t = std::array<uint8_t, 5>;
/// \brief List of the color options (red, green, blue, yellow and no color)
/// with their string name for logging.
private: static const std::array<Colors_t, 5> kColors;
/// \brief All color sequences.
private: std::vector<Pattern_t> allPatterns;
/// \brief The index pointing to one of the potential color sequences.
private: size_t allPatternsIdx = 0u;
/// \brief Collection of visual names.
private: std::vector<std::string> visualNames;
/// \brief Pointer to the visual elements to modify.
private: std::vector<gazebo::rendering::VisualPtr> visuals;
/// \brief Whether shuffle is enabled via a ROS topic or not.
private: bool shuffleEnabled = true;
/// \brief Subscriber to generate and display a new color sequence.
private: ros::Subscriber changePatternSub;
/// \brief ROS Node handle.
private: ros::NodeHandle nh;
// \brief Gazebo Node
private: gazebo::transport::NodePtr gzNode;
// \brief Gazebo subscriber listening for color specification
private: gazebo::transport::SubscriberPtr colorSub;
/// \brief The color pattern.
private: Pattern_t pattern;
/// \brief Track current index in pattern.
private: uint8_t state = 0u;
/// \brief ROS namespace.
private: std::string ns;
/// \brief ROS topic.
private: std::string rosShuffleTopic;
/// \brief gazebo topic.
private: std::string gzColorsTopic;
/// Pointer to the scene node.
private: gazebo::rendering::ScenePtr scene;
/// \brief Connects to rendering update event.
private: gazebo::event::ConnectionPtr updateConnection;
/// \brief Next time where the plugin should be updated.
private: gazebo::common::Time nextUpdateTime;
/// \brief Locks state and pattern member variables.
private: std::mutex mutex;
};
#endif

192
sitl_config/usv/vrx_gazebo/include/vrx_gazebo/navigation_scoring_plugin.hh Normal file
View File

@ -0,0 +1,192 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef VRX_GAZEBO_NAVIGATION_SCORING_PLUGIN_HH_
#define VRX_GAZEBO_NAVIGATION_SCORING_PLUGIN_HH_
#include <string>
#include <vector>
#include <gazebo/common/Events.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/World.hh>
#include <ignition/math/Pose3.hh>
#include <sdf/sdf.hh>
#include "vrx_gazebo/scoring_plugin.hh"
/// \brief A plugin for computing the score of the navigation task.
/// This plugin derives from the generic ScoringPlugin class. Check out that
/// plugin for other required SDF elements.
/// This plugin requires the following SDF parameters:
///
/// <obstacle_penalty>: Specifies how many points are deducted per collision.
/// <gates>: Specifies the collection of gates delimiting the course.
///
/// Each gate accepts the following elements:
///
/// <gate>: A gate is delimited by two markers (left and right).
/// The vessel should pass through the gate with the markers on the defined
/// right and left sides. E.g.:
///
/// <left_marker>: The name of the marker that should stay on the left
/// side of the vessel.
/// <right_marker> The name of the marker that should stay on the right
/// side of the vessel.
///
/// Here's an example:
/// <plugin name="navigation_scoring_plugin"
/// filename="libnavigation_scoring_plugin.so">
/// <vehicle>wamv</vehicle>
/// <task_name>navigation_scoring_plugin</task_name>
/// <course_name>vrx_navigation_course</course_name>
/// <obstacle_penalty>10</obstable_penalty>
/// <gates>
/// <gate>
/// <left_marker>red_bound_0</left_marker>
/// <right_marker>green_bound_0</right_marker>
/// </gate>
/// <gate>
/// <left_marker>red_bound_1</left_marker>
/// <right_marker>green_bound_1</right_marker>
/// </gate>
/// <gate>
/// <left_marker>red_bound_2</left_marker>
/// <right_marker>green_bound_2</right_marker>
/// </gate>
/// <gate>
/// <left_marker>red_bound_3</left_marker>
/// <right_marker>green_bound_3</right_marker>
/// </gate>
/// <gate>
/// <left_marker>red_bound_4</left_marker>
/// <right_marker>green_bound_4</right_marker>
/// </gate>
/// <gate>
/// <left_marker>red_bound_5</left_marker>
/// <right_marker>green_bound_5</right_marker>
/// </gate>
/// <gate>
/// <left_marker>red_bound_6</left_marker>
/// <right_marker>green_bound_6</right_marker>
/// </gate>
/// </gates>
/// </plugin>
class NavigationScoringPlugin : public ScoringPlugin
{
/// \brief All gate states.
private: enum class GateState
{
/// \brief Not "in" the gate.
VEHICLE_OUTSIDE,
/// \brief Before the gate.
VEHICLE_BEFORE,
/// \brief After the gate.
VEHICLE_AFTER,
/// \brief Gate crossed!
CROSSED,
/// \brief Gate invalid. E.g.: if crossed in the wrong direction.
INVALID,
};
/// \brief A gate that is part of the navigation challenge.
private: class Gate
{
/// \brief Constructor.
/// \param[in] _leftMarkerName The left marker's model.
/// \param[in] _rightMarkerName The right marker's model.
public: Gate(const gazebo::physics::LinkPtr _leftMarkerModel,
const gazebo::physics::LinkPtr _rightMarkerModel);
/// \brief Where is the given robot pose with respect to the gate?
/// \param _robotWorldPose Pose of the robot, in the world frame.
/// \return The gate state given the current robot pose.
public: GateState IsPoseInGate(
const ignition::math::Pose3d &_robotWorldPose) const;
/// \brief Recalculate the pose and width of the gate.
public: void Update();
/// \brief The left marker model.
public: gazebo::physics::LinkPtr leftMarkerModel;
/// \brief The right marker model.
public: gazebo::physics::LinkPtr rightMarkerModel;
/// \brief The center of the gate in the world frame. Note that the roll and
/// pitch are ignored. Only yaw is relevant and it points into the direction
/// in which the gate should be crossed.
public: ignition::math::Pose3d pose;
/// \brief The width of the gate in meters.
public: double width;
/// \brief The state of this gate.
public: GateState state = GateState::VEHICLE_OUTSIDE;
};
// Constructor.
public: NavigationScoringPlugin();
// Documentation inherited.
public: void Load(gazebo::physics::WorldPtr _world,
sdf::ElementPtr _sdf);
/// \brief Parse the gates from SDF.
/// \param[in] _sdf The current SDF element.
/// \return True when the gates were successfully parsed or false othwerwise.
private: bool ParseGates(sdf::ElementPtr _sdf);
/// \brief Register a new gate.
/// \param[in] _leftMarkerName The name of the left marker.
/// \param[in] _rightMarkerName The name of the right marker.
/// \return True when the gate has been registered or false otherwise.
private: bool AddGate(const std::string &_leftMarkerName,
const std::string &_rightMarkerName);
/// \brief Callback executed at every world update.
private: void Update();
/// \brief Set the score to 0 and change to state to "finish".
private: void Fail();
// Documentation inherited.
private: void OnCollision() override;
// Name of Course
private: gazebo::physics::ModelPtr course;
/// \brief All the gates.
private: std::vector<Gate> gates;
/// \brief Number of gates
private: int numGates;
/// \brief Pointer to the update event connection.
private: gazebo::event::ConnectionPtr updateConnection;
/// \brief The number of WAM-V collisions.
private: unsigned int numCollisions = 0;
/// \brief Number of points deducted per collision.
private: double obstaclePenalty = 10.0;
};
#endif

224
sitl_config/usv/vrx_gazebo/include/vrx_gazebo/perception_scoring_plugin.hh Normal file
View File

@ -0,0 +1,224 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
/* Note - this code was originally derived from the ARIAC
* PopulationPlugin https://bitbucket.org/osrf/ariac/src/master/osrf_gear/include/osrf_gear/PopulationPlugin.hh
*/
#ifndef VRX_GAZEBO_PERCEPTION_SCORING_PLUGIN_HH_
#define VRX_GAZEBO_PERCEPTION_SCORING_PLUGIN_HH_
#include <geographic_msgs/GeoPoseStamped.h>
#include <ros/ros.h>
#include <vector>
#include <memory>
#include <string>
#include <gazebo/physics/PhysicsTypes.hh>
#include <gazebo/physics/World.hh>
#include <gazebo/transport/transport.hh>
#include <sdf/sdf.hh>
#include "vrx_gazebo/scoring_plugin.hh"
/// \brief Class to store information about each object to be populated.
class PerceptionObject
{
/// \brief Simulation time in which the object should be spawned.
public: double time;
/// \brief amount of time in which the object should be spawned.
public: double duration;
/// \brief PerceptionObject type.
public: std::string type;
/// \brief PerceptionObject type.
public: std::string name;
/// \brief Pose in which the object should be placed in wam-v's frame.
public: ignition::math::Pose3d trialPose;
/// \brief Pose in which the object should be placed in global frame.
private: ignition::math::Pose3d origPose;
/// \brief ModelPtr to the model that this object is representing
public: gazebo::physics::EntityPtr modelPtr;
/// \brief bool to tell weather or not the object is open for attempts
public: bool active = false;
/// \brief error associated with the guess of a moel
public: double error = -1.0;
/// \brief constructor of perception object
public: PerceptionObject(const double& _time,
const double& _duration,
const std::string& _type,
const std::string& _name,
const ignition::math::Pose3d& _trialPose,
const gazebo::physics::WorldPtr _world);
/// \brief set the error of this boject if this object is active
/// and this is the lowest seen error
public: void SetError(const double& _error);
/// \brief move the object to where it is supposed to be relative to the frame
/// \brief of the robot and make it active
public: void StartTrial(const gazebo::physics::EntityPtr& _frame);
/// \brief move the object back to its original location and make inactive
public: void EndTrial();
/// \return a string summarizing this object
public: std::string Str();
};
/// \brief A plugin that allows models to be spawned at a given location in
/// a specific simulation time and then takes care of scoring correct
/// identification and localization of the objects.
///
/// The plugin accepts the following SDF parameters:
///
/// <object_sequence>: Contains the list of objects to be populated. An object
/// should be declared as an <object> element with the
/// following parameters:
/// <time> Simulation time to be spawned.
/// <type> Model.
/// <name> Landmark name.
/// <pose> Initial object pose.
///
/// <loop_forever>: Optional parameter. If true, all objects will be spawned
/// as a circular buffer. After spawning the last element of the collection,
/// the first one will be inserted.
///
/// <frame>: Optional parameter. If present, the poses of the objects will be
/// in the frame of this link/model. Otherwise the world frame is used.
///
/// <robot_namespace>: Optional parameter. If present, specifies ROS namespace.
///
/// <landmark_topic>: Optional parameter. Specify the topic to which the
/// plugin subscribes for receiving identification and localization msgs.
/// Default is "/vrx/perception/landmark"
///
/// <duration>: Optional parameter. Specify the time an object sticks around.
/// defaults to 5
///
/// Here's an example of a valid SDF:
///
/// <plugin filename="libperception_scoring_plugin.so"
/// name="perception_scoring_plugin">
/// <vehicle>wamv</vehicle>
/// <task_name>perception</task_name>
/// <initial_state_duration>1</initial_state_duration>
/// <ready_state_duration>1</ready_state_duration>
/// <running_state_duration>300</running_state_duration>
///
/// <!-- Parameters for PopulationPlugin -->
/// <loop_forever>false</loop_forever>
/// <frame>wamv</frame>
/// <object_sequence>
/// <object>
/// <time>10.0</time>
/// <type>surmark950410</type>
/// <name>red_0</name>
/// <pose>6 0 1 0 0 0</pose>
/// </object>
/// <object>
/// <time>10.0</time>
/// <type>surmark950400</type>
/// <name>green_0</name>
/// <pose>6 6 1 0 0 0</pose>
/// </object>
/// </object_sequence>
/// </plugin>
class PerceptionScoringPlugin : public ScoringPlugin
{
/// \brief Constructor.
public: PerceptionScoringPlugin();
/// \brief Destructor.
public: virtual ~PerceptionScoringPlugin();
// Documentation inherited.
public: virtual void Load(gazebo::physics::WorldPtr _world,
sdf::ElementPtr _sdf);
/// \brief Update the plugin.
protected: void OnUpdate();
private: void OnAttempt(
const geographic_msgs::GeoPoseStamped::ConstPtr &_msg);
/// \brief Restart the object population list
private: void Restart();
// Documentation inherited.
private: void OnRunning() override;
// Documentation inherited.
private: void ReleaseVehicle() override;
private: int attemptBal = 0;
/// \brief ROS namespace.
private: std::string ns;
/// \brief ROS topic where the object id/pose is received.
private: std::string objectTopic;
/// \brief ROS Node handle.
private: ros::NodeHandle nh;
/// \brief ROS subscriber
private: ros::Subscriber objectSub;
/// \brief World pointer.
public: gazebo::physics::WorldPtr world;
/// \brief SDF pointer.
public: sdf::ElementPtr sdf;
/// \brief Collection of objects to be spawned.
public: std::vector<PerceptionObject> objects;
/// \brief Connection event.
public: gazebo::event::ConnectionPtr connection;
/// \brief The time specified in the object is relative to this time.
public: gazebo::common::Time startTime;
/// \brief When true, "objects" will be repopulated when the object queue
/// is empty, creating an infinite supply of objects.
public: bool loopForever = false;
/// \brief Link/model name for the object poses use as their frame of
/// reference
public: std::string frameName = std::string();
/// \brief Link/model that the object poses use as their frame of reference.
public: gazebo::physics::EntityPtr frame;
/// \brief Last time (sim time) that the plugin was updated.
public: gazebo::common::Time lastUpdateTime;
/// \ brief count of how many objects have been despawned
private: int objectsDespawned =0;
};
#endif

174
sitl_config/usv/vrx_gazebo/include/vrx_gazebo/placard_plugin.hh Normal file
View File

@ -0,0 +1,174 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef VRX_GAZEBO_PLACARD_PLUGIN_HH_
#define VRX_GAZEBO_PLACARD_PLUGIN_HH_
#include <ros/ros.h>
#include <std_msgs/ColorRGBA.h>
#include <std_msgs/Empty.h>
#include <dock_placard.pb.h>
#include <array>
#include <cstdint>
#include <map>
#include <mutex>
#include <string>
#include <vector>
#include <gazebo/gazebo.hh>
#include <sdf/sdf.hh>
namespace gazebo
{
typedef const boost::shared_ptr<
const dock_placard_msgs::msgs::DockPlacard>
ConstDockPlacardPtr;
}
/// \brief Controls the shape and color of a symbol.
///
/// It selects a shape (triangle, cross, circle) and a color (red, green, blue)
/// and applies to the current symbol
/// This plugin can be configured with the following SDF tags:
///
/// <shape> "triangle", "cross" or "circle". If ommited, the shape will be
/// randomly selected.
/// <color> "red", "green" or "blue". If ommited, the color will be
/// randomly selected.
/// <shuffle>: True if the topic for shuffling the sequence is enabled.
/// <robot_namespace> ROS namespace of Node, can be used to have multiple
/// plugins.
/// <ros_shuffle_topic>: The ROS topic used to request color changes.
/// <gz_symbol_topic>: The gazebo topic subscribed to set symbol changes
/// defaults to /<robot_namespace>/symbol
/// <visuals>: The set of visual symbols. It contains at least one visual:
/// <visual>: A visual displaying a shape.
///
/// Here's an example:
/// <plugin name="placard1plugin" filename="libplacard_plugin.so">
/// <shape>triangle</shape>
/// <color>red</color>
/// <visuals>
/// <visual>dock_2018_placard1::visual_circle</visual>
/// <visual>dock_2018_placard1::visual_h_cross</visual>
/// <visual>dock_2018_placard1::visual_v_cross</visual>
/// <visual>dock_2018_placard1::visual_triangle</visual>
/// </visuals>
/// <shuffle>true</shuffle>
/// <robot_namespace>vrx</robot_namespace>
/// <topic>dock/placard/shuffle</topic>
/// </plugin>
class PlacardPlugin : public gazebo::VisualPlugin
{
// Documentation inherited.
public: PlacardPlugin();
public: void Load(gazebo::rendering::VisualPtr _parent,
sdf::ElementPtr _sdf);
/// \brief Creates a std_msgs::ColorRGBA message from 4 doubles.
/// \param[in] _r Red.
/// \param[in] _g Green.
/// \param[in] _b Blue.
/// \param[in] _a Alpha.
/// \return The ColorRGBA message.
private: static std_msgs::ColorRGBA CreateColor(const double _r,
const double _g,
const double _b,
const double _a);
/// \brief Initialize all color/symbol sequences.
private: void InitializeAllPatterns();
/// \brief Parse all SDF parameters.
/// \param[in] _sdf SDF elements.
private: bool ParseSDF(sdf::ElementPtr _sdf);
/// \brief Display the symbol in the placard
private: void Update();
/// \brief ROS callback for changing a symbol and its color.
/// \param[in] _msg Not used.
private: void ChangeSymbol(const std_msgs::Empty::ConstPtr &_msg);
/// \brief Gazebo callback for changing light to a specific color pattern.
/// \param[in] _msg New symbol.
private: void ChangeSymbolTo(gazebo::ConstDockPlacardPtr &_msg);
/// \brief List of the color options (red, green, blue, and no color)
/// with their string name for logging.
private: static std::map<std::string, std_msgs::ColorRGBA> kColors;
/// \brief List of the shape options (circle, cross, triangle)
/// with their string name for logging.
private: static std::vector<std::string> kShapes;
/// \brief The current color.
private: std::string color;
/// \brief The current shape.
private: std::string shape;
/// \brief All color/symbol sequences.
private: std::vector<std::array<std::string, 2u>> allPatterns;
/// \brief The index pointing to one of the potential color/symbol sequence.
private: size_t allPatternsIdx = 0u;
/// \brief Collection of visual names.
private: std::vector<std::string> visualNames;
/// \brief Pointer to the visual elements to modify.
private: std::vector<gazebo::rendering::VisualPtr> visuals;
/// \brief Whether shuffle is enabled via a ROS topic or not.
private: bool shuffleEnabled = true;
/// \brief Service to generate and display a new symbol.
private: ros::Subscriber changeSymbolSub;
/// \brief ROS Node handle.
private: ros::NodeHandle nh;
/// \brief ROS namespace.
private: std::string ns;
/// \brief ROS topic.
private: std::string rosShuffleTopic;
/// \brief gazebo Node
private: gazebo::transport::NodePtr gzNode;
/// \brief gazebo symbol sub topic
private: std::string symbolSubTopic;
/// \breif symbol subscriber
private: gazebo::transport::SubscriberPtr symbolSub;
/// Pointer to the scene node.
private: gazebo::rendering::ScenePtr scene;
/// \brief Connects to rendering update event.
private: gazebo::event::ConnectionPtr updateConnection;
/// \brief Next time where the plugin should be updated.
private: gazebo::common::Time nextUpdateTime;
/// \brief Locks state and pattern member variables.
private: std::mutex mutex;
};
#endif

368
sitl_config/usv/vrx_gazebo/include/vrx_gazebo/scan_dock_scoring_plugin.hh Normal file
View File

@ -0,0 +1,368 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef VRX_GAZEBO_SCAN_DOCK_SCORING_PLUGIN_HH_
#define VRX_GAZEBO_SCAN_DOCK_SCORING_PLUGIN_HH_
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <light_buoy_colors.pb.h>
#include <dock_placard.pb.h>
#include <memory>
#include <string>
#include <vector>
#include <gazebo/gazebo.hh>
#if GAZEBO_MAJOR_VERSION >= 8
#include <ignition/transport/Node.hh>
#endif
#include <sdf/sdf.hh>
#include "vrx_gazebo/ColorSequence.h"
#include "vrx_gazebo/scoring_plugin.hh"
/// \brief A class to monitor if the color sequence reported matches the color
/// displayed in the light buoy.
class ColorSequenceChecker
{
/// \brief Constructor.
/// \param[in] _expectedColors The sequence of expected colors.
/// \param[in] _rosNameSpace ROS namespace.
/// \param[in] _rosColorSequenceService The ROS service used to receive
/// the color submisison.
public: ColorSequenceChecker(const std::vector<std::string> &_expectedColors,
const std::string &_rosNameSpace,
const std::string &_rosColorSequenceService);
/// \brief Enable the ROS submission service.
public: void Enable();
/// \brief Disable the ROS submission service.
public: void Disable();
/// \brief Whether a team submitted the color sequence or not.
/// \return True when the submission was received or false otherwise.
public: bool SubmissionReceived() const;
/// \brief Wheter a team submitted the color sequence and is correct or not.
/// \return True when the team submitted the color sequence and it is correct
/// or false otherwise.
public: bool Correct() const;
/// \brief Callback executed when a new color submission is received.
/// \param[in] _request Contains the submission.
/// \param[out] _res The Response. Note that this will be true even if the
/// reported sequence is incorrect.
private: bool OnColorSequence(
ros::ServiceEvent<vrx_gazebo::ColorSequence::Request,
vrx_gazebo::ColorSequence::Response> &_event);
/// \brief The expected color sequence.
private: std::vector<std::string> expectedSequence;
/// \brief ROS namespace.
private: std::string ns;
/// \brief ROS topic where the color sequence should be sent.
private: std::string colorSequenceService;
/// \brief ROS Node handle.
private: ros::NodeHandle nh;
/// \brief Service to generate and display a new color sequence.
private: ros::ServiceServer colorSequenceServer;
/// \brief Whether the color sequence has been received or not.
private: bool colorSequenceReceived = false;
/// \brief Whether the color sequence received is correct or not.
private: bool correctSequence = false;
};
/// \brief A class to monitor if the vehicle docked in a given bay.
class DockChecker
{
/// \brief Constructor.
/// \param[in] _name The name of the checker (only used for debugging).
/// \param[in] _internalActivationTopic The gazebo topic used to receive
/// notifications about the internal activation zone.
/// \param[in] _externalActivationTopic The gazebo topic used to receive
/// notifications about the external activation zone.
/// from the "contain" plugin.
/// \param[in] _minDockTime Minimum amount of seconds to stay docked to be
/// considered a fully successfull dock.
/// \param[in] _dockAllowed Whether is allowed to dock in this bay or not.
/// \param[in] _worldName Gazebo world name.
/// \param[in] _announceSymbol Optional symbol to announce via ROS.
public: DockChecker(const std::string &_name,
const std::string &_internalActivationTopic,
const std::string &_exteriorActivationTopic,
const double _minDockTime,
const bool _dockAllowed,
const std::string &_worldName,
const std::string &_rosNameSpace,
const std::string &_announceSymbol,
const std::string &_gzSymbolTopic);
/// \brief The name of this checker.
public: std::string name;
/// \brief Whether the robot has been successfully docked in this bay or not.
/// \return True when the robot has been docked or false otherwise.
public: bool AnytimeDocked() const;
/// \brief Whether the robot is currently at the entrance of the bay.
/// \return True when the robot is at the entrance or false othwerwise.
public: bool AtEntrance() const;
/// \brief Whether it is allowed to dock in this bay or not.
public: bool Allowed() const;
/// \brief Announce the symbol of the bay via ROS.
public: void AnnounceSymbol();
/// \brief Update function that needs to be executed periodically.
public: void Update();
/// \brief Callback triggered when the vehicle enters or exits the activation
/// zone.
/// \param[in] _msg The current state (0: exiting, 1: entering).
#if GAZEBO_MAJOR_VERSION >= 8
private: void OnInternalActivationEvent(const ignition::msgs::Boolean &_msg);
#else
private: void OnInternalActivationEvent(ConstIntPtr &_msg);
#endif
/// \brief Callback triggered when the vehicle enters or exits the activation
/// zone.
/// \param[in] _msg The current state (0: exiting, 1: entering).
#if GAZEBO_MAJOR_VERSION >= 8
private: void OnExternalActivationEvent(const ignition::msgs::Boolean &_msg);
#else
private: void OnExternalActivationEvent(ConstIntPtr &_msg);
#endif
/// \brief The gazebo topic used to receive notifications
/// from the internal activation zone.
private: std::string internalActivationTopic;
/// \brief The gazebo topic used to receive notifications
/// from the external activation zone.
private: std::string externalActivationTopic;
/// \brief The gazebo topic used to publish symbols to the placards
private: std::string gzSymbolTopic;
/// \brief Minimum amount of seconds to stay docked to be
/// considered a fully successfull dock.
private: double minDockTime;
/// \brief Whether is allowed to dock in this bay or not.
private: bool dockAllowed;
/// \brief Timer used to calculate the elapsed time docked in the bay.
private: gazebo::common::Timer timer;
#if GAZEBO_MAJOR_VERSION >= 8
/// \brief Ignition Transport node used for communication.
private: ignition::transport::Node ignNode;
#endif
/// \brief Create a node for communication.
private: gazebo::transport::NodePtr node;
/// \brief Subscriber to receive notifications from the contain plugin.
private: gazebo::transport::SubscriberPtr containSub;
/// \brief Whether the vehicle has successfully docked or not.
private: bool anytimeDocked = false;
/// \brief Whether the vehicle is at the entrance of the bay or not.
private: bool atEntrance = false;
/// \brief Color and shape of symbol to announce. E.g.: red_cross, blue_circle
private: std_msgs::String announceSymbol;
/// \brief ROS namespace.
private: std::string ns;
/// \brief ROS Node handle.
private: std::unique_ptr<ros::NodeHandle> nh;
/// \brief Publisher for the symbol.
private: ros::Publisher symbolPub;
/// \brief ROS topic where the target symbol will be published.
private: std::string symbolTopic = "/vrx/scan_dock/placard_symbol";
/// \brief Publish the placard symbols
private: gazebo::transport::PublisherPtr dockPlacardPub;
};
/// \brief A plugin for computing the score of the scan and dock task.
/// This plugin derives from the generic ScoringPlugin class. Check out that
/// plugin for other required SDF elements.
/// This plugin requires the following SDF parameters:
///
/// <enable_color_checker>: Optional parameter to turn off color checker
/// service - default is true.
/// <robot_namespace>: Optional parameter with the ROS namespace.
/// <color_sequence_service>: Optional paramter with the ROS service used to
/// receive the color submission.
/// <color_topic>: Optional gazebo topic used to publish the color sequence
/// defaults to /vrx/light_buoy/new_pattern
/// <color_1>: Expected first color of the sequence (RED, GREEN, BLUE, YELLOW).
/// <color_2>: Expected second color of the sequence (RED, GREEN, BLUE, YELLOW).
/// <color_3>: Expected third color of the sequence (RED, GREEN, BLUE, YELLOW).
/// <color_bonus_points>: Points granted when the color sequence is correct.
/// Default value is 10.
/// <bays>: Contains at least one of the following blocks:
/// <bay>: A bay represents a potential play where a vehicle can dock. It has
/// the following required elements:
/// <name>The name of the bay. This is used for debugging only.
/// <internal_activation_topic>The gazebo topic used to receive
/// notifications from the internal activation zone.
/// <external_activation_topic>The gazebo topic used to receive
/// notifications from the external activation zone.
/// <min_dock_time>Minimum amount of seconds to stay docked to be
/// considered a fully successfull dock.
/// <dockAllowed> Whether is allowed to dock in this bay or not.
/// <dock_bonus_points>: Points granted when the vehicle successfully
/// dock-and-undock in any bay.
/// Default value is 10.
/// <correct_dock_bonus_points>: Points granted when the vehicle successfully
/// dock-and-undock in the specified bay.
/// Default value is 10.
/// <symbol>: Required string with format <COLOR>_<SHAPE>, where
/// color can be "red", "green", "blue", "yellow" and color can be "triangle",
/// "circle", "cross". If this parameter is present, a ROS message will be
/// sent in OnReady(). The vehicle should dock in the bay matching this color
/// and shape.
///
/// Here's an example:
/// <plugin name="scan_dock_scoring_plugin"
/// filename="libscan_dock_scoring_plugin.so">
/// <!-- Parameters for scoring_plugin -->
/// <vehicle>wamv</vehicle>
/// <task_name>scan_dock</task_name>
/// <initial_state_duration>3</initial_state_duration>
/// <ready_state_duration>3</ready_state_duration>
/// <running_state_duration>300</running_state_duration>
/// <release_joints>
/// <joint>
/// <name>wamv_external_pivot_joint</name>
/// </joint>
/// <joint>
/// <name>wamv_external_riser</name>
/// </joint>
/// </release_joints>
///
/// <!-- Color sequence checker -->
/// <robot_namespace>vrx</robot_namespace>
/// <color_sequence_service>scan_dock/color_sequence</color_sequence_service>
/// <color_1>red</color_1>
/// <color_2>green</color_2>
/// <color_3>blue</color_3>
///
/// <!-- Dock checkers -->
/// <bays>
/// <bay>
/// <name>bay1</name>
/// <internal_activation_topic>
/// /vrx/dock_2018/bay_1_internal/contain
/// </internal_activation_topic>
/// <external_activation_topic>
/// /vrx/dock_2018/bay_1_external/contain
/// </external_activation_topic>
/// <min_dock_time>10.0</min_dock_time>
/// <dock_allowed>false</dock_allowed>
/// </bay>
///
/// <bay>
/// <name>bay2</name>
/// <internal_activation_topic>
/// /vrx/dock_2018/bay_2_internal/contain
/// </internal_activation_topic>
/// <external_activation_topic>
/// /vrx/dock_2018/bay_2_external/contain
/// </external_activation_topic>
/// <min_dock_time>10.0</min_dock_time>
/// <dock_allowed>true</dock_allowed>
/// <symbol>red_circle</symbol>
/// </bay>
/// </bays>
/// </plugin>
class ScanDockScoringPlugin : public ScoringPlugin
{
// Documentation inherited.
public: ScanDockScoringPlugin();
// Documentation inherited.
private: void Load(gazebo::physics::WorldPtr _world,
sdf::ElementPtr _sdf);
/// \brief Parse all SDF parameters.
/// \param[in] _sdf SDF elements.
private: bool ParseSDF(sdf::ElementPtr _sdf);
/// \brief Callback executed at every world update.
private: void Update();
// Documentation inherited.
private: void OnReady() override;
// Documentation inherited.
private: void OnRunning() override;
/// \brief gazebo Node
private: gazebo::transport::NodePtr node;
/// \brief Publish the color sequence
private: gazebo::transport::PublisherPtr lightBuoySequencePub;
/// \brief Pointer to the update event connection.
private: gazebo::event::ConnectionPtr updateConnection;
/// \brief In charge of receiving the team submission and compare it with
/// the color sequence from the light buoy.
private: std::unique_ptr<ColorSequenceChecker> colorChecker;
/// \brief Monitor all the available bays to decide when the vehicle docks.
private: std::vector<std::unique_ptr<DockChecker>> dockCheckers;
/// \brief To check colors or not
private: bool enableColorChecker = true;
/// \brief Whether we have processed the color sequence submission or not.
private: bool colorSubmissionProcessed = false;
/// \brief Points granted when the color sequence is correct.
private: double colorBonusPoints = 10.0;
/// \brief Points granted when the vehicle successfully
/// dock-and-undock in any bay
private: double dockBonusPoints = 10.0;
/// \brief Points granted when the vehicle successfully
/// dock-and-undock in the specified bay.
private: double correctDockBonusPoints = 10.0;
/// \brief Name of colorTopic for the light buoy
private: std::string colorTopic;
/// \brief Expected color sequence.
private: std::vector<std::string> expectedSequence;
};
#endif

327
sitl_config/usv/vrx_gazebo/include/vrx_gazebo/scoring_plugin.hh Normal file
View File

@ -0,0 +1,327 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef VRX_GAZEBO_SCORING_PLUGIN_HH_
#define VRX_GAZEBO_SCORING_PLUGIN_HH_
#include <ros/ros.h>
#include <gazebo/msgs/gz_string.pb.h>
#include <memory>
#include <string>
#include <vector>
#include <gazebo/common/Events.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/common/Time.hh>
#include <gazebo/physics/World.hh>
#include <sdf/sdf.hh>
#include <gazebo/transport/transport.hh>
#include "vrx_gazebo/Task.h"
#include "vrx_gazebo/Contact.h"
/// \brief A plugin that provides common functionality to any scoring plugin.
/// This plugin defines four different task states:
///
/// * Initial: The vehicle might be locked to the world via some joints with
/// different constraints. This state is used to stabilize the vehicle and let
/// it settle for a while.
/// * Ready: If the vehicle was locked, it will be released in this state. The
/// task hasn't started yet, but the vehicle can be controlled and prepared for
/// the start of the task.
/// * Running: The task has started. The clockwatch task starts too.
/// * Finished: The maximum allowed task time has been reached (time out) or
/// the task has been completed. Other plugins derived from the ScoringPlugin
/// class can call the Finish() method to trigger the completion of the task.
///
/// The plugin also exposes a pair of methods [Set]Score() for setting and
/// getting a score.
///
/// Derived plugins can also override the OnReady(), OnRunning(),
/// and OnFinished() to be notified when the task transitions into the "ready".
/// "running" or "finished" states respectively.
///
/// The plugin publishes task information on a ROS topic at 1Hz.
///
/// This plugin uses the following SDF parameters:
///
/// <vehicle>: Required parameter (string type) with the name of the main
/// vehicle to be under control during the task.
///
/// <task_name>: Required parameter specifying the task name (string type).
///
/// <task_info_topic>: Optional parameter (string type)
/// containing the ROS topic name to publish the task stats. The default
/// topic name is /vrx/task/info .
///
/// <contact_debug_topic>: Optional parameter (string type)
/// containing the ROS topic name to
/// publish every instant a collision with the wamv is happening.
/// Default is /vrx/debug/contact.
///
/// <per_plugin_exit_on_completion>: Specifies whether to shut down after
/// completion, for this specific plugin.
/// Different from environment variable VRX_EXIT_ON_COMPLETION, which is for
/// every plugin in the current shell. Environment variable overwrites this
/// parameter.
///
/// <initial_state_duration>: Optional parameter (double type) specifying the
/// amount of seconds that the plugin will be in the "initial" state.
///
/// <ready_state_duration>: Optional parameter (double type) specifying the
/// amount of seconds that the plugin will be in the "ready" state.
///
/// <running_state_duration>: Optional parameter (double type) specifying the
/// amount of maximum seconds that the plugin will be in the "running" state.
/// Note that this parameter specifies the maximum task time.
///
/// <collision_buffer>: Optional parameter (double type) specifying the
/// minimum amount of seconds between two collisions. If N collisions happen
/// within this time frame, only one will be counted.
///
/// <release_joints>: Optional element specifying the collection of joints that
/// should be dettached when transitioning to the "ready" state.
///
/// This block should contain at least one of these blocks:
/// <joint>: This block should contain a <name> element with the name of the
/// joint to release.
///
/// Here's an example:
/// <plugin name="scoring_plugin"
/// filename="libscoring_plugin.so">
/// <vehicle>wamv</vehicle>
/// <task_name>navigation_course</task_name>
/// <initial_state_duration>10</initial_state_duration>
/// <ready_state_duration>10</ready_state_duration>
/// <running_state_duration>30</running_state_duration>
/// <release_joints>
/// <joint>
/// <name>wamv_external_pivot_joint</name>
/// </joint>
/// <joint>
/// <name>wamv_external_riser</name>
/// </joint>
/// </release_joints>
/// </plugin>
class ScoringPlugin : public gazebo::WorldPlugin
{
/// \brief Class constructor.
public: ScoringPlugin();
/// \brief Shutdown Gazebo and ROS.
public: void Exit();
// Documentation inherited.
protected: void Load(gazebo::physics::WorldPtr _world,
sdf::ElementPtr _sdf);
/// \brief Get the current score.
/// \return The current score.
protected: double Score() const;
/// \brief Set the score.
/// \param[in] _newScore The new score.
protected: void SetScore(double _newScore);
/// \brief Get the task name.
/// \return Task name.
protected: std::string TaskName() const;
/// \brief Get the task state.
/// \return Task state.
protected: std::string TaskState() const;
/// \brief Elapsed time in the running state.
/// \return The elapsed time in the running state.
protected: gazebo::common::Time ElapsedTime() const;
/// \brief Remaining time in the running state.
/// \return The remaining time in the running state.
protected: gazebo::common::Time RemainingTime() const;
/// \brief Finish the current task.
/// This will set the "finished" flag in the task message to true.
protected: void Finish();
/// \brief Tries to release the vehicle in case is locked.
protected: virtual void ReleaseVehicle();
/// \brief Set the score in case of timeout
protected: void SetTimeoutScore(double _timeoutScore);
/// \brief Get the timeoutScore
protected: double GetTimeoutScore();
/// \brief Get running duration
protected: double GetRunningStateDuration();
/// \brief Callback executed at every world update.
private: void Update();
/// \brief Update all time-related variables.
private: void UpdateTime();
/// \brief Update the state of the current task.
private: void UpdateTaskState();
/// \brief Update the task stats message.
private: void UpdateTaskMessage();
/// \brief Publish the task stats over a ROS topic.
private: void PublishStats();
/// \brief Callback executed when the task state transition into "ready".
private: virtual void OnReady();
/// \brief Callback executed when the task state transition into "running".
private: virtual void OnRunning();
/// \brief Callback executed when the task state transition into "finished".
private: virtual void OnFinished();
/// \brief Callback executed when a collision is detected for the WAMV.
private: virtual void OnCollision();
/// \brief Callback function when collision occurs in the world.
/// \param[in] _contacts List of all collisions from last simulation iteration
private: void OnCollisionMsg(ConstContactsPtr &_contacts);
/// \brief Parse all SDF parameters.
/// \return True when all parameters were successfully parsed or false
/// otherwise.
private: bool ParseSDFParameters();
/// \brief Parse the joints section of the SDF block.
/// \return True when all parameters were successfully parsed or false
/// otherwise.
private: bool ParseJoints();
/// \brief A world pointer.
protected: gazebo::physics::WorldPtr world;
/// \brief The name of the task.
protected: std::string taskName = "undefined";
/// \brief The name of the vehicle to score.
protected: std::string vehicleName;
/// \brief Pointer to the vehicle to score.
protected: gazebo::physics::ModelPtr vehicleModel;
/// \brief Last collision time.
protected: gazebo::common::Time lastCollisionTime;
/// \brief gazebo node pointer
private: gazebo::transport::NodePtr gzNode;
/// \brief Collision detection node subscriber
private: gazebo::transport::SubscriberPtr collisionSub;
/// \brief gazebo server control publisher
private: gazebo::transport::PublisherPtr serverControlPub;
/// \brief Pointer to the update event connection.
private: gazebo::event::ConnectionPtr updateConnection;
/// \brief Topic where the task stats are published.
private: std::string taskInfoTopic = "/vrx/task/info";
/// \brief Bool flag for debug.
private: bool debug = true;
/// \brief Topic where debug collision is published.
private: std::string contactDebugTopic = "/vrx/debug/contact";
/// \brief The score.
private: double score = 0.0;
/// \brief Pointer to the SDF plugin element.
private: sdf::ElementPtr sdf;
/// \brief Duration (seconds) of the initial state.
private: double initialStateDuration = 30.0;
/// \brief Duration (seconds) of the ready state.
private: double readyStateDuration = 60.0;
/// \brief Duration (seconds) of the running state (max task time).
protected: double runningStateDuration = 300.0;
/// \brief Absolute time specifying the start of the ready state.
private: gazebo::common::Time readyTime;
/// \brief Absolute time specifying the start of the running state.
private: gazebo::common::Time runningTime;
/// \brief Absolute time specifying the start of the finish state.
private: gazebo::common::Time finishTime;
/// \brief Current time (simulation).
private: gazebo::common::Time currentTime;
// \brief Elapsed time since the start of the task (running state).
private: gazebo::common::Time elapsedTime;
/// \brief Remaining time since the start of the task (running state).
private: gazebo::common::Time remainingTime;
/// \brief Collision buffer.
private: float CollisionBuffer = 3.0;
/// \brief Collisions counter.
private: int collisionCounter = 0;
/// \brief Collision list.
private: std::vector<std::string> collisionList;
/// \brief Collisions timestamps.
private: std::vector<gazebo::common::Time> collisionTimestamps;
/// \brief Whether the current task has timed out or not.
private: bool timedOut = false;
/// \brief Time at which the last message was sent.
private: gazebo::common::Time lastStatsSent = gazebo::common::Time::Zero;
/// \brief The task state.
private: std::string taskState = "initial";
/// \brief The next task message to be published.
protected: vrx_gazebo::Task taskMsg;
/// \brief ROS Contact Msg.
private: vrx_gazebo::Contact contactMsg;
/// \brief The name of the joints to be dettached during ReleaseVehicle().
private: std::vector<std::string> lockJointNames;
/// \brief ROS node handle.
private: std::unique_ptr<ros::NodeHandle> rosNode;
/// \brief Publisher for the task state.
protected: ros::Publisher taskPub;
/// \brief Publisher for the collision.
private: ros::Publisher contactPub;
/// \brief Score in case of timeout - added for Navigation task
private: double timeoutScore = -1.0;
/// \brief Whether to shut down after last gate is crossed.
private: bool perPluginExitOnCompletion = true;
};
#endif

133
sitl_config/usv/vrx_gazebo/include/vrx_gazebo/stationkeeping_scoring_plugin.hh Normal file
View File

@ -0,0 +1,133 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef VRX_GAZEBO_STATIONKEEPING_SCORING_PLUGIN_HH_
#define VRX_GAZEBO_STATIONKEEPING_SCORING_PLUGIN_HH_
#include <geographic_msgs/GeoPoseStamped.h>
#include <ros/ros.h>
#include <memory>
#include <string>
#include <gazebo/common/Events.hh>
#include <gazebo/common/Timer.hh>
#include <gazebo/physics/World.hh>
#include <sdf/sdf.hh>
#include "vrx_gazebo/scoring_plugin.hh"
#include "vrx_gazebo/waypoint_markers.hh"
/// \brief A plugin for computing the score of the station keeping task.
/// This plugin derives from the generic ScoringPlugin class. Refer to that
/// plugin for an explanation of the four states defined (Initial, Ready,
/// Running and Finished) as well as other required SDF elements.
///
/// This plugin publishes a goal pose to a topic when it enters the Ready
/// state.
///
/// In the running state it calculates a 2D pose error distance between the
/// vehicle and the goal as well as a running RMS error of all 2D pose errors
/// calculated so far. The current 2D pose error is published to a topic for
/// pose error, and the RMS error is published to a task score topic. RMS error
/// is also set as the score using the SetScore() method inherited from the
/// parent. This causes it to also appear in the task information topic.
///
/// This plugin requires the following SDF parameters:
///
/// <goal_pose>: Optional parameter (vector type) specifying the latitude,
/// longitude and yaw of the task goal. If not provided, all values default
/// to 0.
/// <markers>: Optional parameter to enable visualization markers. Check the
/// WaypointMarkers class for SDF documentation.
class StationkeepingScoringPlugin : public ScoringPlugin
{
/// \brief Constructor.
public: StationkeepingScoringPlugin();
// Documentation inherited.
public: void Load(gazebo::physics::WorldPtr _world,
sdf::ElementPtr _sdf);
/// \brief Callback executed at every world update.
private: void Update();
// Documentation inherited.
private: void OnReady() override;
// Documentation inherited.
private: void OnRunning() override;
/// \brief Publish the goal pose.
private: void PublishGoal();
/// \brief Pointer to the update event connection.
private: gazebo::event::ConnectionPtr updateConnection;
/// \brief Topic where the task stats are published.
private: std::string goalTopic = "/vrx/station_keeping/goal";
/// \brief Topic where 2D pose error is published
private: std::string poseErrorTopic = "/vrx/station_keeping/pose_error";
/// \brief Topic where mean pose error is published.
private: std::string meanErrorTopic = "/vrx/station_keeping/rms_error";
/// \brief ROS node handle.
private: std::unique_ptr<ros::NodeHandle> rosNode;
/// \brief Publisher for the goal.
private: ros::Publisher goalPub;
/// \brief Publisher for the combined 2D pose error.
private: ros::Publisher poseErrorPub;
/// \brief Publisher for the current mean error.
private: ros::Publisher meanErrorPub;
/// \brief Goal pose in local (Gazebo) coordinates.
private: double goalX;
/// \brief Goal pose in local (Gazebo) coordinates.
private: double goalY;
/// \brief Goal pose in local (Gazebo) coordinates.
private: double goalYaw;
/// \brief Goal pose in spherical (WGS84) coordinates.
private: double goalLat;
/// \brief Goal pose in spherical (WGS84) coordinates.
private: double goalLon;
/// \brief Combined 2D pose error (distance and yaw).
private: double poseError;
/// \brief Number of instant pose error scores calculated so far .
private: unsigned int sampleCount = 0;
/// \brief Sum of all pose error scores calculated so far.
private: double totalPoseError = 0;
/// \brief Cumulative 2D RMS error in meters.
private: double meanError;
/// \brief Timer used to calculate the elapsed time docked in the bay.
private: gazebo::common::Timer timer;
/// \brief Waypoint visualization markers
private: WaypointMarkers waypointMarkers;
};
#endif

130
sitl_config/usv/vrx_gazebo/include/vrx_gazebo/wayfinding_scoring_plugin.hh Normal file
View File

@ -0,0 +1,130 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef VRX_GAZEBO_WAYFINDING_SCORING_PLUGIN_HH_
#define VRX_GAZEBO_WAYFINDING_SCORING_PLUGIN_HH_
#include <ros/ros.h>
#include <memory>
#include <string>
#include <vector>
#include <gazebo/common/Events.hh>
#include <gazebo/common/Timer.hh>
#include <gazebo/physics/World.hh>
#include <sdf/sdf.hh>
#include "vrx_gazebo/scoring_plugin.hh"
#include "vrx_gazebo/waypoint_markers.hh"
/// \brief A plugin for computing the score of the wayfinding navigation task.
/// This plugin derives from the generic ScoringPlugin class. Refer to that
/// plugin for an explanation of the four states defined (Initial, Ready,
/// Running and Finished) as well as other required SDF elements.
///
/// This plugin publishes a series of poses to a topic when it enters the Ready
/// states.
///
/// In the running state it calculates a 2D pose error distance between the
/// vehicle and each of the waypoints and stores the minimum error distance
/// achieved for each waypoint so far. The current recorded minimums are
/// published to a topic as an array of doubles. The average of all minimums is
/// also updated. This value is published to a separate topic for mean error,
/// and set as the current score using the SetScore() method inherited from
/// the parent. This causes it to also appear in the task information topic.
///
/// This plugin requires the following SDF parameters:
///
/// <waypoints>: Required element specifying the set of waypoints that the
/// the vehicle should navigate through.
///
/// This block should contain at least one of these blocks:
/// <waypoint>: This block should contain a <pose> element specifying the
/// lattitude, longitude and yaw of a waypoint.
/// <markers>: Optional parameter to enable visualization markers. Check the
/// WaypointMarkers class for SDF documentation.
class WayfindingScoringPlugin : public ScoringPlugin
{
/// \brief Constructor.
public: WayfindingScoringPlugin();
// Documentation inherited.
public: void Load(gazebo::physics::WorldPtr _world,
sdf::ElementPtr _sdf);
/// \brief Callback executed at every world update.
private: void Update();
// Documentation inherited.
private: void OnReady() override;
// Documentation inherited.
private: void OnRunning() override;
/// \brief Publish the waypoints through which the vehicle must navigate.
private: void PublishWaypoints();
/// \brief Pointer to the update event connection.
private: gazebo::event::ConnectionPtr updateConnection;
/// \brief Pointer to the sdf plugin element.
private: sdf::ElementPtr sdf;
/// \brief Topic where the list of waypoints is published.
private: std::string waypointsTopic = "/vrx/wayfinding/waypoints";
/// \brief Topic where the current minimum pose error distance for each
/// waypoint is published.
private: std::string minErrorsTopic = "/vrx/wayfinding/min_errors";
/// \brief Topic where the current average minimum error is published.
private: std::string meanErrorTopic = "/vrx/wayfinding/mean_error";
/// \brief ROS node handle.
private: std::unique_ptr<ros::NodeHandle> rosNode;
/// \brief Publisher for the goal.
private: ros::Publisher waypointsPub;
/// \brief Publisher for the combined 2D pose error.
private: ros::Publisher minErrorsPub;
/// \brief Publisher for the current rms error.
private: ros::Publisher meanErrorPub;
/// \brief Vector containing waypoints as 3D vectors of doubles representing
/// X Y yaw, where X and Y are local (Gazebo) coordinates.
private: std::vector<ignition::math::Vector3d> localWaypoints;
/// \brief Vector containing waypoints as 3D vectors of doubles representing
/// Lattitude Longitude yaw, where lattitude and longitude are given in
/// spherical (WGS84) coordinates.
private: std::vector<ignition::math::Vector3d> sphericalWaypoints;
/// \brief Vector containing current minimum 2D pose error achieved for each
/// waypoint so far.
private: std::vector<double> minErrors;
/// \brief Current average minimum error for all waypoints.
private: double meanError;
/// \brief Timer used to calculate the elapsed time docked in the bay.
private: gazebo::common::Timer timer;
/// \brief Waypoint visualization markers.
private: WaypointMarkers waypointMarkers;
};
#endif

86
sitl_config/usv/vrx_gazebo/include/vrx_gazebo/waypoint_markers.hh Normal file
View File

@ -0,0 +1,86 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#pragma once
#include <string>
#include <sdf/sdf.hh>
#include <gazebo/gazebo.hh>
#include <ignition/math/Vector3.hh>
#if GAZEBO_MAJOR_VERSION >= 8
#include <ignition/msgs.hh>
#include <ignition/transport.hh>
#endif
/// \brief This class is used to display waypoint markers.
/// Cylindrical Gazebo markers are drawn with text on top
///
/// The marker properties can be set using sdf:
/// material: Optional parameter (string type) to specify the material
/// for marker. Default: Gazebo/Green
/// scaling: Optional parameter (vector type) to specify marker scaling.
/// Default: 0.2 0.2 1.5
/// height: Optional parameter (double type) height of marker above water
/// E.g.
/// <markers>
/// <material>Gazebo/Green</material>
/// <scaling>0.2 0.2 2.0</scaling>
/// <height>0.5</height>
/// </markers>
///
class WaypointMarkers
{
/// \brief Constructor
/// \param[in] _namespace Marker namespace
public: explicit WaypointMarkers(std::string _namespace);
/// \brief Load marker parameters from SDF
/// \param[in] _sdf SDF element pointer with marker parameters
public: void Load(sdf::ElementPtr _sdf);
/// \brief Returns if markers are available for current system
/// \return Returns true if Gazebo version >= 8
public: bool IsAvailable();
/// \brief Draw waypoint marker in Gazebo
/// \param[in] _marker_id Unique marker id for waypoint
/// \param[in] _x X coordinate of waypoint marker
/// \param[in] _y Y coordinate of waypoint marker
/// \param[in] _yaw orientation of waypoint marker in radians
/// \param[in] _text (optional) Text above waypoint marker
/// \return Returns true if marker is successfully sent to Gazebo
public: bool DrawMarker(int _marker_id, double _x, double _y,
double _yaw, std::string _text = "");
/// \brief Namespace for Gazebo markers
private: std::string ns;
/// \brief Name of Gazebo material for marker
private: std::string material;
/// \brief Scaling factor for cylinder marker
private: ignition::math::Vector3d scaling;
/// \brief Height of marker above water
private: double height;
#if GAZEBO_MAJOR_VERSION >= 8
/// \brief gazebo transport node
ignition::transport::Node node;
#endif
};

59
sitl_config/usv/vrx_gazebo/launch/dock.launch Normal file
View File

@ -0,0 +1,59 @@
<?xml version="1.0"?>
<launch>
<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/dock.world" />
<!-- If true, run gazebo GUI -->
<arg name="gui" default="true" />
<!-- If true, run gazebo in verbose mode -->
<arg name="verbose" default="false"/>
<!-- Set various other gazebo arguments-->
<arg name="extra_gazebo_args" default=""/>
<!-- Start in a default namespace -->
<arg name="namespace" default="wamv"/>
<!-- Initial USV location and attitude-->
<arg name="x" default="158" />
<arg name="y" default="108" />
<arg name="z" default="0.1" />
<arg name="P" default="0" />
<arg name="R" default="0" />
<arg name="Y" default="-2.76" />
<!-- Allow user specified thruster configurations
H = stern trusters on each hull
T = H with a lateral thruster
X = "holonomic" configuration -->
<arg name="thrust_config" default="H" />
<!-- Do we lock the vessel to the world? -->
<arg name="wamv_locked" default="true" />
<!-- VRX sensors enabled -->
<arg name="vrx_sensors_enabled" default="true" />
<!-- 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="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="$(arg gui)" />
<arg name="extra_gazebo_args" value="$(arg extra_gazebo_args)"/>
</include>
<!-- Load robot model -->
<arg name="urdf" default="$(find wamv_gazebo)/urdf/wamv_gazebo.urdf.xacro"/>
<param name="$(arg namespace)/robot_description"
command="$(find xacro)/xacro &#x002D;&#x002D;inorder '$(arg urdf)'
locked:=$(arg wamv_locked)
thruster_config:=$(arg thrust_config)
vrx_sensors_enabled:=$(arg vrx_sensors_enabled)
namespace:=$(arg namespace) "/>
<!-- Spawn model in Gazebo -->
<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 wamv "/>
</launch>

27
sitl_config/usv/vrx_gazebo/launch/generate_wamv.launch Normal file
View File

@ -0,0 +1,27 @@
<?xml version="1.0"?>
<!-- Generate wamv given custom configurations. Used by vrx-docker for
competition evaluation -->
<launch>
<arg name="namespace" value="wamv_config"/>
<group ns="$(arg namespace)">
<arg name="sensor_yaml" default="$(find vrx_gazebo)/src/vrx_gazebo_python/generator_scripts/wamv_config/example_sensor_config.yaml"/>
<param name="sensor_yaml" value="$(arg sensor_yaml)"/>
<arg name="thruster_yaml" default="$(find vrx_gazebo)/src/vrx_gazebo_python/generator_scripts/wamv_config/example_thruster_config.yaml"/>
<param name="thruster_yaml" value="$(arg thruster_yaml)"/>
<arg name="wamv_target"/>
<param name="wamv_target" value="$(arg wamv_target)"/>
<param name="wamv_gazebo" value="$(find wamv_gazebo)/urdf/wamv_gazebo.urdf.xacro"/>
<param name="sensors_dir" value="$(find wamv_gazebo)/urdf/sensors"/>
<param name="thrusters_dir" value="$(find wamv_description)/urdf/thrusters"/>
<node name="wamv_generator" pkg="vrx_gazebo" type="generate_wamv" output="screen" required="true"/>
</group>
</launch>

29
sitl_config/usv/vrx_gazebo/launch/generate_worlds.launch Normal file
View File

@ -0,0 +1,29 @@
<?xml version="1.0"?>
<!-- Generates competition worlds. Used by vrx-docker for competition
evaluation -->
<launch>
<!-- Task configuration YAML file -->
<arg name="requested"/>
<param name="requested" value="$(arg requested)"/>
<arg name="world_xacro_target"/>
<param name="world_xacro_target" value="$(arg world_xacro_target)"/>
<arg name="world_target"/>
<param name="world_target" value="$(arg world_target)"/>
<!-- Only used for gymkhana task for pinger position configuration YAML -->
<arg name="config_target" default=""/>
<param name="config_target" value="$(arg config_target)"/>
<arg name="competition_pkg" default="vrx_gazebo"/>
<param name="competition_pkg" value="$(arg competition_pkg)"/>
<arg name="world_name" default="robotx_example_course"/>
<param name="world_name" value="$(arg world_name)"/>
<node name="world_gen" pkg="vrx_gazebo" type="generate_worlds" required="true"
output="screen"/>
</launch>

59
sitl_config/usv/vrx_gazebo/launch/navigation_task.launch Normal file
View File

@ -0,0 +1,59 @@
<?xml version="1.0"?>
<launch>
<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/navigation_task.world" />
<!-- If true, run gazebo GUI -->
<arg name="gui" default="true" />
<!-- If true, run gazebo in verbose mode -->
<arg name="verbose" default="false"/>
<!-- Set various other gazebo arguments-->
<arg name="extra_gazebo_args" default=""/>
<!-- Start in a default namespace -->
<arg name="namespace" default="wamv"/>
<!-- Initial USV location and attitude-->
<arg name="x" default="158" />
<arg name="y" default="108" />
<arg name="z" default="0.1" />
<arg name="P" default="0" />
<arg name="R" default="0" />
<arg name="Y" default="-2.76" />
<!-- Allow user specified thruster configurations
H = stern trusters on each hull
T = H with a lateral thruster
X = "holonomic" configuration -->
<arg name="thrust_config" default="H" />
<!-- Do we lock the vessel to the world? -->
<arg name="wamv_locked" default="true" />
<!-- VRX sensors enabled -->
<arg name="vrx_sensors_enabled" default="true" />
<!-- 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="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="$(arg gui)" />
<arg name="extra_gazebo_args" value="$(arg extra_gazebo_args)"/>
</include>
<!-- Load robot model -->
<arg name="urdf" default="$(find wamv_gazebo)/urdf/wamv_gazebo.urdf.xacro"/>
<param name="$(arg namespace)/robot_description"
command="$(find xacro)/xacro &#x002D;&#x002D;inorder '$(arg urdf)'
locked:=$(arg wamv_locked)
thruster_config:=$(arg thrust_config)
vrx_sensors_enabled:=$(arg vrx_sensors_enabled)
namespace:=$(arg namespace) "/>
<!-- Spawn model in Gazebo -->
<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 wamv"/>
</launch>

61
sitl_config/usv/vrx_gazebo/launch/perception_task.launch Normal file
View File

@ -0,0 +1,61 @@
<?xml version="1.0"?>
<launch>
<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/perception_task.world" />
<!-- If true, run gazebo GUI -->
<arg name="gui" default="true" />
<!-- If true, run gazebo in verbose mode -->
<arg name="verbose" default="false"/>
<!-- Set various other gazebo arguments-->
<arg name="extra_gazebo_args" default=""/>
<!-- Start in a default namespace -->
<arg name="namespace" default="wamv"/>
<arg name="paused" default="false"/>
<!-- Initial USV location and attitude-->
<arg name="x" default="158" />
<arg name="y" default="108" />
<arg name="z" default="0.1" />
<arg name="P" default="0" />
<arg name="R" default="0" />
<arg name="Y" default="-2.76" />
<!-- Allow user specified thruster configurations
H = stern trusters on each hull
T = H with a lateral thruster
X = "holonomic" configuration -->
<arg name="thrust_config" default="H" />
<!-- Do we lock the vessel to the world? -->
<arg name="wamv_locked" default="true" />
<!-- VRX sensors enabled -->
<arg name="vrx_sensors_enabled" default="true" />
<!-- 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="extra_gazebo_args" value="$(arg extra_gazebo_args)"/>
</include>
<!-- Load robot model -->
<arg name="urdf" default="$(find wamv_gazebo)/urdf/wamv_gazebo.urdf.xacro"/>
<param name="$(arg namespace)/robot_description"
command="$(find xacro)/xacro &#x002D;&#x002D;inorder '$(arg urdf)'
locked:=$(arg wamv_locked)
thruster_config:=$(arg thrust_config)
vrx_sensors_enabled:=$(arg vrx_sensors_enabled)
namespace:=$(arg namespace) "/>
<!-- Spawn model in Gazebo -->
<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 wamv "/>
</launch>

7
sitl_config/usv/vrx_gazebo/launch/pinger.launch Normal file
View File

@ -0,0 +1,7 @@
<?xml version="1.0"?>
<launch>
<node name="set_pinger_position" pkg="vrx_gazebo" type="set_pinger_position.py" output="screen">
<rosparam command="load" file="$(find vrx_gazebo)/config/pinger.yaml" />
</node>
<node name="pinger_visualisation" pkg="vrx_gazebo" type="pinger_visualisation.py" />
</launch>

22
sitl_config/usv/vrx_gazebo/launch/playback.launch Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0"?>
<launch>
<!-- Extra gazebo arguments -->
<arg name="log_file"/>
<!-- If true, start in paused state -->
<arg name="paused" default="false"/>
<!-- If true, run gazebo in verbose mode -->
<arg name="verbose" default="false"/>
<!-- Needed to find the WAM-V model during playback -->
<env name="GAZEBO_MODEL_PATH" value="$(find wamv_description)/models:$(find wamv_gazebo)/models:$(optenv GAZEBO_MODEL_PATH)"/>
<!-- Start Gazebo with the recorded log file -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="extra_gazebo_args" value="-p $(arg log_file)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="verbose" value="$(arg verbose)"/>
</include>
</launch>

11
sitl_config/usv/vrx_gazebo/launch/quat2rpy.launch Normal file
View File

@ -0,0 +1,11 @@
<?xml version="1.0"?>
<launch>
<node pkg="vrx_gazebo" type="quat2rpy.py" name="quat2rpy" output="screen">
<remap from="in_topic" to="/gazebo/model_states" />
<remap from="out_topic" to="wamv_rpy" />
<param name="input_msg_type" value="ModelStates" />
<param name="modelstates_index" value="6" type="int" />
</node>
</launch>

78
sitl_config/usv/vrx_gazebo/launch/sandisland.launch Normal file
View File

@ -0,0 +1,78 @@
<?xml version="1.0"?>
<launch>
<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/example_course.world" />
<!-- If true, run gazebo GUI -->
<arg name="gui" default="true" />
<!-- If true, run gazebo in verbose mode -->
<arg name="verbose" default="false"/>
<!-- 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="wamv"/>
<!-- Initial USV location and attitude-->
<arg name="x" default="158" />
<arg name="y" default="108" />
<arg name="z" default="0.1" />
<arg name="P" default="0" />
<arg name="R" default="0" />
<arg name="Y" default="-2.76" />
<!-- If true, show non-competition ROS topics (/gazebo/model_states, /vrx/debug/wind/direction, etc.)-->
<arg name="non_competition_mode" default="true"/>
<arg name="enable_ros_network" value="$(arg non_competition_mode)"/>
<env name="VRX_DEBUG" value="$(arg non_competition_mode)"/>
<env unless="$(arg non_competition_mode)" name="GAZEBO_MODEL_PATH" value="$(find vrx_gazebo)/models:$(find wamv_gazebo)/models:$(find wamv_description)/models:$(optenv GAZEBO_MODEL_PATH)"/>
<!-- Allow user specified thruster configurations
H = stern trusters on each hull
T = H with a lateral thruster
X = "holonomic" configuration -->
<arg name="thrust_config" default="H" />
<!-- Do you want to enable sensors? -->
<arg name="camera_enabled" default="false" />
<arg name="gps_enabled" default="false" />
<arg name="imu_enabled" default="false" />
<arg name="lidar_enabled" default="false" />
<arg name="ground_truth_enabled" default="false" />
<!-- 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="$(arg enable_ros_network)"/>
<arg name="extra_gazebo_args" value="$(arg extra_gazebo_args)"/>
</include>
<!-- Load robot model -->
<arg name="urdf" default="$(find wamv_gazebo)/urdf/wamv_gazebo.urdf.xacro"/>
<param name="$(arg namespace)/robot_description"
command="$(find xacro)/xacro &#x002D;&#x002D;inorder '$(arg urdf)'
thruster_config:=$(arg thrust_config)
camera_enabled:=$(arg camera_enabled)
gps_enabled:=$(arg gps_enabled)
imu_enabled:=$(arg imu_enabled)
lidar_enabled:=$(arg lidar_enabled)
ground_truth_enabled:=$(arg ground_truth_enabled)
namespace:=$(arg namespace) "/>
<!-- Spawn model in Gazebo, script depending on non_competition_mode -->
<node name="spawn_model" pkg="gazebo_ros" type="spawn_model" if="$(arg non_competition_mode)"
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 wamv"/>
<node name="spawn_wamv" pkg="vrx_gazebo" type="spawn_wamv.bash" unless="$(arg non_competition_mode)"
args="-x $(arg x) -y $(arg y) -z $(arg z)
-R $(arg R) -P $(arg P) -Y $(arg Y)
--urdf $(arg urdf) --model wamv"/>
</launch>

59
sitl_config/usv/vrx_gazebo/launch/scan_and_dock.launch Normal file
View File

@ -0,0 +1,59 @@
<?xml version="1.0"?>
<launch>
<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/scan_and_dock.world" />
<!-- If true, run gazebo GUI -->
<arg name="gui" default="true" />
<!-- If true, run gazebo in verbose mode -->
<arg name="verbose" default="false"/>
<!-- Set various other gazebo arguments-->
<arg name="extra_gazebo_args" default=""/>
<!-- Start in a default namespace -->
<arg name="namespace" default="wamv"/>
<!-- Initial USV location and attitude-->
<arg name="x" default="158" />
<arg name="y" default="108" />
<arg name="z" default="0.1" />
<arg name="P" default="0" />
<arg name="R" default="0" />
<arg name="Y" default="-2.76" />
<!-- Allow user specified thruster configurations
H = stern trusters on each hull
T = H with a lateral thruster
X = "holonomic" configuration -->
<arg name="thrust_config" default="H" />
<!-- Do we lock the vessel to the world? -->
<arg name="wamv_locked" default="true" />
<!-- VRX sensors enabled -->
<arg name="vrx_sensors_enabled" default="true" />
<!-- 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="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="$(arg gui)" />
<arg name="extra_gazebo_args" value="$(arg extra_gazebo_args)"/>
</include>
<!-- Load robot model -->
<arg name="urdf" default="$(find wamv_gazebo)/urdf/wamv_gazebo.urdf.xacro"/>
<param name="$(arg namespace)/robot_description"
command="$(find xacro)/xacro &#x002D;&#x002D;inorder '$(arg urdf)'
locked:=$(arg wamv_locked)
thruster_config:=$(arg thrust_config)
vrx_sensors_enabled:=$(arg vrx_sensors_enabled)
namespace:=$(arg namespace) "/>
<!-- Spawn model in Gazebo -->
<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 wamv "/>
</launch>

59
sitl_config/usv/vrx_gazebo/launch/station_keeping.launch Normal file
View File

@ -0,0 +1,59 @@
<?xml version="1.0"?>
<launch>
<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/stationkeeping_task.world" />
<!-- If true, run gazebo GUI -->
<arg name="gui" default="true" />
<!-- If true, run gazebo in verbose mode -->
<arg name="verbose" default="false"/>
<!-- Set various other gazebo arguments-->
<arg name="extra_gazebo_args" default=""/>
<!-- Start in a default namespace -->
<arg name="namespace" default="wamv"/>
<!-- Initial USV location and attitude-->
<arg name="x" default="158" />
<arg name="y" default="108" />
<arg name="z" default="0.1" />
<arg name="P" default="0" />
<arg name="R" default="0" />
<arg name="Y" default="-2.76" />
<!-- Allow user specified thruster configurations
H = stern trusters on each hull
T = H with a lateral thruster
X = "holonomic" configuration -->
<arg name="thrust_config" default="T" />
<!-- Do we lock the vessel to the world? -->
<arg name="wamv_locked" default="true" />
<!-- Do we load the VRX sensor suite? -->
<arg name="vrx_sensors_enabled" default="true" />
<!-- 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="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="$(arg gui)" />
<arg name="extra_gazebo_args" value="$(arg extra_gazebo_args)"/>
</include>
<!-- Load robot model -->
<arg name="urdf" default="$(find wamv_gazebo)/urdf/wamv_gazebo.urdf.xacro"/>
<param name="$(arg namespace)/robot_description"
command="$(find xacro)/xacro &#x002D;&#x002D;inorder '$(arg urdf)'
locked:=$(arg wamv_locked)
thruster_config:=$(arg thrust_config)
vrx_sensors_enabled:=$(arg vrx_sensors_enabled)
namespace:=$(arg namespace) "/>
<!-- Spawn model in Gazebo -->
<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 wamv "/>
</launch>

Some files were not shown because too many files have changed in this diff Show More