From 78cf6c7390232e44d06e517cc69c5541b4576f1c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= <caguero@openrobotics.org>
Date: Tue, 6 Jun 2023 17:32:05 +0200
Subject: [PATCH 1/2] Enable pinger link and visual
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
---
 vrx_urdf/wamv_gazebo/urdf/components/wamv_pinger.xacro | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/vrx_urdf/wamv_gazebo/urdf/components/wamv_pinger.xacro b/vrx_urdf/wamv_gazebo/urdf/components/wamv_pinger.xacro
index ecd943f61..e01ac1d15 100644
--- a/vrx_urdf/wamv_gazebo/urdf/components/wamv_pinger.xacro
+++ b/vrx_urdf/wamv_gazebo/urdf/components/wamv_pinger.xacro
@@ -4,13 +4,19 @@
     <link name="${namespace}/${name}">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <sphere radius="0.01" />
+        </geometry>
       </visual>
       <inertial>
         <mass value="0.1"/>
         <inertia ixx="0.00000542" ixy="0.0" ixz="0.0" iyy="0.00002104" iyz="0.0" izz="0.00002604"/>
       </inertial>
     </link>
-    <joint name="${namespace}/${name}_pinger_joint" type="fixed">
+    <joint name="${namespace}/${name}_pinger_joint" type="revolute">
+      <axis xyz="0 0 1"/>
+      <limit effort="1000.0" lower="0.0" upper="0" velocity="0"/>
+      <origin xyz="0 0 0" rpy="0 0 0" />
       <parent link="${namespace}/base_link"/>
       <child link="${namespace}/${name}"/>
     </joint>

From 6bfcbbca6c8a7591021cb80db92a7e8e5baac0d9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= <caguero@openrobotics.org>
Date: Wed, 21 Jun 2023 15:25:33 +0200
Subject: [PATCH 2/2] Enable/Disable the ROS 2 ping topics.
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
---
 vrx_gz/src/vrx_gz/bridges.py         | 16 ----------------
 vrx_gz/src/vrx_gz/model.py           |  6 +-----
 vrx_gz/src/vrx_gz/payload_bridges.py | 21 +++++++++++++++++++++
 3 files changed, 22 insertions(+), 21 deletions(-)

diff --git a/vrx_gz/src/vrx_gz/bridges.py b/vrx_gz/src/vrx_gz/bridges.py
index 8a79c0dc2..28bbff234 100644
--- a/vrx_gz/src/vrx_gz/bridges.py
+++ b/vrx_gz/src/vrx_gz/bridges.py
@@ -46,22 +46,6 @@ def cmd_vel(model_name):
         ros_type='geometry_msgs/msg/Twist',
         direction=BridgeDirection.ROS_TO_GZ)
 
-def acoustic_pinger(model_name):
-    return Bridge(
-        gz_topic=f'{model_name}/pingers/pinger/range_bearing',
-        ros_topic=f'pingers/pinger/range_bearing',
-        gz_type='ignition.msgs.Param',
-        ros_type='ros_gz_interfaces/msg/ParamVec',
-        direction=BridgeDirection.GZ_TO_ROS)
-
-def set_acoustic_pinger(model_name):
-    return Bridge(
-        gz_topic=f'{model_name}/pingers/pinger/set_pinger_position',
-        ros_topic=f'pingers/pinger/set_pinger_position',
-        gz_type='ignition.msgs.Vector3d',
-        ros_type='geometry_msgs/msg/Vector3',
-        direction=BridgeDirection.ROS_TO_GZ)
-
 def comms_tx(model_name):
     return Bridge(
         gz_topic='/broker/msgs',
diff --git a/vrx_gz/src/vrx_gz/model.py b/vrx_gz/src/vrx_gz/model.py
index f08112e7f..f89c72477 100644
--- a/vrx_gz/src/vrx_gz/model.py
+++ b/vrx_gz/src/vrx_gz/model.py
@@ -87,11 +87,7 @@ def bridges(self, world_name):
                 vrx_gz.bridges.cmd_vel(self.model_name)
             ])
         elif self.is_USV():
-            bridges.extend([
-                # Acoustic pinger
-                vrx_gz.bridges.acoustic_pinger(self.model_name),
-                vrx_gz.bridges.set_acoustic_pinger(self.model_name),
-            ])
+            pass
 
         return [bridges, nodes, custom_launches]
 
diff --git a/vrx_gz/src/vrx_gz/payload_bridges.py b/vrx_gz/src/vrx_gz/payload_bridges.py
index 93768c4be..1117fa729 100644
--- a/vrx_gz/src/vrx_gz/payload_bridges.py
+++ b/vrx_gz/src/vrx_gz/payload_bridges.py
@@ -156,6 +156,22 @@ def thrust_joint_pos(model_name, side):
         ros_type='std_msgs/msg/Float64',
         direction=BridgeDirection.ROS_TO_GZ)
 
+def acoustic_pinger(model_name):
+    return Bridge(
+        gz_topic=f'{model_name}/pingers/pinger/range_bearing',
+        ros_topic=f'pingers/pinger/range_bearing',
+        gz_type='ignition.msgs.Param',
+        ros_type='ros_gz_interfaces/msg/ParamVec',
+        direction=BridgeDirection.GZ_TO_ROS)
+
+def set_acoustic_pinger(model_name):
+    return Bridge(
+        gz_topic=f'{model_name}/pingers/pinger/set_pinger_position',
+        ros_topic=f'pingers/pinger/set_pinger_position',
+        gz_type='ignition.msgs.Vector3d',
+        ros_type='geometry_msgs/msg/Vector3',
+        direction=BridgeDirection.ROS_TO_GZ)
+
 def payload_bridges(world_name, model_name, link_name, sensor_name, sensor_type):
     bridges = []
     if sensor_type == sdf.Sensortype.CAMERA:
@@ -203,5 +219,10 @@ def payload_bridges(world_name, model_name, link_name, sensor_name, sensor_type)
         bridges = [
             thrust_joint_pos(model_name, sensor_type),
         ]
+    elif 'AcousticPinger' in sensor_name:
+        bridges = [
+            acoustic_pinger(model_name),
+            set_acoustic_pinger(model_name),
+        ]
 
     return bridges