机器人学强化训练，适用动态避障，采用算法为RRT做全局规划，人工势场法作为局部规划.zip

机器人学强化训练，适用动态避障，采用算法为RRT做全局规划，人工势场法作为局部规划C++代码。

#define _WINSOCK_DEPRECATED_NO_WARNINGS
#include  //////////////////////////////////////////////////////////////////////////////
#include 
#include 
#include “Ws2tcpip.h“
#include “Winsock2.h“
#include “zss_cmd.pb.h“
#include “vision_detection.pb.h“
#include “zss_debug.pb.h“
#include 
#include 
#include 
#include 
#include  
#include  
#include  
using std::cin;
using std::cout;
using std::cerr;
using std::endl;
double ROBO[30][2];
int NumPoint = 0 IniSizeBlueScore=-1;
double cost_path[500001];//500*500的地图共250000个点，留有冗余
double rrt_point[10000][2];
#define PI 3.14159265
#define ME 0
#define MAX_V 300
#define mid_JS_range 100
#define final_JS_range 120
double dis_lev = 0;
int Count = 0;
double D_heading[2000];
Vision_Detectionframe frame;
Vision_DetectionBall  balls;
Vision_DetectionRobot robots_yellow;
void getROBO（）
{
	for （int ii = 0; ii < frame.robots_blue_size（）; ++ii）
	{
		int i = frame.robots_blue（ii）.robot_id（）;
		//cout << “RBid=   “ << i << endl;
		Vision_DetectionRobot robots_blue = frame.robots_blue（ii）;
		ROBO[i][1] = robots_blue.x（） / 10.0 + 300;
		ROBO[i][0] = 450 - （robots_blue.y（） / 10.0 + 225）;
		//cout << ROBO[i][0] << “ “ << ROBO[i][1] << endl;
		//cout << /*robots_blue.raw_vel_x（） << robots_blue.raw_vel_y（） << */robots_blue.orientation（） << endl;
	}
	for （int ii = 0; ii < frame.robots_yellow_size（）; ++ii）
	{
		int i = frame.robots_yellow（ii）.robot_id（）;
		//cout << “RBid=   “ << i << endl;
		Vision_DetectionRobot robots_yellow = frame.robots_yellow（ii）;
		ROBO[i + 16][1] = robots_yellow.x（） / 10.0 + 300;
		ROBO[i + 16][0] = 450 - （robots_yellow.y（） / 10.0 + 225）;
		//cout << ROBO[i][0] << “ “ << ROBO[i][1] << endl;
	}
}
double barrier_x[35];
double barrier_y[35];
double barrier_angle[35];
double distance[35];
double barrier_V[35];


double board_angle[8];
double board_dis[8];
double board_V[8];

void getBarrier（）
{
	for （int i = 0; i <= 31; ++i） barrier_x[i] = barrier_y[i] = -1;

	for （int ii = 0; ii < frame.robots_blue_size（）; ++ii）
	{
		int i = frame.robots_blue（ii）.robot_id（）;
		//cout << “RBid=   “ << i << endl;
		barrier_x[i] = frame.robots_blue（ii）.x（） / 10;
		barrier_y[i] = -frame.robots_blue（ii）.y（） / 10;
		//cout << barrier_x[i] << “  “ << barrier_y[i] << endl;
	}
	for （int ii = 0; ii < frame.robots_yellow_size（）; ++ii）
	{
		int i = frame.robots_yellow（ii）.robot_id（）;
		//cout << “RBid=   “ << i << endl;
		barrier_x[i + 16] = frame.robots_yellow（ii）.x（） / 10;
		barrier_y[i + 16] = -frame.robots_yellow（ii）.y（） / 10;
		//cout << barrier_x[i+8] << “  “ << barrier_y[i+8] << endl;
	}

}

double MAX（double a double b）
{
	return （a > b） ? a : b;
}
struct point
{
	int parent;
	double pos_x pos_y;
};
int getIndex（double x double y）
{
	return int（int（x - 1） * 600 + int（y））;
}

char* getIP（）
{
	BYTE minorVer = 2;
	BYTE majorVer = 2

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2019-08-31 01:25  RRT-AG-master\
     文件       30429  2019-08-31 01:25  RRT-AG-master\动态3.cpp