pvqf6mep3
/
Air_Ground_CEC
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

修改

Browse Source
pull/3/head
大耳刮子 3 years ago
parent 48d7c4b14c
commit a2e4be81a6
11 changed files with 0 additions and 552 deletions
Show Stats Download Patch File Download Diff File

213
src/Astar/Astar-level6.py
Unescape View File

@ -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)

106
src/Djitellowhite/Djitellowhite.cpp
Unescape View File

@ -1,106 +0,0 @@
#include"Djitellowhite.h"
#include<iostream>
#include<opencv2/opencv.hpp>
#include<stdio.h>
#include<sys/socket.h>
#include<string.h>
#include<arpa/inet.h>
#include<sys/types.h>
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!" <<endl;
}
unsigned long Tello::gethost(){
return this->host;
}
void Tello::takeoff(sockaddr_in serveraddr) {
char tmp[10] = {"takeoff"};
send_message(tmp,serveraddr,0);// 0 -> timeout
cout << "takeoff successfully!" <<endl;
}
void Tello::land(sockaddr_in serveraddr) {
char tmp[10] = {"land"};
send_message(tmp,serveraddr,0);// 0 -> timeout
cout << "land successfully!" <<endl;
}
bool Tello::send_message(char* msg,sockaddr_in server_addr,int timeout){
char buffer[BUFFER_SIZE];
memset(buffer,0,sizeof(buffer));
strcpy(buffer,msg);
//cout << buffer;
sendto(this->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!" <<endl;
// this->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();
}

72
src/Djitellowhite/Djitellowhite.h
Unescape View File

@ -1,72 +0,0 @@
#ifndef _DJITELLOWHITE_H
#define _DJITELLOWHITE_H
#include<iostream>
#include<sys/socket.h>
#include<arpa/inet.h>
#include<pthread.h>
#include<cstring>
#include<unistd.h>
/*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

6
src/Djitellowhite/Hardwarelistener.h
Unescape View File

@ -1,6 +0,0 @@
#ifndef _HARDWARELISTENER_H
#define _HARDWARELISTENER_H
int listenkeyboard();
int scanKeyboard();
#endif

BIN
src/Djitellowhite/Triplet
View File

Binary file not shown.

45
src/Djitellowhite/Triplet.cpp
Unescape View File

@ -1,45 +0,0 @@
#include"Djitellowhite.h"
#include"Hardwarelistener.h"
#include<iostream>
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;
}

BIN
src/Djitellowhite/keyboard
View File

Binary file not shown.

52
src/Djitellowhite/keyboard.cpp
Unescape View File

@ -1,52 +0,0 @@
#include <termio.h>
#include <stdio.h>
#include "Hardwarelistener.h"
#include <iostream>
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" <<endl;
break;
case (int)'d':
cout << "right"<<endl;
break;
}
}
return 0;
// while(1){
// printf("%d\n",scanKeyboard());
// }
}

BIN
src/Djitellowhite/libDjitellowhite.so
View File

Binary file not shown.

BIN
src/Djitellowhite/server
View File

Binary file not shown.

58
src/Djitellowhite/server.cpp
Unescape View File

@ -1,58 +0,0 @@
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#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);
}
Write Preview
Loading…
Cancel
Save