From 20b5c9b38415e1bbf0fa99a3c3adaf99cbb4528b Mon Sep 17 00:00:00 2001
From: Jakub Delicat <109142865+delihus@users.noreply.github.com>
Date: Wed, 12 Jun 2024 09:29:02 +0200
Subject: [PATCH] No namespaces TF fix (#49)

* No namespaces fix

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

* Add lazy, fix and simplify orbbec_astra, remove <ros> from <sensor>

* Delete unused arg

* Rebase branch

* revert astra resolution

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

* without namespace working

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

---------

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>
Co-authored-by: rafal-gorecki <rafal.gorecki@husarion.com>
---
 config/gz_orbbec_astra_remappings.yaml    | 16 ++++-----
 config/gz_ouster_os_remappings.yaml       |  1 +
 config/gz_slamtec_rplidar_remappings.yaml |  1 +
 config/gz_velodyne_remappings.yaml        |  1 +
 launch/gz_orbbec_astra.launch.py          | 44 +++++------------------
 urdf/components.urdf.xacro                | 27 ++++++++------
 urdf/intel_realsense_d435.urdf.xacro      |  8 ++---
 urdf/kinova.urdf.xacro                    |  3 --
 urdf/orbbec_astra.urdf.xacro              | 10 ++----
 urdf/ouster.urdf.xacro                    |  5 ---
 urdf/slamtec_rplidar_a2.urdf.xacro        |  3 --
 urdf/slamtec_rplidar_a3.urdf.xacro        |  3 --
 urdf/slamtec_rplidar_s1.urdf.xacro        |  3 --
 urdf/slamtec_rplidar_s2.urdf.xacro        |  3 --
 urdf/slamtec_rplidar_s3.urdf.xacro        |  3 --
 urdf/stereolabs_zed.urdf.xacro            |  7 +---
 urdf/velodyne_puck.urdf.xacro             |  4 ---
 17 files changed, 40 insertions(+), 102 deletions(-)

diff --git a/config/gz_orbbec_astra_remappings.yaml b/config/gz_orbbec_astra_remappings.yaml
index 111cee2..5daf8f1 100644
--- a/config/gz_orbbec_astra_remappings.yaml
+++ b/config/gz_orbbec_astra_remappings.yaml
@@ -1,31 +1,27 @@
 ---
 # https://github.com/orbbec/OrbbecSDK_ROS2#all-available-topics
-- ros_topic_name: <robot_namespace><device_namespace>/<camera_name>/color/camera_info
-  gz_topic_name: <robot_namespace><device_namespace>/color/camera_info
+- topic_name: <robot_namespace><device_namespace>/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/image_raw
-  gz_topic_name: <robot_namespace><device_namespace>/color/image_raw
+- topic_name: <robot_namespace><device_namespace>/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>/depth/camera_info
-  gz_topic_name: <robot_namespace><device_namespace>/depth/camera_info
+- topic_name: <robot_namespace><device_namespace>/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/image_raw
-  gz_topic_name: <robot_namespace><device_namespace>/depth/image_raw
+- 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_ros_topic_name: <robot_namespace><device_namespace>/depth/image_raw/points
+- ros_topic_name: <robot_namespace><device_namespace>/depth/points
+  gz_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/config/gz_ouster_os_remappings.yaml b/config/gz_ouster_os_remappings.yaml
index eb725fd..34367a3 100644
--- a/config/gz_ouster_os_remappings.yaml
+++ b/config/gz_ouster_os_remappings.yaml
@@ -4,3 +4,4 @@
     gz_topic_name: <robot_namespace><device_namespace>/ouster/points/points
     ros_type_name: sensor_msgs/msg/PointCloud2
     gz_type_name: ignition.msgs.PointCloudPacked
+    lazy: true
diff --git a/config/gz_slamtec_rplidar_remappings.yaml b/config/gz_slamtec_rplidar_remappings.yaml
index 45848b6..e678ea6 100644
--- a/config/gz_slamtec_rplidar_remappings.yaml
+++ b/config/gz_slamtec_rplidar_remappings.yaml
@@ -2,3 +2,4 @@
   - topic_name: <robot_namespace><device_namespace>/scan
     ros_type_name: sensor_msgs/msg/LaserScan
     gz_type_name: ignition.msgs.LaserScan
+    lazy: true
diff --git a/config/gz_velodyne_remappings.yaml b/config/gz_velodyne_remappings.yaml
index b912267..96f7713 100644
--- a/config/gz_velodyne_remappings.yaml
+++ b/config/gz_velodyne_remappings.yaml
@@ -4,3 +4,4 @@
     gz_topic_name: <robot_namespace><device_namespace>/velodyne_points/points
     ros_type_name: sensor_msgs/msg/PointCloud2
     gz_type_name: ignition.msgs.PointCloudPacked
+    lazy: true
diff --git a/launch/gz_orbbec_astra.launch.py b/launch/gz_orbbec_astra.launch.py
index 84bb00b..b106894 100644
--- a/launch/gz_orbbec_astra.launch.py
+++ b/launch/gz_orbbec_astra.launch.py
@@ -26,25 +26,15 @@
 def fix_depth_image_tf(context, *args, **kwargs):
     robot_namespace = LaunchConfiguration("robot_namespace").perform(context)
     device_namespace = LaunchConfiguration("device_namespace").perform(context)
-    tf_prefix = LaunchConfiguration("tf_prefix").perform(context)
-    camera_name = LaunchConfiguration("camera_name").perform(context)
-
-    device_namespace_ext = device_namespace + "/"
-    if device_namespace == "":
-        device_namespace_ext = ""
-
-    tf_prefix_ext = tf_prefix + "_"
-    if tf_prefix == "":
-        tf_prefix_ext = ""
-
-    parent_frame = tf_prefix_ext + camera_name + "_depth_optical_frame"
-    child_frame = (
-        "panther/base_link//"
-        + device_namespace_ext
-        + tf_prefix_ext
-        + camera_name
-        + "_orbbec_astra_depth"
-    )
+
+    if robot_namespace.startswith('/'):
+        robot_namespace = robot_namespace[1:] + "/"
+
+    if device_namespace.startswith('/'):
+        device_namespace = device_namespace[1:]
+
+    parent_frame = robot_namespace + device_namespace + "_depth_optical_frame"
+    child_frame = "panther/base_link/" + robot_namespace + device_namespace + "_orbbec_astra_depth"
 
     static_transform_publisher = Node(
         package="tf2_ros",
@@ -66,7 +56,6 @@ def generate_launch_description():
 
     robot_namespace = LaunchConfiguration("robot_namespace")
     device_namespace = LaunchConfiguration("device_namespace")
-    camera_name = LaunchConfiguration("camera_name")
     gz_bridge_name = LaunchConfiguration("gz_bridge_name")
 
     namespaced_gz_bridge_config_path = ReplaceString(
@@ -74,7 +63,6 @@ def generate_launch_description():
         replacements={
             "<robot_namespace>": robot_namespace,
             "<device_namespace>": device_namespace,
-            "<camera_name>": camera_name,
         },
     )
 
@@ -84,24 +72,12 @@ def generate_launch_description():
         description="Sensor namespace that will appear before all non absolute topics and TF frames, used for distinguishing multiple cameras on the same robot.",
     )
 
-    declare_tf_prefix = DeclareLaunchArgument(
-        "tf_prefix",
-        default_value="",
-        description="Prefix added for all links of device. Here used as fix for static transform publisher.",
-    )
-
     declare_robot_namespace = DeclareLaunchArgument(
         "robot_namespace",
         default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
         description="Namespace which will appear in front of all topics (including /tf and /tf_static).",
     )
 
-    declare_camera_name = DeclareLaunchArgument(
-        "camera_name",
-        default_value="camera",
-        description="Name of the camera. It will appear before all tfs and topics.",
-    )
-
     declare_gz_bridge_name = DeclareLaunchArgument(
         "gz_bridge_name",
         default_value="gz_bridge",
@@ -121,8 +97,6 @@ def generate_launch_description():
         [
             declare_device_namespace,
             declare_robot_namespace,
-            declare_tf_prefix,
-            declare_camera_name,
             declare_gz_bridge_name,
             gz_bridge,
             OpaqueFunction(function=fix_depth_image_tf),
diff --git a/urdf/components.urdf.xacro b/urdf/components.urdf.xacro
index 0fda963..b92e621 100644
--- a/urdf/components.urdf.xacro
+++ b/urdf/components.urdf.xacro
@@ -28,6 +28,13 @@
       <xacro:property name="device_namespace" value="${component['device_namespace']}"
         scope="parent" />
 
+        <xacro:if value="${robot_namespace == ''}">
+          <xacro:property name="ns" value="None" scope="parent" />
+        </xacro:if>
+        <xacro:unless value="${robot_namespace == ''}">
+          <xacro:property name="ns" value="${robot_namespace}" scope="parent" />
+        </xacro:unless>
+
       <xacro:if value="${type == 'CAM01'}">
         <xacro:include
           filename="$(find ros_components_description)/urdf/orbbec_astra.urdf.xacro"
@@ -37,7 +44,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
-          namespace="${robot_namespace}"
+          namespace="${ns}"
           device_namespace="${device_namespace}"
         />
       </xacro:if>
@@ -52,7 +59,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
-          namespace="${robot_namespace}"
+          namespace="${ns}"
           device_namespace="${device_namespace}"
         />
       </xacro:if>
@@ -67,7 +74,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
-          namespace="${robot_namespace}"
+          namespace="${ns}"
           device_namespace="${device_namespace}"
         />
       </xacro:if>
@@ -91,7 +98,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
-          namespace="${robot_namespace}"
+          namespace="${ns}"
           device_namespace="${device_namespace}"
           model="${ouster_models_dict[type]}"
         />
@@ -107,7 +114,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
-          namespace="${robot_namespace}"
+          namespace="${ns}"
           device_namespace="${device_namespace}"
         />
       </xacro:if>
@@ -153,7 +160,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
-          namespace="${robot_namespace}"
+          namespace="${ns}"
           device_namespace="${device_namespace}"
           kinova_type="gen3"
           dof="6"
@@ -172,7 +179,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
-          namespace="${robot_namespace}"
+          namespace="${ns}"
           device_namespace="${device_namespace}"
           kinova_type="gen3"
           dof="6"
@@ -191,7 +198,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
-          namespace="${robot_namespace}"
+          namespace="${ns}"
           device_namespace="${device_namespace}"
           kinova_type="gen3"
           dof="7"
@@ -210,7 +217,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
-          namespace="${robot_namespace}"
+          namespace="${ns}"
           kinova_type="gen3"
           dof="7"
           gripper="robotiq_2f_85"
@@ -227,7 +234,7 @@
           parent_link="${parent_link}"
           xyz="${xyz}"
           rpy="${rpy}"
-          namespace="${robot_namespace}"
+          namespace="${ns}"
           device_namespace="${device_namespace}"
           robotiq_type="2f_85"
         />
diff --git a/urdf/intel_realsense_d435.urdf.xacro b/urdf/intel_realsense_d435.urdf.xacro
index 93b4d74..a59ca34 100644
--- a/urdf/intel_realsense_d435.urdf.xacro
+++ b/urdf/intel_realsense_d435.urdf.xacro
@@ -192,9 +192,7 @@
           <near>0.02</near>
           <far>300.0</far>
         </clip>
-        <ros>
-          <namespace>${ns}${device_namespace}</namespace>
-        </ros>
+
       </sensor>
 
       <sensor type="depth_camera" name="${ns}${device_namespace}_intel_realsense_d435_depth">
@@ -222,9 +220,7 @@
             <stddev>0.005</stddev>
           </noise>
         </camera>
-        <ros>
-          <namespace>${ns}${device_namespace}</namespace>
-        </ros>
+
       </sensor>
     </gazebo>
   </xacro:macro>
diff --git a/urdf/kinova.urdf.xacro b/urdf/kinova.urdf.xacro
index 9b31acd..6896ffb 100644
--- a/urdf/kinova.urdf.xacro
+++ b/urdf/kinova.urdf.xacro
@@ -186,9 +186,6 @@
           <visualize>true</visualize>
           <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 c7998a8..32f9fde 100644
--- a/urdf/orbbec_astra.urdf.xacro
+++ b/urdf/orbbec_astra.urdf.xacro
@@ -100,7 +100,7 @@
         <topic>${ns}${device_namespace}/color/image_raw</topic>
         <visualize>false</visualize>
 
-        <ignition_frame_id>${device_namespace}_color_optical_frame</ignition_frame_id>
+        <ignition_frame_id>${ns}${device_namespace}_color_optical_frame</ignition_frame_id>
         <horizontal_fov>${60.0/180.0*pi}</horizontal_fov>
         <image>
           <width>640</width>
@@ -111,9 +111,6 @@
           <near>0.6</near>
           <far>8.0</far>
         </clip>
-        <ros>
-          <namespace>${ns}${device_namespace}</namespace>
-        </ros>
       </sensor>
 
       <sensor type="depth_camera" name="${ns}${device_namespace}_orbbec_astra_depth">
@@ -124,7 +121,7 @@
         <topic>${ns}${device_namespace}/depth/image_raw</topic>
         <visualize>false</visualize>
 
-        <ignition_frame_id>${device_namespace}_depth_optical_frame</ignition_frame_id>
+        <ignition_frame_id>${ns}${device_namespace}_depth_optical_frame</ignition_frame_id>
         <camera>
           <horizontal_fov>${60.0/180.0*pi}</horizontal_fov>
           <image>
@@ -142,9 +139,6 @@
             <stddev>0.005</stddev>
           </noise>
         </camera>
-        <ros>
-          <namespace>${ns}${device_namespace}</namespace>
-        </ros>
       </sensor>
     </gazebo>
 
diff --git a/urdf/ouster.urdf.xacro b/urdf/ouster.urdf.xacro
index c82f135..811fe14 100644
--- a/urdf/ouster.urdf.xacro
+++ b/urdf/ouster.urdf.xacro
@@ -146,11 +146,6 @@
         <frame_id>${ns}${prefix}os_lidar</frame_id>
         <ignition_frame_id>${ns}${prefix}os_lidar</ignition_frame_id>
         <always_on>true</always_on>
-
-        <ros>
-          <namespace>${ns}${device_ns}</namespace>
-        </ros>
-
       </sensor>
     </gazebo>
   </xacro:macro>
diff --git a/urdf/slamtec_rplidar_a2.urdf.xacro b/urdf/slamtec_rplidar_a2.urdf.xacro
index c99f7c0..be7846e 100644
--- a/urdf/slamtec_rplidar_a2.urdf.xacro
+++ b/urdf/slamtec_rplidar_a2.urdf.xacro
@@ -123,9 +123,6 @@
         </ray>
         <always_on>true</always_on>
         <visualize>false</visualize>
-        <ros>
-          <namespace>${ns}${device_ns}</namespace>
-        </ros>
       </sensor>
     </gazebo>
   </xacro:macro>
diff --git a/urdf/slamtec_rplidar_a3.urdf.xacro b/urdf/slamtec_rplidar_a3.urdf.xacro
index 223f8e9..4c4feb7 100644
--- a/urdf/slamtec_rplidar_a3.urdf.xacro
+++ b/urdf/slamtec_rplidar_a3.urdf.xacro
@@ -99,9 +99,6 @@
         </ray>
         <always_on>true</always_on>
         <visualize>false</visualize>
-        <ros>
-          <namespace>${ns}${device_ns}</namespace>
-        </ros>
       </sensor>
     </gazebo>
 
diff --git a/urdf/slamtec_rplidar_s1.urdf.xacro b/urdf/slamtec_rplidar_s1.urdf.xacro
index baf3820..28ee959 100644
--- a/urdf/slamtec_rplidar_s1.urdf.xacro
+++ b/urdf/slamtec_rplidar_s1.urdf.xacro
@@ -105,9 +105,6 @@
         </ray>
         <always_on>true</always_on>
         <visualize>false</visualize>
-        <ros>
-          <namespace>${ns}${device_ns}</namespace>
-        </ros>
       </sensor>
     </gazebo>
 
diff --git a/urdf/slamtec_rplidar_s2.urdf.xacro b/urdf/slamtec_rplidar_s2.urdf.xacro
index b739e75..d5ba416 100644
--- a/urdf/slamtec_rplidar_s2.urdf.xacro
+++ b/urdf/slamtec_rplidar_s2.urdf.xacro
@@ -97,9 +97,6 @@
         </ray>
         <always_on>1</always_on>
         <visualize>false</visualize>
-        <ros>
-          <namespace>${ns}${device_ns}</namespace>
-        </ros>
       </sensor>
     </gazebo>
 
diff --git a/urdf/slamtec_rplidar_s3.urdf.xacro b/urdf/slamtec_rplidar_s3.urdf.xacro
index 0c13a43..8c37afc 100644
--- a/urdf/slamtec_rplidar_s3.urdf.xacro
+++ b/urdf/slamtec_rplidar_s3.urdf.xacro
@@ -97,9 +97,6 @@
         </ray>
         <always_on>1</always_on>
         <visualize>false</visualize>
-        <ros>
-          <namespace>${ns}${device_ns}</namespace>
-        </ros>
       </sensor>
     </gazebo>
 
diff --git a/urdf/stereolabs_zed.urdf.xacro b/urdf/stereolabs_zed.urdf.xacro
index 7168733..04c0e38 100644
--- a/urdf/stereolabs_zed.urdf.xacro
+++ b/urdf/stereolabs_zed.urdf.xacro
@@ -199,9 +199,6 @@
             <far>300.0</far>
           </clip>
         </camera>
-        <ros>
-          <namespace>${ns}</namespace>
-        </ros>
       </sensor>
 
       <sensor type="depth_camera" name="${ns}${device_namespace}_stereolabs_zed_depth">
@@ -228,9 +225,7 @@
             <stddev>0.03</stddev>
           </noise>
         </camera>
-        <ros>
-          <namespace>${ns}${device_namespace}</namespace>
-        </ros>
+
       </sensor>
     </gazebo>
 
diff --git a/urdf/velodyne_puck.urdf.xacro b/urdf/velodyne_puck.urdf.xacro
index 3dfd6e7..27b2787 100644
--- a/urdf/velodyne_puck.urdf.xacro
+++ b/urdf/velodyne_puck.urdf.xacro
@@ -103,10 +103,6 @@
         <frame_id>${ns}${prefix}velodyne</frame_id>
         <ignition_frame_id>${ns}${prefix}velodyne</ignition_frame_id>
         <always_on>true</always_on>
-
-        <ros>
-          <namespace>${ns}${device_ns}</namespace>
-        </ros>
       </sensor>
     </gazebo>
   </xacro:macro>