无人机UAV获取程序和配置过程

无人机UAV获取程序和配置过程

通过socket，对局域网中的无人机进行连接，并获取其对应的uav和ip的字典

一. 地面站程序

最开始的思路是先获取所有在线主机的ip，对这些ip进行socket连接，并发送A1以获得其ip。但所使用的scapy库，无法获取多个网卡的主机ip，只能获得默认的网卡下的ip。因此选择暴力连接，对网段下所有的ip进行socket连接，并开启多线程。
程序返回为一个字典，包含uav值和ip数组。{‘uav3’: [‘192.168.3.11’, ‘192.168.1.128’]}
测试结果，win可以很快的对256*2个ip进行连接测试，但同样的程序在ubuntu中需要2分钟10s。解决方法，设置sock.settimeout(1)，可以迅速完成扫描。

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 显示局域网下所有主机的MAC地址和ip

# # 获取网段所有ip主机，只能获得默认的一张网卡的主机ip
# from scapy.all import srp, Ether, ARP, conf
# ipscans=['192.168.1.1/24', '192.168.3.1/24']         # 24是子网掩码，对应255.255.255.0
# def get_all_ip(ipscans=['192.168.1.1/24'], sacn_time=2):
#     ip = []
#     while sacn_time>0:
#         sacn_time = sacn_time-1
#         for ipscan in ipscans:
#             try:
#                 ans, unans = srp(Ether(dst="FF:FF:FF:FF:FF:FF")/ARP(pdst=ipscan), timeout=2, verbose=False)
#             except Exception as e:
#                 print(str(e))
#             else:
#                 for snd,rcv in ans:
#                     list_mac = rcv.sprintf("%Ether.src% - %ARP.psrc%")
#                     ip.append(str(rcv.sprintf("%ARP.psrc%")))
#     ip = list(set(ip))
#     return ip

import socket
import threading
# 强行获取所有的ip主机
ipscans=['192.168.1.', '192.168.3.']         # 24是子网掩码，对应255.255.255.0
def get_all_ip_max(ipscans=['192.168.1']):
    ip = []
    for ipscan in ipscans:
        for i in range(256):
            ip.append(ipscan+str(i))
            pass
    ip = list(set(ip))
    return ip

# 获得ip对应的uav
# def get_all_uav(ip=[], port=12321):
#     uav_ip = {}
#     if len(ip)==0:
#         return
    
#     for ip_host in ip:
#         sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
#         sock.settimeout(1)
#         BUFSIZE = 1024
#         ADDR = (ip_host, port)
#         try:
#             # print(ADDR)
#             client = sock.connect(ADDR)
#         except Exception as e:
#             # print('error',e)
#             # sock.close()
#             pass
#         else:
#             data = 'A1'.encode("utf-8")
#             sock.sendall((data)) #不要用send()
#             recv_data = sock.recv(BUFSIZE).decode("utf-8")
#             # 判断该uav是否已有
#             if(recv_data[0] == 'u'):
#                 if recv_data[:-1] in uav_ip:
#                     uav_ip[recv_data[:-1]].append(ip_host)
#                 else:
#                     uav_ip[recv_data[:-1]] = [ip_host]
#         finally:
#             sock.close()
#     return uav_ip


# 获得ip对应的uav，使用多线程，并且设置连接timeout为1，减少扫描时长
class ip_Thread(threading.Thread):
    def __init__(self, name, ip_host, port=12321):
        threading.Thread.__init__(self)
        self.name = name
        self.ip_host = ip_host
        self.port = port
        self.uav_ip = []        # 结果
    def run(self):
        # print ("开始线程：" + self.name)
        # 读取uav
        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        sock.settimeout(1)
        BUFSIZE = 1024
        ADDR = (self.ip_host, self.port)
        try:
            # print(ADDR)
            client = sock.connect(ADDR)
        except Exception as e:
            # print('error',e)
            # sock.close()
            pass
        else:
            data = 'A1'.encode("utf-8")
            sock.sendall((data)) #不要用send()
            recv_data = sock.recv(BUFSIZE).decode("utf-8")
            # 标准1：直接使用uav?，Ubuntu下不需要其他字符，Win下需要加一个回车
            if len(recv_data) > 0:
                if recv_data[0] == 'u':
                    # print(recv_data[:-1], self.ip_host)
                    # uav_ip[recv_data[:-1]] = [ip_host]
                    # self.uav_ip[recv_data[:-1]] = [self.ip_host]
                    self.uav_ip.append(recv_data[:-1])
                    self.uav_ip.append(self.ip_host)
                # 标准2：使用[uav?]，不需要考虑其他
                elif '[' in recv_data and ']' in recv_data:
                    self.uav_ip.append(recv_data[recv_data.index('[')+1 : recv_data.index(']')])
                    self.uav_ip.append(self.ip_host)
        finally:
            sock.close()
    def get_res(self):
        # 返回ip结果
        return self.uav_ip


