diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidance/rover-path-planning-fence.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceAuto/rover-path-planning-fence.txt similarity index 100% rename from Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidance/rover-path-planning-fence.txt rename to Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceAuto/rover-path-planning-fence.txt diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidance/rover-path-planning-mission.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceAuto/rover-path-planning-mission.txt similarity index 100% rename from Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidance/rover-path-planning-mission.txt rename to Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceAuto/rover-path-planning-mission.txt diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasier/rover-path-bendyruler-fence.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierAuto/rover-path-bendyruler-fence.txt similarity index 100% rename from Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasier/rover-path-bendyruler-fence.txt rename to Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierAuto/rover-path-bendyruler-fence.txt diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasier/rover-path-bendyruler-mission-easier.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierAuto/rover-path-bendyruler-mission-easier.txt similarity index 100% rename from Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasier/rover-path-bendyruler-mission-easier.txt rename to Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierAuto/rover-path-bendyruler-mission-easier.txt diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierGuided/rover-path-bendyruler-fence.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierGuided/rover-path-bendyruler-fence.txt new file mode 100644 index 0000000000..ace97c223c --- /dev/null +++ b/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 diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 9f4c8e08fa..46f1f68b59 100644 --- a/Tools/autotest/rover.py +++ b/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 - # twosquares is currently disabled because of the requirement to have an inclusion fence (which it doesn't have ATM) - # self.test_poly_fence_object_avoidance_guided_two_squares( - # target_system=target_system, - # target_component=target_component) + self.test_poly_fence_object_avoidance_guided_two_squares( + 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 - try: - self.set_parameters({ - "WENC_TYPE": 10, - "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.set_parameters({ + "WENC_TYPE": 10, + "EK3_ENABLE": 1, + "AHRS_EKF_TYPE": 3, + }) self.reboot_sitl() - if ex is not None: - raise ex + 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() 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(): - return - # bendy Ruler isn't as flexible as Dijkstra for planning, so - # it gets a simpler test: - self.test_poly_fence_object_avoidance_guided_bendy_ruler( - target_system=target_system, - target_component=target_component, - ) + 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": 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, diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 19b9f946d9..67cdbf54e4 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/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("Loading mission (%s)" % os.path.basename(filepath)) - wploader = mavwp.MAVWPLoader( + # self.progress("filepath: %s" % filepath) + self.progress("Loading {loaderclass.itemstype()} (%s)" % os.path.basename(filepath)) + 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: