Ejemplo n.º 1
0
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
Ejemplo n.º 2
0
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
Ejemplo n.º 3
0
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
Ejemplo n.º 4
0
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
Ejemplo n.º 5
0
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
Ejemplo n.º 6
0
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
Ejemplo n.º 7
0
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
Ejemplo n.º 8
0
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