autotest: tidy Rover autotests

autotest: tidy WheelEncoders test

autotest: rover: tidy PolyFenceObjectAvoidance code

autotest: add load_fence_using_mavwp

Also corrects the mission item types being returned by the mission-item-to-misison-item-int converter

autotest: fix Rover PolyFenceAvoidanceBendyRulerEasier test

these tests weren't being run becausethey were disabled based on support for loading fence items from QGC files.

autotest: fix Rover PolyFenceAvoidanceBendyRuler test

Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierGuided/rover-path-bendyruler-fence.txt

@@ -0,0 +1,11 @@
+QGC WPL 110
+0	0	0	5001	8.000000	0.000000	0.000000	0.000000	40.071766	-105.230202	0.000000	0
+1	0	0	5001	8.000000	0.000000	0.000000	0.000000	40.071014	-105.230247	0.000000	0
+2	0	0	5001	8.000000	0.000000	0.000000	0.000000	40.071014	-105.228821	0.000000	0
+3	0	0	5001	8.000000	0.000000	0.000000	0.000000	40.071609	-105.228867	0.000000	0
+4	0	0	5001	8.000000	0.000000	0.000000	0.000000	40.071602	-105.228172	0.000000	0
+5	0	0	5001	8.000000	0.000000	0.000000	0.000000	40.070858	-105.227982	0.000000	0
+6	0	0	5001	8.000000	0.000000	0.000000	0.000000	40.070789	-105.226219	0.000000	0
+7	0	0	5001	8.000000	0.000000	0.000000	0.000000	40.072453	-105.226379	0.000000	0
+8	0	0	5004	20.000000	0.000000	0.000000	0.000000	40.071609	-105.228172	0.000000	0
+9	0	0	5004	20.000000	0.000000	0.000000	0.000000	40.071625	-105.227982	0.000000	0

Tools/autotest/rover.py

@@ -4794,20 +4794,20 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
         self.wait_servo_channel_value(3, self.get_parameter("RC3_TRIM", 5), timeout=10)
         self.wait_disarmed()
 
-    def test_poly_fence_object_avoidance_guided(self, target_system=1, target_component=1):
+    def PolyFenceObjectAvoidanceGuided(self, target_system=1, target_component=1):
+        '''PolyFence object avoidance tests - guided mode'''
         if not self.mavproxy_can_do_mision_item_protocols():
             return
 
         self.test_poly_fence_object_avoidance_guided_pathfinding(
             target_system=target_system,
             target_component=target_component)
-        return
+        self.test_poly_fence_object_avoidance_guided_two_squares(
-        # twosquares is currently disabled because of the requirement to have an inclusion fence (which it doesn't have ATM)
+            target_system=target_system,
-        # self.test_poly_fence_object_avoidance_guided_two_squares(
+            target_component=target_component)
-        #     target_system=target_system,
-        #     target_component=target_component)
 
-    def test_poly_fence_object_avoidance_auto(self, target_system=1, target_component=1):
+    def PolyFenceObjectAvoidanceAuto(self, target_system=1, target_component=1):
+        '''PolyFence object avoidance tests - auto mode'''
         mavproxy = self.start_mavproxy()
         self.load_fence_using_mavproxy(mavproxy, "rover-path-planning-fence.txt")
         self.stop_mavproxy(mavproxy)
@@ -4823,8 +4823,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
         self.wait_ready_to_arm()
         self.arm_vehicle()
         self.set_parameter("FENCE_ENABLE", 1)
-        if self.mavproxy is not None:
-            self.mavproxy.send("fence list\n")
         # target_loc is copied from the mission file
         target_loc = mavutil.location(40.073799, -105.229156)
         self.wait_location(target_loc, height_accuracy=None, timeout=300)
@@ -4872,41 +4870,32 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
 
     def WheelEncoders(self):
         '''make sure wheel encoders are generally working'''
