From 1377f467a55635e6bf86229818d5fe8b1030e348 Mon Sep 17 00:00:00 2001
From: Jakub Delicat <jakub.delicat@husarion.com>
Date: Wed, 29 May 2024 16:17:00 +0200
Subject: [PATCH] Fixed gz_bridges after may humble sync

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>
---
 config/gz_kinova_remappings.yaml       |  8 ++---
 config/gz_orbbec_astra_remappings.yaml | 46 ++++++++++++++------------
 launch/gz_components.launch.py         |  2 +-
 urdf/components.urdf.xacro             |  6 +++-
 urdf/kinova.urdf.xacro                 |  5 ++-
 urdf/orbbec_astra.urdf.xacro           |  2 +-
 6 files changed, 40 insertions(+), 29 deletions(-)

diff --git a/config/gz_kinova_remappings.yaml b/config/gz_kinova_remappings.yaml
index 77d0510..ebb819a 100644
--- a/config/gz_kinova_remappings.yaml
+++ b/config/gz_kinova_remappings.yaml
@@ -1,20 +1,20 @@
 ---
-  - topic_name: <robot_namespace><device_namespace>/wrist_mounted_camera/camera_info
+  - topic_name: /<robot_namespace>/<device_namespace>/wrist_mounted_camera/camera_info
     ros_type_name: sensor_msgs/msg/CameraInfo
     gz_type_name: ignition.msgs.CameraInfo
     lazy: true
 
-  - topic_name: <robot_namespace><device_namespace>/wrist_mounted_camera/image
+  - topic_name: /<robot_namespace>/<device_namespace>/wrist_mounted_camera/image
     ros_type_name: sensor_msgs/msg/Image
     gz_type_name: ignition.msgs.Image
     lazy: true
 
-  - topic_name: <robot_namespace><device_namespace>/wrist_mounted_camera/depth_image
+  - topic_name: /<robot_namespace>/<device_namespace>/wrist_mounted_camera/depth_image
     ros_type_name: sensor_msgs/msg/Image
     gz_type_name: ignition.msgs.Image
     lazy: true
 
-  - ros_topic_name: <robot_namespace><device_namespace>/wrist_mounted_camera/points
+  - ros_topic_name: /<robot_namespace>/<device_namespace>/wrist_mounted_camera/points
     ros_type_name: sensor_msgs/msg/PointCloud2
     gz_type_name: ignition.msgs.PointCloudPacked
     lazy: true
diff --git a/config/gz_orbbec_astra_remappings.yaml b/config/gz_orbbec_astra_remappings.yaml
index 3c76814..111cee2 100644
--- a/config/gz_orbbec_astra_remappings.yaml
+++ b/config/gz_orbbec_astra_remappings.yaml
@@ -1,27 +1,31 @@
 ---
 # https://github.com/orbbec/OrbbecSDK_ROS2#all-available-topics
-  - topic_name: <robot_namespace><device_namespace>/<camera_name>/color/camera_info
-    ros_type_name: sensor_msgs/msg/CameraInfo
-    gz_type_name: ignition.msgs.CameraInfo
-    lazy: true
+- ros_topic_name: <robot_namespace><device_namespace>/<camera_name>/color/camera_info
+  gz_topic_name: <robot_namespace><device_namespace>/color/camera_info
+  ros_type_name: sensor_msgs/msg/CameraInfo
+  gz_type_name: ignition.msgs.CameraInfo
+  lazy: true
 
-  - topic_name: <robot_namespace><device_namespace>/<camera_name>/color/image_raw
-    ros_type_name: sensor_msgs/msg/Image
-    gz_type_name: ignition.msgs.Image
-    lazy: true
+- ros_topic_name: <robot_namespace><device_namespace>/<camera_name>/color/image_raw
+  gz_topic_name: <robot_namespace><device_namespace>/color/image_raw
+  ros_type_name: sensor_msgs/msg/Image
+  gz_type_name: ignition.msgs.Image
+  lazy: true
 
-  - topic_name: <robot_namespace><device_namespace>/<camera_name>/depth/camera_info
-    ros_type_name: sensor_msgs/msg/CameraInfo
-    gz_type_name: ignition.msgs.CameraInfo
-    lazy: true
+- ros_topic_name: <robot_namespace><device_namespace>/<camera_name>/depth/camera_info
+  gz_topic_name: <robot_namespace><device_namespace>/depth/camera_info
+  ros_type_name: sensor_msgs/msg/CameraInfo
+  gz_type_name: ignition.msgs.CameraInfo
+  lazy: true
 
