moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
gtest_with_move_group.py
Go to the documentation of this file.
1import os
2import unittest
3import launch_testing
4
5from ament_index_python.packages import get_package_share_directory
6from moveit_configs_utils import MoveItConfigsBuilder
7
8from launch import LaunchDescription
9from launch.actions import (
10 DeclareLaunchArgument,
11 ExecuteProcess,
12 RegisterEventHandler,
13 TimerAction,
14)
15from launch.event_handlers import OnProcessExit
16from launch_ros.actions import Node
17from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
18from launch_testing.actions import ReadyToTest
19
20from moveit_configs_utils import MoveItConfigsBuilder
21
22
24 moveit_config = (
25 MoveItConfigsBuilder("moveit_resources_panda")
26 .robot_description(file_path="config/panda.urdf.xacro")
27 .planning_pipelines("ompl", ["ompl"])
28 .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
29 .to_moveit_configs()
30 )
31
32 run_move_group_node = Node(
33 package="moveit_ros_move_group",
34 executable="move_group",
35 output="log",
36 parameters=[moveit_config.to_dict()],
37 )
38
39 static_tf = Node(
40 package="tf2_ros",
41 executable="static_transform_publisher",
42 output="log",
43 arguments=["--frame-id", "world", "--child-frame-id", "panda_link0"],
44 )
45
46 robot_state_publisher = Node(
47 package="robot_state_publisher",
48 executable="robot_state_publisher",
49 name="robot_state_publisher",
50 output="log",
51 parameters=[moveit_config.robot_description],
52 )
53
54 # ros2_control using FakeSystem as hardware
55 ros2_controllers_path = os.path.join(
56 get_package_share_directory("moveit_resources_panda_moveit_config"),
57 "config",
58 "ros2_controllers.yaml",
59 )
60 ros2_control_node = Node(
61 package="controller_manager",
62 executable="ros2_control_node",
63 parameters=[moveit_config.robot_description, ros2_controllers_path],
64 output="log",
65 )
66
67 load_controllers = []
68 for controller in [
69 "panda_arm_controller",
70 "panda_hand_controller",
71 ]:
72 load_controllers += [
73 ExecuteProcess(
74 cmd=[
75 "ros2",
76 "run",
77 "controller_manager",
78 "spawner",
79 controller,
80 "--param-file",
81 ros2_controllers_path,
82 ],
83 output="log",
84 )
85 ]
86
87 # Spawn the joint_state_broadcaster separately so the test binary can be
88 # gated on its activation. The spawner process exits 0 only once the
89 # controller is loaded, configured, and activated, at which point
90 # /joint_states is guaranteed to be publishing. Gating the test on this
91 # event (instead of a fixed wall-clock timer) avoids a startup race where
92 # the test runs before joint_states exists and
93 # MoveGroupInterface::getCurrentState() fails with
94 # "Failed to fetch current robot state".
95 joint_state_broadcaster_spawner = ExecuteProcess(
96 cmd=[
97 "ros2",
98 "run",
99 "controller_manager",
100 "spawner",
101 "joint_state_broadcaster",
102 "--param-file",
103 ros2_controllers_path,
104 ],
105 output="log",
106 )
107
108 gtest_node = Node(
109 executable=PathJoinSubstitution(
110 [
111 LaunchConfiguration("test_binary_dir"),
112 LaunchConfiguration("test_executable"),
113 ]
114 ),
115 parameters=[moveit_config.to_dict()],
116 output="screen",
117 )
118
119 return LaunchDescription(
120 [
121 DeclareLaunchArgument(
122 name="test_binary_dir",
123 description="Binary directory of package "
124 "containing test executables",
125 ),
126 static_tf,
127 robot_state_publisher,
128 ros2_control_node,
129 *load_controllers,
130 joint_state_broadcaster_spawner,
131 TimerAction(period=1.0, actions=[run_move_group_node]),
132 # Launch the test binary only once joint_state_broadcaster has
133 # activated, guaranteeing /joint_states is available.
134 RegisterEventHandler(
135 OnProcessExit(
136 target_action=joint_state_broadcaster_spawner,
137 on_exit=[gtest_node],
138 )
139 ),
140 ReadyToTest(),
141 ]
142 ), {
143 "gtest_node": gtest_node,
144 }
145
146
147class TestGTestWaitForCompletion(unittest.TestCase):
148 # Waits for test to complete, then waits a bit to make sure result files are generated
149 def test_gtest_run_complete(self, gtest_node):
150 self.proc_info.assertWaitForShutdown(gtest_node, timeout=4000.0)
151
152
153@launch_testing.post_shutdown_test()
154class TestGTestProcessPostShutdown(unittest.TestCase):
155 # Checks if the test has been completed with acceptable exit codes (successful codes)
156 def test_gtest_pass(self, proc_info, gtest_node):
157 launch_testing.asserts.assertExitCodes(
158 proc_info,
159 process=gtest_node,
160 # Allow -11 since ros2_control or warehouse_ros sometimes segfaults
161 # for unrelated reasons.
162 allowable_exit_codes=[0, -11],
163 )