|
| 1 | +# Copyright 2023 ArduPilot.org. |
| 2 | +# |
| 3 | +# This program is free software: you can redistribute it and/or modify |
| 4 | +# it under the terms of the GNU General Public License as published by |
| 5 | +# the Free Software Foundation, either version 3 of the License, or |
| 6 | +# (at your option) any later version. |
| 7 | +# |
| 8 | +# This program is distributed in the hope that it will be useful, |
| 9 | +# but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 10 | +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 11 | +# GNU General Public License for more details. |
| 12 | +# |
| 13 | +# You should have received a copy of the GNU General Public License |
| 14 | +# along with this program. If not, see <https://www.gnu.org/licenses/>. |
| 15 | + |
| 16 | +""" |
| 17 | +Tests the Rally Point interface. |
| 18 | +
|
| 19 | +Clear the list, adds points and verifies that they are correctly updated. |
| 20 | +
|
| 21 | +colcon test --packages-select ardupilot_dds_tests \ |
| 22 | +--event-handlers=console_cohesion+ --pytest-args -k test_rally_point |
| 23 | +
|
| 24 | +""" |
| 25 | + |
| 26 | +import rclpy |
| 27 | +import time |
| 28 | +import launch_pytest |
| 29 | +from rclpy.node import Node |
| 30 | +from builtin_interfaces.msg import Time |
| 31 | +from ardupilot_msgs.msg import Rally |
| 32 | +from ardupilot_msgs.srv import RallyGet |
| 33 | +from ardupilot_msgs.srv import RallySet |
| 34 | +import pytest |
| 35 | +from launch_pytest.tools import process as process_tools |
| 36 | +from launch import LaunchDescription |
| 37 | +import threading |
| 38 | + |
| 39 | +RALLY_0 = Rally() |
| 40 | +RALLY_0.point.latitude =-35.0 |
| 41 | +RALLY_0.point.longitude = 149.0 |
| 42 | +RALLY_0.point.altitude = 400.0 |
| 43 | +RALLY_0.altitude_frame = 2 |
| 44 | + |
| 45 | + |
| 46 | +class RallyControl(Node): |
| 47 | + """Push/Pull Rally points.""" |
| 48 | + |
| 49 | + def __init__(self): |
| 50 | + """Initialise the node.""" |
| 51 | + super().__init__("rally_service") |
| 52 | + self.clear_rally_event = threading.Event() |
| 53 | + self.add_rally_event = threading.Event() |
| 54 | + self.get_rally_event = threading.Event() |
| 55 | + |
| 56 | + self._client_rally_get = self.create_client(RallyGet, "/ap/rally_get") |
| 57 | + self._client_rally_set = self.create_client(RallySet, "/ap/rally_set") |
| 58 | + # Add a spin thread. |
| 59 | + self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,)) |
| 60 | + self.ros_spin_thread.start() |
| 61 | + |
| 62 | + def _clear_list(self): |
| 63 | + req = RallySet.Request() |
| 64 | + req.clear = True |
| 65 | + self.future = self._client_rally_set.call_async(req) |
| 66 | + time.sleep(1.0) |
| 67 | + if self.future.result() is None: |
| 68 | + return False |
| 69 | + else: |
| 70 | + print(self.future) |
| 71 | + print(self.future.result()) |
| 72 | + return self.future.result().size == 0 |
| 73 | + |
| 74 | + def _send_rally(self, rally): |
| 75 | + req = RallySet.Request() |
| 76 | + req.clear = False |
| 77 | + req.rally = rally |
| 78 | + self.future = self._client_rally_set.call_async(req) |
| 79 | + time.sleep(1.0) |
| 80 | + return self.future.result() |
| 81 | + |
| 82 | + def _get_rally(self): |
| 83 | + req = RallyGet.Request() |
| 84 | + req.index = 0 |
| 85 | + self.future = self._client_rally_get.call_async(req) |
| 86 | + time.sleep(1.0) |
| 87 | + return self.future.result() |
| 88 | + |
| 89 | + #-------------- PROCESSES ----------------- |
| 90 | + def process_clear(self): |
| 91 | + print("---> Process start") |
| 92 | + while True: |
| 93 | + if self._clear_list(): |
| 94 | + print("---> List Cleared") |
| 95 | + self.clear_rally_event.set() |
| 96 | + return |
| 97 | + time.sleep(1) |
| 98 | + |
| 99 | + def clear_list(self): |
| 100 | + try: |
| 101 | + self.clear_thread.stop() |
| 102 | + except: |
| 103 | + print("---> Starting thread") |
| 104 | + self.clear_thread = threading.Thread(target=self.process_clear) |
| 105 | + self.clear_thread.start() |
| 106 | + |
| 107 | + def process_send(self, rally): |
| 108 | + while True: |
| 109 | + response = self._send_rally(rally) |
| 110 | + if response is not None: |
| 111 | + if response.success: |
| 112 | + self.add_rally_event.set() |
| 113 | + return |
| 114 | + time.sleep(1) |
| 115 | + |
| 116 | + def send_rally(self, rally): |
| 117 | + try: |
| 118 | + self.send_thread.stop() |
| 119 | + except: |
| 120 | + self.send_thread = threading.Thread(target=self.process_send, args=[rally]) |
| 121 | + self.send_thread.start() |
| 122 | + |
| 123 | + def process_compare(self, rally): |
| 124 | + while True: |
| 125 | + response = self._get_rally() |
| 126 | + if response is not None: |
| 127 | + if response.success: |
| 128 | + if (abs(response.rally.point.latitude - rally.point.latitude) < 1e-7 |
| 129 | + and abs(response.rally.point.longitude - rally.point.longitude) < 1e-7 |
| 130 | + and response.rally.altitude_frame == rally.altitude_frame): |
| 131 | + self.get_rally_event.set() |
| 132 | + return |
| 133 | + time.sleep(1) |
| 134 | + |
| 135 | + def get_and_compare_rally(self, rally): |
| 136 | + try: |
| 137 | + self.get_thread.stop() |
| 138 | + except: |
| 139 | + self.get_thread = threading.Thread(target=self.process_compare, args=[rally]) |
| 140 | + self.get_thread.start() |
| 141 | + |
| 142 | +@launch_pytest.fixture |
| 143 | +def launch_sitl_copter_dds_serial(sitl_copter_dds_serial): |
| 144 | + """Fixture to create the launch description.""" |
| 145 | + sitl_ld, sitl_actions = sitl_copter_dds_serial |
| 146 | + |
| 147 | + ld = LaunchDescription( |
| 148 | + [ |
| 149 | + sitl_ld, |
| 150 | + launch_pytest.actions.ReadyToTest(), |
| 151 | + ] |
| 152 | + ) |
| 153 | + actions = sitl_actions |
| 154 | + yield ld, actions |
| 155 | + |
| 156 | +@launch_pytest.fixture |
| 157 | +def launch_sitl_copter_dds_udp(sitl_copter_dds_udp): |
| 158 | + """Fixture to create the launch description.""" |
| 159 | + sitl_ld, sitl_actions = sitl_copter_dds_udp |
| 160 | + |
| 161 | + ld = LaunchDescription( |
| 162 | + [ |
| 163 | + sitl_ld, |
| 164 | + launch_pytest.actions.ReadyToTest(), |
| 165 | + ] |
| 166 | + ) |
| 167 | + actions = sitl_actions |
| 168 | + yield ld, actions |
| 169 | + |
| 170 | +@pytest.mark.launch(fixture=launch_sitl_copter_dds_udp) |
| 171 | +def test_dds_udp_rally(launch_context, launch_sitl_copter_dds_udp): |
| 172 | + """Test Rally Points with AP_DDS.""" |
| 173 | + _, actions = launch_sitl_copter_dds_udp |
| 174 | + micro_ros_agent = actions["micro_ros_agent"].action |
| 175 | + mavproxy = actions["mavproxy"].action |
| 176 | + sitl = actions["sitl"].action |
| 177 | + |
| 178 | + # Wait for process to start. |
| 179 | + process_tools.wait_for_start_sync(launch_context, micro_ros_agent, timeout=2) |
| 180 | + process_tools.wait_for_start_sync(launch_context, mavproxy, timeout=2) |
| 181 | + process_tools.wait_for_start_sync(launch_context, sitl, timeout=2) |
| 182 | + |
| 183 | + rclpy.init() |
| 184 | + time.sleep(5) |
| 185 | + try: |
| 186 | + node = RallyControl() |
| 187 | + node.clear_list() |
| 188 | + clear_flag = node.clear_rally_event.wait(20) |
| 189 | + assert clear_flag, "Could not clear" |
| 190 | + node.send_rally(RALLY_0) |
| 191 | + |
| 192 | + send_flag = node.add_rally_event.wait(10) |
| 193 | + assert send_flag, "Could not send Rally" |
| 194 | + |
| 195 | + node.get_and_compare_rally(RALLY_0) |
| 196 | + send_flag = node.get_rally_event.wait(10) |
| 197 | + assert send_flag, "Wrong Rally point back" |
| 198 | + finally: |
| 199 | + rclpy.shutdown() |
| 200 | + yield |
0 commit comments