-
Notifications
You must be signed in to change notification settings - Fork 487
/
servo_utils.test.py
131 lines (117 loc) · 4.46 KB
/
servo_utils.test.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
import os
import launch
import unittest
import launch_ros
import launch_testing
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder
from launch_param_builder import ParameterBuilder
def generate_test_description():
moveit_config = (
MoveItConfigsBuilder("moveit_resources_panda")
.robot_description(file_path="config/panda.urdf.xacro")
.to_moveit_configs()
)
# Get parameters for the Servo node
servo_params = {
"moveit_servo_test": ParameterBuilder("moveit_servo")
.yaml("config/panda_simulated_config.yaml")
.to_dict()
}
# This filter parameter should be >1. Increase it for greater smoothing but slower motion.
low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5}
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"ros2_controllers.yaml",
)
ros2_control_node = launch_ros.actions.Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[ros2_controllers_path],
remappings=[
("/controller_manager/robot_description", "/robot_description"),
],
output="screen",
)
joint_state_broadcaster_spawner = launch_ros.actions.Node(
package="controller_manager",
executable="spawner",
arguments=[
"joint_state_broadcaster",
"--controller-manager-timeout",
"300",
"--controller-manager",
"/controller_manager",
],
)
panda_arm_controller_spawner = launch_ros.actions.Node(
package="controller_manager",
executable="spawner",
arguments=["panda_arm_controller", "-c", "/controller_manager"],
)
# Component nodes for tf and Servo
test_container = launch_ros.actions.ComposableNodeContainer(
name="servo_utils_tests_container",
namespace="/",
package="rclcpp_components",
executable="component_container_mt",
composable_node_descriptions=[
launch_ros.descriptions.ComposableNode(
package="robot_state_publisher",
plugin="robot_state_publisher::RobotStatePublisher",
name="robot_state_publisher",
parameters=[moveit_config.robot_description],
),
launch_ros.descriptions.ComposableNode(
package="tf2_ros",
plugin="tf2_ros::StaticTransformBroadcasterNode",
name="static_tf2_broadcaster",
parameters=[{"/child_frame_id": "panda_link0", "/frame_id": "world"}],
),
],
output="screen",
)
servo_gtest = launch_ros.actions.Node(
executable=launch.substitutions.PathJoinSubstitution(
[
launch.substitutions.LaunchConfiguration("test_binary_dir"),
"moveit_servo_utils_test",
]
),
parameters=[
servo_params,
low_pass_filter_coeff,
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.robot_description_kinematics,
],
output="screen",
)
return launch.LaunchDescription(
[
launch.actions.DeclareLaunchArgument(
name="test_binary_dir",
description="Binary directory of package "
"containing test executables",
),
ros2_control_node,
joint_state_broadcaster_spawner,
panda_arm_controller_spawner,
test_container,
launch.actions.TimerAction(period=2.0, actions=[servo_gtest]),
launch_testing.actions.ReadyToTest(),
]
), {
"servo_gtest": servo_gtest,
}
class TestGTestWaitForCompletion(unittest.TestCase):
# Waits for test to complete, then waits a bit to make sure result files are generated
def test_gtest_run_complete(self, servo_gtest):
self.proc_info.assertWaitForShutdown(servo_gtest, timeout=4000.0)
@launch_testing.post_shutdown_test()
class TestGTestProcessPostShutdown(unittest.TestCase):
# Checks if the test has been completed with acceptable exit codes (successful codes)
def test_gtest_pass(self, proc_info, servo_gtest):
launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest)