Skip to content

Commit 03a8b13

Browse files
hmakelinhmakelin
authored
Merge pull request #13 from hmakelin/hmakelin-sitl-tests
Add preliminary SITL test using mavsdk
2 parents de0e403 + 05f1f57 commit 03a8b13

File tree

5 files changed

+196
-22
lines changed

5 files changed

+196
-22
lines changed

‎.gitignore

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,4 +19,7 @@ docs/_build/
1919
.coverage*
2020

2121
# Optional weights folder (LoFTR)
22-
weights/
22+
weights/
23+
24+
# SITL test log output
25+
test/sitl/output/

‎docs/pages/developer_guide.rst

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -301,6 +301,8 @@ See the provided ``loftr_params.yaml`` and ``superglue_params.yaml`` for example
301301

302302
Testing
303303
====================================================
304+
Unit & ROS 2 integration tests
305+
____________________________________________________
304306
First you must install the dev dependencies for your workspace:
305307

306308
.. code-block:: bash
@@ -324,6 +326,14 @@ to measure:
324326
python3 -m coverage run --branch --include */site-packages/gisnav/* src/gisnav/test/test_mock_gps_node.py
325327
python3 -m coverage report
326328
329+
SITL tests
330+
____________________________________________________
331+
SITL tests are under the ``test/sitl`` folder. They are simple Python scripts:
332+
333+
.. code-block:: bash
334+
335+
cd ~/px4_ros_com_ros2/src/gisnav/test/sitl
336+
python sitl_test_mock_gps_node.py
327337
328338
Documentation
329339
====================================================

‎requirements-dev.txt

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,14 @@
1+
# Sphinx
12
Sphinx==4.3.2
23
pydata-sphinx-theme
34
autodocsumm==0.2.7
45
docutils>=0.17
56
myst_parser==0.16.1
67
pygments==2.11.2 # make Sphinx highlighting work
8+
9+
# Unit & ROS 2 integration tests
710
pytest
8-
coverage
11+
coverage
12+
13+
# SITL tests
14+
mavsdk

‎test/assets/ksql_airport.plan

Lines changed: 39 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@
3636
"doJumpId": 2,
3737
"frame": 2,
3838
"params": [
39-
-90,
39+
-85,
4040
0,
4141
0,
4242
0,
@@ -47,7 +47,7 @@
4747
"type": "SimpleItem"
4848
},
4949
{
50-
"AMSLAltAboveTerrain": null,
50+
"AMSLAltAboveTerrain": 130,
5151
"Altitude": 130,
5252
"AltitudeMode": 1,
5353
"autoContinue": true,
@@ -59,14 +59,14 @@
5959
0,
6060
0,
6161
null,
62-
37.523640799999995,
63-
-122.2551034,
62+
37.5236489,
63+
-122.2551101,
6464
130
6565
],
6666
"type": "SimpleItem"
6767
},
6868
{
69-
"AMSLAltAboveTerrain": null,
69+
"AMSLAltAboveTerrain": 130,
7070
"Altitude": 130,
7171
"AltitudeMode": 1,
7272
"autoContinue": true,
@@ -78,14 +78,14 @@
7878
0,
7979
0,
8080
null,
81-
37.5243677,
82-
-122.256137,
81+
37.52371697,
82+
-122.25300218,
8383
130
8484
],
8585
"type": "SimpleItem"
8686
},
8787
{
88-
"AMSLAltAboveTerrain": null,
88+
"AMSLAltAboveTerrain": 130,
8989
"Altitude": 130,
9090
"AltitudeMode": 1,
9191
"autoContinue": true,
@@ -97,14 +97,14 @@
9797
0,
9898
0,
9999
null,
100-
37.5253963,
101-
-122.2548594,
100+
37.52176837,
101+
-122.2531202,
102102
130
103103
],
104104
"type": "SimpleItem"
105105
},
106106
{
107-
"AMSLAltAboveTerrain": null,
107+
"AMSLAltAboveTerrain": 130,
108108
"Altitude": 130,
109109
"AltitudeMode": 1,
110110
"autoContinue": true,
@@ -116,14 +116,14 @@
116116
0,
117117
0,
118118
null,
119-
37.5225601,
120-
-122.25639389999999,
119+
37.52190452,
120+
-122.25684311,
121121
130
122122
],
123123
"type": "SimpleItem"
124124
},
125125
{
126-
"AMSLAltAboveTerrain": null,
126+
"AMSLAltAboveTerrain": 130,
127127
"Altitude": 130,
128128
"AltitudeMode": 1,
129129
"autoContinue": true,
@@ -135,16 +135,35 @@
135135
0,
136136
0,
137137
null,
138-
37.5226161,
139-
-122.2543137,
138+
37.52347872,
139+
-122.25807692,
140140
130
141141
],
142142
"type": "SimpleItem"
143143
},
144144
{
145+
"AMSLAltAboveTerrain": 130,
146+
"Altitude": 130,
147+
"AltitudeMode": 1,
145148
"autoContinue": true,
146-
"command": 20,
149+
"command": 16,
147150
"doJumpId": 8,
151+
"frame": 3,
152+
"params": [
153+
0,
154+
0,
155+
0,
156+
null,
157+
37.52461893,
158+
-122.25681092,
159+
130
160+
],
161+
"type": "SimpleItem"
162+
},
163+
{
164+
"autoContinue": true,
165+
"command": 20,
166+
"doJumpId": 9,
148167
"frame": 2,
149168
"params": [
150169
0,
@@ -159,9 +178,9 @@
159178
}
160179
],
161180
"plannedHomePosition": [
162-
37.52364104097109,
163-
-122.25510340000132,
164-
2.7443970797736887
181+
37.5236489,
182+
-122.2551101,
183+
2.673999185660817
165184
],
166185
"vehicleType": 2,
167186
"version": 2

‎test/sitl/sitl_test_mock_gps_node.py

Lines changed: 136 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,136 @@
1+
#!/usr/bin/env python3
2+
"""Script for testing :class:`.MockGPSNode` in SITL simulation"""
3+
import sys
4+
import os
5+
import asyncio
6+
7+
from functools import partial
8+
9+
from mavsdk import System
10+
from mavsdk.log_files import LogFilesResult, LogFilesError
11+
12+
DOCKER_CONTAINERS = ['gisnav-docker_mapserver_1', 'gisnav-docker_px4-sitl_1']
13+
SYS_ADDR = 'udp://0.0.0.0:14550'
14+
MISSION_FILE = os.path.join(os.path.dirname(__file__), '../assets/ksql_airport.plan')
15+
MAVLINK_CONNECTION_TIMEOUT_SEC = 15
16+
PRE_FLIGHT_HEALTH_CHECK_TIMEOUT_SEC = 15
17+
LOG_OUTPUT_PATH = os.path.join(os.path.dirname(__file__), 'output')
18+
19+
20+
async def run():
21+
"""Runs the SITL test"""
22+
drone = System()
23+
24+
try:
25+
connect_mavlink_ = partial(connect_mavlink, drone)
26+
await asyncio.wait_for(connect_mavlink_(), timeout=MAVLINK_CONNECTION_TIMEOUT_SEC)
27+
except asyncio.TimeoutError as _:
28+
raise asyncio.TimeoutError(f'MAVLink connection attempt timed out at {MAVLINK_CONNECTION_TIMEOUT_SEC} seconds.')
29+
30+
# TODO: poll mapserver until a valid response is received
31+
32+
print(f'Importing mission from file {MISSION_FILE}')
33+
mission_import_data = await drone.mission_raw.import_qgroundcontrol_mission(MISSION_FILE)
34+
35+
print(f'Uploading mission...')
36+
await drone.mission_raw.upload_mission(mission_import_data.mission_items)
37+
38+
try:
39+
check_health_ = partial(check_health, drone)
40+
await asyncio.wait_for(check_health_(), timeout=PRE_FLIGHT_HEALTH_CHECK_TIMEOUT_SEC)
41+
except asyncio.TimeoutError as _:
42+
raise asyncio.TimeoutError(f'Pre-flight health checks failed.')
43+
44+
# TODO: figure out why this is needed
45+
await asyncio.sleep(5)
46+
47+
print(f'Arming...')
48+
await drone.action.arm()
49+
50+
print(f'Starting mission...')
51+
await drone.mission_raw.start_mission()
52+
53+
async for mission_progress in drone.mission_raw.mission_progress():
54+
if 0 < mission_progress.current < mission_progress.total:
55+
print(f'Mission progress: {mission_progress.current}/{mission_progress.total}')
56+
else:
57+
print(f'Mission finished.')
58+
break
59+
60+
print('Getting log entries...')
61+
entries = await drone.log_files.get_entries()
62+
entry = entries[0]
63+
64+
if not os.path.exists(LOG_OUTPUT_PATH):
65+
print(f'Creating missing {LOG_OUTPUT_PATH} directory...')
66+
os.makedirs(LOG_OUTPUT_PATH)
67+
filename = f'gisnav-sitl-test-log_{entry.date.replace(":", "-")}.ulog'
68+
output_path = os.path.join(LOG_OUTPUT_PATH, filename)
69+
70+
print(f'Downloading log {entry.id} from {entry.date} to {output_path}.')
71+
try:
72+
await drone.log_files.download_log_file(entry, output_path)
73+
except LogFilesError as e:
74+
if e is LogFilesResult.Result.INVALID_ARGUMENT:
75+
print(f'File {output_path} possibly already exists, could not download log. Exception was: "{e}"')
76+
else:
77+
raise
78+
79+
80+
async def connect_mavlink(drone):
81+
"""Connects to drone via MAVLink"""
82+
print(f'Connecting to drone at "{SYS_ADDR}"...')
83+
await drone.connect(system_address=SYS_ADDR)
84+
async for state in drone.core.connection_state():
85+
# This might take a while assuming the Docker containers have not had time to start yet
86+
if state.is_connected:
87+
print(f'Connection successful.')
88+
break
89+
90+
91+
async def check_health(drone):
92+
"""Goes through (pre-flight) health checks"""
93+
print("Going through health-checks...")
94+
async for health in drone.telemetry.health():
95+
if health.is_global_position_ok \
96+
and health.is_local_position_ok \
97+
and health.is_home_position_ok \
98+
and health.is_accelerometer_calibration_ok \
99+
and health.is_gyrometer_calibration_ok \
100+
and health.is_magnetometer_calibration_ok \
101+
and health.is_armable:
102+
print(f'Health-checks OK')
103+
break
104+
105+
106+
def setup():
107+
"""Sets up SITL testing environment
108+
109+
This most likely means starting supporting services with docker
110+
"""
111+
print('Starting SITL environment...')
112+
os.system('docker start ' + ' '.join(DOCKER_CONTAINERS))
113+
114+
115+
def cleanup():
116+
"""Cleans up after tests
117+
118+
Should clean up whatever was set in :meth:`.setup` (e.g. docker containers so that they won't be left running if
119+
the tests fail because of an error)
120+
"""
121+
print('Shutting down SITL environment...')
122+
for container in DOCKER_CONTAINERS:
123+
os.system(f'docker kill {container}')
124+
125+
126+
if __name__ == "__main__":
127+
setup()
128+
loop = asyncio.get_event_loop()
129+
try:
130+
loop.run_until_complete(run())
131+
except Exception as _:
132+
# TODO: handle exceptions here
133+
raise
134+
finally:
135+
cleanup()
136+
sys.exit(0)

0 commit comments

Comments
 (0)