-  - topic_name: <robot_namespace><device_namespace>/<camera_name>/depth/image_raw
-    ros_type_name: sensor_msgs/msg/Image
-    gz_type_name: ignition.msgs.Image
-    lazy: true
+- ros_topic_name: <robot_namespace><device_namespace>/<camera_name>/depth/image_raw
+  gz_topic_name: <robot_namespace><device_namespace>/depth/image_raw
+  ros_type_name: sensor_msgs/msg/Image
+  gz_type_name: ignition.msgs.Image
+  lazy: true
 
-  - ros_topic_name: <robot_namespace><device_namespace>/<camera_name>/depth/points
-    gz_topic_name: <robot_namespace><device_namespace>/<camera_name>/depth/image_raw/points
-    ros_type_name: sensor_msgs/msg/PointCloud2
-    gz_type_name: ignition.msgs.PointCloudPacked
-    lazy: true
+- ros_topic_name: <robot_namespace><device_namespace>/<camera_name>/depth/points
+  gz_ros_topic_name: <robot_namespace><device_namespace>/depth/image_raw/points
+  ros_type_name: sensor_msgs/msg/PointCloud2
+  gz_type_name: ignition.msgs.PointCloudPacked
+  lazy: true
diff --git a/launch/gz_components.launch.py b/launch/gz_components.launch.py
index 5089692..98980a5 100644
--- a/launch/gz_components.launch.py
+++ b/launch/gz_components.launch.py
@@ -41,7 +41,7 @@ def get_launch_description(name: str, package: str, namespace: str, component: y
     device_namespace = get_value(component, "device_namespace")
     robot_namespace = namespace
 
-    if  "ur3" not in name and "kinova" not in name and "robotiq" not in name:
+    if "ur" not in name and "kinova" not in name and "robotiq" not in name:
         if len(robot_namespace) and robot_namespace[0] != "/":
             robot_namespace = "/" + robot_namespace
         if len(device_namespace) and device_namespace[0] != "/":
diff --git a/urdf/components.urdf.xacro b/urdf/components.urdf.xacro
index 6751723..0fda963 100644
--- a/urdf/components.urdf.xacro
+++ b/urdf/components.urdf.xacro
@@ -153,6 +153,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
+          namespace="${robot_namespace}"
           device_namespace="${device_namespace}"
           kinova_type="gen3"
           dof="6"
@@ -171,6 +172,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
+          namespace="${robot_namespace}"
           device_namespace="${device_namespace}"
           kinova_type="gen3"
           dof="6"
@@ -179,7 +181,6 @@
         />
       </xacro:if>
 
-
       <xacro:if value="${type == 'MAN06'}">
         <xacro:include
           filename="$(find ros_components_description)/urdf/kinova.urdf.xacro"
@@ -190,6 +191,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
+          namespace="${robot_namespace}"
           device_namespace="${device_namespace}"
           kinova_type="gen3"
           dof="7"
@@ -208,6 +210,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
+          namespace="${robot_namespace}"
           kinova_type="gen3"
           dof="7"
           gripper="robotiq_2f_85"
@@ -224,6 +227,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
+          namespace="${robot_namespace}"
           device_namespace="${device_namespace}"
           robotiq_type="2f_85"
         />
diff --git a/urdf/kinova.urdf.xacro b/urdf/kinova.urdf.xacro
index a466e0e..9b31acd 100644
--- a/urdf/kinova.urdf.xacro
+++ b/urdf/kinova.urdf.xacro
@@ -184,8 +184,11 @@
           <always_on>1</always_on>
           <update_rate>${camera_fps}</update_rate>
           <visualize>true</visualize>
-          <topic>${ns}wrist_mounted_camera</topic>
+          <topic>${ns}${device_namespace}/wrist_mounted_camera</topic>
           <enable_metrics>false</enable_metrics>
+          <ros>
+            <namespace>${ns}${device_namespace}</namespace>
+          </ros>
         </sensor>
       </gazebo>
 
diff --git a/urdf/orbbec_astra.urdf.xacro b/urdf/orbbec_astra.urdf.xacro
index c273fa9..c7998a8 100644
--- a/urdf/orbbec_astra.urdf.xacro
+++ b/urdf/orbbec_astra.urdf.xacro
@@ -112,7 +112,7 @@
           <far>8.0</far>
         </clip>
         <ros>
-          <namespace>${ns}</namespace>
+          <namespace>${ns}${device_namespace}</namespace>
         </ros>
       </sensor>