diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index 83122dca..35f3a3f2 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -1,12 +1,15 @@ from typing import List from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity -from launch.actions import (DeclareLaunchArgument, OpaqueFunction, - RegisterEventHandler) +from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnProcessExit, OnProcessStart -from launch.substitutions import (AndSubstitution, LaunchConfiguration, - NotSubstitution, PathJoinSubstitution) +from launch.substitutions import ( + AndSubstitution, + LaunchConfiguration, + NotSubstitution, + PathJoinSubstitution, +) from lbr_bringup import LBRMoveGroupMixin from lbr_description import LBRDescriptionMixin, RVizMixin @@ -23,8 +26,12 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld.add_action(ros2_control_node) # joint state broad caster and controller on ros2 control node start - joint_state_broadcaster = LBRROS2ControlMixin.node_joint_state_broadcaster() - controller = LBRROS2ControlMixin.node_controller() + joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( + controller="joint_state_broadcaster" + ) + controller = LBRROS2ControlMixin.node_controller_spawner( + controller=LaunchConfiguration("ctrl") + ) controller_event_handler = RegisterEventHandler( OnProcessStart( diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/sim.launch.py index fbef3c8e..00c684ac 100644 --- a/lbr_bringup/launch/sim.launch.py +++ b/lbr_bringup/launch/sim.launch.py @@ -23,7 +23,9 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld.add_action(GazeboMixin.include_gazebo()) # Gazebo has its own controller manager spawn_entity = GazeboMixin.node_spawn_entity() ld.add_action(spawn_entity) - joint_state_broadcaster = LBRROS2ControlMixin.node_joint_state_broadcaster() + joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( + controller="joint_state_broadcaster" + ) ld.add_action(joint_state_broadcaster) robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( robot_description=robot_description, use_sim_time=True @@ -31,7 +33,11 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld.add_action( robot_state_publisher ) # Do not condition robot state publisher on joint state broadcaster as Gazebo uses robot state publisher to retrieve robot description - ld.add_action(LBRROS2ControlMixin.node_controller()) + ld.add_action( + LBRROS2ControlMixin.node_controller_spawner( + controller=LaunchConfiguration("ctrl") + ) + ) # MoveIt 2 ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution()) diff --git a/lbr_ros2_control/launch/system_interface.launch.py b/lbr_ros2_control/launch/system_interface.launch.py index 3141f038..9d1a1460 100644 --- a/lbr_ros2_control/launch/system_interface.launch.py +++ b/lbr_ros2_control/launch/system_interface.launch.py @@ -1,6 +1,7 @@ from launch import LaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessStart +from launch.substitutions import LaunchConfiguration from lbr_description import LBRDescriptionMixin from lbr_ros2_control import LBRROS2ControlMixin @@ -24,8 +25,12 @@ def generate_launch_description() -> LaunchDescription: robot_description=robot_description ) ld.add_action(ros2_control_node) - joint_state_broadcaster = LBRSystemInterface.node_joint_state_broadcaster() - controller = LBRSystemInterface.node_controller() + joint_state_broadcaster = LBRSystemInterface.node_controller_spawner( + controller="joint_state_broadcaster" + ) + controller = LBRSystemInterface.node_controller_spawner( + controller=LaunchConfiguration("ctrl") + ) controller_event_handler = RegisterEventHandler( OnProcessStart( target_action=ros2_control_node, diff --git a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py index 85ad1df5..8f11ce92 100644 --- a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py +++ b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py @@ -72,38 +72,21 @@ def node_ros2_control( ) @staticmethod - def node_joint_state_broadcaster( + def node_controller_spawner( robot_name: Optional[Union[LaunchConfiguration, str]] = None, + controller: Optional[Union[LaunchConfiguration, str]] = None, **kwargs, ) -> Node: if robot_name is None: robot_name = LaunchConfiguration("robot_name", default="lbr") + if controller is None: + controller = LaunchConfiguration("ctrl") return Node( package="controller_manager", executable="spawner", output="screen", arguments=[ - "joint_state_broadcaster", - "--controller-manager", - "controller_manager", - ], - namespace=robot_name, - **kwargs, - ) - - @staticmethod - def node_controller( - robot_name: Optional[Union[LaunchConfiguration, str]] = None, - **kwargs, - ) -> Node: - if robot_name is None: - robot_name = LaunchConfiguration("robot_name", default="lbr") - return Node( - package="controller_manager", - executable="spawner", - output="screen", - arguments=[ - LaunchConfiguration("ctrl", default="position_trajectory_controller"), + controller, "--controller-manager", "controller_manager", ],