moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
servo_cpp_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
10
11
13 moveit_config = (
14 MoveItConfigsBuilder("moveit_resources_panda")
15 .robot_description(file_path="config/panda.urdf.xacro")
16 .to_moveit_configs()
17 )
18
19 # Get parameters for the Servo node
20 servo_params = {
21 "moveit_servo_test": ParameterBuilder("moveit_servo")
22 .yaml("config/test_config_panda.yaml")
23 .to_dict()
24 }
25
26 # This sets the update rate and planning group name for the acceleration limiting filter.
27 acceleration_filter_update_period = {"update_period": 0.01}
28 planning_group_name = {"planning_group_name": "panda_arm"}
29
30 # ros2_control using FakeSystem as hardware
31 ros2_controllers_path = os.path.join(
32 get_package_share_directory("moveit_resources_panda_moveit_config"),
33 "config",
34 "ros2_controllers.yaml",
35 )
36 ros2_control_node = launch_ros.actions.Node(
37 package="controller_manager",
38 executable="ros2_control_node",
39 parameters=[ros2_controllers_path],
40 remappings=[
41 ("/controller_manager/robot_description", "/robot_description"),
42 ],
43 output="screen",
44 )
45
46 joint_state_broadcaster_spawner = launch_ros.actions.Node(
47 package="controller_manager",
48 executable="spawner",
49 arguments=[
50 "joint_state_broadcaster",
51 "--controller-manager-timeout",
52 "300",
53 "--controller-manager",
54 "/controller_manager",
55 "--param-file",
56 ros2_controllers_path,
57 ],
58 )
59
60 panda_arm_controller_spawner = launch_ros.actions.Node(
61 package="controller_manager",
62 executable="spawner",
63 arguments=[
64 "panda_arm_controller",
65 "-c",
66 "/controller_manager",
67 "--param-file",
68 ros2_controllers_path,
69 ],
70 )
71
72 # Component nodes for tf and Servo
73 test_container = launch_ros.actions.ComposableNodeContainer(
74 name="servo_integration_tests_container",
75 namespace="/",
76 package="rclcpp_components",
77 executable="component_container_mt",
78 composable_node_descriptions=[
79 launch_ros.descriptions.ComposableNode(
80 package="robot_state_publisher",
81 plugin="robot_state_publisher::RobotStatePublisher",
82 name="robot_state_publisher",
83 parameters=[moveit_config.robot_description],
84 ),
85 launch_ros.descriptions.ComposableNode(
86 package="tf2_ros",
87 plugin="tf2_ros::StaticTransformBroadcasterNode",
88 name="static_tf2_broadcaster",
89 parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}],
90 ),
91 ],
92 output="screen",
93 )
94
95 servo_gtest = launch_ros.actions.Node(
96 executable=launch.substitutions.PathJoinSubstitution(
97 [
98 launch.substitutions.LaunchConfiguration("test_binary_dir"),
99 "moveit_servo_cpp_integration_test",
100 ]
101 ),
102 parameters=[
103 servo_params,
104 acceleration_filter_update_period,
105 planning_group_name,
106 moveit_config.robot_description,
107 moveit_config.robot_description_semantic,
108 moveit_config.robot_description_kinematics,
109 ],
110 output="screen",
111 )
112
113 return launch.LaunchDescription(
114 [
115 launch.actions.DeclareLaunchArgument(
116 name="test_binary_dir",
117 description="Binary directory of package "
118 "containing test executables",
119 ),
120 ros2_control_node,
121 joint_state_broadcaster_spawner,
122 panda_arm_controller_spawner,
123 test_container,
124 # Gate the gtest launch on panda_arm_controller_spawner exiting so
125 # controllers are guaranteed active before the test starts.
126 launch.actions.RegisterEventHandler(
127 OnProcessExit(
128 target_action=panda_arm_controller_spawner,
129 on_exit=[servo_gtest],
130 )
131 ),
132 launch_testing.actions.ReadyToTest(),
133 ]
134 ), {
135 "servo_gtest": servo_gtest,
136 }
137
138
139class TestGTestWaitForCompletion(unittest.TestCase):
140 # Waits for test to complete, then waits a bit to make sure result files are generated
141 def test_gtest_run_complete(self, servo_gtest):
142 self.proc_info.assertWaitForShutdown(servo_gtest, timeout=4000.0)
143
144
145@launch_testing.post_shutdown_test()
146class TestGTestProcessPostShutdown(unittest.TestCase):
147 # Checks if the test has been completed with acceptable exit codes (successful codes)
148 def test_gtest_pass(self, proc_info, servo_gtest):
149 launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest)