Ejemplo n.º 1
0
def test():
    '''
    run test program as a module to avoid namespace conflicts with serial module:
    
    python -p pprzlink.serial

    pprzlink should be installed in a python standard path or included to your PYTHONPATH
    '''
    import time
    import argparse
    from pprzlink import messages_xml_map

    parser = argparse.ArgumentParser()
    parser.add_argument("-f", "--file", help="path to messages.xml file")
    parser.add_argument("-c", "--class", help="message class", dest='msg_class', default='telemetry')
    parser.add_argument("-d", "--device", help="device name", dest='dev', default='/dev/ttyUSB0')
    parser.add_argument("-b", "--baudrate", help="baudrate", dest='baud', default=115200, type=int)
    parser.add_argument("-id", "--ac_id", help="aircraft id (receiver)", dest='ac_id', default=42, type=int)
    parser.add_argument("--interface_id", help="interface id (sender)", dest='id', default=0, type=int)
    args = parser.parse_args()
    messages_xml_map.parse_messages(args.file)
    serial_interface = SerialMessagesInterface(lambda s, m: print("new message from %i: %s" % (s, m)), device=args.dev,
                                               baudrate=args.baud, msg_class=args.msg_class, interface_id=args.id, verbose=True)

    print("Starting serial interface on %s at %i baud" % (args.dev, args.baud))
    try:
        serial_interface.start()

        # give the thread some time to properly start
        time.sleep(0.1)

        # send some datalink messages to aicraft for test
        ac_id = args.ac_id
        print("sending ping")
        ping = PprzMessage('datalink', 'PING')
        serial_interface.send(ping, 0)

        get_setting = PprzMessage('datalink', 'GET_SETTING')
        get_setting['index'] = 0
        get_setting['ac_id'] = ac_id
        serial_interface.send(get_setting, 0)

        # change setting with index 0 (usually the telemetry mode)
        set_setting = PprzMessage('datalink', 'SETTING')
        set_setting['index'] = 0
        set_setting['ac_id'] = ac_id
        set_setting['value'] = 1
        serial_interface.send(set_setting, 0)

        # block = PprzMessage('datalink', 'BLOCK')
        # block['block_id'] = 3
        # block['ac_id'] = ac_id
        # serial_interface.send(block, 0)

        while serial_interface.isAlive():
            serial_interface.join(1)
    except (KeyboardInterrupt, SystemExit):
        print('Shutting down...')
        serial_interface.stop()
        exit()
Ejemplo n.º 2
0
def callback_depth(depth):

    image_buffer = drone.bridge.imgmsg_to_cv2(depth,
                                              desired_encoding="passthrough")

    mean_left = 0
    mean_right = 0
    matrix = depth.height * depth.width

    for row in range(depth.height):
        for col in range(depth.width):
            pix = image_buffer[row][col]
            if pix < 1000:
                if (col < depth.width / 2):
                    mean_left += pix
                else:
                    mean_right += pix

    mean_left /= matrix * 0.5
    mean_right /= matrix * 0.5

    message = PprzMessage("intermcu", "IMCU_LR_MEAN_DIST")
    message.set_values([np.float(mean_left), np.float(mean_right)])

    serial.interface.send(message, drone.id)
Ejemplo n.º 3
0
def test():
    import time
    import argparse
    from pprzlink import messages_xml_map

    parser = argparse.ArgumentParser()
    parser.add_argument("-f", "--file", help="path to messages.xml file")
    parser.add_argument("-c", "--class", help="message class of incoming messages", dest='msg_class', default='telemetry')
    parser.add_argument("-a", "--address", help="destination address", dest='address', default='127.0.0.1')
    parser.add_argument("-id", "--ac_id", help="aircraft id (receiver)", dest='ac_id', default=42, type=int)
    parser.add_argument("--interface_id", help="interface id (sender)", dest='id', default=0, type=int)
    parser.add_argument("-up", "--uplink_port", help="uplink port", dest='uplink', default=UPLINK_PORT, type=int)
    parser.add_argument("-dp", "--downlink_port", help="downlink port", dest='downlink', default=DOWNLINK_PORT, type=int)
    args = parser.parse_args()
    messages_xml_map.parse_messages(args.file)
    udp_interface = UdpMessagesInterface(lambda s, a, m: print("new message from %i (%s): %s" % (s, a, m)),
                                               uplink_port=args.uplink, downlink_port=args.downlink,
                                               msg_class=args.msg_class, interface_id=args.id, verbose=True)

    print("Starting UDP interface with '%s' with id '%d'" % (args.address, args.ac_id))
    address = (args.address, args.uplink)
    try:
        udp_interface.start()

        # give the thread some time to properly start
        time.sleep(0.1)

        # send some datalink messages to aicraft for test
        ac_id = args.ac_id
        print("sending ping")
        ping = PprzMessage('datalink', 'PING')
        udp_interface.send(ping, 0, address)

        print("sending get_setting")
        get_setting = PprzMessage('datalink', 'GET_SETTING')
        get_setting['index'] = 0
        get_setting['ac_id'] = ac_id
        udp_interface.send(get_setting, 0, address)

        # change setting with index 0 (usually the telemetry mode)
        print("sending setting")
        set_setting = PprzMessage('datalink', 'SETTING')
        set_setting['index'] = 0
        set_setting['ac_id'] = ac_id
        set_setting['value'] = 1
        udp_interface.send(set_setting, 0, address)

        # print("sending block")
        # block = PprzMessage('datalink', 'BLOCK')
        # block['block_id'] = 3
        # block['ac_id'] = ac_id
        # udp_interface.send(block, 0, address)

        while udp_interface.isAlive():
            udp_interface.join(1)
    except (KeyboardInterrupt, SystemExit):
        print('Shutting down...')
        udp_interface.stop()
        exit()
Ejemplo n.º 4
0
    def circular_formation(self):
        '''
        circular formation control algorithm
        '''

        ready = True
        for ac in self.aircraft:
            if (not ac.initialized_nav) or (not ac.initialized_gvf):
                if self.verbose:
                    print("Waiting for state of aircraft ", ac.id)
                ready = False

        if not ready:
            return

        i = 0
        for ac in self.aircraft:
            ac.sigma = np.arctan2(ac.XY[1] - ac.XYc[1], ac.XY[0] - ac.XYc[0])
            self.sigmas[i] = ac.sigma
            i = i + 1

        inter_sigma = self.B.transpose().dot(self.sigmas)
        error_sigma = inter_sigma - self.Zdesired

        if np.size(error_sigma) > 1:
            for i in range(0, np.size(error_sigma)):
                if error_sigma[i] > np.pi:
                    error_sigma[i] = error_sigma[i] - 2 * np.pi
                elif error_sigma[i] <= -np.pi:
                    error_sigma[i] = error_sigma[i] + 2 * np.pi
        else:
            if error_sigma > np.pi:
                error_sigma = error_sigma - 2 * np.pi
            elif error_sigma <= -np.pi:
                error_sigma = error_sigma + 2 * np.pi

        u = -self.aircraft[0].s * self.k * self.B.dot(error_sigma)

        if self.verbose:
            print(
                "Inter-vehicle errors: ",
                str(error_sigma * 180.0 / np.pi).replace('[',
                                                         '').replace(']', ''))

        i = 0
        for ac in self.aircraft:
            msga = PprzMessage("ground", "DL_SETTING")
            msga['ac_id'] = ac.id
            msga['index'] = ac.a_index
            msga['value'] = self.radius + u[i]
            msgb = PprzMessage("ground", "DL_SETTING")
            msgb['ac_id'] = ac.id
            msgb['index'] = ac.b_index
            msgb['value'] = self.radius + u[i]

            self._interface.send(msga)
            self._interface.send(msgb)

            i = i + 1
Ejemplo n.º 5
0
    def __init__(self, ac_id, plateform_id, tag_id):
        # Start a ivy messages listener named PIR DRONE
        self._interface = IvyMessagesInterface("PIR", ivy_bus="192.168.1:2010")

        self.drone = FC_Rotorcraft(ac_id, tag_id, self._interface)
        self.plateform = Platform(plateform_id)

        # bind to GROUND_REF message
        def ground_ref_cb(ground_id, msg):
            ac_id = int(msg['ac_id'])
            if ac_id == self.drone.id:
                # X and V in NED
                self.drone.X_opti[0] = float(msg['pos'][1])
                self.drone.X_opti[1] = float(msg['pos'][0])
                self.drone.X_opti[2] = -float(msg['pos'][2])
                self.drone.V_opti[0] = float(msg['speed'][1])
                self.drone.V_opti[1] = float(msg['speed'][0])
                self.drone.V_opti[2] = -float(msg['speed'][2])
                self.drone.quaternions[0] = float(msg['quat'][0])
                self.drone.quaternions[1] = float(msg['quat'][1])
                self.drone.quaternions[2] = float(msg['quat'][2])
                self.drone.quaternions[3] = float(msg['quat'][3])
                self.drone.timeLastUpdate = time.time()
                self.drone.initialized = True
            if ac_id == self.plateform.id:
                # X and V in NED
                self.plateform.X[0] = float(msg['pos'][1])
                self.plateform.X[1] = float(msg['pos'][0])
                self.plateform.X[2] = -float(msg['pos'][2])
                self.plateform.V[0] = float(msg['speed'][1])
                self.plateform.V[1] = float(msg['speed'][0])
                self.plateform.V[2] = -float(msg['speed'][2])
                self.plateform.quaternions[0] = float(msg['quat'][0])
                self.plateform.quaternions[1] = float(msg['quat'][1])
                self.plateform.quaternions[2] = float(msg['quat'][2])
                self.plateform.quaternions[3] = float(msg['quat'][3])
                self.plateform.timeLastUpdate = time.time()
                self.plateform.initialized = True

        self._interface.subscribe(ground_ref_cb,
                                  PprzMessage("ground", "GROUND_REF"))

        # Recuperation du x et y via l'UWB
        def get_xy_uwb(uwb_id, msg):
            # X and V in NED
            #msg est un tableau contenant : d1corr, d2corr, d3corr, d1vrai, d2vrai, d3vrai, x, y, z, vx, vy, vz
            x = float(msg['values'][6])
            y = float(msg['values'][7])
            z = self.drone.X_opti[3]
            psi = cr.getEulerAngles(
                self.drone.quaternions)[2] - cr.getEulerAngles(
                    self.plateform.quaternions)[2]
            #self.drone.V_uwb[0] = float(msg['values'][9])
            #self.drone.V_uwb[1] = float(msg['values'][10])
            self.drone.X_uwb = cr.uwb2pseudoBody(x, y, z)

        self._interface.subscribe(get_xy_uwb,
                                  PprzMessage("telemetry", "PAYLOAD_FLOAT"))
def formation(B, ds, radius, k):

    waiting_for_msgs = 0
    for ac in list_aircraft:
        if ac.a == -999:
            print("Waiting for GVF msg of aircraft ", ac.id)
            waiting_for_msgs = 1
        if ac.XY[0] == -999:
            print("Waiting for NAV msg of aircraft ", ac.id)
            waiting_for_msgs = 1
    
    if waiting_for_msgs == 1:
        return

    sigma = np.zeros(len(list_aircraft))
    i = 0
    for ac in list_aircraft:
        ac.sigma = np.arctan2(ac.XY[1]-ac.XYc[1], ac.XY[0]-ac.XYc[0])
        sigma[i] = ac.sigma
        i = i + 1

    inter_sigma = B.transpose().dot(sigma)
    error_sigma = inter_sigma - ds
    
    if np.size(error_sigma) > 1:
        for i in range(0, np.size(error_sigma)):
            if error_sigma[i] > np.pi:
                error_sigma[i] = error_sigma[i] - 2*np.pi
            elif error_sigma[i] <= -np.pi:
                error_sigma[i] = error_sigma[i] + 2*np.pi
    else:
        if error_sigma > np.pi:
            error_sigma = error_sigma - 2*np.pi
        elif error_sigma <= -np.pi:
            error_sigma = error_sigma + 2*np.pi


    u = -list_aircraft[0].s*k*B.dot(error_sigma)

    print(list_aircraft[0].time, " ", str(error_sigma*180.0/np.pi).replace('[','').replace(']',''))

    i = 0
    for ac in list_aircraft:
        msga = PprzMessage("ground", "DL_SETTING")
        msga['ac_id'] = ac.id
        msga['index'] = ac.a_index
        msga['value'] = radius + u[i]
        msgb = PprzMessage("ground", "DL_SETTING")
        msgb['ac_id'] = ac.id
        msgb['index'] = ac.b_index
        msgb['value'] = radius + u[i]

        interface.send(msga)
        interface.send(msgb)

        i = i + 1
    return
Ejemplo n.º 7
0
    def __init__(self, config, freq=10., verbose=False):
        self.config = config
        self.step = 1. / freq
        self.verbose = verbose
        self.ids = self.config['ids']
        self.B = np.array(self.config['topology'])
        self.Zdesired = np.array(self.config['desired_intervehicle_angles'])
        self.k = np.array(self.config['gain'])
        self.radius = np.array(self.config['desired_stationary_radius'])
        self.aircraft = [Aircraft(i) for i in self.ids]
        self.sigmas = np.zeros(len(self.aircraft))

        for ac in self.aircraft:
            settings = PaparazziACSettings(ac.id)
            list_of_indexes = ['ell_a', 'ell_b']

            for setting_ in list_of_indexes:
                try:
                    index = settings.name_lookup[setting_].index
                    if setting_ == 'ell_a':
                        ac.a_index = index
                    if setting_ == 'ell_b':
                        ac.b_index = index
                except Exception as e:
                    print(e)
                    print(setting_ + " setting not found, \
                            have you forgotten to check gvf.xml for your settings?"
                          )

        # Start IVY interface
        self._interface = IvyMessagesInterface("Circular Formation")

        # bind to NAVIGATION message
        def nav_cb(ac_id, msg):
            if ac_id in self.ids and msg.name == "NAVIGATION":
                ac = self.aircraft[self.ids.index(ac_id)]
                ac.XY[0] = float(msg.get_field(2))
                ac.XY[1] = float(msg.get_field(3))
                ac.initialized_nav = True

        self._interface.subscribe(nav_cb, PprzMessage("telemetry",
                                                      "NAVIGATION"))

        def gvf_cb(ac_id, msg):
            if ac_id in self.ids and msg.name == "GVF":
                if int(msg.get_field(1)) == 1:
                    ac = self.aircraft[self.ids.index(ac_id)]
                    param = msg.get_field(4)
                    ac.XYc[0] = float(param[0])
                    ac.XYc[1] = float(param[1])
                    ac.a = float(param[2])
                    ac.b = float(param[3])
                    ac.s = float(msg.get_field(2))
                    ac.initialized_gvf = True

        self._interface.subscribe(gvf_cb, PprzMessage("telemetry", "GVF"))
