Fix issue #691 (#692)

* Rename pinger topics. Signed-off-by: Carlos Agüero <[email protected]> Co-authored-by: M1chaelM <[email protected]>
osrf · Jun 30, 2023 · 5b501ea · 5b501ea
1 parent da7aed4
commit 5b501ea
Show file tree

Hide file tree

Showing 15 changed files with 29 additions and 30 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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,23 +13,23 @@
         <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>
       <plugin
         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>${pinger_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
@@ -15,15 +15,15 @@
   <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 -->
   <xacro:arg name="namespace" default="wamv"/>
   <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"/>