HardWare/MobileNode/Programming: uav_guided_mode.py

#!/usr/bin/python

"""
This program reads the RAW GPS readings from a file every INTERVAL seconds, converts them to GPS coordinates and directs the uav to traverse to those. After all the waypoints are traversed, it sets the UAV to RTL mode.
"""
import re
import sys, os
import socket
import select
import time

sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '../lib'))

import mavlink_apm, FTL_util

INTERVAL = 5
MAX_SIZE = 1024

# Following are used for converting RAW GPS data into GPS coordinates
raw_int_to_float_lat_lon = 10**7
raw_int_to_float_alt = 10**3

# Object used for accessing utility functions
FTL_util_obj = FTL_util.FTL_util()

""" This function sets the next waypoint the UAV has to traverse to"""
def send_wp (mav_obj, mavproxy_sock, latitude, longitude,altitude,
                        address_of_mavproxy):
    global MAX_SIZE, INTERVAL
    # A simpler way of assigning would be a,b,c,d = p1,p2,p3,p4. But for 
    # readability sake, we stick with the usual assignment
    target_system = target_component = 1
    seq = 0
    #frame = mavlink_apm.MAV_FRAME_GLOBAL_RELATIVE_ALT
    frame = mavlink_apm.MAV_FRAME_GLOBAL
    command = mavlink_apm.MAV_CMD_NAV_WAYPOINT
     # signifies whether this is the current waypoint to proceed to, 
     # any positive value is true
    current = 2
    autocontinue = 1 # continue to next waypoint
    param1 = param2 = param3 = param4 = PARAM_IGNORE = 0

    # Send the waypoint message and wait till the ACK is received for 2 seconds
    list_read_sockets = [mavproxy_sock]
    list_write_sockets = list_error_sockets = []
    while 1:
        msg = mav_obj.mission_item_encode(target_system, target_component,seq,frame,command,current,autocontinue,param1,param2,param3,param4,latitude,longitude,altitude-584.0)
        try:
            mavproxy_sock.sendto(msg.get_msgbuf(),(address_of_mavproxy))
        except socket.error as v:
            print "Exception when setting WP:"
            print os.strerror(v.errno)
            time.sleep(1)
            continue
        readable, writable, error = select.select(list_read_sockets,
                              list_write_sockets,
                              list_error_sockets,INTERVAL)
        if readable:
            data_from_mavproxy,address_of_mavproxy = mavproxy_sock.recvfrom (MAX_SIZE)
            decoded_message = mav_obj.decode(data_from_mavproxy)
            msg_id = decoded_message.get_msgId()

            # Check if this is a waypoint request message
            if msg_id == mavlink_apm.MAVLINK_MSG_ID_MISSION_ACK:
                print 'ACK for this way-point received received...'
                break
            else:
                #print "Expected message ID 47, received %d" % msg_id
                continue
        else:
            # Send the message again
            print "Timeout on receiving waypoint ACK message"
            continue
    
    return

""" This function parses the RAW GPS data and returns latitude, longitude
and altitude"""
def parse_gps_info(gps_info):
    latitude = longitude = altitude = 0
    # Get Latitude
    match_for_latitude =  re.search(r'lat : (.+?),',gps_info)
    if not match_for_latitude:
        print "Error - no latitude information found, return!"
        return 0,0,0
    else:
        latitude = match_for_latitude.group(1) 

    # Get longitude
    match_for_longitude =  re.search(r'lon : (.+?),',gps_info)
    if not match_for_longitude:
        print "Error - no longitude information found, return!"
        return 0,0,0
    else:
        longitude = match_for_longitude.group(1) 

    # Get altitude
    match_for_altitude =  re.search(r'alt : (.+?),',gps_info)
    if not match_for_altitude:
        print "Error - no altitude information found, return!"
        return 0,0,0
    else:
        altitude = match_for_altitude.group(1)

    return latitude, longitude, altitude

""" This function is invoked to obtain the  address of MAVProxy"""
def get_mavproxy_address (mav_obj,mavproxy_sock):
    MAX_SIZE = 1024
    heartbeat_received = 'False'
    mavproxy_sock.setblocking(1)
    print "Waiting for heartbeat message..."
    while not heartbeat_received.lower() == 'TRUE'.lower():
        # Wait for heartbeat message to get the remote address
        # used by MAVProxy
        try:
            data_from_mavproxy,address_of_mavproxy = mavproxy_sock.recvfrom (MAX_SIZE)
        except socket.error as v:
            print "Exception when trying to obtain address of MAVProxy"
            print os.strerror(v.errno)
        decoded_message = mav_obj.decode(data_from_mavproxy)
        msg_id = decoded_message.get_msgId()
        if msg_id == mavlink_apm.MAVLINK_MSG_ID_HEARTBEAT:
            # Undo the change made
            heartbeat_received = 'True'
            print 'Got the address of MAV,  proceeding..'
            print address_of_mavproxy
            mavproxy_sock.setblocking(0)
    return address_of_mavproxy

""" The main function """
def main():
    global MAX_SIZE, raw_int_to_float_lat_lon, raw_int_to_float_alt
    file_handle = ""

    if len(sys.argv) != 3:
        print "Usage: ./uav_guided_mode.py <MAVProxy port> <path to file with GPS info>"
        sys.exit(1)
    mavproxy_port = int(sys.argv[1])
    file_name_with_gps_info = sys.argv[2]
    # Check if the file exists
    if not os.path.exists(file_name_with_gps_info):
        print "File name %s does not exist" % file_name_with_gps_info
        sys.exit(1)

    # Check if the file is not empty
    if not (os.path.getsize(file_name_with_gps_info) > 0):
        print "File %s is empty" % file_name_with_gps_info
        sys.exit(1)
    
    try:
        HOST = '' # Listen on all interfaces
        # Create a server socket for MAVProxy
        mavproxy_sock = socket.socket (socket.AF_INET,socket.SOCK_DGRAM)
        mavproxy_sock.setblocking(0)
        print 'created UDP socket for MAVProxy'
        mavproxy_sock.bind((HOST,mavproxy_port))
        print 'Binding socket for MAVProxy connection'
        # Create the mavproxy object
        mav_obj = mavlink_apm.MAVLink (mavproxy_sock)

        # Get the address used by MAVProxy 
        address_of_mavproxy = get_mavproxy_address (mav_obj, mavproxy_sock)

        # ARM the UAV
        component_id = mavlink_apm.MAV_COMP_ID_SYSTEM_CONTROL
        # Same command for arming or disarming, arm_flag controls whether the UAV
        # armed or disarmed. arm_flag=1->arm, arm_flag=0->disarm
        command = mavlink_apm.MAV_CMD_COMPONENT_ARM_DISARM
        arm_flag = 1
        # Number of confirmations needed for this command. 0 means immediately
        confirmation = 0
        # Other parameters are ignored by this command and are to be set to zero.
        PARAM_IGNORE = 0
        msg = mav_obj.command_long_encode (1,component_id,command,confirmation,
                                          arm_flag,PARAM_IGNORE,PARAM_IGNORE,
                                          PARAM_IGNORE,PARAM_IGNORE,PARAM_IGNORE,
                                          PARAM_IGNORE)
        try:
            mavproxy_sock.sendto(msg.get_msgbuf(),(address_of_mavproxy))
        except socket.error as v:
            print "Exception when trying to ARM the copter:"
            print os.strerror(v.errno)
        print "ARMED"
        file_handle = open(file_name_with_gps_info, 'r')

        # For each line which represents raw GPS data, generate and set a waypoint      
        for line in file_handle:
            # Skip empty lines
            if line not in ['\n','\r\n']:
                latitude, longitude, altitude = parse_gps_info(line)
                # Filter unexpected values
                if not longitude or not latitude or not altitude:
                    print "Erroneous values, skip.."
                    continue
                latitude = float(latitude)/raw_int_to_float_lat_lon
                longitude = float(longitude)/raw_int_to_float_lat_lon
                altitude = float(altitude)/raw_int_to_float_alt
                send_wp (mav_obj, mavproxy_sock, latitude,longitude,
                                altitude,address_of_mavproxy)
                # Sleep for INTERVAL seconds
                time.sleep(float(INTERVAL))
        
        return_status = FTL_util_obj.set_mav_mode(FTL_util_obj.rtl_mode,mav_obj, 
                                                mavproxy_sock, address_of_mavproxy)
        if return_status < 0:
            print "Error while setting mode, please check the parameters passed..."
            sys.exit(1)
                
    except Exception as ex:
        template = "An exception of type {0} occured. Arguments:\n{1!r}"
        message = template.format(type(ex).__name__, ex.args)
        print message
        sys.exit(1)

    if file_handle:
        file_handle.close()

if __name__ == '__main__':
    main()