Ejemplo n.º 8
0
 def unpack_pprz_msg(self, data):
     """Unpack a raw PPRZ message"""
     sender_id = data[0]
     receiver_id = data[1]
     class_id = data[2] & 0x0F
     component_id = (data[2] & 0xF0) >> 4
     msg_id = data[3]
     msg = PprzMessage(class_id, msg_id)
     msg.binary_to_payload(data[4:])
     return sender_id, receiver_id, component_id, msg
Ejemplo n.º 9
0
    def parse_pprz_msg(callback, ivy_msg):
        """
        Parse an Ivy message into a PprzMessage.
        Basically parts/args in string are separated by space, but char array can also contain a space:
        |f,o,o, ,b,a,r| in old format or "foo bar" in new format

        :param callback: function to call with ac_id and parsed PprzMessage as params
        :param ivy_msg: Ivy message string to parse into PprzMessage
        """
        # first split on array delimiters
        l = re.split('([|\"][^|\"]*[|\"])', ivy_msg)
        # strip spaces and filter out emtpy strings
        l = [str.strip(s) for s in l if str.strip(s) is not '']
        data = []
        for s in l:
            # split non-array strings further up
            if '|' not in s and '"' not in s:
                data += s.split(' ')
            else:
                data.append(s)
        # ignore ivy message with less than 3 elements
        if len(data) < 3:
            return

        # normal format is "sender_name msg_name msg_payload..."
        # advanced format has requests and answers (with request_id as 'pid_index')
        # request: "sender_name request_id msg_name_REQ msg_payload..."
        # answer:  "request_id sender_name msg_name msg_payload..."

        # check for request_id in first or second string (-> advanced format with msg_name in third string)
        advanced = False
        if re.search("[0-9]+_[0-9]+", data[0]) or re.search(
                "[0-9]+_[0-9]+", data[1]):
            advanced = True
        msg_name = data[2] if advanced else data[1]
        # check which message class it is
        msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
        if msg_class is None:
            print("Ignoring unknown message " + ivy_msg)
            return
        # pass non-telemetry messages with ac_id 0
        if msg_class == "telemetry":
            try:
                ac_id = int(data[0])
            except ValueError:
                print("ignoring message " + ivy_msg)
                sys.stdout.flush()
        else:
            ac_id = 0
        payload = data[3:] if advanced else data[2:]
        values = list(filter(None, payload))
        msg = PprzMessage(msg_class, msg_name)
        msg.set_values(values)
        # finally call the callback, passing the aircraft id and parsed message
        callback(ac_id, msg)
Ejemplo n.º 10
0
    def parse_pprz_msg(callback, ivy_msg):
        """
        Parse an Ivy message into a PprzMessage.
        Basically parts/args in string are separated by space, but char array can also contain a space:
        |f,o,o, ,b,a,r| in old format or "foo bar" in new format

        :param callback: function to call with ac_id and parsed PprzMessage as params
        :param ivy_msg: Ivy message string to parse into PprzMessage
        """
        # first split on array delimiters
        l = re.split('([|\"][^|\"]*[|\"])', ivy_msg)
        # strip spaces and filter out emtpy strings
        l = [str.strip(s) for s in l if str.strip(s) is not '']
        data = []
        for s in l:
            # split non-array strings further up
            if '|' not in s and '"' not in s:
                data += s.split(' ')
            else:
                data.append(s)
        # ignore ivy message with less than 3 elements
        if len(data) < 3:
            return

        # normal format is "sender_name msg_name msg_payload..."
        # advanced format has requests and answers (with request_id as 'pid_index')
        # request: "sender_name request_id msg_name_REQ msg_payload..."
        # answer:  "request_id sender_name msg_name msg_payload..."

        # check for request_id in first or second string (-> advanced format with msg_name in third string)
        advanced = False
        if re.search("[0-9]+_[0-9]+", data[0]) or re.search("[0-9]+_[0-9]+", data[1]):
            advanced = True
        msg_name = data[2] if advanced else data[1]
        # check which message class it is
        msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
        if msg_class is None:
            print("Ignoring unknown message " + ivy_msg)
            return
        # pass non-telemetry messages with ac_id 0
        if msg_class == "telemetry":
            try:
                ac_id = int(data[0])
            except ValueError:
                print("ignoring message " + ivy_msg)
                sys.stdout.flush()
        else:
            ac_id = 0
        payload = data[3:] if advanced else data[2:]
        values = list(filter(None, payload))
        msg = PprzMessage(msg_class, msg_name)
        msg.set_values(values)
        # finally call the callback, passing the aircraft id and parsed message
        callback(ac_id, msg)
Ejemplo n.º 11
0
    def parse_pprz_msg(callback, ivy_msg):
        """
        Parse an Ivy message into a PprzMessage.
        :param callback: function to call with ac_id and parsed PprzMessage as params
        :param ivy_msg: Ivy message string to parse into PprzMessage
        """
        # normal format is "sender_name msg_name msg_payload..."
        # advanced format has requests and answers (with request_id as 'pid_index')
        # request: "sender_name request_id msg_name_REQ msg_payload..."
        # answer:  "request_id sender_name msg_name msg_payload..."

        data = re.search("(\S+) +(\S+) +(.*)", ivy_msg)
        # check for request_id in first or second string (-> advanced format with msg_name in third string)
        if data is None:
            return
        if re.search("[0-9]+_[0-9]+", data.group(1)) or re.search("[0-9]+_[0-9]+", data.group(2)):
            if re.search("[0-9]+_[0-9]+", data.group(1)):
                sender_name = data.group(2)
            else:
                sender_name = data.group(1)
            # this is an advanced type, split again
            data = re.search("(\S+) +(.*)", data.group(3))
            msg_name = data.group(1)
            payload = data.group(2)
        else:
            # this was a normal message
            sender_name = data.group(1)
            msg_name = data.group(2)
            payload = data.group(3)
        # check which message class it is
        msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
        if msg_class is None:
            print("Ignoring unknown message " + ivy_msg)
            return
        msg = PprzMessage(msg_class, msg_name)
        msg.ivy_string_to_payload(payload)
        # pass non-telemetry messages with ac_id 0 or ac_id attrib value
        if msg_class == "telemetry":
            try:
                if(sender_name[0:6] == 'replay'):
                    ac_id = int(sender_name[6:])
                else:
                    ac_id = int(sender_name)
            except ValueError:
                print("ignoring message " + ivy_msg)
                sys.stdout.flush()
        else:
            if 'ac_id' in msg.fieldnames:
                ac_id_idx = msg.fieldnames.index('ac_id')
                ac_id = msg.fieldvalues[ac_id_idx]
            else:
                ac_id = 0
        # finally call the callback, passing the aircraft id and parsed message
        callback(ac_id, msg)
Ejemplo n.º 12
0
    def init_dcftables(self):
        time.sleep(2)

        for count, column in enumerate(self.B.T):
            index = np.nonzero(column)
            i = index[0]

            # nei_id = 0, special msg to clean the table onboard
            msg_clean_a = PprzMessage("datalink", "DCF_REG_TABLE")
            msg_clean_a['ac_id'] = int(self.list_ids[i[0]])
            msg_clean_a['nei_id'] = 0
            msg_clean_b = PprzMessage("datalink", "DCF_REG_TABLE")
            msg_clean_b['ac_id'] = int(self.list_ids[i[1]])
            msg_clean_b['nei_id'] = 0

            self._interface.send(msg_clean_a)
            self._interface.send(msg_clean_b)

            if self.verbose:
                print(msg_clean_a)
                print(msg_clean_b)

        for count, column in enumerate(self.B.T):
            index = np.nonzero(column)
            i = index[0]

            msga = PprzMessage("datalink", "DCF_REG_TABLE")
            msga['ac_id'] = int(self.list_ids[i[0]])
            msga['nei_id'] = int(self.list_ids[i[1]])
            if len(self.list_ids) == 2:
                msga['desired_sigma'] = (column[index])[0] * int(self.Zdesired)
            else:
                msga['desired_sigma'] = (column[index])[0] * int(
                    self.Zdesired[count])
            self._interface.send(msga)

            msgb = PprzMessage("datalink", "DCF_REG_TABLE")
            msgb['ac_id'] = int(self.list_ids[i[1]])
            msgb['nei_id'] = int(self.list_ids[i[0]])
            if len(self.list_ids) == 2:
                msgb['desired_sigma'] = (column[index])[1] * int(self.Zdesired)
            else:
                msgb['desired_sigma'] = (column[index])[1] * int(
                    self.Zdesired[count])
            self._interface.send(msgb)

            if self.verbose:
                print(msga)
                print(msgb)

        time.sleep(2)
Ejemplo n.º 13
0
 def __init__(self, verbose=False):
     # init cam related part
     uEyePprzlink.__init__(self, verbose)
     # build fake message
     msg = PprzMessage("telemetry", "DC_SHOT")
     msg['photo_nr'] = 0
     self.process_msg(0, msg)
Ejemplo n.º 14
0
    def mission_segment(self,
                        s_pos,
                        d_pos,
                        altitude,
                        duration=0,
                        insert_mode=0):
        """Send a MISSION_SEGMENT command to paparazzi.

        Arguments:
            s_pos: source position as tuple (east, north)
            d_pos: destination position as tuple (east, north)
            altitude: altitude of the segment (in meters above sea level)
            duration: duration of mission (in seconds) before being discarded.
                Default is 0, meaning no specified duration.
            insert_mode: default APPEND
                0: APPEND
                1: PREPEND
                2: REPLACE_CURRENT
                3: REPLACE_ALL
        """
        msg = PprzMessage("datalink", "MISSION_SEGMENT")
        msg['ac_id'] = self.ac_id
        # APPEND, PREPEND, REPLACE_CURRENT, REPLACE_ALL
        msg['insert'] = insert_mode
        msg['segment_east_1'] = s_pos[0]
        msg['segment_north_1'] = s_pos[1]
        msg['segment_east_2'] = d_pos[0]
        msg['segment_north_2'] = d_pos[1]
        msg['segment_alt'] = altitude
        msg['duration'] = duration
        self.ivy_mes.send_raw_datalink(msg)
Ejemplo n.º 15
0
def receiveRigidBodyList(rigidBodyList, stamp):
    for (ac_id, pos, quat) in rigidBodyList:
        i = str(ac_id)
        if i not in id_dict.keys():
            continue

        store_track(i, pos, stamp)
        if timestamp[i] is None or abs(stamp - timestamp[i]) < period:
            if timestamp[i] is None:
                timestamp[i] = stamp
            continue  # too early for next message
        timestamp[i] = stamp

        msg = PprzMessage("datalink", "REMOTE_GPS_LOCAL")
        msg['ac_id'] = id_dict[i]
        msg['pad'] = 0
        msg['enu_x'] = pos[0]
        msg['enu_y'] = pos[1]
        msg['enu_z'] = pos[2]
        vel = compute_velocity(i)
        msg['enu_xd'] = vel[0]
        msg['enu_yd'] = vel[1]
        msg['enu_zd'] = vel[2]
        msg['tow'] = int(stamp)  # TODO convert to GPS itow ?
        # convert quaternion to psi euler angle
        dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
        dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
        msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / 3.14
        ivy.send(msg)
Ejemplo n.º 16
0
 def send_pprz_fault_info(self):
     if self.verbose: print('Sending the FAULT_INFO')
     set_fault = PprzMessage('datalink', 'FAULT_INFO')
     set_fault['info'] = self.fault_info
     set_fault['type'] = self.fault_type
     set_fault['class'] = self.fault_class
     self.interface.send(set_fault, 0)
Ejemplo n.º 17
0
def guidance_setmode(ac_id, value):
    retval = ''

    try:
        settings = PaparazziACSettings(ac_id)
    except Exception as e:
        print(e)
        return
    try:
        index = settings.name_lookup['auto2'].index
    except Exception as e:
        print(e)
        print("auto2 setting not found, mode change not possible.")
        return

    if index is not None:
        msg = PprzMessage("ground", "DL_SETTING")
        msg['ac_id'] = ac_id
        msg['index'] = index
        msg['value'] = value
        if verbose:
            print_ivy_trace(msg)
            retval = 'Guidance mode: ac_id=%d, index=%d, value=%d\n' % (
                ac_id, index, value)
        ivy_interface.send(msg)
        if curl: print_curl_format()
        return retval
Ejemplo n.º 18
0
    def __init__(self, my_ac_id, tag_id, interface_):

        # Start a ivy messages listener named PIR DRONE
        self._interface = interface_
        self.tag = ArUcoTag(tag_id)
        self.ac_id = my_ac_id

        # bind to jevois message
        def jevois_cb(jevois_id, msg):

            # print("entree dans callback : {} {}".format(self.tag.id , str(msg['id'])))
            if self.tag.id == str(msg['id']).replace('"', ""):
                self.tag.X[0] = float(
                    msg['coord']
                    [0]) / 1000  # data is sent in milimeter , stocked in meter
                self.tag.X[1] = float(msg['coord'][1]) / 1000
                self.tag.X[2] = float(msg['coord'][2]) / 1000
                self.tag.quaternions[0] = float(msg['quat'][0])
                self.tag.quaternions[1] = float(msg['quat'][1])
                self.tag.quaternions[2] = float(msg['quat'][2])
                self.tag.quaternions[3] = float(msg['quat'][3])

                self.tag.whd[0] = float(msg['dim'][0])
                self.tag.whd[1] = float(msg['dim'][1])
                self.tag.whd[2] = float(msg['dim'][2])

                self.tag.initialized = True
                self.tag.timeLastUpdate = time.time()

        self._interface.subscribe(jevois_cb,
                                  PprzMessage("telemetry", "JEVOIS"))
Ejemplo n.º 19
0
 def __setitem__(self, key, value):
     setting = self.settings_grp[key]
     msg = PprzMessage("ground", "DL_SETTING")
     msg['ac_id'] = self.ac_id
     msg['index'] = setting.index
     msg['value'] = value
     self.ivy.send(msg)
Ejemplo n.º 20
0
    def on_mouse(self, event, x, y, flags, param):
        if event == cv2.EVENT_LBUTTONDOWN:
            self.mouse['start'] = (x, y)

        if event == cv2.EVENT_RBUTTONDOWN:
            self.mouse['start'] = None

        if event == cv2.EVENT_MOUSEMOVE:
            self.mouse['now'] = (x, y)

        if event == cv2.EVENT_LBUTTONUP:
            # If mouse start is defined, a region has been selected
            if not self.mouse.get('start'):
                return

            # Obtain mouse start coordinates
            sx, sy = self.mouse['start']

            # Create a new message
            msg = PprzMessage("datalink", "VIDEO_ROI")
            msg['ac_id'] = None
            msg['startx'] = sx
            msg['starty'] = sy
            msg['width'] = abs(x - sx)
            msg['height'] = abs(y - sy)
            msg['downsized_width'] = self.frame.shape[1]

            # Send message via the ivy interface
            self.ivy.send_raw_datalink(msg)

            # Reset mouse start
            self.mouse['start'] = None
