moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
servo_utils.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/panda_simulated_config.yaml")
23 .to_dict()
24 }
25
26 # This filter parameter should be >1. Increase it for greater smoothing but slower motion.
27 low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5}
28
29 # ros2_control using FakeSystem as hardware
30 ros2_controllers_path = os.path.join(
31 get_package_share_directory("moveit_resources_panda_moveit_config"),
32 "config",
33 "ros2_controllers.yaml",
34 )
35 ros2_control_node = launch_ros.actions.Node(
36 package="controller_manager",
37 executable="ros2_control_node",
38 parameters=[ros2_controllers_path],
39 remappings=[
40 ("/controller_manager/robot_description", "/robot_description"),
41 ],
42 output="screen",
43 )
44
45 joint_state_broadcaster_spawner = launch_ros.actions.Node(
46 package="controller_manager",
47 executable="spawner",
48 arguments=[
49 "joint_state_broadcaster",
50 "--controller-manager-timeout",
51 "300",
52 "--controller-manager",
53 "/controller_manager",
54 "--param-file",
55 ros2_controllers_path,
56 ],
57 )
58
59 panda_arm_controller_spawner = launch_ros.actions.Node(
60 package="controller_manager",
61 executable="spawner",
62 arguments=[
63 "panda_arm_controller",
64 "-c",
65 "/controller_manager",
66 "--param-file",
67 ros2_controllers_path,
68 ],
69 )
70
71 # Component nodes for tf and Servo
72 test_container = launch_ros.actions.ComposableNodeContainer(
73 name="servo_utils_tests_container",
74 namespace="/",
75 package="rclcpp_components",
76 executable="component_container_mt",
77 composable_node_descriptions=[
78 launch_ros.descriptions.ComposableNode(
79 package="robot_state_publisher",
80 plugin="robot_state_publisher::RobotStatePublisher",
81 name="robot_state_publisher",
82 parameters=[moveit_config.robot_description],
83 ),
84 launch_ros.descriptions.ComposableNode(
85 package="tf2_ros",
86 plugin="tf2_ros::StaticTransformBroadcasterNode",
87 name="static_tf2_broadcaster",
88 parameters=[{"/child_frame_id": "panda_link0", "/frame_id": "world"}],
89 ),
90 ],
91 output="screen",
92 )
93
94 servo_gtest = launch_ros.actions.Node(
95 executable=launch.substitutions.PathJoinSubstitution(
96 [
97 launch.substitutions.LaunchConfiguration("test_binary_dir"),
98 "moveit_servo_utils_test",
99 ]
100 ),
101 parameters=[
102 servo_params,
103 low_pass_filter_coeff,
104 moveit_config.robot_description,
105 moveit_config.robot_description_semantic,
106 moveit_config.robot_description_kinematics,
107 ],
108 output="screen",
109 )
110
111 return launch.LaunchDescription(
112 [
113 launch.actions.DeclareLaunchArgument(
114 name="test_binary_dir",
115 description="Binary directory of package "
116 "containing test executables",
117 ),
118 ros2_control_node,
119 joint_state_broadcaster_spawner,
120 panda_arm_controller_spawner,
121 test_container,
122 # Gate the gtest launch on the last spawner exiting (== controller
123 # activated). The prior fixed TimerAction raced controller
124 # activation under load and caused this family to flake.
125 launch.actions.RegisterEventHandler(
126 OnProcessExit(
127 target_action=panda_arm_controller_spawner,
128 on_exit=[servo_gtest],
129 )
130 ),
131 launch_testing.actions.ReadyToTest(),
132 ]
133 ), {
134 "servo_gtest": servo_gtest,
135 }
136
137
138class TestGTestWaitForCompletion(unittest.TestCase):
139 # Waits for test to complete, then waits a bit to make sure result files are generated
140 def test_gtest_run_complete(self, servo_gtest):
141 self.proc_info.assertWaitForShutdown(servo_gtest, timeout=4000.0)
142
143
144@launch_testing.post_shutdown_test()
145class TestGTestProcessPostShutdown(unittest.TestCase):
146 # Checks if the test has been completed with acceptable exit codes (successful codes)
147 def test_gtest_pass(self, proc_info, servo_gtest):
148 launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest)
test_gtest_pass(self, proc_info, servo_gtest)