mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
Peter Barker
Peter Barker
parent
d7bc9a420f
commit
2245399ff9
|
|
@ -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
|
||||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
Loading…
Reference in New Issue