Skip to content

Commit 4e5175f

Browse files
mhubiimhubii
committed
initial lego launch re-factor
1 parent 5085157 commit 4e5175f

21 files changed

+260
-436
lines changed

‎lbr_bringup/CMakeLists.txt

+1-1
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,6 @@ install(
1515
)
1616

1717
# Launch mixins
18-
ament_python_install_package(launch_mixins)
18+
ament_python_install_package(lbr_bringup)
1919

2020
ament_package()

‎lbr_bringup/launch/bringup.launch.py

+24-7
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,16 @@
11
from launch import LaunchDescription
22
from launch.actions import IncludeLaunchDescription
3-
from launch.conditions import IfCondition, UnlessCondition
3+
from launch.conditions import LaunchConfigurationEquals
44
from launch.launch_description_sources import PythonLaunchDescriptionSource
55
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
6-
from launch_mixins.lbr_description import LBRDescriptionMixin
76
from launch_ros.substitutions import FindPackageShare
7+
from lbr_bringup.description import LBRDescriptionMixin
88

99

1010
def generate_launch_description() -> LaunchDescription:
1111
ld = LaunchDescription()
1212

13-
ld.add_action(LBRDescriptionMixin.arg_sim())
13+
ld.add_action(LBRDescriptionMixin.arg_mode())
1414

1515
ld.add_action(
1616
IncludeLaunchDescription(
@@ -19,11 +19,28 @@ def generate_launch_description() -> LaunchDescription:
1919
[
2020
FindPackageShare("lbr_bringup"),
2121
"launch",
22-
"sim.launch.py",
22+
"hardware.launch.py",
2323
]
2424
)
2525
),
26-
condition=IfCondition(LaunchConfiguration("sim")),
26+
condition=LaunchConfigurationEquals(
27+
LaunchConfiguration("mode"), "hardware"
28+
),
29+
)
30+
)
31+
32+
ld.add_action(
33+
IncludeLaunchDescription(
34+
PythonLaunchDescriptionSource(
35+
PathJoinSubstitution(
36+
[
37+
FindPackageShare("lbr_bringup"),
38+
"launch",
39+
"mock.launch.py",
40+
]
41+
)
42+
),
43+
condition=LaunchConfigurationEquals(LaunchConfiguration("mode"), "mock"),
2744
)
2845
)
2946

@@ -34,11 +51,11 @@ def generate_launch_description() -> LaunchDescription:
3451
[
3552
FindPackageShare("lbr_bringup"),
3653
"launch",
37-
"real.launch.py",
54+
"gazebo.launch.py",
3855
]
3956
)
4057
),
41-
condition=UnlessCondition(LaunchConfiguration("sim")),
58+
condition=LaunchConfigurationEquals(LaunchConfiguration("mode"), "gazebo"),
4259
)
4360
)
4461

‎lbr_bringup/launch/gazebo.launch.py

+31-128
Original file line numberDiff line numberDiff line change
@@ -1,150 +1,53 @@
1-
from typing import List
1+
from launch import LaunchDescription
2+
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
3+
from lbr_bringup.description import GazeboMixin, LBRDescriptionMixin
4+
from lbr_bringup.ros2_control import LBRROS2ControlMixin
25

3-
from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
4-
from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler
5-
from launch.conditions import IfCondition
6-
from launch.event_handlers import OnProcessExit
7-
from launch.substitutions import (
8-
AndSubstitution,
9-
LaunchConfiguration,
10-
NotSubstitution,
11-
PathJoinSubstitution,
12-
)
13-
from launch_mixins.lbr_bringup import LBRMoveGroupMixin
14-
from launch_mixins.lbr_description import GazeboMixin, LBRDescriptionMixin, RVizMixin
15-
from launch_mixins.lbr_ros2_control import LBRROS2ControlMixin
166

17-
18-
def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
7+
def generate_launch_description() -> LaunchDescription:
198
ld = LaunchDescription()
209

21-
robot_description = LBRDescriptionMixin.param_robot_description(sim=True)
22-
world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero
23-
ld.add_action(GazeboMixin.include_gazebo()) # Gazebo has its own controller manager
24-
spawn_entity = GazeboMixin.node_spawn_entity(tf=world_robot_tf)
25-
ld.add_action(spawn_entity)
26-
joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
27-
controller="joint_state_broadcaster"
28-
)
29-
ld.add_action(joint_state_broadcaster)
30-
robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
31-
robot_description=robot_description, use_sim_time=True
32-
)
33-
ld.add_action(
34-
robot_state_publisher
35-
) # Do not condition robot state publisher on joint state broadcaster as Gazebo uses robot state publisher to retrieve robot description
10+
# launch arguments
11+
ld.add_action(LBRDescriptionMixin.arg_model())
12+
ld.add_action(LBRDescriptionMixin.arg_robot_name())
3613
ld.add_action(
37-
LBRROS2ControlMixin.node_controller_spawner(
38-
controller=LaunchConfiguration("ctrl")
39-
)
40-
)
41-
42-
# MoveIt 2
43-
ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution())
44-
ld.add_action(LBRMoveGroupMixin.arg_capabilities())
45-
ld.add_action(LBRMoveGroupMixin.arg_disable_capabilities())
46-
ld.add_action(LBRMoveGroupMixin.arg_monitor_dynamics())
47-
ld.add_action(LBRMoveGroupMixin.args_publish_monitored_planning_scene())
14+
LBRROS2ControlMixin.arg_ctrl()
15+
) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml
4816

