From 8caea63c576626adefb8d3d61de6a6e120ab3425 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= <caguero@openrobotics.org>
Date: Wed, 28 Jun 2023 16:37:13 +0200
Subject: [PATCH 1/2] Rename pinger 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/AcousticTrackingScoringPlugin.cc        |  3 +--
 vrx_gz/src/vrx_gz/payload_bridges.py               | 12 ++++++------
 .../practice_2023_acoustic_perception0_task.sdf    |  2 +-
 .../practice_2023_acoustic_perception1_task.sdf    |  2 +-
 .../practice_2023_acoustic_perception2_task.sdf    |  2 +-
 .../practice_2023_acoustic_tracking0_task.sdf      |  2 +-
 .../practice_2023_acoustic_tracking1_task.sdf      |  2 +-
 .../practice_2023_acoustic_tracking2_task.sdf      |  2 +-
 vrx_gz/worlds/acoustic_perception_task.sdf         |  2 +-
 vrx_gz/worlds/acoustic_tracking_task.sdf           |  2 +-
 vrx_gz/worlds/gymkhana_task.sdf                    |  2 +-
 .../wamv_config/component_compliance/numeric.yaml  |  2 +-
 .../wamv_config/example_component_config.yaml      |  2 +-
 .../wamv_gazebo/urdf/components/wamv_pinger.xacro  | 14 +++++++-------
 vrx_urdf/wamv_gazebo/urdf/wamv_gazebo.urdf.xacro   |  8 ++++----
 15 files changed, 29 insertions(+), 30 deletions(-)

diff --git a/vrx_gz/src/AcousticTrackingScoringPlugin.cc b/vrx_gz/src/AcousticTrackingScoringPlugin.cc
index 32e221bd1..3d149b309 100644
--- a/vrx_gz/src/AcousticTrackingScoringPlugin.cc
+++ b/vrx_gz/src/AcousticTrackingScoringPlugin.cc
@@ -78,8 +78,7 @@ class AcousticTrackingScoringPlugin::Implementation
   public: double obstaclePenalty = 0.1;
 
   /// /brief The topic used to set the pinger position.
-  public: std::string setPingerTopicName =
-    "/wamv/pingers/pinger/set_pinger_position";
+  public: std::string setPingerTopicName = "/pinger/set_pinger_position";
 
   /// \brief Topic where 2D pose error is published.
   public: std::string poseErrorTopic = "/vrx/acoustic_wayfinding/pose_error";
