From 00d3af6fc5afbc1d95da6bd6e3f2006e3bdf56be Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 1 Aug 2022 15:00:22 +1000 Subject: [PATCH] autotest: handle terrain requests internally to autotest We will cache all required SRTM data within the autotest branch --- .gitignore | 1 + Tools/autotest/arducopter.py | 19 ++------- Tools/autotest/arduplane.py | 10 ++--- Tools/autotest/common.py | 83 ++++++++++++++++++++++++++++++------ Tools/autotest/pysim/util.py | 2 +- Tools/autotest/quadplane.py | 11 ++--- 6 files changed, 83 insertions(+), 43 deletions(-) diff --git a/.gitignore b/.gitignore index 9ed0f2d14f..58ef1b120e 100644 --- a/.gitignore +++ b/.gitignore @@ -36,6 +36,7 @@ *.vbrain *.vrx *.zip +!Tools/autotest/tilecache/**/*.zip *~ .*.swo .*.swp diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 1b54b30e1a..02b95074f3 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -488,9 +488,8 @@ class AutoTestCopter(AutoTest): self.change_mode('LOITER') - mavproxy = self.start_mavproxy() - self.wait_ready_to_arm(timeout=120*60) # terrain takes time - self.stop_mavproxy(mavproxy) + self.install_terrain_handlers_context() + self.wait_ready_to_arm() self.arm_vehicle() @@ -3225,10 +3224,7 @@ class AutoTestCopter(AutoTest): ex = None self.context_push() - # we must start mavproxy here as otherwise we can't get the - # terrain database tiles - this leads to random failures in - # CI! - mavproxy = self.start_mavproxy() + self.install_terrain_handlers_context() try: self.set_analog_rangefinder_parameters() @@ -3278,8 +3274,6 @@ class AutoTestCopter(AutoTest): self.disarm_vehicle(force=True) ex = e - self.stop_mavproxy(mavproxy) - self.context_pop() self.reboot_sitl() if ex is not None: @@ -5779,10 +5773,7 @@ class AutoTestCopter(AutoTest): ''' - # we must start mavproxy here as otherwise we can't get the - # terrain database tiles - this leads to random failures in - # CI! - mavproxy = self.start_mavproxy() + self.install_terrain_handlers_context() self.set_parameter("FS_GCS_ENABLE", 0) self.change_mode('GUIDED') @@ -5843,8 +5834,6 @@ class AutoTestCopter(AutoTest): self.wait_disarmed() - self.stop_mavproxy(mavproxy) - def PrecisionLoiterCompanion(self): """Use Companion PrecLand backend precision messages to loiter.""" diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index ce3aeb9dd0..2cb29250fc 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -2565,11 +2565,11 @@ function''' def TerrainMission(self): - mavproxy = self.start_mavproxy() + self.install_terrain_handlers_context() num_wp = self.load_mission("ap-terrain.txt") - self.wait_ready_to_arm(timeout=120*60) # time to get terrain + self.wait_ready_to_arm() self.arm_vehicle() global max_alt @@ -2586,8 +2586,6 @@ function''' self.fly_mission_waypoints(num_wp-1, mission_timeout=600) - self.stop_mavproxy(mavproxy) - if max_alt < 200: raise NotAchievedException("Did not follow terrain") @@ -2595,7 +2593,7 @@ function''' '''test AP_Terrain''' self.reboot_sitl() # we know the terrain height at CMAC - mavproxy = self.start_mavproxy() + self.install_terrain_handlers_context() self.wait_ready_to_arm() loc = self.mav.location() @@ -2636,8 +2634,6 @@ function''' raise NotAchievedException("Expected terrain height=%f got=%f" % (expected_terrain_height, report.terrain_height)) - self.stop_mavproxy(mavproxy) - def test_loiter_terrain(self): default_rad = self.get_parameter("WP_LOITER_RAD") self.set_parameters({ diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 04e0798f61..941e170fa4 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -30,6 +30,7 @@ import threading import enum from MAVProxy.modules.lib import mp_util +from MAVProxy.modules.lib import mp_elevation from pymavlink import mavparm from pymavlink import mavwp, mavutil, DFReader @@ -1542,6 +1543,17 @@ class AutoTest(ABC): self.last_sim_time_cached = 0 self.last_sim_time_cached_wallclock = 0 + # to autopilot we do not want to go to the internet for tiles, + # usually. Set this to False to gather tiles from internet in + # the cae there are new tiles required, then add them to the + # repo and set this back to false: + self.terrain_in_offline_mode = True + self.elevationmodel = mp_elevation.ElevationModel( + cachedir=util.reltopdir("Tools/autotest/tilecache/srtm"), + offline=self.terrain_in_offline_mode + ) + self.terrain_data_messages_sent = 0 # count of messages back + def __del__(self): if self.rc_thread is not None: self.progress("Joining RC thread in __del__") @@ -7029,6 +7041,8 @@ Also, ignores heartbeats not from our target system''' self.progress(fmt % ("**--tests_total_time--**", tests_total_time)) self.progress("mavproxy_start was called %u times" % (self.start_mavproxy_count,)) + self.progress("Supplied terrain data to autopilot in %u messages" % + (self.terrain_data_messages_sent,)) def send_statustext(self, text): if sys.version_info.major >= 3 and not isinstance(text, bytes): @@ -9510,6 +9524,61 @@ Also, ignores heartbeats not from our target system''' self.context_pop() self.reboot_sitl() + def install_terrain_handlers_context(self): + '''install a message handler into the current context which will + listen for an fulfill terrain requests from ArduPilot. Will + die if the data is not available - but + self.terrain_in_offline_mode can be set to true in the + constructor to change this behaviour + ''' + + def check_terrain_requests(mav, m): + if m.get_type() != 'TERRAIN_REQUEST': + return + self.progress("Processing TERRAIN_REQUEST (%s)" % + self.dump_message_verbose(m)) + # swiped from mav_terrain.py + for bit in range(56): + if m.mask & (1 << bit) == 0: + continue + + lat = m.lat * 1.0e-7 + lon = m.lon * 1.0e-7 + bit_spacing = m.grid_spacing * 4 + (lat, lon) = mp_util.gps_offset(lat, lon, + east=bit_spacing * (bit % 8), + north=bit_spacing * (bit // 8)) + data = [] + for i in range(4*4): + y = i % 4 + x = i // 4 + (lat2, lon2) = mp_util.gps_offset(lat, lon, + east=m.grid_spacing * y, + north=m.grid_spacing * x) + # if we are in online mode then we'll try to fetch + # from the internet into the cache dir: + for i in range(120): + alt = self.elevationmodel.GetElevation(lat2, lon2) + if alt is not None: + break + if self.terrain_in_offline_mode: + break + self.progress("No elevation data for (%f %f); retry" % + (lat2, lon2)) + time.sleep(1) + if alt is None: + # no data - we can't send the packet + raise ValueError("No elevation data for (%f %f)" % (lat2, lon2)) + data.append(int(alt)) + self.terrain_data_messages_sent += 1 + self.mav.mav.terrain_data_send(m.lat, + m.lon, + m.grid_spacing, + bit, + data) + + self.install_message_hook_context(check_terrain_requests) + def test_set_position_global_int(self, timeout=100): """Test set position message in guided mode.""" # Disable heading and yaw test on rover type @@ -9523,10 +9592,7 @@ Also, ignores heartbeats not from our target system''' test_heading = True test_yaw_rate = True - # we must start mavproxy here as otherwise we can't get the - # terrain database tiles - this leads to random failures in - # CI! - mavproxy = self.start_mavproxy() + self.install_terrain_handlers_context() self.set_parameter("FS_GCS_ENABLE", 0) self.change_mode("GUIDED") @@ -9732,8 +9798,6 @@ Also, ignores heartbeats not from our target system''' called_function=lambda plop, empty: send_yaw_rate( target_rate, None), minimum_duration=5) - self.stop_mavproxy(mavproxy) - self.progress("Getting back to home and disarm") self.do_RTL(distance_min=0, distance_max=wp_accuracy) self.disarm_vehicle() @@ -9750,10 +9814,7 @@ Also, ignores heartbeats not from our target system''' test_heading = True test_yaw_rate = True - # we must start mavproxy here as otherwise we can't get the - # terrain database tiles - this leads to random failures in - # CI! - mavproxy = self.start_mavproxy() + self.install_terrain_handlers_context() self.set_parameter("FS_GCS_ENABLE", 0) self.change_mode("GUIDED") @@ -10099,8 +10160,6 @@ Also, ignores heartbeats not from our target system''' minimum_duration=2 ) - self.stop_mavproxy(mavproxy) - self.progress("Getting back to home and disarm") self.do_RTL(distance_min=0, distance_max=wp_accuracy) self.disarm_vehicle() diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index 12237e21b6..4d6e099dc2 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -556,7 +556,7 @@ def start_MAVProxy_SITL(atype, aircraft = 'test.%s' % atype cmd.extend(['--aircraft', aircraft]) cmd.extend(options) - cmd.extend(['--default-modules', 'misc,terrain,wp,rally,fence,param,arm,mode,rc,cmdlong,output']) + cmd.extend(['--default-modules', 'misc,wp,rally,fence,param,arm,mode,rc,cmdlong,output']) print("PYTHONPATH: %s" % str(env['PYTHONPATH'])) print("Running: %s" % cmd_as_shell(cmd)) diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 6a853e6d72..ba216be702 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -290,12 +290,8 @@ class AutoTestQuadPlane(AutoTest): self.load_fence(fence) if self.mavproxy is not None: self.mavproxy.send('wp list\n') - # terrain can take a *long* time to load: - timeout = 120 - if include_terrain_timeout: - timeout *= 1000 # yes, *that* long - mavproxy = self.start_mavproxy() - self.wait_ready_to_arm(timeout=timeout) + self.install_terrain_handlers_context() + self.wait_ready_to_arm() self.arm_vehicle() self.change_mode('AUTO') self.wait_waypoint(1, 19, max_dist=60, timeout=1200) @@ -303,13 +299,12 @@ class AutoTestQuadPlane(AutoTest): self.wait_disarmed(timeout=120) # give quadplane a long time to land # wait for blood sample here self.set_current_waypoint(20) - self.wait_ready_to_arm(timeout=timeout) + self.wait_ready_to_arm() self.arm_vehicle() self.wait_waypoint(20, 34, max_dist=60, timeout=1200) self.wait_disarmed(timeout=120) # give quadplane a long time to land self.progress("Mission OK") - self.stop_mavproxy(mavproxy) def enum_state_name(self, enum_name, state, pretrim=None): e = mavutil.mavlink.enums[enum_name]