Ejemplo n.º 21
0
    def mission_goto_wp(self, east, north, up, duration=0, insert_mode=0):
        """Send a MISSION_GOTO_WP command to paparazzi.

        Arguments:
            east: position east
            north: position north
            up: position up (in meters above sea level)
            duration: duration of mission before being discarded. Default is 0,
                meaning no specified duration.
            insert_mode: default APPEND
                0: APPEND
                1: PREPEND
                2: REPLACE_CURRENT
                3: REPLACE_ALL
        """
        msg = PprzMessage("datalink", "MISSION_GOTO_WP")
        msg['ac_id'] = self.ac_id
        # APPEND, PREPEND, REPLACE_CURRENT, REPLACE_ALL
        msg['insert'] = insert_mode
        # ENU to NED coordinates
        msg['wp_east'] = east
        msg['wp_north'] = north
        msg['wp_alt'] = up
        msg['duration'] = duration
        self.ivy_mes.send_raw_datalink(msg)
Ejemplo n.º 22
0
    def _message_req(self, msg_name, cb, params=None):
        bind_id = None

        def _cb(sender, msg):
            if bind_id is not None:
                self._ivy.unsubscribe(bind_id)
            cb(sender, msg)

        req_id = self._get_req_id()
        req_regex = '^{} ([^ ]* +{}( .*|$))'.format(req_id, msg_name)
        bind_id = self._ivy.subscribe(_cb, req_regex)
        req_msg = PprzMessage('ground', '{}_REQ'.format(msg_name))
        if params is not None:
            req_msg.set_values(params)
        #FIXME we shouldn't use directly Ivy, but pprzlink python API is not supporting the request id for now
        IvySendMsg('pprz_connect {} {} {}'.format(
            req_id, req_msg.name, req_msg.payload_to_ivy_string()))
Ejemplo n.º 23
0
def receiveRigidBodyList( rigidBodyList, stamp ):
    for (ac_id, pos, quat, valid) in rigidBodyList:
        if not valid:
            # skip if rigid body is not valid
            continue

        i = str(ac_id)
        if i not in id_dict.keys():
            continue

        store_track(i, pos, stamp)
        if timestamp[i] is None or abs(stamp - timestamp[i]) < period:
            if timestamp[i] is None:
                timestamp[i] = stamp
            continue # too early for next message
        timestamp[i] = stamp

        msg = PprzMessage("datalink", "REMOTE_GPS_LOCAL")
        msg['ac_id'] = id_dict[i]
        msg['pad'] = 0
        msg['enu_x'] = pos[0]
        msg['enu_y'] = pos[1]
        msg['enu_z'] = pos[2]
        vel = compute_velocity(i)
        msg['enu_xd'] = vel[0]
        msg['enu_yd'] = vel[1]
        msg['enu_zd'] = vel[2]
        msg['tow'] = int(1000. * stamp) # TODO convert to GPS itow ?
        # convert quaternion to psi euler angle
        dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
        dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
        msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / 3.14
        ivy.send(msg)

        # send GROUND_REF message if needed
        if args.ground_ref:
            gr = PprzMessage("ground", "GROUND_REF")
            gr['ac_id'] = str(id_dict[i])
            gr['frame'] = "LTP_ENU"
            gr['pos'] = pos
            gr['speed'] = vel
            gr['quat'] = [quat[3], quat[0], quat[1], quat[2]]
            gr['rate'] = [ 0., 0., 0. ]
            gr['timestamp'] = stamp
            ivy.send(gr)
Ejemplo n.º 24
0
 def move_waypoint(self, ac_id, wp_id, lat, lon, alt):
     msg = PprzMessage("ground", "MOVE_WAYPOINT")
     msg['ac_id'] = ac_id
     msg['wp_id'] = wp_id
     msg['lat'] = lat
     msg['long'] = lon
     msg['alt'] = alt
     print("Sending message: %s" % msg)
     self._interface.send(msg)
Ejemplo n.º 25
0
 def send_accelerations(self):
     msg = PprzMessage("datalink", "DESIRED_SETPOINT")
     msg['ac_id'] = self.id
     msg['flag'] = 0  # full 3D not supported yet
     # Receiving position in ENU, but sending in NED:
     msg['uy'] = self.U[0][0]
     msg['ux'] = self.U[1][0]
     msg['uz'] = 20  #self.U[2][0]     # Sending altitude, not acceleration
     self._interface.send(msg)
Ejemplo n.º 26
0
    def __init__(self, verbose=False):
        from pprzlink.ivy import IvyMessagesInterface

        # init Ivy interface
        self.pprzivy = IvyMessagesInterface("pyueye")
        # init cam related part
        uEyePprzlink.__init__(self, verbose)
        # bind to message
        self.pprzivy.subscribe(self.process_msg,
                               PprzMessage("telemetry", "DC_SHOT"))
