def do_land_cur_gps(yaw, altitude):
fix = values.get_value(gps_topic)
if not fix:
rospy.logerr("No GPS fix is latched")
return
rospy.loginfo("Landing on current coord: Lat: %f Long: %f", \
fix.latitude, fix.longitude)
rospy.loginfo("With desired Altitude: %f, Yaw: %f", \
altitude, yaw)
try:
ret = land(min_pitch=0.0, \
yaw=yaw, \
latitude=fix.latitude, \
longitude=fix.longitude, \
altitude=altitude)
except rospy.ServiceException as e:
logexc('Error landing')
return False
if not ret.success:
rospy.logerr('Error landing: unknown')
return False
return True
def do_takeoff_cur_gps(min_pitch, yaw, altitude):
fix = values.get_value(gps_topic)
if not fix:
rospy.logerr("No GPS fix is latched")
return
rospy.loginfo("Taking-off from current coord: Lat: %f Long %f", \
fix.latitude, fix.longitude)
rospy.loginfo("With desired Altitude: %f, Yaw: %f, Pitch angle: %f", \
altitude, yaw, min_pitch)
try:
ret = takeoff(min_pitch=min_pitch, \
yaw=yaw, \
latitude=fix.latitude, \
longitude=fix.longitude, \
altitude=altitude)
except rospy.ServiceException as e:
logexc('Error taking off')
return False
if not ret.success:
rospy.logerr('Error taking off: unknown')
return False
rospy.loginfo("Take-off command accepted")
return True
def arm(state):
try:
ret = arming(value=state)
except rospy.ServiceException as e:
logexc('Arming failed')
return False
if not ret.success:
rospy.logerr("Arming failed: unknown")
return False
return True
def set_current_wp(wp_seq):
try:
ret = wp_set(wp_seq)
except rospy.ServiceException as e:
logexc("Exception setting current waypoint")
return False
if not ret.success:
rospy.logerr('Error setting current waypoint: unknown')
return False
return True
def set_custom_mode(custom_mode):
global current_mode, mode_sub
if current_mode == custom_mode:
return
rospy.logdebug("Mode requested: %s", custom_mode)
if mode_sub and mode_sub.type:
rospy.logerr("Mode change is already in progress")
return
def clean():
global mode_sub
if mode_sub and mode_sub.type:
rospy.logdebug("Clearing mode sub")
mode_sub.unregister()
mode_sub = None
done_evt = threading.Event()
def state_cb(state):
global mode_sub
if state.mode == custom_mode:
global current_mode
current_mode = state.mode
rospy.loginfo("Mode changed to %s", state.mode)
done_evt.set()
clean()
mode_sub = rospy.Subscriber('/mavros/state', State, state_cb)
success = False
try:
ret = set_mode(base_mode=0, custom_mode=custom_mode)
if not ret.mode_sent:
rospy.logerr("Set mode %s failed: unknown" % custom_mode)
elif not done_evt.wait(10):
rospy.logerr("Set mode %s failed: timed out" % custom_mode)
else:
success = True
except rospy.ServiceException as e:
logexc('Set mode failed')
return False
if not success:
clean()
return success
def set_int_param(param, value):
try:
val = ParamValue()
val.integer = value
val.real = 0.0
ret = set_param(param, val)
except rospy.ServiceException as e:
logexc('Set param failed')
return False
if not ret.success:
rospy.logerr("Set param failed: unknown")
return False
return True
def push_waypoints(waypoints):
try:
ret = wp_push(start_index=0, waypoints=waypoints)
except rospy.ServiceException as e:
logexc("Exception pushing waypoints")
return False
if not ret.success:
rospy.logerr('Error setting waypoints: unknown')
return False
if not ret.wp_transfered == len(waypoints):
rospy.logerr("Only %d/%d waypoints were transferred" %
(ret.wp_transfered, len(waypoints)))
return False
return True
def set_airspeed(airspeed):
""" Sets the airspeed in m/s
"""
rospy.loginfo("Setting desired airspeed to %2.2f m/s" % airspeed)
try:
ret = command(
command=rhc.MAV_CMD_DO_CHANGE_SPEED, \
confirmation=0, \
param1=0, \
param2=airspeed, \
param3=-1, \
param4=0)
except rospy.ServiceException as e:
logexc('Error setting airspeed')
return False
if not ret.success:
rospy.logerr('Error setting airspeed: unknown')
return False
return True