diff --git a/jazzy_ws/src/humanoid_locomotion_policy_example/h1_fullbody_controller/launch/h1_fullbody_controller.launch.py b/jazzy_ws/src/humanoid_locomotion_policy_example/h1_fullbody_controller/launch/h1_fullbody_controller.launch.py index 37cdd16..925336a 100644 --- a/jazzy_ws/src/humanoid_locomotion_policy_example/h1_fullbody_controller/launch/h1_fullbody_controller.launch.py +++ b/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'), }] - + ), ]) diff --git a/jazzy_ws/src/humanoid_locomotion_policy_example/h1_fullbody_controller/scripts/h1_fullbody_controller.py b/jazzy_ws/src/humanoid_locomotion_policy_example/h1_fullbody_controller/scripts/h1_fullbody_controller.py index 3bd7a2b..510b2a2 100755 --- a/jazzy_ws/src/humanoid_locomotion_policy_example/h1_fullbody_controller/scripts/h1_fullbody_controller.py +++ b/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