Ejemplo n.º 27
0
    def on_ivy_msg(self, agent, *larg):
        """ Split ivy message up into the separate parts
        Basically parts/args in string are separated by space, but char array can also contain a space:
        |f,o,o, ,b,a,r| in old format or "foo bar" in new format
        """
        # return if no callback is set
        if self.callback is None:
            return

        # first split on array delimiters
        l = re.split('([|\"][^|\"]*[|\"])', larg[0])
        # strip spaces and filter out emtpy strings
        l = [str.strip(s) for s in l if str.strip(s) is not '']
        data = []
        for s in l:
            # split non-array strings further up
            if '|' not in s and '"' not in s:
                data += s.split(' ')
            else:
                data.append(s)
        # ignore ivy message with less than 3 elements
        if len(data) < 3:
            return

        # check which message class it is
        msg_name = data[1]
        msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
        if msg_class is None:
            print("Ignoring unknown message " + larg[0])
            return
        # pass non-telemetry messages with ac_id 0
        if msg_class == "telemetry":
            try:
                ac_id = int(data[0])
            except ValueError:
                print("ignoring message " + larg[0])
                sys.stdout.flush()
        else:
            ac_id = 0
        values = list(filter(None, data[2:]))
        msg = PprzMessage(msg_class, msg_name)
        msg.set_values(values)
        self.callback(ac_id, msg)
Ejemplo n.º 28
0
 def get_parameter_list(missionType):
     if missionType not in MissionBuilder.missionMessagesNames:
         raise ValueError(missionType + " is not a MISSION")
     msg = PprzMessage('datalink', missionType)
     params = []
     for field in msg.fieldnames:
         # not outputing params common to all missions
         if field not in MissionBuilder.commonParameters:
             params.append(field)
     return params
Ejemplo n.º 29
0
    def on_ivy_msg(self, agent, *larg):
        """ Split ivy message up into the separate parts
        Basically parts/args in string are separated by space, but char array can also contain a space:
        |f,o,o, ,b,a,r| in old format or "foo bar" in new format
        """
        # return if no callback is set
        if self.callback is None:
            return

        # first split on array delimiters
        l = re.split('([|\"][^|\"]*[|\"])', larg[0])
        # strip spaces and filter out emtpy strings
        l = [str.strip(s) for s in l if str.strip(s) is not '']
        data = []
        for s in l:
            # split non-array strings further up
            if '|' not in s and '"' not in s:
                data += s.split(' ')
            else:
                data.append(s)
        # ignore ivy message with less than 3 elements
        if len(data) < 3:
            return

        # check which message class it is
        msg_name = data[1]
        msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
        if msg_class is None:
            print("Ignoring unknown message " + larg[0])
            return
        # pass non-telemetry messages with ac_id 0
        if msg_class == "telemetry":
            try:
                ac_id = int(data[0])
            except ValueError:
                print("ignoring message " + larg[0])
                sys.stdout.flush()
        else:
            ac_id = 0
        values = list(filter(None, data[2:]))
        msg = PprzMessage(msg_class, msg_name)
        msg.set_values(values)
        self.callback(ac_id, msg)
Ejemplo n.º 30
0
    def envoi_cmd(self, phi, theta, psi, throttle):

        msg = PprzMessage("datalink", "JOYSTICK_RAW")
        msg['ac_id'] = self.drone.id
        msg['roll'] = phi
        msg['pitch'] = theta
        msg['yaw'] = psi
        msg['throttle'] = throttle

        self._interface.send_raw_datalink(msg)
def line(shape_id):
    msg_shape = PprzMessage("ground", "SHAPE")
    msg_shape['id'] = shape_id
    msg_shape['linecolor'] = 'purple'
    msg_shape['shape'] = Shape.LINE.value
    msg_shape['status'] = Status.CREATE.value
    msg_shape['latarr'] = [int(43.46296 * 1e7), int(43.46216 * 1e7)]
    msg_shape['lonarr'] = [int(1.272 * 1e7), int(1.2744 * 1e7)]
    msg_shape['text'] = "runway"
    return msg_shape
Ejemplo n.º 32
0
 def set_nav_mode(self):
     """
     change auto2 mode to NAV.
     """
     if self.auto2_index is not None:
         msg = PprzMessage("ground", "DL_SETTING")
         msg['ac_id'] = self.ac_id
         msg['index'] = self.auto2_index
         msg['value'] = 13  # AP_MODE_NAV
         print("Setting mode to NAV: %s" % msg)
         self._interface.send(msg)
Ejemplo n.º 33
0
def worldenv_cb(ac_id, msg):
    """
        Callback for paparazzi WORLD_ENV requests
    """
    # request location (in meters)
    east, north, up = float(msg.get_field(3)),\
        float(msg.get_field(4)),\
        float(msg.get_field(5))
    up *= -1
    # convert in km + translation with mesoNH origin
    weast, wnorth, wup = get_wind(east, north, up)
    print("wind_est:")
    print(weast)
    print(wnorth)
    print(wup)
    msg_back=PprzMessage("ground", "WORLD_ENV")
    msg_back.set_value_by_name("wind_east",weast)
    msg_back.set_value_by_name("wind_north",wnorth)
    msg_back.set_value_by_name("wind_up",wup)
    msg_back.set_value_by_name("ir_contrast",400)
    msg_back.set_value_by_name("time_scale",1)
    msg_back.set_value_by_name("gps_availability",1)
    ivy.send(msg_back,None)