-        ex = None
+        self.set_parameters({
-        try:
+            "WENC_TYPE": 10,
-            self.set_parameters({
+            "EK3_ENABLE": 1,
-                "WENC_TYPE": 10,
+            "AHRS_EKF_TYPE": 3,
-                "EK3_ENABLE": 1,
+        })
-                "AHRS_EKF_TYPE": 3,
-            })
-            self.reboot_sitl()
-            self.change_mode("LOITER")
-            self.wait_ready_to_arm()
-            self.change_mode("MANUAL")
-            self.arm_vehicle()
-            self.set_rc(3, 1600)
-
-            m = self.assert_receive_message('WHEEL_DISTANCE', timeout=5)
-
-            tstart = self.get_sim_time()
-            while True:
-                if self.get_sim_time_cached() - tstart > 10:
-                    break
-                dist_home = self.distance_to_home(use_cached_home=True)
-                m = self.mav.messages.get("WHEEL_DISTANCE")
-                delta = abs(m.distance[0] - dist_home)
-                self.progress("dist-home=%f wheel-distance=%f delta=%f" %
-                              (dist_home, m.distance[0], delta))
-                if delta > 5:
-                    raise NotAchievedException("wheel distance incorrect")
-            self.disarm_vehicle()
-        except Exception as e:
-            self.print_exception_caught(e)
-            self.disarm_vehicle()
-            ex = e
         self.reboot_sitl()
-        if ex is not None:
+        self.change_mode("LOITER")
-            raise ex
+        self.wait_ready_to_arm()
+        self.change_mode("MANUAL")
+        self.arm_vehicle()
+        self.set_rc(3, 1600)
+
+        m = self.assert_receive_message('WHEEL_DISTANCE', timeout=5)
+
+        tstart = self.get_sim_time()
+        while True:
+            if self.get_sim_time_cached() - tstart > 10:
+                break
+            dist_home = self.distance_to_home(use_cached_home=True)
+            m = self.mav.messages.get("WHEEL_DISTANCE")
+            delta = abs(m.distance[0] - dist_home)
+            self.progress("dist-home=%f wheel-distance=%f delta=%f" %
+                          (dist_home, m.distance[0], delta))
+            if delta > 5:
+                raise NotAchievedException("wheel distance incorrect")
+        self.disarm_vehicle()
 
     def test_poly_fence_object_avoidance_guided_two_squares(self, target_system=1, target_component=1):
         self.start_subtest("Ensure we can steer around obstacles in guided mode")
@@ -5018,159 +5007,86 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
 
         self.disarm_vehicle()
 
-    def test_poly_fence_object_avoidance_guided_bendy_ruler(self, target_system=1, target_component=1):
-        if not self.mavproxy_can_do_mision_item_protocols():
-            return
-        self.load_fence("rover-path-bendyruler-fence.txt")
-        self.context_push()
-        ex = None
-        try:
-            self.set_parameters({
-                "AVOID_ENABLE": 3,
-                "OA_TYPE": 1,
-                "OA_LOOKAHEAD": 50,
-            })
-            self.reboot_sitl()
-            self.change_mode('GUIDED')
-            self.wait_ready_to_arm()
-            self.arm_vehicle()
-            self.set_parameters({
-                "FENCE_ENABLE": 1,
-                "WP_RADIUS": 5,
-            })
-            if self.mavproxy is not None:
-                self.mavproxy.send("fence list\n")
-            target_loc = mavutil.location(40.071060, -105.227734, 0, 0)
-            self.send_guided_mission_item(target_loc,
-                                          target_system=target_system,
-                                          target_component=target_component)
-            # FIXME: we don't get within WP_RADIUS of our target?!
-            self.wait_location(target_loc, timeout=300, accuracy=15)
-            self.do_RTL(timeout=300)
-            self.disarm_vehicle()
-        except Exception as e:
-            self.print_exception_caught(e)
-            ex = e
-        self.context_pop()
-        self.disarm_vehicle()
-        self.reboot_sitl()
-        if ex is not None:
-            raise ex
-
-    def PolyFenceObjectAvoidanceBendyRulerEasier(self, target_system=1, target_component=1):
-        '''PolyFence object avoidance tests - easier bendy ruler test'''
-        if not self.mavproxy_can_do_mision_item_protocols():
-            return
-        self.test_poly_fence_object_avoidance_auto_bendy_ruler_easier(
-            target_system=target_system, target_component=target_component)
-        self.test_poly_fence_object_avoidance_guided_bendy_ruler_easier(
-            target_system=target_system, target_component=target_component)
-
-    def test_poly_fence_object_avoidance_guided_bendy_ruler_easier(self, target_system=1, target_component=1):
-        '''finish-line issue means we can't complete the harder one.  This
-        test can go away once we've nailed that one.  The only
-        difference here is the target point.
-        '''
-        if not self.mavproxy_can_do_mision_item_protocols():
-            return
-        self.load_fence("rover-path-bendyruler-fence.txt")
-        self.context_push()
-        ex = None
-        try:
-            self.set_parameters({
-                "AVOID_ENABLE": 3,
-                "OA_TYPE": 1,
-                "OA_LOOKAHEAD": 50,
-            })
-            self.reboot_sitl()
-            self.change_mode('GUIDED')
-            self.wait_ready_to_arm()
-            self.arm_vehicle()
-            self.set_parameters({
-                "FENCE_ENABLE": 1,
-                "WP_RADIUS": 5,
-            })
-            if self.mavproxy is not None:
-                self.mavproxy.send("fence list\n")
-            target_loc = mavutil.location(40.071260, -105.227000, 0, 0)
-            self.send_guided_mission_item(target_loc,
-                                          target_system=target_system,
-                                          target_component=target_component)
-            # FIXME: we don't get within WP_RADIUS of our target?!
-            self.wait_location(target_loc, timeout=300, accuracy=15)
-            self.do_RTL(timeout=300)
-            self.disarm_vehicle()
-        except Exception as e:
-            self.print_exception_caught(e)
-            ex = e
-        self.context_pop()
-        self.disarm_vehicle()
-        self.reboot_sitl()
-        if ex is not None:
-            raise ex
-
-    def test_poly_fence_object_avoidance_auto_bendy_ruler_easier(self, target_system=1, target_component=1):
-        '''finish-line issue means we can't complete the harder one.  This
-        test can go away once we've nailed that one.  The only
-        difference here is the target point.
-        '''
-        if not self.mavproxy_can_do_mision_item_protocols():
-            return
-
-        self.load_fence("rover-path-bendyruler-fence.txt")
-        self.load_mission("rover-path-bendyruler-mission-easier.txt")
-        self.context_push()
-        ex = None
-        try:
-            self.set_parameters({
-                "AVOID_ENABLE": 3,
-                "OA_TYPE": 1,
-                "OA_LOOKAHEAD": 50,
-            })
-            self.reboot_sitl()
-            self.change_mode('AUTO')
-            self.wait_ready_to_arm()
-            self.arm_vehicle()
-            self.set_parameters({
-                "FENCE_ENABLE": 1,
-                "WP_RADIUS": 5,
-            })
-            if self.mavproxy is not None:
-                self.mavproxy.send("fence list\n")
-            target_loc = mavutil.location(40.071260, -105.227000, 0, 0)
-            # target_loc is copied from the mission file
-            self.wait_location(target_loc, timeout=300)
-            # mission has RTL as last item
-            self.wait_distance_to_home(3, 7, timeout=300)
-            self.disarm_vehicle()
-        except Exception as e:
-            self.print_exception_caught(e)
-            ex = e
-        self.context_pop()
-        self.disarm_vehicle()
-        self.reboot_sitl()
-        if ex is not None:
-            raise ex
-
-    def PolyFenceObjectAvoidance(self, target_system=1, target_component=1):
-        '''PolyFence object avoidance tests'''
-        self.test_poly_fence_object_avoidance_auto(
-            target_system=target_system,
-            target_component=target_component)
-        self.test_poly_fence_object_avoidance_guided(
-            target_system=target_system,
-            target_component=target_component)
-
     def PolyFenceObjectAvoidanceBendyRuler(self, target_system=1, target_component=1):
         '''PolyFence object avoidance tests - bendy ruler'''
