Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

autotest: correct uploading of exclusion circles

This commit is contained in:
Peter Barker 2024-07-16 17:31:13 +10:00 committed by Peter Barker
parent 1de60a31b0
commit 8dcd3986c7
3 changed files with 215 additions and 247 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
Tools/autotest/arduplane.py
View File

@ -1608,18 +1608,14 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
# Test arming outside inclusion zone
self.progress("Test arming while vehicle outside of inclusion zone")
self.set_parameter("FENCE_TYPE", 4) # Enables polygon fence types
locs = [
self.upload_fences_from_locations([(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
mavutil.location(1.000, 1.000, 0, 0),
mavutil.location(1.000, 1.001, 0, 0),
mavutil.location(1.001, 1.001, 0, 0),
mavutil.location(1.001, 1.000, 0, 0)
]
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
[
locs
]
)
)])
self.delay_sim_time(10) # let fence check run so it loads-from-eeprom
self.do_fence_enable()
self.assert_fence_enabled()
@ -1637,12 +1633,9 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.001, 0, 0),
mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0),
]
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
[
locs
]
)
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, locs),
])
self.delay_sim_time(10) # let fence check run so it loads-from-eeprom
self.do_fence_enable()
self.assert_fence_enabled()
@ -3720,12 +3713,9 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.001, 0, 0),
mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0),
]
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
[
locs
]
)
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, locs),
])
self.delay_sim_time(1)
self.wait_ready_to_arm()
self.takeoff(alt=50)
@ -3783,12 +3773,9 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.003, 0, 0),
mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0),
]
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
[
locs
]
)
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, locs),
])
self.delay_sim_time(1)
self.wait_ready_to_arm()
self.takeoff(alt=50)

194
Tools/autotest/rover.py
View File