def get_all_uav_threading(ip=[], port=12321):
    uav_ip = {}
    if len(ip)==0:
        return
    threads = []
    # 添加线程到线程列表
    for ip_host in ip:
        threads.append(ip_Thread(str(ip_host), ip_host))
    # 开启新线程
    for ip_t in threads:
        ip_t.start()
    # 等待所有线程完成
    for ip_t in threads:
        ip_t.join()
    # 合并所有的uav和ip
    for ip_t in threads:
        ip_res = ip_t.get_res()
        if len(ip_res) > 0:
            if ip_res[0] in uav_ip:
                uav_ip[ip_res[0]].append(ip_res[1])
            else:
                uav_ip[ip_res[0]] = [ip_res[1]]
    return uav_ip




if __name__ == '__main__':
    # ip = (get_all_ip(ipscans))
    # print(ip)
    # uav_ip = get_all_uav(ip)
    # print(uav_ip)

    ip = get_all_ip_max(ipscans)
    ip.append('localhost')
    uav_ip = get_all_uav_threading(ip)
    print(uav_ip)

二. 无人机配置

1. 新建文件夹uav_server

cd
mkdir uav_server
cd uav_server

2. 创建文件uav_id，里面第一行写入uav？，不要加任何其他东西包括空格和回车(但对于win系统，需要加一个回车)。或者标准二为[uav?]。

nano uav_id

3. 创建程序server.py

nano server.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

from socketserver import BaseRequestHandler,ThreadingTCPServer
import threading
import os

BUF_SIZE=1024
id_path  = './uav_id'

class Handler(BaseRequestHandler):
    def handle(self):
        while True:
            data = self.request.recv(BUF_SIZE)
            data = data.decode("utf-8")
            if len(data)>0 and data[0]=='A':
                cur_thread = threading.current_thread()
                response = ''
                with open(id_path, 'r') as id_file:
                    uav_id = str(id_file.readline())
                    response = uav_id
                self.request.sendall(response.encode("utf-8"))
                print('send:',response)
            else:
                print('close')
                break



if __name__ == '__main__':
    HOST = '0.0.0.0'
    PORT = 12321
    ADDR = (HOST, PORT)
    server = ThreadingTCPServer(ADDR, Handler)  #参数为监听地址和已建立连接的处理类
    print('listening')
    server.serve_forever()  #监听，建立好TCP连接后，为该连接创建新的socket和线程，并由处理类中的handle方法处理

三、无人机服务器开机自启动

1. 默认地址下新建脚本uav.sh

cd
nano uav.sh

#!/bin/bash

cd uav_server
python3 ./server.py

wait
exit 0

sudo chmod +x uav.sh

2. 设置开机自启

cd /etc/init.d/
sudo nano uav_on

#!/bin/bash

### BEGIN INIT INFO
# Provides:          tju
# Required-Start:    $local_fs $syslog $network
# Required-Stop:     $local_fs $syslog $network
# Default-Start:     2 3 4 5
# Default-Stop:      0 1 6
# Short-Description: ros service
# Description:       ros service daemon

cd /home/tju
sudo echo '123456' | sudo -S "./uav.sh"

wait
exit 0

sudo chmod +x uav_on
sudo systemctl enable uav_on
sudo systemctl start uav_on

四、rosbridge安装

sudo apt-get install -y ros-melodic-rosbridge-server
sudo apt-get install -y ros-melodic-tf2-web-republisher

sudo apt-get install -y ros-noetic-rosbridge-server
sudo apt-get install -y ros-noetic-tf2-web-republisher

五、ROS PX4开机自启配置

1. 新建ros.sh

cd 
nano ros.sh

#!/bin/bash

source /opt/ros/melodic/setup.bash
# source /opt/ros/noetic/setup.bash
sudo chmod +777 /dev/ttyACM0
sleep 2;
roslaunch mavros px4.launch fcu_url:="/dev/ttyACM0:921600" &
sleep 10;
roslaunch rosbridge_server rosbridge_websocket.launch &
wait
exit 0

sudo chmod +x ros.sh

2. 新建ros_on

cd
cd /etc/init.d/
sudo nano ros_on

#!/bin/bash

### BEGIN INIT INFO
# Provides:          tju
# Required-Start:    $local_fs $syslog $network
# Required-Stop:     $local_fs $syslog $network
# Default-Start:     2 3 4 5
# Default-Stop:      0 1 6
# Short-Description: ros service
# Description:       ros service daemon

cd /home/tju
echo '123456' | sudo -S "./ros.sh"

wait
exit 0

sudo chmod +x ros_on
sudo systemctl enable ros_on