diff --git a/vrx_gz/src/vrx_gz/payload_bridges.py b/vrx_gz/src/vrx_gz/payload_bridges.py
index 1117fa729..c21f5ec81 100644
--- a/vrx_gz/src/vrx_gz/payload_bridges.py
+++ b/vrx_gz/src/vrx_gz/payload_bridges.py
@@ -158,16 +158,16 @@ def thrust_joint_pos(model_name, side):
 
 def acoustic_pinger(model_name):
     return Bridge(
-        gz_topic=f'{model_name}/pingers/pinger/range_bearing',
-        ros_topic=f'pingers/pinger/range_bearing',
+        gz_topic=f'{model_name}/sensors/acoustics/receiver/range_bearing',
+        ros_topic=f'sensors/acoustics/receiver/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):
+def set_acoustic_pinger():
     return Bridge(
-        gz_topic=f'{model_name}/pingers/pinger/set_pinger_position',
-        ros_topic=f'pingers/pinger/set_pinger_position',
+        gz_topic=f'/pinger/set_pinger_position',
+        ros_topic=f'/pinger/set_pinger_position',
         gz_type='ignition.msgs.Vector3d',
         ros_type='geometry_msgs/msg/Vector3',
         direction=BridgeDirection.ROS_TO_GZ)
@@ -222,7 +222,7 @@ def payload_bridges(world_name, model_name, link_name, sensor_name, sensor_type)
     elif 'AcousticPinger' in sensor_name:
         bridges = [
             acoustic_pinger(model_name),
-            set_acoustic_pinger(model_name),
+            set_acoustic_pinger(),
         ]
 
     return bridges
diff --git a/vrx_gz/worlds/2023_practice/practice_2023_acoustic_perception0_task.sdf b/vrx_gz/worlds/2023_practice/practice_2023_acoustic_perception0_task.sdf
index 1c2aabd75..c5fc26929 100644
--- a/vrx_gz/worlds/2023_practice/practice_2023_acoustic_perception0_task.sdf
+++ b/vrx_gz/worlds/2023_practice/practice_2023_acoustic_perception0_task.sdf
@@ -426,7 +426,7 @@
 
     <!-- Publish the pinger pose -->
     <plugin filename="libPublisherPlugin.so" name="vrx::PublisherPlugin">
-      <message type="gz.msgs.Vector3d" topic="/wamv/pingers/pinger/set_pinger_position" at="1.0">
+      <message type="gz.msgs.Vector3d" topic="/pinger/set_pinger_position" at="1.0">
         x: -560,
         y: 185,
         z: -2
diff --git a/vrx_gz/worlds/2023_practice/practice_2023_acoustic_perception1_task.sdf b/vrx_gz/worlds/2023_practice/practice_2023_acoustic_perception1_task.sdf
index 0105bc9f1..f7d9e7914 100644
--- a/vrx_gz/worlds/2023_practice/practice_2023_acoustic_perception1_task.sdf
+++ b/vrx_gz/worlds/2023_practice/practice_2023_acoustic_perception1_task.sdf
@@ -426,7 +426,7 @@
 
     <!-- Publish the pinger pose -->
     <plugin filename="libPublisherPlugin.so" name="vrx::PublisherPlugin">
-      <message type="gz.msgs.Vector3d" topic="/wamv/pingers/pinger/set_pinger_position" at="1.0">
+      <message type="gz.msgs.Vector3d" topic="/pinger/set_pinger_position" at="1.0">
         x: -528,
         y: 225,
         z: -2
diff --git a/vrx_gz/worlds/2023_practice/practice_2023_acoustic_perception2_task.sdf b/vrx_gz/worlds/2023_practice/practice_2023_acoustic_perception2_task.sdf
index e771471f2..ec0d06e3a 100644
--- a/vrx_gz/worlds/2023_practice/practice_2023_acoustic_perception2_task.sdf
+++ b/vrx_gz/worlds/2023_practice/practice_2023_acoustic_perception2_task.sdf
@@ -426,7 +426,7 @@
 
     <!-- Publish the pinger pose -->
     <plugin filename="libPublisherPlugin.so" name="vrx::PublisherPlugin">
-      <message type="gz.msgs.Vector3d" topic="/wamv/pingers/pinger/set_pinger_position" at="1.0">
+      <message type="gz.msgs.Vector3d" topic="/pinger/set_pinger_position" at="1.0">
         x: -490,
         y: 175,
         z: -2
diff --git a/vrx_gz/worlds/2023_practice/practice_2023_acoustic_tracking0_task.sdf b/vrx_gz/worlds/2023_practice/practice_2023_acoustic_tracking0_task.sdf
index 13e52f2f1..6eef388b7 100644
--- a/vrx_gz/worlds/2023_practice/practice_2023_acoustic_tracking0_task.sdf
+++ b/vrx_gz/worlds/2023_practice/practice_2023_acoustic_tracking0_task.sdf
@@ -750,7 +750,7 @@
       <ready_state_duration>10</ready_state_duration>
       <running_state_duration>300</running_state_duration>
       <release_topic>/vrx/release</release_topic>      
-      <set_pinger_position_topic>/wamv/pingers/pinger/set_pinger_position</set_pinger_position_topic>
+      <set_pinger_position_topic>/pinger/set_pinger_position</set_pinger_position_topic>
       <pinger_model>pinger</pinger_model>
       <pinger_depth>2.0</pinger_depth>
     </plugin>
diff --git a/vrx_gz/worlds/2023_practice/practice_2023_acoustic_tracking1_task.sdf b/vrx_gz/worlds/2023_practice/practice_2023_acoustic_tracking1_task.sdf
index e60ef4231..67de761e6 100644
--- a/vrx_gz/worlds/2023_practice/practice_2023_acoustic_tracking1_task.sdf
+++ b/vrx_gz/worlds/2023_practice/practice_2023_acoustic_tracking1_task.sdf
@@ -954,7 +954,7 @@
       <ready_state_duration>10</ready_state_duration>
       <running_state_duration>300</running_state_duration>
       <release_topic>/vrx/release</release_topic>      
-      <set_pinger_position_topic>/wamv/pingers/pinger/set_pinger_position</set_pinger_position_topic>
+      <set_pinger_position_topic>/pinger/set_pinger_position</set_pinger_position_topic>
       <pinger_model>pinger</pinger_model>
       <pinger_depth>2.0</pinger_depth>
     </plugin>
diff --git a/vrx_gz/worlds/2023_practice/practice_2023_acoustic_tracking2_task.sdf b/vrx_gz/worlds/2023_practice/practice_2023_acoustic_tracking2_task.sdf
index 0bf98ffda..a6c4ce2e6 100644
--- a/vrx_gz/worlds/2023_practice/practice_2023_acoustic_tracking2_task.sdf
+++ b/vrx_gz/worlds/2023_practice/practice_2023_acoustic_tracking2_task.sdf
@@ -851,7 +851,7 @@
       <ready_state_duration>10</ready_state_duration>
       <running_state_duration>300</running_state_duration>
       <release_topic>/vrx/release</release_topic>      
-      <set_pinger_position_topic>/wamv/pingers/pinger/set_pinger_position</set_pinger_position_topic>
+      <set_pinger_position_topic>/pinger/set_pinger_position</set_pinger_position_topic>
       <pinger_model>pinger</pinger_model>
       <pinger_depth>2.0</pinger_depth>
     </plugin>
diff --git a/vrx_gz/worlds/acoustic_perception_task.sdf b/vrx_gz/worlds/acoustic_perception_task.sdf
index c91a05fe3..63081e351 100644
--- a/vrx_gz/worlds/acoustic_perception_task.sdf
+++ b/vrx_gz/worlds/acoustic_perception_task.sdf
@@ -426,7 +426,7 @@
 
     <!-- Publish the pinger pose -->
     <plugin filename="libPublisherPlugin.so" name="vrx::PublisherPlugin">
-      <message type="gz.msgs.Vector3d" topic="/wamv/pingers/pinger/set_pinger_position" at="1.0">
+      <message type="gz.msgs.Vector3d" topic="/pinger/set_pinger_position" at="1.0">
         x: -540,
         y: 180,
         z: -2
diff --git a/vrx_gz/worlds/acoustic_tracking_task.sdf b/vrx_gz/worlds/acoustic_tracking_task.sdf
index 00e969bbe..a415b014a 100644
--- a/vrx_gz/worlds/acoustic_tracking_task.sdf
+++ b/vrx_gz/worlds/acoustic_tracking_task.sdf
@@ -750,7 +750,7 @@
       <ready_state_duration>10</ready_state_duration>
       <running_state_duration>300</running_state_duration>
       <release_topic>/vrx/release</release_topic>      
-      <set_pinger_position_topic>/wamv/pingers/pinger/set_pinger_position</set_pinger_position_topic>
+      <set_pinger_position_topic>/pinger/set_pinger_position</set_pinger_position_topic>
       <pinger_model>pinger</pinger_model>
       <pinger_depth>2.0</pinger_depth>
     </plugin>
diff --git a/vrx_gz/worlds/gymkhana_task.sdf b/vrx_gz/worlds/gymkhana_task.sdf
index 63d01cd6f..861f563d3 100644
--- a/vrx_gz/worlds/gymkhana_task.sdf
+++ b/vrx_gz/worlds/gymkhana_task.sdf
@@ -453,7 +453,7 @@
            Respect top-level plugin finished status. -->
       <per_plugin_exit_on_completion>true</per_plugin_exit_on_completion>
       <obstacle_penalty>1</obstacle_penalty>
-      <set_position_topic_name>/wamv/pingers/pinger/set_pinger_position</set_position_topic_name>
+      <set_position_topic_name>/pinger/set_pinger_position</set_position_topic_name>
       <pinger_position>-483 295.5 0</pinger_position>
     </plugin>
 
diff --git a/vrx_urdf/vrx_gazebo/config/wamv_config/component_compliance/numeric.yaml b/vrx_urdf/vrx_gazebo/config/wamv_config/component_compliance/numeric.yaml
index 5815e66f4..6b67960bd 100644
--- a/vrx_urdf/vrx_gazebo/config/wamv_config/component_compliance/numeric.yaml
+++ b/vrx_urdf/vrx_gazebo/config/wamv_config/component_compliance/numeric.yaml
@@ -59,7 +59,7 @@ wamv_ball_shooter:
 wamv_pinger:
     num: 1
     allowed_params:
-        name
+        sensor_name
         position
 
 wamv_p3d:
diff --git a/vrx_urdf/vrx_gazebo/config/wamv_config/example_component_config.yaml b/vrx_urdf/vrx_gazebo/config/wamv_config/example_component_config.yaml
index 7a0ef5fd2..ed5aedfe1 100644
--- a/vrx_urdf/vrx_gazebo/config/wamv_config/example_component_config.yaml
+++ b/vrx_urdf/vrx_gazebo/config/wamv_config/example_component_config.yaml
@@ -60,5 +60,5 @@ wamv_ball_shooter:
       pitch: ${radians(-20)}
       yaw: 0.0
 wamv_pinger:
-    - name: pinger
+    - sensor_name: receiver
       position: 1.0 0 -1.0
diff --git a/vrx_urdf/wamv_gazebo/urdf/components/wamv_pinger.xacro b/vrx_urdf/wamv_gazebo/urdf/components/wamv_pinger.xacro
index e01ac1d15..0e691e494 100644
--- a/vrx_urdf/wamv_gazebo/urdf/components/wamv_pinger.xacro
+++ b/vrx_urdf/wamv_gazebo/urdf/components/wamv_pinger.xacro
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:macro name="wamv_pinger" params="name:='pinger' frameId:='wamv/pinger' position:='0 0 0' orientation:='0 0 0'">
-    <link name="${namespace}/${name}">
+  <xacro:macro name="wamv_pinger" params="pinger_name:='pinger' sensor_name:='receiver' frameId:='/pinger' position:='0 0 0' orientation:='0 0 0'">
+    <link name="${namespace}/${sensor_name}">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
@@ -13,12 +13,12 @@
         <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="revolute">
+    <joint name="${namespace}/${sensor_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}"/>
+      <child link="${namespace}/${sensor_name}"/>
     </joint>
 
     <gazebo>
@@ -26,10 +26,10 @@
         filename="libAcousticPingerPlugin.so"
         name="vrx::AcousticPingerPlugin">
         <position>${position}</position>
-        <topic>${namespace}/${pinger_namespace}${name}/range_bearing</topic>
-        <set_position_topic>${namespace}/${pinger_namespace}${name}/set_pinger_position
+        <topic>/${namespace}/${sensor_namespace}${acoustic_namespace}${sensor_name}/range_bearing</topic>
+        <set_position_topic>/${pinger_name}/set_pinger_position
         </set_position_topic>
-        <frame_id>${namespace}/${name}</frame_id>
+        <frame_id>${namespace}/${sensor_name}</frame_id>
         <range_noise>
           <noise>
             <type>gaussian</type>
diff --git a/vrx_urdf/wamv_gazebo/urdf/wamv_gazebo.urdf.xacro b/vrx_urdf/wamv_gazebo/urdf/wamv_gazebo.urdf.xacro
index fa9e0c0fc..de182f896 100644
--- a/vrx_urdf/wamv_gazebo/urdf/wamv_gazebo.urdf.xacro
+++ b/vrx_urdf/wamv_gazebo/urdf/wamv_gazebo.urdf.xacro
@@ -15,7 +15,7 @@
   <xacro:arg name="thruster_namespace" default="thrusters/"/>
   <xacro:arg name="camera_namespace" default="cameras/"/>
   <xacro:arg name="sensor_namespace" default="sensors/"/>
-  <xacro:arg name="pinger_namespace" default="pingers/"/>
+  <xacro:arg name="acoustic_namespace" default="acoustics/"/>
   <xacro:arg name="shooter_namespace" default="shooters/"/>
   <!-- Note: this is only used for some sensors that do not correctly use the
   robotNamespace parameter -->
@@ -23,7 +23,7 @@
   <xacro:property name="thruster_namespace" value="$(arg thruster_namespace)" scope="global" />
   <xacro:property name="camera_namespace" value="$(arg camera_namespace)" scope="global" />
   <xacro:property name="sensor_namespace" value="$(arg sensor_namespace)" scope="global" />
-  <xacro:property name="pinger_namespace" value="$(arg pinger_namespace)" scope="global" />
+  <xacro:property name="acoustic_namespace" value="$(arg acoustic_namespace)" scope="global" />
   <xacro:property name="shooter_namespace" value="$(arg shooter_namespace)" scope="global" />
   <xacro:property name="namespace" value="$(arg namespace)" scope="global" />
 
@@ -186,7 +186,7 @@
 
     <!-- Add pinger -->
     <xacro:if value="$(arg pinger_enabled)">
-      <xacro:wamv_pinger name="pinger" position="-528 191 -2.0" />
+      <xacro:wamv_pinger sensor_name="receiver" position="-528 191 -2.0" />
     </xacro:if>
 
     <!-- Add ball shooter (default pitch angle: ~-60 deg) -->
@@ -218,7 +218,7 @@
       <xacro:lidar name="lidar_wamv" type="16_beam"/>
 
       <!-- Add pinger -->
-      <xacro:wamv_pinger name="pinger" position="-528 191 -2.0" />
+      <xacro:wamv_pinger sensor_name="receiver" position="-528 191 -2.0" />
 
       <!-- Add ball shooter (default pitch angle: ~-60 deg) -->
       <xacro:wamv_ball_shooter name="ball_shooter" x="0.54" y="0.30" z="1.296" pitch="-1.04"/>

From 33d3331515066753ab1d9a0f7081e789ca66291c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= <caguero@openrobotics.org>
Date: Wed, 28 Jun 2023 16:40:16 +0200
Subject: [PATCH 2/2] Tweak
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 | 2 +-
 1 file changed, 1 insertion(+), 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 0e691e494..e22fef62d 100644
--- a/vrx_urdf/wamv_gazebo/urdf/components/wamv_pinger.xacro
+++ b/vrx_urdf/wamv_gazebo/urdf/components/wamv_pinger.xacro
@@ -29,7 +29,7 @@
         <topic>/${namespace}/${sensor_namespace}${acoustic_namespace}${sensor_name}/range_bearing</topic>
         <set_position_topic>/${pinger_name}/set_pinger_position
         </set_position_topic>
-        <frame_id>${namespace}/${sensor_name}</frame_id>
+        <frame_id>${pinger_name}</frame_id>
         <range_noise>
           <noise>
             <type>gaussian</type>