-        if not self.mavproxy_can_do_mision_item_protocols():
+        self.load_fence_using_mavwp("rover-path-bendyruler-fence.txt")
-            return
+        self.set_parameters({
-        # bendy Ruler isn't as flexible as Dijkstra for planning, so
+            "AVOID_ENABLE": 3,
-        # it gets a simpler test:
+            "OA_TYPE": 1,
-        self.test_poly_fence_object_avoidance_guided_bendy_ruler(
+            "FENCE_ENABLE": 1,
-            target_system=target_system,
+            "WP_RADIUS": 5,
-            target_component=target_component,
+        })
-        )
+        self.reboot_sitl()
+        self.set_parameters({
+            "OA_BR_LOOKAHEAD": 50,
+        })
+        self.change_mode('GUIDED')
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        target_loc = mavutil.location(40.071060, -105.227734, 1584, 0)
+        self.send_guided_mission_item(target_loc,
+                                      target_system=target_system,
+                                      target_component=target_component)
+        # FIXME: we don't get within WP_RADIUS of our target?!
+        self.wait_location(target_loc, timeout=300, accuracy=15)
+        self.do_RTL(timeout=300)
+        self.disarm_vehicle()
+
+    def PolyFenceObjectAvoidanceBendyRulerEasierGuided(self, target_system=1, target_component=1):
+        '''finish-line issue means we can't complete the harder one.  This
+        test can go away once we've nailed that one.  The only
+        difference here is the target point.
+        '''
+        self.load_fence_using_mavwp("rover-path-bendyruler-fence.txt")
+        self.set_parameters({
+            "AVOID_ENABLE": 3,
+            "OA_TYPE": 1,
+            "FENCE_ENABLE": 1,
+            "WP_RADIUS": 5,
+        })
+        self.reboot_sitl()
+        self.set_parameters({
+            "OA_BR_LOOKAHEAD": 60,
+        })
+        self.change_mode('GUIDED')
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        target_loc = mavutil.location(40.071260, -105.227000, 1584, 0)
+        self.send_guided_mission_item(target_loc,
+                                      target_system=target_system,
+                                      target_component=target_component)
+        # FIXME: we don't get within WP_RADIUS of our target?!
+        self.wait_location(target_loc, timeout=300, accuracy=15)
+        self.do_RTL(timeout=300)
+        self.disarm_vehicle()
+
+    def PolyFenceObjectAvoidanceBendyRulerEasierAuto(self, target_system=1, target_component=1):
+        '''finish-line issue means we can't complete the harder one.  This
+        test can go away once we've nailed that one.  The only
+        difference here is the target point.
+        '''
+        self.load_fence_using_mavwp("rover-path-bendyruler-fence.txt")
+        self.load_mission("rover-path-bendyruler-mission-easier.txt")
+
+        self.set_parameters({
+            "AVOID_ENABLE": 3,
+            "OA_TYPE": 1,  # BendyRuler
+            "FENCE_ENABLE": 1,
+            "WP_RADIUS": 5,
+        })
+        self.reboot_sitl()
+        self.set_parameters({
+            "OA_BR_LOOKAHEAD": 60,
+        })
+        self.change_mode('AUTO')
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        target_loc = mavutil.location(40.071260, -105.227000, 1584, 0)
+        # target_loc is copied from the mission file
+        self.wait_location(target_loc, timeout=300)
+        # mission has RTL as last item
+        self.wait_distance_to_home(3, 7, timeout=300)
+        self.disarm_vehicle()
 
     def test_scripting_simple_loop(self):
         self.start_subtest("Scripting simple loop")
@@ -6861,10 +6777,12 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
             self.SkidSteer,
             self.PolyFence,
             self.PolyFenceAvoidance,
-            self.PolyFenceObjectAvoidance,
+            self.PolyFenceObjectAvoidanceAuto,
+            self.PolyFenceObjectAvoidanceGuided,
             self.PolyFenceObjectAvoidanceBendyRuler,
             self.SendToComponents,
-            self.PolyFenceObjectAvoidanceBendyRulerEasier,
+            self.PolyFenceObjectAvoidanceBendyRulerEasierGuided,
+            self.PolyFenceObjectAvoidanceBendyRulerEasierAuto,
             self.SlewRate,
             self.Scripting,
             self.ScriptingSteeringAndThrottle,

