Add namespace for multi-humanoid support

jazzy_ws/src/humanoid_locomotion_policy_example/h1_fullbody_controller/launch/h1_fullbody_controller.launch.py

@@ -16,7 +16,11 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch_ros.actions import Node
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import (
+    LaunchConfiguration,
+    IfElseSubstitution,
+    TextSubstitution,
+)
 from ament_index_python.packages import get_package_share_directory
 import os
 
@@ -39,16 +43,29 @@ def generate_launch_description():
             "use_sim_time",
             default_value="True",
             description="Use simulation (Omniverse Isaac Sim) clock if true"),
+        DeclareLaunchArgument(
+            "namespace",
+            default_value="h1_01",
+            description="ROS namespace for the H1 controller"),
+        DeclareLaunchArgument(
+            "use_namespace",
+            default_value="False",
+            description="Whether to apply the ROS namespace to the node"),
         Node(
             package='h1_fullbody_controller',
             executable='h1_fullbody_controller.py',
             name='h1_fullbody_controller',
             output="screen",
+            namespace=IfElseSubstitution(
+                [LaunchConfiguration('use_namespace')],
+                [LaunchConfiguration('namespace')],
+                [TextSubstitution(text='')]
+            ),
             parameters=[{
                 'publish_period_ms': LaunchConfiguration('publish_period_ms'),
                 'policy_path': LaunchConfiguration('policy_path'),
                 "use_sim_time": LaunchConfiguration('use_sim_time'),
             }]
-            
+
         ),
     ])

jazzy_ws/src/humanoid_locomotion_policy_example/h1_fullbody_controller/scripts/h1_fullbody_controller.py

@@ -61,27 +61,27 @@ def __init__(self):
         # Create subscription for velocity commands
         self._cmd_vel_subscription = self.create_subscription(
             Twist,
-            '/cmd_vel',
+            'cmd_vel',
             self._cmd_vel_callback,
             qos_profile=10)
 
         # Create publisher for joint commands
         self._joint_publisher = self.create_publisher(
             JointState,
-            '/joint_command',
+            'joint_command',
             qos_profile=sim_qos_profile)
 
         # Setup synchronized subscribers for IMU and joint state data
         self._imu_sub_filter = Subscriber(
             self,
             Imu,
-            '/imu',
+            'imu',
             qos_profile=sim_qos_profile,
         )
         self._joint_states_sub_filter = Subscriber(
             self,
             JointState,
-            '/joint_states',
+            'joint_states',
             qos_profile=sim_qos_profile,
         )
         queue_size = 10