diff --git a/src/Astar/Astar-level6.py b/src/Astar/Astar-level6.py deleted file mode 100644 index b4dca24..0000000 --- a/src/Astar/Astar-level6.py +++ /dev/null @@ -1,213 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt - -#Astar算法 -def A_star(org, des): - open_list = [] - close_list = [] - open_list.append(org) - while len(open_list) > 0: - - temp_node = open_list[0] - for node in open_list: - if node.f < temp_node.f: - temp_node = node - current_node= temp_node - - - open_list.remove(current_node) - close_list.append(current_node) - #找到当前节点的所有邻近节点 - node_list = [] - if Is_valid(current_node.x, current_node.y - 1, open_list, close_list): - node_list.append(Node(current_node.x, current_node.y - 1)) - if Is_valid(current_node.x, current_node.y + 1, open_list, close_list): - node_list.append(Node(current_node.x, current_node.y + 1)) - if Is_valid(current_node.x - 1, current_node.y, open_list, close_list): - node_list.append(Node(current_node.x - 1, current_node.y)) - if Is_valid(current_node.x + 1, current_node.y, open_list, close_list): - node_list.append(Node(current_node.x + 1, current_node.y)) - if Is_valid(current_node.x - 1, current_node.y - 1, open_list, close_list): - node_list.append(Node(current_node.x - 1, current_node.y - 1)) - if Is_valid(current_node.x + 1, current_node.y + 1, open_list, close_list): - node_list.append(Node(current_node.x + 1, current_node.y + 1)) - if Is_valid(current_node.x - 1, current_node.y + 1, open_list, close_list): - node_list.append(Node(current_node.x - 1, current_node.y + 1)) - if Is_valid(current_node.x + 1, current_node.y - 1, open_list, close_list): - node_list.append(Node(current_node.x + 1, current_node.y - 1)) - neighbors = node_list - for node in neighbors: - if node not in open_list: - - node.init_node(current_node, des) - open_list.append(node) - - for node in open_list: - if (node.x == des.x) and (node.y == des.y): - return node - - return None - -#邻点是否可用 -def Is_valid(x,y,open_list=[],close_list=[]): - - - if x < 0 or x >=len(MAZE) or y < 0 or y >= len(MAZE[0]): - return False - - if MAZE[x][y] == 1: - return False - - if Is_contain(open_list,x,y): - return False - - if Is_contain(close_list,x,y): - return False - return True - - -def Is_contain(nodes, x, y): - for node in nodes: - if (node.x == x) and (node.y == y): - return True - return False - - -class Node: - def __init__(self, x, y): - self.x = x - self.y = y - self.f = 0 - self.g = 0 - self.h = 0 - self.parent = None - - -#对角线移动是直行移动代价的1.4倍 -#如果相邻节点已有父节点，则父节点是否更改以使g值最小为准 - def init_node(self, parent, end): - if self.parent is not None: - if parent is not None: - if abs(self.x - parent.x) + abs(self.y - parent.y) == 1: - if self.g > parent.g + 10: - self.g = parent.g + 10 - self.parent = parent - self.h = (abs(self.x - end.x) + abs(self.y - end.y))*10 - self.f = self.g + self.h - else: - if self.g > parent.g + 20: - self.g = parent.g + 20 - self.parent = parent - self.h = (abs(self.x - end.x) + abs(self.y - end.y))*10 - self.f = self.g + self.h - else: - if parent is not None: - if abs(self.x - parent.x) + abs(self.y - parent.y) == 1: - self.g = parent.g + 10 - self.parent = parent - self.h = (abs(self.x - end.x) + abs(self.y - end.y))*10 - self.f = self.g + self.h - else: - self.g = parent.g + 20 - self.parent = parent - self.h = (abs(self.x - end.x) + abs(self.y - end.y))*10 - self.f = self.g + self.h - -def draw_path(map_grid,path): - obstacle_x=[] - obstacle_y=[] - path_x=[] - path_y=[] - close_list_x=[] - close_list_y=[] - open_list_x=[] - open_list_y=[] - for i in range(len(map_grid)): - for j in range(len(map_grid[0])): - if map_grid[i][j]==1: #栅格地图上obstacle为障碍物标识 - obstacle_x.append(j) - obstacle_y.append(len(map_grid)-1-i) - plt.figure(figsize=(10,10)) #为了防止x,y轴间隔不一样长，影响最后的表现效果，所以手动设定等长 - plt.ylim(-0.5,len(map_grid)-0.5) - plt.xlim(-0.5,len(map_grid[0])-0.5) - my_y_ticks = np.arange(0, len(map_grid), 1) - my_x_ticks = np.arange(0, len(map_grid[0]), 1) - plt.xticks(my_x_ticks)#竖线的位置与间隔 - plt.yticks(my_y_ticks) - plt.grid(True) #开启栅格 - for point in path: - path_x.append(point.y) - path_y.append(len(map_grid)-1-point.x) - plt.plot(path_x,path_y,linewidth=3) - plt.scatter(open_list_x,open_list_y,s=500,c='cyan',marker='s') - plt.scatter(obstacle_x,obstacle_y,s=500,c='k',marker='s') - plt.scatter(close_list_x,close_list_y,s=500,c='g',marker='s') - plt.title("grid map simulation ") - plt.show() -""" - if map_grid[i][j]==closelist: #栅格地图上closelist为为闭列表中记录的位置标志 - close_list_x.append(i) - close_list_y.append(j) - if map_grid[i][j]==openlist: #栅格地图上openlist为为闭列表中记录的位置标志 - open_list_x.append(i) - open_list_y.append(j) -""" -#暂时不画closelist和openlist - -#迷宫地图 -MAZE = [ - [0,0,0,0,0,0,0,0,0,0,0,1,1,0,1,0,1,0,1,1,0,1,0], - [0,0,0,0,0,0,1,1,0,1,0,1,0,0,0,0,0,1,1,0,0,0,1], - [0,0,0,1,0,0,0,1,0,0,0,1,1,0,1,0,1,0,0,1,0,0,1], - [0,0,1,1,0,0,0,0,0,0,0,0,0,1,1,0,1,0,1,1,0,1,0], - [0,0,0,0,0,0,0,1,1,0,0,0,0,1,1,0,1,0,0,0,0,0,0], - [0,0,0,1,1,1,0,1,0,0,0,1,0,1,0,0,0,1,1,0,1,1,0], - [0,1,0,0,1,1,0,0,0,0,0,1,1,0,1,0,0,0,0,0,1,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,1,0,1,0,1,0], - [1,0,0,0,0,0,1,1,0,1,0,0,0,0,0,0,1,1,0,0,1,0,1], - [0,0,0,1,0,0,0,1,0,0,0,1,0,0,0,1,0,0,0,0,1,1,0], - [1,0,1,1,0,0,0,0,0,0,0,0,1,1,0,1,0,1,1,0,1,0,1], - [0,0,0,0,0,0,0,1,1,0,0,0,0,0,1,0,0,0,1,0,1,0,1], - [0,0,0,1,1,1,0,1,0,0,0,0,1,1,1,1,0,1,0,0,0,1,1], - [0,1,0,0,1,1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,1], - [0,0,0,0,0,1,0,0,0,0,0,0,0,1,1,0,1,0,1,0,0,1,1], - [0,0,0,0,0,0,0,0,0,0,0,1,1,0,1,0,1,0,1,1,0,1,0], - [0,0,0,0,0,0,1,1,0,1,0,1,0,0,0,0,0,1,1,0,0,0,1], - [0,0,0,1,0,0,0,1,0,0,0,1,1,0,1,0,1,0,0,1,0,0,1], - [0,0,1,1,0,0,0,0,0,0,0,0,0,1,1,0,1,0,1,1,0,1,0], - [0,0,0,0,0,0,0,1,1,0,0,0,0,1,1,0,1,0,0,0,0,0,0], - [0,0,0,1,1,1,0,1,0,0,0,1,0,1,0,0,0,1,1,0,1,1,0], - [0,1,0,0,1,1,0,0,0,0,0,1,1,0,1,0,0,0,0,0,1,0,0], - [0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,1,0,1,0,1,0], -] - -#设置起始点和终点 -org = Node(22, 0) -des = Node(0, 22) - -result_node = A_star(org, des) - -path = [] - -minpath = result_node.g -count=0 - -while result_node is not None: - path.append(Node(result_node.x, result_node.y)) - if result_node.parent is not None: - if abs(result_node.x - result_node.parent.x) + abs(result_node.y - result_node.parent.y) == 2: - count+=1 - result_node = result_node.parent - -minpath-=count*6 - -for i in range(0,len(MAZE)): - for j in range(0,len(MAZE[0])): - if Is_contain(path, i, j): - MAZE[i][j]='*' - print("*, ", end='') - else: - print(str(MAZE[i][j]) + ", ", end='') - print() -print("最短路为：",minpath) -draw_path(MAZE,path) \ No newline at end of file diff --git a/src/Djitellowhite/Djitellowhite.cpp b/src/Djitellowhite/Djitellowhite.cpp deleted file mode 100644 index 12ad5ed..0000000 --- a/src/Djitellowhite/Djitellowhite.cpp +++ /dev/null @@ -1,106 +0,0 @@ -#include"Djitellowhite.h" -#include -#include - - -#include -#include -#include - -#include -#include - -using namespace cv; -using namespace std; -Tello::Tello(const char* IP) { - this->stream_on=false; - this->host = inet_addr(IP); - cout << "create successfully!" << endl; - -} -Tello::~Tello() {} -void Tello::set_conf(int socket){ - this->sock = socket; -} -void Tello::get_connect(sockaddr_in serveraddr) { - char tmp[10] = {"command"}; - send_message(tmp,serveraddr,0);// 0 -> timeout - cout << "connect successfully!" <host; -} -void Tello::takeoff(sockaddr_in serveraddr) { - char tmp[10] = {"takeoff"}; - send_message(tmp,serveraddr,0);// 0 -> timeout - cout << "takeoff successfully!" < timeout - cout << "land successfully!" <sock,buffer,strlen(buffer),0,(struct sockaddr *)&server_addr, sizeof(server_addr)); - return true; -} -void* Tello::udp_response_receiver(void* arg){ - //int m_SockServer; //创建socket对象 - //sockaddr_in serveraddr; //创建sockaddr_in对象储存自身信息(当有多个端口，可以多个绑定) - //sockaddr_in serveraddrfrom; - - //serveraddr.sin_family = AF_INET; //设置服务器地址家族 - //serveraddr.sin_port = htons(8889); //设置服务器端口号 - //serveraddr.sin_addr.s_addr = inet_addr("0.0.0.0"); - //m_SockServer = socket(AF_INET, SOCK_DGRAM, 0); //创建一个临时变量并赋值给m_SockServer - //int i = bind(m_SockServer, (sockaddr*)&serveraddr, sizeof(serveraddr)); //把名字和套接字绑定 - //std::cout << "bind:" << i << std::endl; - - int socket = *(int *)arg; - - cout << "receive start" << endl; - while(1){ - char buffer[20]; - int iret; - memset(buffer,0,sizeof(buffer)); - - iret = recv(socket, buffer,20,MSG_WAITALL); - if(iret > 0 && iret <10){ - printf("Message from tello: %s\n", buffer); - } - } -} -void Tello::get_video(sockaddr_in serveraddr){ - - char tmp[10] = {"streamon"}; - send_message(tmp,serveraddr,0);// 0 -> timeout -// cout << "stream_on successfully!" <stream_on = true; -// -// cout << "vid"; -// VideoCapture cap("udp://@0.0.0.0:11111"); -// cout << "cap"; -// if(!cap.isOpened()){ -// cout << "fuck off"; -// return; -// } - -// Mat frame; -// cout << "continue!"; -// while(1){ -// cap>>frame; -// if(frame.empty()) -// break; -// imshow("video",frame); -// waitKey(20); -// -// } -// cap.release(); - - -} diff --git a/src/Djitellowhite/Djitellowhite.h b/src/Djitellowhite/Djitellowhite.h deleted file mode 100644 index b311e46..0000000 --- a/src/Djitellowhite/Djitellowhite.h +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef _DJITELLOWHITE_H -#define _DJITELLOWHITE_H -#include -#include -#include -#include -#include -#include -/*Library for interacting with DJI Ryze Tello drones.*/ - - -//coding = utf - 8 - -/* """C++ wrapper to interact with the Ryze Tello drone using the official Tello api. - Tello API documentation: - [1.3](https://dl-cdn.ryzerobotics.com/downloads/tello/20180910/Tello%20SDK%20Documentation%20EN_1.3.pdf) -*/ - -#define RESPONSE_TIMEOUT 7 //in seconds -#define TAKEOFF_TIMEOUT 20 //in seconds -#define FRAME_GRAB_TIMEOUT 3 -#define TIME_BTW_COMMANDS 0.1 //in seconds -#define TIME_BTW_RC_CONTROL_COMMANDS 0.001 //in seconds -#define RETRY_COUNT 3 //number of retries after a failed command - - -//Video stream, server socket -#define VS_UDP_IP "0.0.0.0" -#define VS_UDP_PORT 11111 - -#define CONTROL_UDP_PORT 8889 -#define STATE_UDP_PORT 8890 - -#define BUFFER_SIZE 1024 -//Constants for video settings -#define BITRATE_AUTO 0 -#define BITRATE_1MBPS 1 -#define BITRATE_2MBPS 2 -#define BITRATE_3MBPS 3 -#define BITRATE_4MBPS 4 -#define BITRATE_5MBPS 5 -#define RESOLUTION_480P "low" -#define RESOLUTION_720P "high" -#define FPS_5 "low" -#define FPS_15 "middle" -#define FPS_30 "high" -#define CAMERA_FORWARD 0 -#define CAMERA_DOWNWARD 1 -//日志模块暂时不写 - -class Tello { -private: - int sock; - bool stream_on; - unsigned long host; -public: - Tello(const char* IP); - ~Tello(); - void set_conf(int socket); - unsigned long gethost(); - void get_connect(sockaddr_in serveraddr); - void takeoff(sockaddr_in serveraddr); - void land(sockaddr_in serveraddr); - bool send_message(char* msg,sockaddr_in server_addr,int timeout = RESPONSE_TIMEOUT); - static void *udp_response_receiver(void* arg); - void get_video(sockaddr_in serveraddr); -}; - - - - -#endif diff --git a/src/Djitellowhite/Hardwarelistener.h b/src/Djitellowhite/Hardwarelistener.h deleted file mode 100644 index 176cde8..0000000 --- a/src/Djitellowhite/Hardwarelistener.h +++ /dev/null @@ -1,6 +0,0 @@ -#ifndef _HARDWARELISTENER_H -#define _HARDWARELISTENER_H - -int listenkeyboard(); -int scanKeyboard(); -#endif diff --git a/src/Djitellowhite/Triplet b/src/Djitellowhite/Triplet deleted file mode 100755 index b514f91..0000000 Binary files a/src/Djitellowhite/Triplet and /dev/null differ diff --git a/src/Djitellowhite/Triplet.cpp b/src/Djitellowhite/Triplet.cpp deleted file mode 100644 index 39c2de7..0000000 --- a/src/Djitellowhite/Triplet.cpp +++ /dev/null @@ -1,45 +0,0 @@ -#include"Djitellowhite.h" -#include"Hardwarelistener.h" -#include -using namespace std; -int main(void){ - Tello T("192.168.10.1"); - int socket_fd, err; - - if((socket_fd = socket(PF_INET,SOCK_DGRAM,IPPROTO_UDP))==-1) - { - cout << "initialized error" << endl; - cout << 0; - return -1; - } - - cout << 1 << socket_fd; - T.set_conf(socket_fd); - - pthread_t thread_receive; - err = pthread_create(&thread_receive,nullptr,T.udp_response_receiver,&socket_fd); - sleep(3); - struct sockaddr_in serveraddr; - memset(&serveraddr,0,sizeof(serveraddr)); - serveraddr.sin_family=AF_INET; - serveraddr.sin_addr.s_addr=T.gethost(); - serveraddr.sin_port=htons(CONTROL_UDP_PORT); - - - - if(err!=0) - { - cout << err << " " << "can't create thread"; - } - - T.get_connect(serveraddr); - //sleep(1); - //T.takeoff(serveraddr); - T.get_video(serveraddr); - sleep(5); - //T.land(serveraddr); - - pthread_join(thread_receive,NULL); - listenkeyboard(); - return 0; -} diff --git a/src/Djitellowhite/keyboard b/src/Djitellowhite/keyboard deleted file mode 100755 index fb6acf5..0000000 Binary files a/src/Djitellowhite/keyboard and /dev/null differ diff --git a/src/Djitellowhite/keyboard.cpp b/src/Djitellowhite/keyboard.cpp deleted file mode 100644 index 3006537..0000000 --- a/src/Djitellowhite/keyboard.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include -#include -#include "Hardwarelistener.h" -#include -using namespace std; -int scanKeyboard() -{ - - int in; - - struct termios new_settings; - struct termios stored_settings; - //设置终端参数 - tcgetattr(0,&stored_settings); - new_settings = stored_settings; - new_settings.c_lflag &= (~ICANON); - new_settings.c_cc[VTIME] = 0; - tcgetattr(0,&stored_settings); - new_settings.c_cc[VMIN] = 1; - tcsetattr(0,TCSANOW,&new_settings); - in = getchar(); - tcsetattr(0,TCSANOW,&stored_settings); - - return in; - -} - -//测试函数 -int listenkeyboard(){ - while(1){ - switch(scanKeyboard()) - { - case (int)'w': - cout << "forward"<< endl; - break; - case (int)'s': - cout << "back" << endl; - break; - case (int)'a': - cout << "left" < -#include -#include -#include -#include -#include -#include -#include -#define PORT 8890 - -void udp_server(int sockfd) -{ - - socklen_t len; - char buf[1024] = {0}; - struct sockaddr_in server_addr; - int n; - int opt = 1; - len = sizeof(server_addr); - memset(&buf, 0, sizeof(buf)); - server_addr.sin_family = AF_INET; - server_addr.sin_addr.s_addr = htonl(INADDR_ANY); - server_addr.sin_port = htons(PORT); - - setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); //当服务器非正常断开的时候重启服务器，不会进入TIME_WAIT状态 - - if (bind(sockfd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) { - printf("can not bind\n"); - exit(1); - } - - while (1) { - printf("========wait for client's request========\n"); - n = recvfrom(sockfd, buf, sizeof(buf), 0, (struct sockaddr *)&server_addr, &len); - buf[n] = '\0'; - printf("receive client's data: %s\n", buf); - sendto(sockfd, buf, n, 0, (struct sockaddr *)&server_addr, len); - printf("send data to client: %s\n", buf); - } - - close(sockfd); -} - -int main(int argc, char **argv) -{ - int sockfd; - - if ((sockfd = socket(PF_INET, SOCK_DGRAM, 0)) < 0) { - printf("create socket false\n"); - exit(1); - } - - udp_server(sockfd); - - exit(0); -} - -