osrf · Jul 12, 2022 · Jul 15, 2022 · Jul 15, 2022 · Jul 25, 2022 · Jul 25, 2022
diff --git a/vrx_ign/CMakeLists.txt b/vrx_ign/CMakeLists.txt
@@ -20,6 +20,8 @@ find_package(ignition-plugin1 REQUIRED COMPONENTS loader register)
 set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
 find_package(ignition-rendering6 REQUIRED)
 set(IGN_RENDERING_VER ${ignition-rendering6_VERSION_MAJOR})
+find_package(ignition-utils1 REQUIRED)
+set(IGN_UTILS_VER ${ignition-utils1_VERSION_MAJOR})
 find_package(sdformat12 REQUIRED)
 
 find_package(std_msgs REQUIRED)
@@ -44,10 +46,43 @@ install(
   TARGETS Waves
   DESTINATION lib)
 
+# Buoyancy
+add_library(SimpleBuoyancy SHARED
+  src/SimpleBuoyancy.cc
+  src/PolyhedronVolume.cc
+  src/ShapeVolume.cc
+)
+target_link_libraries(SimpleBuoyancy PUBLIC
+  ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+  ignition-gazebo${IGN_GAZEBO_VER}::core
+  ignition-math${IGN_MATH_VER}
+  Waves
+)
+install(
+  TARGETS SimpleBuoyancy
+  DESTINATION lib)
+
+
+# Base scoring plugin
+add_library(ScoringPlugin SHARED
+  src/ScoringPlugin.cc
+)
+target_link_libraries(ScoringPlugin PUBLIC
+ignition-gazebo${IGN_GAZEBO_VER}::core
+ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
+ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER}
+ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER}
+Waves
+)
+install(
+  TARGETS ScoringPlugin
+  DESTINATION lib)
+
 # Plugins
 list(APPEND VRX_IGN_PLUGINS
+  WildlifeScoringPlugin
+  StationkeepingScoringPlugin
   SimpleHydrodynamics
-  Surface
   WaveVisual
 )
 
@@ -57,8 +92,10 @@ foreach(PLUGIN ${VRX_IGN_PLUGINS})
     ignition-gazebo${IGN_GAZEBO_VER}::core
     ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
     ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER}
+    ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER}
     Waves
-  )
+    ScoringPlugin
+)
 endforeach()
 
 install(
@@ -78,5 +115,4 @@ install(DIRECTORY
   launch
   DESTINATION share/${PROJECT_NAME})
 
-
 ament_package()
diff --git a/vrx_ign/models/crocodile_buoy/model.config b/vrx_ign/models/crocodile_buoy/model.config
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<model>
+  <name>crocodile_static</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+
+  <author>
+    <name>Carlos Agüero</name>
+    <email>caguero@openrobotics.org</email>
+  </author>
+
+  <author>
+    <name>Rafi Abdullah</name>
+    <email>rafi@openrobotics.org</email>
+  </author>
+
+  <description>
+    A crocodile buoy without motion plugin for the RobotX 2022 competition.
+  </description>
+</model>
diff --git a/vrx_ign/models/crocodile_buoy/model.sdf b/vrx_ign/models/crocodile_buoy/model.sdf
@@ -0,0 +1,55 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="crocodile_buoy">
+    <link name="link">
+      <inertial>
+        <pose>0 0 -2 0 0 0</pose>
+        <!-- Mass determined based on buoyancy geometry so object 
+             floats at mid-point of sphere -->
+        <mass>20</mass>
+        <!-- Mark only - based on cylinder -->
+        <inertia>
+          <ixx>1.0</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>1.0</iyy>
+          <iyz>0</iyz>
+          <izz>0.1</izz>
+        </inertia>
+      </inertial>
+      <collision name="crocodile_collision">
+        <pose>0 0 0.06 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.80 0.40 0.15</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="crocodile_visual">
+        <geometry>
+          <box>
+            <size>0.80 0.40 0.15</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+    <plugin name="CrocodileBuoyancyPlugin" filename="libbuoyancy_gazebo_plugin.so">
+      <wave_model>ocean_waves</wave_model>
+      <fluid_density>1000</fluid_density>
+      <fluid_level>0.0</fluid_level>
+      <linear_drag>25.0</linear_drag>
+      <angular_drag>2.0</angular_drag>
+      <buoyancy name="buoyancy_sphere">
+        <link_name>link</link_name>
+        <pose>0 0 0.2 0 0 0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.325</radius>
+          </sphere>
+        </geometry>
+      </buoyancy>
+    </plugin>
+  </model>
+</sdf>
+
+
diff --git a/vrx_ign/models/wam-v/model.sdf.erb b/vrx_ign/models/wam-v/model.sdf.erb
@@ -28,7 +28,7 @@ end
 
   <!-- Base model-->
   <include merge="true">
-    <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/wam-v</uri>
+    <uri>https://fuel.gazebosim.org/1.0/openrobotics/models/wam-v</uri>
   </include>
 
   <plugin
@@ -70,15 +70,54 @@ end
   </plugin>
 
   <plugin
-    filename="libSurface.so"
-    name="vrx::Surface">
-    <link_name>base_link</link_name>
-    <vehicle_length>4.9</vehicle_length>
-    <vehicle_width>2.4</vehicle_width>
-    <hull_radius>0.213</hull_radius>
-    <fluid_level>0</fluid_level>
-
-    <!-- Waves -->
+    filename="libSimpleBuoyancy.so"
+    name="vrx::SimpleBuoyancy">
+    <fluid_density>1000</fluid_density>
+    <fluid_level>0.0</fluid_level>
+    <linear_drag>20.0</linear_drag>
+    <angular_drag>2.0</angular_drag>
+    <!-- Left Hull -->
+    <buoyancy name="left_hull_mid">
+      <link_name>base_link</link_name>
+      <pose>0.6 1.03 0.2 0 1.57 0</pose>
+      <geometry>
+        <cylinder>
+          <length>2</length>
+          <radius>0.2</radius>
+        </cylinder>
+      </geometry>
+    </buoyancy>
+    <buoyancy name="left_hull_rear">
+      <link_name>base_link</link_name>
+      <pose>-1.4 1.03 0.2 0 1.57 0</pose>
+      <geometry>
+        <cylinder>
+          <length>2</length>
+          <radius>0.2</radius>
+        </cylinder>
+      </geometry>
+    </buoyancy>
+    <!-- Right Hull -->
+    <buoyancy name="right_hull_mid">
+      <link_name>base_link</link_name>
+      <pose>0.6 -1.03 0.2 0 1.57 0</pose>
+      <geometry>
+        <cylinder>
+          <length>2</length>
+          <radius>0.2</radius>
+        </cylinder>
+      </geometry>
+    </buoyancy>
+    <buoyancy name="right_hull_rear">
+      <link_name>base_link</link_name>
+      <pose>-1.4 -1.03 0.2 0 1.57 0</pose>
+      <geometry>
+        <cylinder>
+          <length>2</length>
+          <radius>0.2</radius>
+        </cylinder>
+      </geometry>
+    </buoyancy>
     <wavefield>
       <size><%= $wavefield_size%> <%= $wavefield_size%></size>
       <cell_count><%= $wavefield_cell_count%> <%=$wavefield_cell_count%></cell_count>
@@ -87,7 +126,7 @@ end
         <period>5.0</period>
         <number>3</number>
         <scale>1.1</scale>
-        <gain>0.5</gain>
+        <gain>0.3</gain>
         <direction>1 0</direction>
         <angle>0.4</angle>
         <tau>2.0</tau>