diff --git a/clearpath_control/launch/control.launch.py b/clearpath_control/launch/control.launch.py index fc1e70b8..fd24d01f 100644 --- a/clearpath_control/launch/control.launch.py +++ b/clearpath_control/launch/control.launch.py @@ -39,21 +39,27 @@ from clearpath_config.common.utils.dictionary import unflatten_dict from clearpath_config.common.utils.yaml import read_yaml -REMAPPINGS = [ - ('joint_states', 'platform/joint_states'), - ('dynamic_joint_states', 'platform/dynamic_joint_states'), - ('platform_velocity_controller/odom', 'platform/odom'), - ('platform_velocity_controller/odometry', 'platform/odom'), - ('platform_velocity_controller/cmd_vel', 'platform/cmd_vel'), - ('platform_velocity_controller/cmd_vel_out', 'platform/cmd_vel_out'), - ('platform_velocity_controller/reference', 'platform/cmd_vel'), - ('platform_velocity_controller/transition_event', 'platform/transition_event'), +CONTROLLER_MANAGER_REMAPPINGS = [ ('/diagnostics', 'diagnostics'), - ('/tf', 'tf'), - ('/tf_static', 'tf_static'), ('~/robot_description', 'robot_description'), ] +JOINT_STATE_BROADCASTER_REMAPPINGS = [ + '-r', 'joint_states:=platform/joint_states', + '-r', 'dynamic_joint_states:=platform/dynamic_joint_states', +] + +PLATFORM_VELOCITY_CONTROLLER_REMAPPINGS = [ + '-r', '~/odom:=platform/odom', + '-r', '~/odometry:=platform/odom', + '-r', '~/cmd_vel:=platform/cmd_vel', + '-r', '~/cmd_vel_out:=platform/cmd_vel_out', + '-r', '~/reference:=platform/cmd_vel', + '-r', '~/transition_event:=platform/transition_event', + '-r', '/tf:=tf', + '-r', '/tf_static:=tf_static', +] + def launch_setup(context, *args, **kwargs): setup_path = LaunchConfiguration('setup_path') @@ -76,14 +82,17 @@ def launch_setup(context, *args, **kwargs): 'stdout': 'screen', 'stderr': 'screen', }, - remappings=REMAPPINGS, + remappings=CONTROLLER_MANAGER_REMAPPINGS, condition=UnlessCondition(use_sim_time) )) # Add Joint State Broadcaster controllers.append(Node( package='controller_manager', executable='spawner', - arguments=['--controller-manager-timeout', '60', 'joint_state_broadcaster'], + arguments=[ + '--controller-manager-timeout', '60', 'joint_state_broadcaster', + '--controller-ros-args', + ] + JOINT_STATE_BROADCASTER_REMAPPINGS, output='screen', additional_env={'ROS_SUPER_CLIENT': 'True'}, )) @@ -91,7 +100,10 @@ def launch_setup(context, *args, **kwargs): controllers.append(Node( package='controller_manager', executable='spawner', - arguments=['--controller-manager-timeout', '60', 'platform_velocity_controller'], + arguments=[ + '--controller-manager-timeout', '60', 'platform_velocity_controller', + '--controller-ros-args', + ] + PLATFORM_VELOCITY_CONTROLLER_REMAPPINGS, output='screen', additional_env={'ROS_SUPER_CLIENT': 'True'}, )) diff --git a/clearpath_manipulators/launch/control.launch.py b/clearpath_manipulators/launch/control.launch.py index 260cea32..739ab4fc 100644 --- a/clearpath_manipulators/launch/control.launch.py +++ b/clearpath_manipulators/launch/control.launch.py @@ -62,14 +62,6 @@ def launch_setup(context, *args, **kwargs): }, remappings=[ ('~/robot_description', 'robot_description'), - ('dynamic_joint_states', - PathJoinSubstitution([ - '/', namespace, 'platform', 'dynamic_joint_states' - ])), - ('joint_states', - PathJoinSubstitution([ - '/', namespace, 'platform', 'joint_states' - ])), ], )) # Add Joint State Broadcaster @@ -78,6 +70,15 @@ def launch_setup(context, *args, **kwargs): executable='spawner', arguments=[ 'joint_state_broadcaster', '--controller-manager-timeout', '60', + '--controller-ros-args', + '-r', ['dynamic_joint_states:=', + PathJoinSubstitution([ + '/', namespace, 'platform', 'dynamic_joint_states' + ])], + '-r', ['joint_states:=', + PathJoinSubstitution([ + '/', namespace, 'platform', 'joint_states' + ])], ], output='screen', ))