moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
test_basic_integration.test.py
Go to the documentation of this file.
1import launch_testing
2import os
3import sys
4import unittest
5
6from ament_index_python.packages import get_package_share_directory
7from launch import LaunchDescription
8from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction
9from launch.event_handlers import OnProcessExit
10from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
11from launch_ros.actions import Node
12from moveit_configs_utils import MoveItConfigsBuilder
13
14sys.path.append(os.path.dirname(__file__))
15from hybrid_planning_common import (
16 generate_common_hybrid_launch_description,
17 load_yaml,
18)
19
20
22 # generate_common_hybrid_launch_description() returns a list of nodes to launch
23 common_launch = generate_common_hybrid_launch_description()
24 moveit_config = (
25 MoveItConfigsBuilder("moveit_resources_panda")
26 .robot_description(file_path="config/panda.urdf.xacro")
27 .to_moveit_configs()
28 )
29
30 hybrid_planning_gtest = Node(
31 executable=PathJoinSubstitution(
32 [LaunchConfiguration("test_binary_dir"), "test_basic_integration"]
33 ),
34 parameters=[
35 moveit_config.to_dict(),
36 ],
37 output="screen",
38 )
39
40 # Static TF
41 static_tf = Node(
42 package="tf2_ros",
43 executable="static_transform_publisher",
44 name="static_transform_publisher",
45 output="log",
46 arguments=["--frame-id", "world", "--child-frame-id", "panda_link0"],
47 )
48
49 # Publish TF
50 robot_state_publisher = Node(
51 package="robot_state_publisher",
52 executable="robot_state_publisher",
53 name="robot_state_publisher",
54 output="both",
55 parameters=[moveit_config.robot_description],
56 )
57
58 # ros2_control using FakeSystem as hardware
59 ros2_controllers_path = os.path.join(
60 get_package_share_directory("moveit_hybrid_planning"),
61 "config",
62 "demo_controller.yaml",
63 )
64 ros2_control_node = Node(
65 package="controller_manager",
66 executable="ros2_control_node",
67 parameters=[moveit_config.robot_description, ros2_controllers_path],
68 output="screen",
69 )
70
71 joint_state_broadcaster_spawner = Node(
72 package="controller_manager",
73 executable="spawner",
74 arguments=[
75 "joint_state_broadcaster",
76 "--controller-manager",
77 "/controller_manager",
78 "--param-file",
79 ros2_controllers_path,
80 ],
81 )
82
83 panda_joint_group_position_controller_spawner = Node(
84 package="controller_manager",
85 executable="spawner",
86 arguments=[
87 "panda_joint_group_position_controller",
88 "-c",
89 "/controller_manager",
90 "--param-file",
91 ros2_controllers_path,
92 ],
93 )
94
95 return LaunchDescription(
96 [
97 DeclareLaunchArgument(
98 name="test_binary_dir",
99 description="Binary directory of package "
100 "containing test executables",
101 ),
102 # Gate the gtest launch on the last spawner exiting (== controller
103 # activated). The prior fixed TimerAction raced controller
104 # activation under load and caused this family to flake.
105 RegisterEventHandler(
106 OnProcessExit(
107 target_action=panda_joint_group_position_controller_spawner,
108 on_exit=[hybrid_planning_gtest],
109 )
110 ),
111 launch_testing.actions.ReadyToTest(),
112 ]
113 + common_launch
114 + [
115 static_tf,
116 robot_state_publisher,
117 ros2_control_node,
118 joint_state_broadcaster_spawner,
119 panda_joint_group_position_controller_spawner,
120 ]
121 ), {
122 "hybrid_planning_gtest": hybrid_planning_gtest,
123 }
124
125
126class TestGTestWaitForCompletion(unittest.TestCase):
127 # Waits for test to complete, then waits a bit to make sure result files are generated
128 def test_gtest_run_complete(self, hybrid_planning_gtest):
129 self.proc_info.assertWaitForShutdown(hybrid_planning_gtest, timeout=4000.0)
130
131
132@launch_testing.post_shutdown_test()
133class TestGTestProcessPostShutdown(unittest.TestCase):
134 # Checks if the test has been completed with acceptable exit codes (successful codes)
135 def test_gtest_pass(self, proc_info, hybrid_planning_gtest):
136 launch_testing.asserts.assertExitCodes(proc_info, process=hybrid_planning_gtest)