moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
move_group_api.test.py
Go to the documentation of this file.
1import os
2import launch
3import unittest
4import launch_ros
5import launch_testing
6from ament_index_python.packages import get_package_share_directory
7from moveit_configs_utils import MoveItConfigsBuilder
8
9
11 moveit_config = (
12 MoveItConfigsBuilder("moveit_resources_panda")
13 .robot_description(
14 file_path="config/panda.urdf.xacro",
15 )
16 .robot_description_semantic(file_path="config/panda.srdf")
18 publish_robot_description=True, publish_robot_description_semantic=True
19 )
20 .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
21 .planning_pipelines(
22 pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"]
23 )
24 .to_moveit_configs()
25 )
26
27 # Start the actual move_group node/action server
28 move_group_node = launch_ros.actions.Node(
29 package="moveit_ros_move_group",
30 executable="move_group",
31 output="screen",
32 parameters=[moveit_config.to_dict()],
33 arguments=["--ros-args", "--log-level", "info"],
34 )
35
36 # ros2_control using FakeSystem as hardware
37 ros2_controllers_path = os.path.join(
38 get_package_share_directory("moveit_resources_panda_moveit_config"),
39 "config",
40 "ros2_controllers.yaml",
41 )
42 ros2_control_node = launch_ros.actions.Node(
43 package="controller_manager",
44 executable="ros2_control_node",
45 parameters=[ros2_controllers_path],
46 remappings=[
47 ("/controller_manager/robot_description", "/robot_description"),
48 ],
49 output="screen",
50 )
51
52 joint_state_broadcaster_spawner = launch_ros.actions.Node(
53 package="controller_manager",
54 executable="spawner",
55 arguments=[
56 "joint_state_broadcaster",
57 "--controller-manager",
58 "/controller_manager",
59 "--param-file",
60 ros2_controllers_path,
61 ],
62 output="screen",
63 )
64
65 panda_arm_controller_spawner = launch_ros.actions.Node(
66 package="controller_manager",
67 executable="spawner",
68 arguments=[
69 "panda_arm_controller",
70 "-c",
71 "/controller_manager",
72 "--param-file",
73 ros2_controllers_path,
74 ],
75 )
76
77 panda_hand_controller_spawner = launch_ros.actions.Node(
78 package="controller_manager",
79 executable="spawner",
80 arguments=[
81 "panda_hand_controller",
82 "-c",
83 "/controller_manager",
84 "--param-file",
85 ros2_controllers_path,
86 ],
87 )
88
89 # Static TF
90 static_tf_node = launch_ros.actions.Node(
91 package="tf2_ros",
92 executable="static_transform_publisher",
93 name="static_transform_publisher",
94 output="log",
95 arguments=["--frame-id", "world", "--child-frame-id", "panda_link0"],
96 )
97
98 # Publish TF
99 robot_state_publisher = launch_ros.actions.Node(
100 package="robot_state_publisher",
101 executable="robot_state_publisher",
102 name="robot_state_publisher",
103 output="both",
104 parameters=[moveit_config.robot_description],
105 )
106
107 move_group_gtest = launch_ros.actions.Node(
108 executable=launch.substitutions.PathJoinSubstitution(
109 [
110 launch.substitutions.LaunchConfiguration("test_binary_dir"),
111 "move_group_api_test",
112 ]
113 ),
114 parameters=[moveit_config.to_dict()],
115 output="screen",
116 )
117
118 return launch.LaunchDescription(
119 [
120 launch.actions.DeclareLaunchArgument(
121 name="test_binary_dir",
122 description="Binary directory of package "
123 "containing test executables",
124 ),
125 static_tf_node,
126 robot_state_publisher,
127 move_group_node,
128 ros2_control_node,
129 joint_state_broadcaster_spawner,
130 panda_arm_controller_spawner,
131 panda_hand_controller_spawner,
132 move_group_gtest,
133 # launch.actions.TimerAction(period=15.0, actions=[move_group_gtest]),
134 launch_testing.actions.ReadyToTest(),
135 ]
136 ), {
137 "move_group_gtest": move_group_gtest,
138 }
139
140
141class TestGTestWaitForCompletion(unittest.TestCase):
142 # Waits for test to complete, then waits a bit to make sure result files are generated
143 def test_gtest_run_complete(self, move_group_gtest):
144 self.proc_info.assertWaitForShutdown(move_group_gtest, timeout=4000.0)
145
146
147@launch_testing.post_shutdown_test()
148class TestGTestProcessPostShutdown(unittest.TestCase):
149 # Checks if the test has been completed with acceptable exit codes (successful codes)
150 def test_gtest_pass(self, proc_info, move_group_gtest):
151 launch_testing.asserts.assertExitCodes(proc_info, process=move_group_gtest)
test_gtest_pass(self, proc_info, move_group_gtest)