Tools/autotest/vehicle_test_suite.py

@@ -2112,6 +2112,14 @@ class TestSuite(ABC):
             (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, locs),
         ])
 
+    def load_fence_using_mavwp(self, filename):
+        filepath = os.path.join(testdir, self.current_test_name_directory, filename)
+        if not os.path.exists(filepath):
+            filepath = self.generic_mission_filepath_for_filename(filename)
+        self.progress("Loading fence from (%s)" % str(filepath))
+        items = self.mission_item_protocol_items_from_filepath(mavwp.MissionItemProtocol_Fence, filepath)
+        self.check_fence_upload_download(items)
+
     def send_reboot_command(self):
         self.mav.mav.command_long_send(self.sysid_thismav(),
                                        1,
@@ -5124,7 +5132,7 @@ class TestSuite(ABC):
             os.path.join(testdir, self.current_test_name_directory, filename),
             strict=strict)
 
-    def wp_to_mission_item_int(self, wp):
+    def wp_to_mission_item_int(self, wp, mission_type):
         '''convert a MISSION_ITEM to a MISSION_ITEM_INT. We always send as
            MISSION_ITEM_INT to give cm level accuracy
            Swiped from mavproxy_wp.py
@@ -5145,19 +5153,35 @@ class TestSuite(ABC):
             wp.param4,
             int(wp.x*1.0e7),
             int(wp.y*1.0e7),
-            wp.z)
+            wp.z,
+            mission_type,
+        )
         return wp_int
 
-    def mission_from_filepath(self, filepath, target_system=1, target_component=1):
+    def mission_item_protocol_items_from_filepath(self,
+                                                  loaderclass,
+                                                  filepath,
+                                                  target_system=1,
+                                                  target_component=1,
+                                                  ):
         '''returns a list of mission-item-ints from filepath'''
-        self.progress("filepath: %s" % filepath)
+        # self.progress("filepath: %s" % filepath)
-        self.progress("Loading mission (%s)" % os.path.basename(filepath))
+        self.progress("Loading {loaderclass.itemstype()} (%s)" % os.path.basename(filepath))
-        wploader = mavwp.MAVWPLoader(
+        wploader = loaderclass(
             target_system=target_system,
             target_component=target_component
         )
         wploader.load(filepath)
-        return [self.wp_to_mission_item_int(x) for x in wploader.wpoints]
+        return [self.wp_to_mission_item_int(x, wploader.mav_mission_type()) for x in wploader.wpoints]  # noqa:502
+
+    def mission_from_filepath(self, filepath, target_system=1, target_component=1):
+        '''returns a list of mission-item-ints from filepath'''
+        return self.mission_item_protocol_items_from_filepath(
+            mavwp.MAVWPLoader,
+            filepath,
+            target_system=target_system,
+            target_component=target_component,
+        )
 
     def sitl_home_string_from_mission(self, filename):
         '''return a string of the form "lat,lng,yaw,alt" from the home
@@ -5178,6 +5202,10 @@ class TestSuite(ABC):
             os.path.join(testdir, self.current_test_name_directory, filename)
         )
 
+    def get_home_location_from_mission(self, filename):
+        (home_lat, home_lon, home_alt, heading) = self.get_home_tuple_from_mission("rover-path-planning-mission.txt")
+        return mavutil.location(home_lat, home_lon)
+
     def get_home_tuple_from_mission_filepath(self, filepath):
         '''gets item 0 from the mission file, returns a tuple suitable for
         passing to customise_SITL_commandline as --home.  Yaw will be
@@ -9078,7 +9106,7 @@ Also, ignores heartbeats not from our target system'''
                 raise NotAchievedException("received request for item from wrong mission type")
 
             if items[m.seq].mission_type != mission_type:
-                raise NotAchievedException("supplied item not of correct mission type")
+                raise NotAchievedException(f"supplied item not of correct mission type (want={mission_type} got={items[m.seq].mission_type}")  # noqa:501
             if items[m.seq].target_system != target_system:
                 raise NotAchievedException("supplied item not of correct target system")
             if items[m.seq].target_component != target_component: