moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
demo_joint_jog.launch.py
Go to the documentation of this file.
1import os
2import launch
3import launch_ros
4from ament_index_python.packages import get_package_share_directory
5from launch_param_builder import ParameterBuilder
6from moveit_configs_utils import MoveItConfigsBuilder
7
8
10 moveit_config = (
11 MoveItConfigsBuilder("moveit_resources_panda")
12 .robot_description(file_path="config/panda.urdf.xacro")
13 .joint_limits(file_path="config/hard_joint_limits.yaml")
14 .robot_description_kinematics()
15 .to_moveit_configs()
16 )
17
18 # Get parameters for the Servo node
19 servo_params = {
20 "moveit_servo": ParameterBuilder("moveit_servo")
21 .yaml("config/panda_simulated_config.yaml")
22 .to_dict()
23 }
24
25 # This sets the update rate and planning group name for the acceleration limiting filter.
26 acceleration_filter_update_period = {"update_period": 0.01}
27 planning_group_name = {"planning_group_name": "panda_arm"}
28
29 # RViz
30 rviz_config_file = (
31 get_package_share_directory("moveit_servo") + "/config/demo_rviz_config.rviz"
32 )
33 rviz_node = launch_ros.actions.Node(
34 package="rviz2",
35 executable="rviz2",
36 name="rviz2",
37 output="log",
38 arguments=["-d", rviz_config_file],
39 parameters=[
40 moveit_config.robot_description,
41 moveit_config.robot_description_semantic,
42 ],
43 )
44
45 # ros2_control using FakeSystem as hardware
46 ros2_controllers_path = os.path.join(
47 get_package_share_directory("moveit_resources_panda_moveit_config"),
48 "config",
49 "ros2_controllers.yaml",
50 )
51 ros2_control_node = launch_ros.actions.Node(
52 package="controller_manager",
53 executable="ros2_control_node",
54 parameters=[ros2_controllers_path],
55 remappings=[
56 ("/controller_manager/robot_description", "/robot_description"),
57 ],
58 output="screen",
59 )
60
61 joint_state_broadcaster_spawner = launch_ros.actions.Node(
62 package="controller_manager",
63 executable="spawner",
64 arguments=[
65 "joint_state_broadcaster",
66 "--controller-manager-timeout",
67 "300",
68 "--controller-manager",
69 "/controller_manager",
70 ],
71 )
72
73 panda_arm_controller_spawner = launch_ros.actions.Node(
74 package="controller_manager",
75 executable="spawner",
76 arguments=["panda_arm_controller", "-c", "/controller_manager"],
77 )
78
79 # Launch as much as possible in components
80 container = launch_ros.actions.ComposableNodeContainer(
81 name="moveit_servo_demo_container",
82 namespace="/",
83 package="rclcpp_components",
84 executable="component_container_mt",
85 composable_node_descriptions=[
86 launch_ros.descriptions.ComposableNode(
87 package="robot_state_publisher",
88 plugin="robot_state_publisher::RobotStatePublisher",
89 name="robot_state_publisher",
90 parameters=[moveit_config.robot_description],
91 ),
92 launch_ros.descriptions.ComposableNode(
93 package="tf2_ros",
94 plugin="tf2_ros::StaticTransformBroadcasterNode",
95 name="static_tf2_broadcaster",
96 parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}],
97 ),
98 ],
99 output="screen",
100 )
101 # Launch a standalone Servo node.
102 # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC
103 servo_node = launch_ros.actions.Node(
104 package="moveit_servo",
105 executable="demo_joint_jog",
106 parameters=[
107 servo_params,
108 acceleration_filter_update_period,
109 planning_group_name,
110 moveit_config.robot_description,
111 moveit_config.robot_description_semantic,
112 moveit_config.robot_description_kinematics,
113 moveit_config.joint_limits,
114 ],
115 output="screen",
116 )
117
118 return launch.LaunchDescription(
119 [
120 rviz_node,
121 ros2_control_node,
122 joint_state_broadcaster_spawner,
123 panda_arm_controller_spawner,
124 servo_node,
125 container,
126 ]
127 )