Skip to content

Commit

Permalink
single controller spawner
Browse files Browse the repository at this point in the history
  • Loading branch information
mhubii committed Nov 20, 2023
1 parent 1f8dad8 commit 34880e5
Show file tree
Hide file tree
Showing 4 changed files with 33 additions and 32 deletions.
19 changes: 13 additions & 6 deletions lbr_bringup/launch/real.launch.py
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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(
Expand Down
10 changes: 8 additions & 2 deletions lbr_bringup/launch/sim.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -23,15 +23,21 @@ 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
)
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())
Expand Down
9 changes: 7 additions & 2 deletions lbr_ros2_control/launch/system_interface.launch.py
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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,
Expand Down
27 changes: 5 additions & 22 deletions lbr_ros2_control/lbr_ros2_control/launch_mixin.py
Original file line number Diff line number Diff line change
Expand Up @@ -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",
],
Expand Down

0 comments on commit 34880e5

Please sign in to comment.