moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
servo_ros_integration.test.py
Go to the documentation of this file.
1import os
2import launch
3from launch.event_handlers import OnProcessExit
4import unittest
5import launch_ros
6import launch_testing
7from ament_index_python.packages import get_package_share_directory
8from moveit_configs_utils import MoveItConfigsBuilder
9from launch_param_builder import ParameterBuilder
10from launch.conditions import IfCondition, UnlessCondition
11from launch.substitutions import LaunchConfiguration
12
13
15 moveit_config = (
16 MoveItConfigsBuilder("moveit_resources_panda")
17 .robot_description(file_path="config/panda.urdf.xacro")
18 .to_moveit_configs()
19 )
20
21 # Launch Servo as a standalone node or as a "node component" for better latency/efficiency
22 launch_as_standalone_node = LaunchConfiguration(
23 "launch_as_standalone_node", default="true"
24 )
25
26 # Get parameters for the Servo node
27 servo_params = {
28 "moveit_servo": ParameterBuilder("moveit_servo")
29 .yaml("config/test_config_panda.yaml")
30 .to_dict()
31 }
32
33 # This sets the update rate and planning group name for the acceleration limiting filter.
34 acceleration_filter_update_period = {"update_period": 0.01}
35 planning_group_name = {"planning_group_name": "panda_arm"}
36
37 # ros2_control using FakeSystem as hardware
38 ros2_controllers_path = os.path.join(
39 get_package_share_directory("moveit_resources_panda_moveit_config"),
40 "config",
41 "ros2_controllers.yaml",
42 )
43 ros2_control_node = launch_ros.actions.Node(
44 package="controller_manager",
45 executable="ros2_control_node",
46 parameters=[ros2_controllers_path],
47 remappings=[
48 ("/controller_manager/robot_description", "/robot_description"),
49 ],
50 output="screen",
51 )
52
53 joint_state_broadcaster_spawner = launch_ros.actions.Node(
54 package="controller_manager",
55 executable="spawner",
56 arguments=[
57 "joint_state_broadcaster",
58 "--controller-manager-timeout",
59 "300",
60 "--controller-manager",
61 "/controller_manager",
62 "--param-file",
63 ros2_controllers_path,
64 ],
65 output="screen",
66 )
67
68 panda_arm_controller_spawner = launch_ros.actions.Node(
69 package="controller_manager",
70 executable="spawner",
71 arguments=[
72 "panda_arm_controller",
73 "-c",
74 "/controller_manager",
75 "--param-file",
76 ros2_controllers_path,
77 ],
78 )
79
80 # Launch as much as possible in components
81 container = launch_ros.actions.ComposableNodeContainer(
82 name="moveit_servo_demo_container",
83 namespace="/",
84 package="rclcpp_components",
85 executable="component_container_mt",
86 composable_node_descriptions=[
87 # Example of launching Servo as a node component
88 # Launching as a node component makes ROS 2 intraprocess communication more efficient.
89 launch_ros.descriptions.ComposableNode(
90 package="moveit_servo",
91 plugin="moveit_servo::ServoNode",
92 name="servo_node",
93 parameters=[
94 servo_params,
95 acceleration_filter_update_period,
96 planning_group_name,
97 moveit_config.robot_description,
98 moveit_config.robot_description_semantic,
99 moveit_config.robot_description_kinematics,
100 ],
101 condition=UnlessCondition(launch_as_standalone_node),
102 ),
103 launch_ros.descriptions.ComposableNode(
104 package="robot_state_publisher",
105 plugin="robot_state_publisher::RobotStatePublisher",
106 name="robot_state_publisher",
107 parameters=[moveit_config.robot_description],
108 ),
109 launch_ros.descriptions.ComposableNode(
110 package="tf2_ros",
111 plugin="tf2_ros::StaticTransformBroadcasterNode",
112 name="static_tf2_broadcaster",
113 parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}],
114 ),
115 ],
116 output="screen",
117 )
118 # Launch a standalone Servo node.
119 # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC
120 servo_node = launch_ros.actions.Node(
121 package="moveit_servo",
122 executable="servo_node",
123 name="servo_node",
124 parameters=[
125 servo_params,
126 acceleration_filter_update_period,
127 planning_group_name,
128 moveit_config.robot_description,
129 moveit_config.robot_description_semantic,
130 moveit_config.robot_description_kinematics,
131 ],
132 output="screen",
133 condition=IfCondition(launch_as_standalone_node),
134 )
135
136 servo_gtest = launch_ros.actions.Node(
137 executable=launch.substitutions.PathJoinSubstitution(
138 [
139 launch.substitutions.LaunchConfiguration("test_binary_dir"),
140 "moveit_servo_ros_integration_test",
141 ]
142 ),
143 output="screen",
144 )
145
146 return launch.LaunchDescription(
147 [
148 servo_node,
149 container,
150 launch.actions.TimerAction(period=3.0, actions=[ros2_control_node]),
151 launch.actions.TimerAction(
152 period=5.0, actions=[joint_state_broadcaster_spawner]
153 ),
154 launch.actions.TimerAction(
155 period=7.0, actions=[panda_arm_controller_spawner]
156 ),
157 # Gate the gtest launch on the last-in-chain spawner completing so the
158 # test does not race controller activation. Prior fixed 9s timer flaked
159 # under load (ServoRosFixture.testJointJog).
160 launch.actions.RegisterEventHandler(
161 OnProcessExit(
162 target_action=panda_arm_controller_spawner,
163 on_exit=[servo_gtest],
164 )
165 ),
166 launch_testing.actions.ReadyToTest(),
167 ]
168 ), {
169 "servo_gtest": servo_gtest,
170 }
171
172
173class TestGTestWaitForCompletion(unittest.TestCase):
174 # Waits for test to complete, then waits a bit to make sure result files are generated
175 def test_gtest_run_complete(self, servo_gtest):
176 self.proc_info.assertWaitForShutdown(servo_gtest, timeout=4000.0)
177
178
179@launch_testing.post_shutdown_test()
180class TestGTestProcessPostShutdown(unittest.TestCase):
181 # Checks if the test has been completed with acceptable exit codes (successful codes)
182 def test_gtest_pass(self, proc_info, servo_gtest):
183 launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest)