Browse Source
-created a test base class to centralize redundant methods among the different tests -added mission waypoint list topic listener (this also helps make sure the simulation is ready) -check number of mission waypoints in FCU against mission -increase time for mavros topics to be ready from 30 to 60 seconds -reduce position check loop rates -clean up logging -support QGC plan for mission file format, see #8619 -vehicle is an arg for mission test launch file, working toward other airframes -Jenkins: fix vtol vehicle arg value -get MAV_TYPE param and use FW radius for pure fixed-wing mission position check -remove unused vehicle arg from test in multiple tests launch, clearing runtime warningsbg
Anthony Lamping
7 years ago
committed by
Daniel Agar
8 changed files with 529 additions and 591 deletions
@ -0,0 +1,375 @@ |
|||||||
|
#!/usr/bin/env python2 |
||||||
|
from __future__ import division |
||||||
|
|
||||||
|
import unittest |
||||||
|
import rospy |
||||||
|
import math |
||||||
|
from geometry_msgs.msg import PoseStamped |
||||||
|
from mavros_msgs.msg import Altitude, ExtendedState, HomePosition, State, \ |
||||||
|
WaypointList |
||||||
|
from mavros_msgs.srv import CommandBool, ParamGet, SetMode, WaypointClear, \ |
||||||
|
WaypointPush |
||||||
|
from sensor_msgs.msg import NavSatFix |
||||||
|
|
||||||
|
|
||||||
|
class MavrosTestCommon(unittest.TestCase): |
||||||
|
# dictionaries correspond to mavros ExtendedState msg |
||||||
|
LAND_STATES = { |
||||||
|
0: 'UNDEFINED', |
||||||
|
1: 'ON_GROUND', |
||||||
|
2: 'IN_AIR', |
||||||
|
3: 'TAKEOFF', |
||||||
|
4: 'LANDING' |
||||||
|
} |
||||||
|
VTOL_STATES = { |
||||||
|
0: 'VTOL UNDEFINED', |
||||||
|
1: 'VTOL MC-to-FW', |
||||||
|
2: 'VTOL FW-to-MC', |
||||||
|
3: 'VTOL MC', |
||||||
|
4: 'VTOL FW' |
||||||
|
} |
||||||
|
|
||||||
|
def __init__(self, *args): |
||||||
|
super(MavrosTestCommon, self).__init__(*args) |
||||||
|
|
||||||
|
def setUp(self): |
||||||
|
self.altitude = Altitude() |
||||||
|
self.extended_state = ExtendedState() |
||||||
|
self.global_position = NavSatFix() |
||||||
|
self.home_position = HomePosition() |
||||||
|
self.local_position = PoseStamped() |
||||||
|
self.mission_wp = WaypointList() |
||||||
|
self.state = State() |
||||||
|
self.mav_type = None |
||||||
|
|
||||||
|
self.sub_topics_ready = { |
||||||
|
key: False |
||||||
|
for key in [ |
||||||
|
'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos', |
||||||
|
'mission_wp', 'state' |
||||||
|
] |
||||||
|
} |
||||||
|
|
||||||
|
# ROS services |
||||||
|
service_timeout = 30 |
||||||
|
rospy.loginfo("waiting for ROS services") |
||||||
|
try: |
||||||
|
rospy.wait_for_service('mavros/param/get', service_timeout) |
||||||
|
rospy.wait_for_service('mavros/cmd/arming', service_timeout) |
||||||
|
rospy.wait_for_service('mavros/mission/push', service_timeout) |
||||||
|
rospy.wait_for_service('mavros/mission/clear', service_timeout) |
||||||
|
rospy.wait_for_service('mavros/set_mode', service_timeout) |
||||||
|
rospy.loginfo("ROS services are up") |
||||||
|
except rospy.ROSException: |
||||||
|
self.fail("failed to connect to services") |
||||||
|
self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet) |
||||||
|
self.set_arming_srv = rospy.ServiceProxy('/mavros/cmd/arming', |
||||||
|
CommandBool) |
||||||
|
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) |
||||||
|
self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', |
||||||
|
WaypointClear) |
||||||
|
self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', |
||||||
|
WaypointPush) |
||||||
|
|
||||||
|
# ROS subscribers |
||||||
|
self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, |
||||||
|
self.altitude_callback) |
||||||
|
self.ext_state_sub = rospy.Subscriber('mavros/extended_state', |
||||||
|
ExtendedState, |
||||||
|
self.extended_state_callback) |
||||||
|
self.global_pos_sub = rospy.Subscriber('mavros/global_position/global', |
||||||
|
NavSatFix, |
||||||
|
self.global_position_callback) |
||||||
|
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', |
||||||
|
HomePosition, |
||||||
|
self.home_position_callback) |
||||||
|
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', |
||||||
|
PoseStamped, |
||||||
|
self.local_position_callback) |
||||||
|
self.mission_wp_sub = rospy.Subscriber( |
||||||
|
'mavros/mission/waypoints', WaypointList, self.mission_wp_callback) |
||||||
|
self.state_sub = rospy.Subscriber('mavros/state', State, |
||||||
|
self.state_callback) |
||||||
|
|
||||||
|
# |
||||||
|
# Callback functions |
||||||
|
# |
||||||
|
def altitude_callback(self, data): |
||||||
|
self.altitude = data |
||||||
|
|
||||||
|
# amsl has been observed to be nan while other fields are valid |
||||||
|
if not self.sub_topics_ready['alt'] and not math.isnan(data.amsl): |
||||||
|
self.sub_topics_ready['alt'] = True |
||||||
|
|
||||||
|
def extended_state_callback(self, data): |
||||||
|
if self.extended_state.vtol_state != data.vtol_state: |
||||||
|
rospy.loginfo("VTOL state changed from {0} to {1}".format( |
||||||
|
self.VTOL_STATES.get(self.extended_state.vtol_state), |
||||||
|
self.VTOL_STATES.get(data.vtol_state))) |
||||||
|
|
||||||
|
if self.extended_state.landed_state != data.landed_state: |
||||||
|
rospy.loginfo("landed state changed from {0} to {1}".format( |
||||||
|
self.LAND_STATES.get(self.extended_state.landed_state), |
||||||
|
self.LAND_STATES.get(data.landed_state))) |
||||||
|
|
||||||
|
self.extended_state = data |
||||||
|
|
||||||
|
if not self.sub_topics_ready['ext_state']: |
||||||
|
self.sub_topics_ready['ext_state'] = True |
||||||
|
|
||||||
|
def global_position_callback(self, data): |
||||||
|
self.global_position = data |
||||||
|
|
||||||
|
if not self.sub_topics_ready['global_pos']: |
||||||
|
self.sub_topics_ready['global_pos'] = True |
||||||
|
|
||||||
|
def home_position_callback(self, data): |
||||||
|
self.home_position = data |
||||||
|
|
||||||
|
if not self.sub_topics_ready['home_pos']: |
||||||
|
self.sub_topics_ready['home_pos'] = True |
||||||
|
|
||||||
|
def local_position_callback(self, data): |
||||||
|
self.local_position = data |
||||||
|
|
||||||
|
if not self.sub_topics_ready['local_pos']: |
||||||
|
self.sub_topics_ready['local_pos'] = True |
||||||
|
|
||||||
|
def mission_wp_callback(self, data): |
||||||
|
if self.mission_wp.current_seq != data.current_seq: |
||||||
|
rospy.loginfo("current mission waypoint sequence updated: {0}". |
||||||
|
format(data.current_seq)) |
||||||
|
|
||||||
|
self.mission_wp = data |
||||||
|
|
||||||
|
if not self.sub_topics_ready['mission_wp']: |
||||||
|
self.sub_topics_ready['mission_wp'] = True |
||||||
|
|
||||||
|
def state_callback(self, data): |
||||||
|
if self.state.armed != data.armed: |
||||||
|
rospy.loginfo("armed state changed from {0} to {1}".format( |
||||||
|
self.state.armed, data.armed)) |
||||||
|
|
||||||
|
if self.state.mode != data.mode: |
||||||
|
rospy.loginfo("mode changed from {0} to {1}".format( |
||||||
|
self.state.mode, data.mode)) |
||||||
|
|
||||||
|
self.state = data |
||||||
|
|
||||||
|
# mavros publishes a disconnected state message on init |
||||||
|
if not self.sub_topics_ready['state'] and data.connected: |
||||||
|
self.sub_topics_ready['state'] = True |
||||||
|
|
||||||
|
# |
||||||
|
# Helper methods |
||||||
|
# |
||||||
|
def set_arm(self, arm, timeout): |
||||||
|
"""arm: True to arm or False to disarm, timeout(int): seconds""" |
||||||
|
rospy.loginfo("setting FCU arm: {0}".format(arm)) |
||||||
|
old_arm = self.state.armed |
||||||
|
loop_freq = 1 # Hz |
||||||
|
rate = rospy.Rate(loop_freq) |
||||||
|
arm_set = False |
||||||
|
for i in xrange(timeout * loop_freq): |
||||||
|
if self.state.armed == arm: |
||||||
|
arm_set = True |
||||||
|
rospy.loginfo( |
||||||
|
"set arm success | new arm: {0}, old arm: {1} | seconds: {2} of {3}". |
||||||
|
format(arm, old_arm, i / loop_freq, timeout)) |
||||||
|
break |
||||||
|
else: |
||||||
|
try: |
||||||
|
res = self.set_arming_srv(arm) |
||||||
|
if not res.success: |
||||||
|
rospy.logerr("failed to send arm command") |
||||||
|
except rospy.ServiceException as e: |
||||||
|
rospy.logerr(e) |
||||||
|
|
||||||
|
rate.sleep() |
||||||
|
|
||||||
|
self.assertTrue(arm_set, ( |
||||||
|
"failed to set arm | new arm: {0}, old arm: {1} | timeout(seconds): {2}". |
||||||
|
format(arm, old_arm, timeout))) |
||||||
|
|
||||||
|
def set_mode(self, mode, timeout): |
||||||
|
"""mode: PX4 mode string, timeout(int): seconds""" |
||||||
|
rospy.loginfo("setting FCU mode: {0}".format(mode)) |
||||||
|
old_mode = self.state.mode |
||||||
|
loop_freq = 1 # Hz |
||||||
|
rate = rospy.Rate(loop_freq) |
||||||
|
mode_set = False |
||||||
|
for i in xrange(timeout * loop_freq): |
||||||
|
if self.state.mode == mode: |
||||||
|
mode_set = True |
||||||
|
rospy.loginfo( |
||||||
|
"set mode success | new mode: {0}, old mode: {1} | seconds: {2} of {3}". |
||||||
|
format(mode, old_mode, i / loop_freq, timeout)) |
||||||
|
break |
||||||
|
else: |
||||||
|
try: |
||||||
|
res = self.set_mode_srv(0, mode) # 0 is custom mode |
||||||
|
if not res.mode_sent: |
||||||
|
rospy.logerr("failed to send mode command") |
||||||
|
except rospy.ServiceException as e: |
||||||
|
rospy.logerr(e) |
||||||
|
|
||||||
|
rate.sleep() |
||||||
|
|
||||||
|
self.assertTrue(mode_set, ( |
||||||
|
"failed to set mode | new mode: {0}, old mode: {1} | timeout(seconds): {2}". |
||||||
|
format(mode, old_mode, timeout))) |
||||||
|
|
||||||
|
def wait_for_topics(self, timeout): |
||||||
|
"""wait for simulation to be ready, make sure we're getting topic info |
||||||
|
from all topics by checking dictionary of flag values set in callbacks, |
||||||
|
timeout(int): seconds""" |
||||||
|
rospy.loginfo("waiting for subscribed topics to be ready") |
||||||
|
loop_freq = 1 # Hz |
||||||
|
rate = rospy.Rate(loop_freq) |
||||||
|
simulation_ready = False |
||||||
|
for i in xrange(timeout * loop_freq): |
||||||
|
if all(value for value in self.sub_topics_ready.values()): |
||||||
|
simulation_ready = True |
||||||
|
rospy.loginfo("simulation topics ready | seconds: {0} of {1}". |
||||||
|
format(i / loop_freq, timeout)) |
||||||
|
break |
||||||
|
|
||||||
|
rate.sleep() |
||||||
|
|
||||||
|
self.assertTrue(simulation_ready, ( |
||||||
|
"failed to hear from all subscribed simulation topics | topic ready flags: {0} | timeout(seconds): {1}". |
||||||
|
format(self.sub_topics_ready, timeout))) |
||||||
|
|
||||||
|
def wait_on_landed_state(self, desired_landed_state, timeout, index): |
||||||
|
rospy.loginfo( |
||||||
|
"waiting for landed state | state: {0}, index: {1}".format( |
||||||
|
self.LAND_STATES.get(desired_landed_state), index)) |
||||||
|
loop_freq = 10 # Hz |
||||||
|
rate = rospy.Rate(loop_freq) |
||||||
|
landed_state_confirmed = False |
||||||
|
for i in xrange(timeout * loop_freq): |
||||||
|
if self.extended_state.landed_state == desired_landed_state: |
||||||
|
landed_state_confirmed = True |
||||||
|
rospy.loginfo( |
||||||
|
"landed state confirmed | state: {0}, index: {1} | seconds: {2} of {3}". |
||||||
|
format( |
||||||
|
self.LAND_STATES.get(desired_landed_state), index, i / |
||||||
|
loop_freq, timeout)) |
||||||
|
break |
||||||
|
|
||||||
|
rate.sleep() |
||||||
|
|
||||||
|
self.assertTrue(landed_state_confirmed, ( |
||||||
|
"landed state not detected | desired: {0}, current: {1} | index: {2}, timeout(seconds): {3}". |
||||||
|
format( |
||||||
|
self.LAND_STATES.get(desired_landed_state), |
||||||
|
self.LAND_STATES.get(self.extended_state.landed_state), index, |
||||||
|
timeout))) |
||||||
|
|
||||||
|
def wait_on_transition(self, transition, timeout, index): |
||||||
|
"""Wait for VTOL transition, timeout(int): seconds""" |
||||||
|
rospy.loginfo( |
||||||
|
"waiting for VTOL transition | transition: {0}, index: {1}".format( |
||||||
|
self.VTOL_STATES.get(transition), index)) |
||||||
|
loop_freq = 10 # Hz |
||||||
|
rate = rospy.Rate(loop_freq) |
||||||
|
transitioned = False |
||||||
|
for i in xrange(timeout * loop_freq): |
||||||
|
if transition == self.extended_state.vtol_state: |
||||||
|
rospy.loginfo( |
||||||
|
"transitioned | index: {0} | seconds: {1} of {2}".format( |
||||||
|
index, i / loop_freq, timeout)) |
||||||
|
transitioned = True |
||||||
|
break |
||||||
|
|
||||||
|
rate.sleep() |
||||||
|
|
||||||
|
self.assertTrue(transitioned, ( |
||||||
|
"transition not detected | index: {0} | timeout(seconds): {1}". |
||||||
|
format(index, timeout))) |
||||||
|
|
||||||
|
def clear_wps(self, timeout): |
||||||
|
"""timeout(int): seconds""" |
||||||
|
loop_freq = 1 # Hz |
||||||
|
rate = rospy.Rate(loop_freq) |
||||||
|
wps_cleared = False |
||||||
|
for i in xrange(timeout * loop_freq): |
||||||
|
if not self.mission_wp.waypoints: |
||||||
|
wps_cleared = True |
||||||
|
rospy.loginfo("clear waypoints success | seconds: {0} of {1}". |
||||||
|
format(i / loop_freq, timeout)) |
||||||
|
break |
||||||
|
else: |
||||||
|
try: |
||||||
|
res = self.wp_clear_srv() |
||||||
|
if not res.success: |
||||||
|
rospy.logerr("failed to send waypoint clear command") |
||||||
|
except rospy.ServiceException as e: |
||||||
|
rospy.logerr(e) |
||||||
|
|
||||||
|
rate.sleep() |
||||||
|
|
||||||
|
self.assertTrue(wps_cleared, ( |
||||||
|
"failed to clear waypoints | timeout(seconds): {0}".format(timeout) |
||||||
|
)) |
||||||
|
|
||||||
|
def send_wps(self, waypoints, timeout): |
||||||
|
"""waypoints, timeout(int): seconds""" |
||||||
|
rospy.loginfo("sending mission waypoints") |
||||||
|
if self.mission_wp.waypoints: |
||||||
|
rospy.loginfo("FCU already has mission waypoints") |
||||||
|
|
||||||
|
loop_freq = 1 # Hz |
||||||
|
rate = rospy.Rate(loop_freq) |
||||||
|
wps_sent = False |
||||||
|
wps_verified = False |
||||||
|
for i in xrange(timeout * loop_freq): |
||||||
|
if not wps_sent: |
||||||
|
try: |
||||||
|
res = self.wp_push_srv(start_index=0, waypoints=waypoints) |
||||||
|
wps_sent = res.success |
||||||
|
if wps_sent: |
||||||
|
rospy.loginfo("waypoints successfully transferred") |
||||||
|
except rospy.ServiceException as e: |
||||||
|
rospy.logerr(e) |
||||||
|
else: |
||||||
|
if len(waypoints) == len(self.mission_wp.waypoints): |
||||||
|
rospy.loginfo("number of waypoints transferred: {0}". |
||||||
|
format(len(waypoints))) |
||||||
|
wps_verified = True |
||||||
|
|
||||||
|
if wps_sent and wps_verified: |
||||||
|
rospy.loginfo("send waypoints success | seconds: {0} of {1}". |
||||||
|
format(i / loop_freq, timeout)) |
||||||
|
break |
||||||
|
|
||||||
|
rate.sleep() |
||||||
|
|
||||||
|
self.assertTrue(( |
||||||
|
wps_sent and wps_verified |
||||||
|
), "mission could not be transferred and verified | timeout(seconds): {0}". |
||||||
|
format(timeout)) |
||||||
|
|
||||||
|
def wait_for_mav_type(self, timeout): |
||||||
|
"""Wait for MAV_TYPE parameter, timeout(int): seconds""" |
||||||
|
rospy.loginfo("waiting for MAV_TYPE") |
||||||
|
loop_freq = 1 # Hz |
||||||
|
rate = rospy.Rate(loop_freq) |
||||||
|
res = False |
||||||
|
for i in xrange(timeout * loop_freq): |
||||||
|
try: |
||||||
|
res = self.get_param_srv('MAV_TYPE') |
||||||
|
if res.success: |
||||||
|
self.mav_type = res.value.integer |
||||||
|
rospy.loginfo( |
||||||
|
"MAV_TYPE received | value: {0} | seconds: {1} of {2}". |
||||||
|
format(self.mav_type, i / loop_freq, timeout)) |
||||||
|
break |
||||||
|
except rospy.ServiceException as e: |
||||||
|
rospy.logerr(e) |
||||||
|
|
||||||
|
rate.sleep() |
||||||
|
|
||||||
|
self.assertTrue(res.success, ( |
||||||
|
"MAV_TYPE param get failed | timeout(seconds): {0}".format(timeout) |
||||||
|
)) |
||||||
Loading…
Reference in new issue