lengthen offboard tests

* land after offboard flying complete
* lengthen rostest time limit for tests (5 min ea)
This commit is contained in:
Anthony Lamping 2018-03-30 10:35:54 -04:00 committed by Daniel Agar
parent ac61a04cd9
commit 752d43d94c
7 changed files with 28 additions and 20 deletions

View File

@ -83,7 +83,7 @@ class MavrosOffboardAttctlTest(MavrosTestCommon):
self.att.body_rate = Vector3()
self.att.header = Header()
self.att.header.frame_id = "base_footprint"
self.att.orientation = Quaternion(*quaternion_from_euler(0.25, 0.25,
self.att.orientation = Quaternion(*quaternion_from_euler(-0.25, 0.5,
0))
self.att.thrust = 0.7
self.att.type_mask = 7 # ignore body rate
@ -102,9 +102,9 @@ class MavrosOffboardAttctlTest(MavrosTestCommon):
def test_attctl(self):
"""Test offboard attitude control"""
# boundary to cross
boundary_x = 5
boundary_y = 5
boundary_z = -5
boundary_x = 200
boundary_y = 100
boundary_z = 50
# make sure the simulation is ready to start the mission
self.wait_for_topics(60)
@ -119,14 +119,14 @@ class MavrosOffboardAttctlTest(MavrosTestCommon):
rospy.loginfo("attempting to cross boundary | x: {0}, y: {1}, z: {2}".
format(boundary_x, boundary_y, boundary_z))
# does it cross expected boundaries in 'timeout' seconds?
timeout = 12 # (int) seconds
timeout = 90 # (int) seconds
loop_freq = 2 # Hz
rate = rospy.Rate(loop_freq)
crossed = False
for i in xrange(timeout * loop_freq):
if (self.local_position.pose.position.x > boundary_x and
self.local_position.pose.position.z > boundary_y and
self.local_position.pose.position.y < boundary_z):
self.local_position.pose.position.y > boundary_y and
self.local_position.pose.position.z > boundary_z):
rospy.loginfo("boundary crossed | seconds: {0} of {1}".format(
i / loop_freq, timeout))
crossed = True
@ -143,6 +143,9 @@ class MavrosOffboardAttctlTest(MavrosTestCommon):
self.local_position.pose.position.y,
self.local_position.pose.position.z, timeout)))
self.set_mode("AUTO.LAND", 5)
self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND,
90, 0)
self.set_arm(False, 5)

View File

@ -166,12 +166,16 @@ class MavrosOffboardPosctlTest(MavrosTestCommon):
self.set_arm(True, 5)
rospy.loginfo("run mission")
positions = ((0, 0, 0), (2, 2, 2), (2, -2, 2), (-2, -2, 2), (2, 2, 2))
positions = ((0, 0, 0), (50, 50, 20), (50, -50, 20), (-50, -50, 20),
(0, 0, 20))
for i in xrange(len(positions)):
self.reach_position(positions[i][0], positions[i][1],
positions[i][2], 18)
positions[i][2], 30)
self.set_mode("AUTO.LAND", 5)
self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND,
45, 0)
self.set_arm(False, 5)

View File

@ -107,6 +107,7 @@ def read_plan_file(f):
else:
raise IOError("no mission items")
class MavrosMissionTest(MavrosTestCommon):
"""
Run a mission
@ -198,8 +199,8 @@ class MavrosMissionTest(MavrosTestCommon):
# advanced to next wp
(index < self.mission_wp.current_seq)
# end of mission
or (index == (mission_length - 1)
and self.mission_item_reached == index))
or (index == (mission_length - 1) and
self.mission_item_reached == index))
if reached:
rospy.loginfo(
@ -257,8 +258,8 @@ class MavrosMissionTest(MavrosTestCommon):
rospy.loginfo("run mission {0}".format(self.mission_name))
for index, waypoint in enumerate(wps):
# only check position for waypoints where this makes sense
if (waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT
or waypoint.frame == Waypoint.FRAME_GLOBAL):
if (waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT or
waypoint.frame == Waypoint.FRAME_GLOBAL):
alt = waypoint.z_alt
if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT:
alt += self.altitude.amsl - self.altitude.relative
@ -280,8 +281,8 @@ class MavrosMissionTest(MavrosTestCommon):
self.wait_for_vtol_state(transition, 60, index)
# after reaching position, wait for landing detection if applicable
if (waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND
or waypoint.command == mavutil.mavlink.MAV_CMD_NAV_LAND):
if (waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND or
waypoint.command == mavutil.mavlink.MAV_CMD_NAV_LAND):
self.wait_for_landed_state(
mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, 60, index)

View File

@ -13,6 +13,6 @@
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>
<!-- ROStest -->
<test test-name="mavros_flow_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" time-limit="120.0"/>
<test test-name="mavros_flow_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" time-limit="120.0"/>
<test test-name="mavros_flow_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" time-limit="300.0"/>
<test test-name="mavros_flow_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" time-limit="300.0"/>
</launch>

View File

@ -13,7 +13,7 @@
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>
<!-- ROStest -->
<test test-name="multicopter_box" pkg="px4" type="mission_test.py" time-limit="120.0" args="multirotor_box.plan"/>
<test test-name="multicopter_box" pkg="px4" type="mission_test.py" time-limit="300.0" args="multirotor_box.plan"/>
<test test-name="mission_test_new_1" pkg="px4" type="mission_test.py" time-limit="300.0" args="vtol_new_1.plan"/>
<test test-name="mission_test_new_2" pkg="px4" type="mission_test.py" time-limit="300.0" args="vtol_new_2.plan"/>
<test test-name="mission_test_old_1" pkg="px4" type="mission_test.py" time-limit="300.0" args="vtol_old_1.plan"/>

View File

@ -13,5 +13,5 @@
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>
<!-- ROStest -->
<test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" time-limit="120.0"/>
<test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" time-limit="300.0"/>
</launch>

View File

@ -13,5 +13,5 @@
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>
<!-- ROStest -->
<test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" time-limit="120.0"/>
<test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" time-limit="300.0"/>
</launch>