49-
# MoveGroup:
50-
# - requires world frame
51-
# - urdf only has robot_name/world
52-
# This transform needs publishing
53-
robot_name = LaunchConfiguration("robot_name").perform(context)
17+
# static transform world -> robot_name/world
18+
world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero
5419
ld.add_action(
5520
LBRDescriptionMixin.node_static_tf(
5621
tf=world_robot_tf,
5722
parent="world",
58-
child=PathJoinSubstitution(
59-
[
60-
robot_name,
61-
"world",
62-
] # results in robot_name/world
63-
),
64-
parameters=[{"use_sim_time": True}],
23+
child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]),
6524
)
6625
)
6726

68-
model = LaunchConfiguration("model").perform(context)
69-
moveit_configs_builder = LBRMoveGroupMixin.moveit_configs_builder(
70-
robot_name=model,
71-
package_name=f"{model}_moveit_config",
72-
)
73-
move_group_params = LBRMoveGroupMixin.params_move_group()
27+
# robot description
28+
robot_description = LBRDescriptionMixin.param_robot_description(mode="gazebo")
7429

75-
ld.add_action(
76-
LBRMoveGroupMixin.node_move_group(
77-
parameters=[
78-
moveit_configs_builder.to_dict(),
79-
move_group_params,
80-
{"use_sim_time": True},
81-
],
82-
condition=IfCondition(LaunchConfiguration("moveit")),
83-
namespace=robot_name,
84-
)
85-
)
86-
87-
# RViz and MoveIt
88-
rviz_moveit = RVizMixin.node_rviz(
89-
rviz_config_pkg=f"{model}_moveit_config",
90-
rviz_config="config/moveit.rviz",
91-
parameters=LBRMoveGroupMixin.params_rviz(
92-
moveit_configs=moveit_configs_builder.to_moveit_configs()
93-
)
94-
+ [{"use_sim_time": True}],
95-
condition=IfCondition(
96-
AndSubstitution(LaunchConfiguration("moveit"), LaunchConfiguration("rviz"))
97-
),
98-
remappings=[
99-
("display_planned_path", robot_name + "/display_planned_path"),
100-
("joint_states", robot_name + "/joint_states"),
101-
("monitored_planning_scene", robot_name + "/monitored_planning_scene"),
102-
("planning_scene", robot_name + "/planning_scene"),
103-
("planning_scene_world", robot_name + "/planning_scene_world"),
104-
("robot_description", robot_name + "/robot_description"),
105-
("robot_description_semantic", robot_name + "/robot_description_semantic"),
106-
],
107-
)
108-
109-
# RViz no MoveIt
110-
rviz = RVizMixin.node_rviz(
111-
rviz_config_pkg="lbr_bringup",
112-
rviz_config="config/config.rviz",
113-
condition=IfCondition(
114-
AndSubstitution(
115-
LaunchConfiguration("rviz"),
116-
NotSubstitution(LaunchConfiguration("moveit")),
117-
)
118-
),
119-
)
120-
121-
# RViz event handler
122-
rviz_event_handler = RegisterEventHandler(
123-
OnProcessExit(target_action=spawn_entity, on_exit=[rviz_moveit, rviz])
30+
# robot state publisher
31+
robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
32+
robot_description=robot_description, use_sim_time=True
12433
)
125-
ld.add_action(rviz_event_handler)
126-
127-
return ld.entities
34+
ld.add_action(
35+
robot_state_publisher
36+
) # Do not condition robot state publisher on joint state broadcaster as Gazebo uses robot state publisher to retrieve robot description
12837

38+
# Gazebo
39+
ld.add_action(GazeboMixin.include_gazebo()) # Gazebo has its own controller manager
40+
spawn_entity = GazeboMixin.node_spawn_entity(tf=world_robot_tf)
41+
ld.add_action(spawn_entity)
12942

130-
def generate_launch_description() -> LaunchDescription:
131-
ld = LaunchDescription()
132-
ld.add_action(LBRDescriptionMixin.arg_model())
133-
ld.add_action(LBRDescriptionMixin.arg_robot_name())
134-
ld.add_action(
135-
DeclareLaunchArgument(
136-
name="moveit",
137-
default_value="false",
138-
description="Whether to launch MoveIt 2.",
139-
)
43+
# controllers
44+
joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
45+
controller="joint_state_broadcaster"
14046
)
47+
ld.add_action(joint_state_broadcaster)
14148
ld.add_action(
142-
DeclareLaunchArgument(
143-
name="rviz", default_value="true", description="Whether to launch RViz."
49+
LBRROS2ControlMixin.node_controller_spawner(
50+
controller=LaunchConfiguration("ctrl")
14451
)
14552
)
146-
ld.add_action(
147-
LBRROS2ControlMixin.arg_ctrl()
148-
) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml
149-
ld.add_action(OpaqueFunction(function=launch_setup))
15053
return ld

‎lbr_bringup/launch/hardware.launch.py

+68
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
from launch import LaunchDescription
2+
from launch.actions import RegisterEventHandler
3+
from launch.event_handlers import OnProcessStart
4+
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
5+
from lbr_bringup.description import LBRDescriptionMixin
6+
from lbr_bringup.ros2_control import LBRROS2ControlMixin
7+
8+
9+
def generate_launch_description() -> LaunchDescription:
10+
ld = LaunchDescription()
11+
12+
# launch arguments
13+
ld.add_action(LBRDescriptionMixin.arg_model())
14+
ld.add_action(LBRDescriptionMixin.arg_robot_name())
15+
ld.add_action(LBRDescriptionMixin.arg_port_id())
16+
ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg())
17+
ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg())
18+
ld.add_action(LBRROS2ControlMixin.arg_ctrl())
19+
20+
# static transform world -> robot_name/world
21+
ld.add_action(
22+
LBRDescriptionMixin.node_static_tf(
23+
tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
24+
parent="world",
25+
child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]),
26+
)
27+
)
28+
29+
# robot description
30+
robot_description = LBRDescriptionMixin.param_robot_description(mode="hardware")
31+
32+
# robot state publisher
33+
robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
34+
robot_description=robot_description, use_sim_time=False
35+
)
36+
ld.add_action(robot_state_publisher)
37+
38+
# ros2 control node
39+
ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False)
40+
ld.add_action(ros2_control_node)
41+
42+
# joint state broad caster and controller on ros2 control node start
43+
joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
44+
controller="joint_state_broadcaster"
45+
)
46+
force_torque_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
47+
controller="force_torque_broadcaster"
48+
)
49+
lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
50+
controller="lbr_state_broadcaster"
51+
)
52+
controller = LBRROS2ControlMixin.node_controller_spawner(
53+
controller=LaunchConfiguration("ctrl")
54+
)
55+
56+
controller_event_handler = RegisterEventHandler(
57+
OnProcessStart(
58+
target_action=ros2_control_node,
59+
on_start=[
60+
joint_state_broadcaster,
61+
force_torque_broadcaster,
62+
lbr_state_broadcaster,
63+
controller,
64+
],
65+
)
66+
)
67+
ld.add_action(controller_event_handler)
68+
return ld

‎lbr_bringup/launch/mock.launch.py

+59
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
from launch import LaunchDescription
2+
from launch.actions import RegisterEventHandler
3+
from launch.event_handlers import OnProcessStart
4+
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
5+
from lbr_bringup.description import LBRDescriptionMixin
6+
from lbr_bringup.ros2_control import LBRROS2ControlMixin
7+
8+
9+
def generate_launch_description() -> LaunchDescription:
10+
ld = LaunchDescription()
11+
12+
# launch arguments
13+
ld.add_action(LBRDescriptionMixin.arg_model())
14+
ld.add_action(LBRDescriptionMixin.arg_robot_name())
15+
ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg())
16+
ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg())
17+
ld.add_action(LBRROS2ControlMixin.arg_ctrl())
18+
19+
# static transform world -> robot_name/world
20+
ld.add_action(
21+
LBRDescriptionMixin.node_static_tf(
22+
tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
23+
parent="world",
24+
child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]),
25+
)
26+
)
27+
28+
# robot description
29+
robot_description = LBRDescriptionMixin.param_robot_description(mode="mock")
30+
31+
# robot state publisher
32+
robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
33+
robot_description=robot_description, use_sim_time=False
34+
)
35+
ld.add_action(robot_state_publisher)
36+
37+
# ros2 control node
38+
ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False)
39+
ld.add_action(ros2_control_node)
40+
41+
# joint state broad caster and controller on ros2 control node start
42+
joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
43+
controller="joint_state_broadcaster"
44+
)
45+
controller = LBRROS2ControlMixin.node_controller_spawner(
46+
controller=LaunchConfiguration("ctrl")
47+
)
48+
49+
controller_event_handler = RegisterEventHandler(
50+
OnProcessStart(
51+
target_action=ros2_control_node,
52+
on_start=[
53+
joint_state_broadcaster,
54+
controller,
55+
],
56+
)
57+
)
58+
ld.add_action(controller_event_handler)
59+
return ld

0 commit comments

Comments
 (0)