@ -4195,22 +4195,22 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
here = self.mav.location()
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
[
[ # east
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# east
self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, 50, 40), # tr
self.offset_location_ne(here, -50, 40), # tl,
], [ # over the top of the vehicle
]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# over the top of the vehicle
self.offset_location_ne(here, -50, -50), # bl
self.offset_location_ne(here, -50, 50), # br
self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, -50), # tl,
]
]
)
]),
])
self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz
self.drain_mav()
self.assert_fence_breached()
@ -4231,22 +4231,22 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
here = self.mav.location()
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
[
[ # east
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
# east
self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, 50, 40), # tr
self.offset_location_ne(here, -50, 40), # tl,
], [ # over the top of the vehicle
]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
# over the top of the vehicle
self.offset_location_ne(here, -50, -50), # bl
self.offset_location_ne(here, -50, 50), # br
self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, -50), # tl,
]
]
)
]),
])
self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz
self.drain_mav()
self.assert_fence_breached()
@ -4605,22 +4605,22 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
def test_poly_fence_reboot_survivability(self):
here = self.mav.location()
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
[
[ # east
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# east
self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, 50, 40), # tr
self.offset_location_ne(here, -50, 40), # tl,
], [ # over the top of the vehicle
]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# over the top of the vehicle
self.offset_location_ne(here, -50, -50), # bl
self.offset_location_ne(here, -50, 50), # br
self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, -50), # tl,
]
]
)
]),
])
self.reboot_sitl()
downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
downloaded_len = len(downloaded_items)
@ -4663,17 +4663,15 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
def test_poly_fence_inclusion_overlapping_inclusion_circles(self, here, target_system=1, target_component=1):
self.start_subtest("Overlapping circular inclusion")
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION,
[
{
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, {
"radius": 30,
"loc": self.offset_location_ne(here, -20, 0),
},
{
}),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, {
"radius": 30,
"loc": self.offset_location_ne(here, 20, 0),
},
}),
])
if self.mavproxy is not None:
# handy for getting pretty pictures
@ -4702,19 +4700,17 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
target_system=target_system,
target_component=target_component)
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
[
[
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
self.offset_location_ne(here, -40, -20), # tl
self.offset_location_ne(here, 50, -20), # tr
self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, -40, 20), # bl,
],
{
]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, {
"radius": 30,
"loc": self.offset_location_ne(here, -20, 0),
},
}),
])
self.delay_sim_time(5)
@ -4735,21 +4731,19 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
target_system=target_system,
target_component=target_component)
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
[
[
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
self.offset_location_ne(here, -20, -25), # tl
self.offset_location_ne(here, 50, -25), # tr
self.offset_location_ne(here, 50, 15), # br
self.offset_location_ne(here, -20, 15), # bl,
],
[
]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
self.offset_location_ne(here, 20, -20), # tl
self.offset_location_ne(here, -50, -20), # tr
self.offset_location_ne(here, -50, 20), # br
self.offset_location_ne(here, 20, 20), # bl,
],
]),
])
self.delay_sim_time(5)
@ -4772,23 +4766,25 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
def test_poly_fence_exclusion(self, here, target_system=1, target_component=1):
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
[
[ # east
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# east
self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, 50, 40), # tr
self.offset_location_ne(here, -50, 40), # tl,
], [ # west
]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# west
self.offset_location_ne(here, -50, -20), # tl
self.offset_location_ne(here, 50, -20), # tr
self.offset_location_ne(here, 50, -40), # br
self.offset_location_ne(here, -50, -40), # bl,
], {
]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, {
"radius": 30,
"loc": self.offset_location_ne(here, -60, 0),
},
}),
])
self.delay_sim_time(5)
if self.mavproxy is not None:
@ -5009,21 +5005,21 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
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")
here = self.mav.location()
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
[
[ # east
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# east
self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 10), # tl
self.offset_location_ne(here, 50, 30), # tr
self.offset_location_ne(here, -50, 40), # br,
],
[ # further east (and south
]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# further east (and south
self.offset_location_ne(here, -60, 60), # bl
self.offset_location_ne(here, 40, 70), # tl
self.offset_location_ne(here, 40, 90), # tr
self.offset_location_ne(here, -60, 80), # br,
],
]),
])
if self.mavproxy is not None:
self.mavproxy.send("fence list\n")
@ -5061,23 +5057,25 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
def test_poly_fence_avoidance_dont_breach_exclusion(self, target_system=1, target_component=1):
self.start_subtest("Ensure we stop before breaching an exclusion fence")
here = self.mav.location()
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
[
[ # east
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# east
self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, 50, 40), # tr
self.offset_location_ne(here, -50, 40), # tl,
], [ # west
]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# west
self.offset_location_ne(here, -50, -20), # tl
self.offset_location_ne(here, 50, -20), # tr
self.offset_location_ne(here, 50, -40), # br
self.offset_location_ne(here, -50, -40), # bl,
], {
]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, {
"radius": 30,
"loc": self.offset_location_ne(here, -60, 0),
},
}),
])
if self.mavproxy is not None:
self.mavproxy.send("fence list\n")
@ -6631,22 +6629,22 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
'''ensure MAV_CMD_DO_FENCE_ENABLE mavlink command works'''
here = self.mav.location()
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
[
[ # east
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# east
self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, 50, 40), # tr
self.offset_location_ne(here, -50, 40), # tl,
], [ # over the top of the vehicle
]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# over the top of the vehicle
self.offset_location_ne(here, -50, -50), # bl
self.offset_location_ne(here, -50, 50), # br
self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, -50), # tl,
]
]
)
]),
])
# enable:
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1)
@ -6847,17 +6845,15 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.progress("Ensure we can arm when we have an inclusion fence we are inside of")
here = self.mav.location()
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
[
[ # over the top of the vehicle
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
# over the top of the vehicle
self.offset_location_ne(here, -50, -50), # bl
self.offset_location_ne(here, -50, 50), # br
self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, -50), # tl,
]
]
)
]),
])
self.delay_sim_time(5)
self.wait_ready_to_arm()
@ -6869,17 +6865,15 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.progress("Now create a fence we are in breach of")
here = self.mav.location()
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
[
[ # over the top of the vehicle
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
# over the top of the vehicle
self.offset_location_ne(here, 20, 20), # bl
self.offset_location_ne(here, 20, 50), # br
self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, 20), # tl,
]
]
)
]),
])
self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False)
self.reboot_sitl()
@ -6890,34 +6884,30 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.clear_fence()
self.wait_ready_to_arm()
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
[
[ # over the top of the vehicle
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
# over the top of the vehicle
self.offset_location_ne(here, 20, 20), # bl
self.offset_location_ne(here, 20, 50), # br
self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, 20), # tl,
]
]
)
]),
])
self.reboot_sitl()
self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120)
self.clear_fence()
self.wait_ready_to_arm()
self.progress("Ensure we can arm after clearing polygon fence type enabled")
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
[
[ # over the top of the vehicle
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
# over the top of the vehicle
self.offset_location_ne(here, 20, 20), # bl
self.offset_location_ne(here, 20, 50), # br
self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, 20), # tl,
]
]
)
]),
])
self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120)
self.set_parameter('FENCE_TYPE', 2)
self.wait_ready_to_arm()

21
Tools/autotest/vehicle_test_suite.py
View File

@ -2106,10 +2106,8 @@ class TestSuite(ABC):
locs2.append(copy.copy(locs2[1]))
return self.roundtrip_fence_using_fencepoint_protocol(locs2)
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
[
locs
self.upload_fences_from_locations([
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, locs),
])
def send_reboot_command(self):
@ -11884,26 +11882,19 @@ Also, ignores heartbeats not from our target system'''
'''return mode vehicle should start in with default RC inputs set'''
return None
def upload_fences_from_locations(self,
vertex_type,
list_of_list_of_locs,
target_system=1,
target_component=1):
def upload_fences_from_locations(self, fences, target_system=1, target_component=1):
seq = 0
items = []
for locs in list_of_list_of_locs:
for (vertex_type, locs) in fences:
if isinstance(locs, dict):
# circular fence
if vertex_type == mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION:
v = mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION
else:
v = mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION
item = self.mav.mav.mission_item_int_encode(
target_system,
target_component,
seq, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL,
v,
vertex_type,
0, # current
0, # autocontinue
locs["radius"], # p1