4 from ament_index_python.packages
import get_package_share_directory
5 from launch.conditions
import IfCondition, UnlessCondition
6 from launch.substitutions
import LaunchConfiguration
7 from launch_param_builder
import ParameterBuilder
8 from moveit_configs_utils
import MoveItConfigsBuilder
14 .robot_description(file_path=
"config/panda.urdf.xacro")
20 launch_as_standalone_node = LaunchConfiguration(
21 "launch_as_standalone_node", default=
"false"
26 "moveit_servo": ParameterBuilder(
"moveit_servo")
27 .yaml(
"config/panda_simulated_config.yaml")
32 acceleration_filter_update_period = {
"update_period": 0.01}
33 planning_group_name = {
"planning_group_name":
"panda_arm"}
37 +
"/config/demo_rviz_config_ros.rviz"
39 rviz_node = launch_ros.actions.Node(
44 arguments=[
"-d", rviz_config_file],
46 moveit_config.robot_description,
47 moveit_config.robot_description_semantic,
52 ros2_controllers_path = os.path.join(
55 "ros2_controllers.yaml",
57 ros2_control_node = launch_ros.actions.Node(
58 package=
"controller_manager",
59 executable=
"ros2_control_node",
60 parameters=[ros2_controllers_path],
62 (
"/controller_manager/robot_description",
"/robot_description"),
67 joint_state_broadcaster_spawner = launch_ros.actions.Node(
68 package=
"controller_manager",
71 "joint_state_broadcaster",
72 "--controller-manager-timeout",
74 "--controller-manager",
75 "/controller_manager",
79 panda_arm_controller_spawner = launch_ros.actions.Node(
80 package=
"controller_manager",
82 arguments=[
"panda_arm_controller",
"-c",
"/controller_manager"],
86 container = launch_ros.actions.ComposableNodeContainer(
87 name=
"moveit_servo_demo_container",
89 package=
"rclcpp_components",
90 executable=
"component_container_mt",
91 composable_node_descriptions=[
94 launch_ros.descriptions.ComposableNode(
95 package=
"moveit_servo",
96 plugin=
"moveit_servo::ServoNode",
100 acceleration_filter_update_period,
102 moveit_config.robot_description,
103 moveit_config.robot_description_semantic,
104 moveit_config.robot_description_kinematics,
105 moveit_config.joint_limits,
107 condition=UnlessCondition(launch_as_standalone_node),
109 launch_ros.descriptions.ComposableNode(
110 package=
"robot_state_publisher",
111 plugin=
"robot_state_publisher::RobotStatePublisher",
112 name=
"robot_state_publisher",
113 parameters=[moveit_config.robot_description],
115 launch_ros.descriptions.ComposableNode(
117 plugin=
"tf2_ros::StaticTransformBroadcasterNode",
118 name=
"static_tf2_broadcaster",
119 parameters=[{
"child_frame_id":
"/panda_link0",
"frame_id":
"/world"}],
126 servo_node = launch_ros.actions.Node(
127 package=
"moveit_servo",
128 executable=
"servo_node",
132 acceleration_filter_update_period,
134 moveit_config.robot_description,
135 moveit_config.robot_description_semantic,
136 moveit_config.robot_description_kinematics,
137 moveit_config.joint_limits,
140 condition=IfCondition(launch_as_standalone_node),
143 return launch.LaunchDescription(
147 joint_state_broadcaster_spawner,
148 panda_arm_controller_spawner,
def get_package_share_directory(pkg_name)
def generate_launch_description()