#include<iostream>
#include<cmath>
#include <stdexcept>
#include <iostream>
#include <vector>
#include <cmath>
#include <ros/ros
.h>
#include <sensor_msgs/Po
intCloud2
.h>
#include <pcl_conversions/pcl_conversions
.h>
#include "std_msgs/Float64
.h"
#include "geometry_msgs/Po
int.h"
#include "sensor_msgs/Imu
.h"
#include "Spline
.h"
#include "ins/ASENSING
.h"
#include <std_msgs/Bool
.h>
#include <visualization_msgs/Marker
.h>
#include "jsk_recognition_msgs/BoundingBoxArray
.h"
#define D_ROAD_W 0
.08 //道路宽度采样间隔 [m]
using namespace std;
int load_go=0;
// bool start_go=true;//跑包注释
bool start_go;
bool load_yaw=false;
bool load_location=false;
bool correct_data_get=false;
bool po
int_cloud_get = false;
double imu_speed;
pcl::Po
intCloud<pcl::Po
intXYZRGB>::Ptr cloud
(new pcl::Po
intCloud<pcl::Po
intXYZRGB>
);
double x_array[26];///改点注释1111111111111111111111111111111
double y_array[26];///改点注释2222222222222222222222
class Po
int
{
public:
double x ; // m
double y ; // m
};
class Vehicle_State
{
public:
double x; // m
double y; // m
double yaw; // degree
};
class Vehicle_Trajectory
{
public:
vector<double> x;
vector<double> y;
double cf;
};
int last_tem_index =9;
ros::Publisher pub;
ros::Publisher pub_stop;
/**************/
ros::Publisher marker_pub;
ros::Publisher marker_all;
ros::Publisher marker_all2;
ros::Publisher pub_brake; //2023
.8
.11
/****************/
std_msgs::Float64 stop_sign;
std_msgs::Float64 break_flag; //2023
.8
.11
Vehicle_State state;
double correct_angle;
double correct_x;
double correct_y;
double angleDegrees = 0;
Po
int center1;//第一次更新后的左圆中心
Po
int center2;//第一次更新后的右圆中心
vector<Po
int> state_all;
vector<Po
int> obstruct;
vector<Po
int> obstruct_
left;
vector<Po
int> obstruct_
right;
vector<Po
int> center_
left;
vector<Po
int> center_
right;
vector<Po
int> obstruct_
left_nei;
vector<Po
int> obstruct_
left_wai;
vector<Po
int> obstruct_
right_nei;
vector<Po
int> obstruct_
right_wai;
vector<Po
int> obstruct_
left_nei_new;
vector<Po
int> obstruct_
left_wai_new;
vector<Po
int> obstruct_
right_nei_new;
vector<Po
int> obstruct_
right_wai_new;
vector<Po
int> path_global_location;
Vehicle_Trajectory tra_best;
int location_index = 0;
int load = 0;
Po
int center_
left_final;
Po
int center_
right_final;
vector<Po
int>
left_center_sum;
vector<Po
int>
right_center_sum;
double way_x[26];//改点注释141414141414141414
double way_y[26];
double distance
(double x1,double y1,double x2,double y2
)
{
return sqrt
((x2-x1
)*
(x2-x1
)+
(y2-y1
)*
(y2-y1
));
}
Po
int findCircleCenter
(const std::vector<Po
int>& po
ints, double radius
) {
int n = po
ints
.size
();
// Create matrices for the linear system
Eigen::MatrixXd A
(n, 3
);
Eigen::VectorXd b
(n
);
for
(int i = 0; i < n; ++i
) {
A
(i, 0
) = 2 *
(po
ints[i]
.x - po
ints[0]
.x
);
A
(i, 1
) = 2 *
(po
ints[i]
.y - po
ints[0]
.y
);
A
(i, 2
) = -1;
b
(i
) = pow
(po
ints[i]
.x - po
ints[0]
.x, 2
) + pow
(po
ints[i]
.y - po
ints[0]
.y, 2
) - pow
(radius, 2
);
}
// Solve the least squares problem A * [cx, cy, d] = b
Eigen::VectorXd solution = A
.jacobiSvd
(Eigen::ComputeThinU | Eigen::ComputeThinV
).solve
(b
);
Po
int center;
// Extract circle center coordinates
center
.x = solution
(0
) + po
ints[0]
.x;
center
.y = solution
(1
) + po
ints[0]
.y;
return center;
}
void fitCircle
(const std::vector<Po
int>& po
ints, double& centerX, double& centerY, double& radius
) {
int n = po
ints
.size
();
if
(n < 3
) {
std::cerr << "At least 3 po
ints are required to fit a circle
." << std::endl;
return;
}
double sumX = 0, sumY = 0, sumX2 = 0, sumY2 = 0, sumXY = 0;
double sumX3 = 0, sumY3 = 0, sumX1Y2 = 0, sumX2Y1 = 0;
for
(const auto& p : po
ints
) {
double x = p
.x;
double y = p
.y;
double x2 = x * x;
double y2 = y * y;
sumX += x;
sumY += y;
sumX2 += x2;
sumY2 += y2;
sumXY += x * y;
sumX3 += x2 * x;
sumY3 += y2 * y;
sumX1Y2 += x * y2;
sumX2Y1 += x2 * y;
}
double C = n * sumX2 - sumX * sumX;
double D = n * sumXY - sumX * sumY;
double E = n * sumX3 + n * sumX1Y2 -
(sumX2 + sumY2
) * sumX;
double G = n * sumY2 - sumY * sumY;
double H = n * sumY3 + n * sumX2Y1 -
(sumX2 + sumY2
) * sumY;
double denominator = 2 *
(C * G - D * D
);
if
(denominator == 0
) {
std::cerr << "Denominator in circle fitting is zero
." << std::endl;
return;
}
centerX =
(G * E - D * H
) / denominator;
centerY =
(C * H - D * E
) / denominator;
radius = std::sqrt
((sumX2 + sumY2 - 2 * centerX * sumX - 2 * centerY * sumY + n *
(centerX * centerX + centerY * centerY
)) / n
);
}
void
interpolatePo
ints
(double*
original_x, double*
original_y, double* new_x, double* new_y,
int SIZE ,
int INTERVALS
) {
int index = 0;
for
(int i = 0; i < SIZE - 1; ++i
) {
double x1 =
original_x[i];
double y1 =
original_y[i];
double x2 =
original_x[i + 1];
double y2 =
original_y[i + 1];
double dx =
(x2 - x1
) /
INTERVALS;
double dy =
(y2 - y1
) /
INTERVALS;
for
(int j = 0; j <
INTERVALS; ++j
) {
new_x[index] = x1 + j * dx;
new_y[index] = y1 + j * dy;
index++;
}
}
// Add the last po
int from
original arrays
new_x[index] =
original_x[SIZE - 1];
new_y[index] =
original_y[SIZE - 1];
}
void set_path_global_location
()
{
//起点长12 yuanshi
double relative_x_chushi[190] = {1
.5,3
.5, 4
.50000000000000, 6, 7
.50000000000000, 9,\
10
.5000000000000, 12, 13
.5000000000000, 15, 16
.4929784907258, 17
.9458293799335, 19
.3198327455343,\
20
.5757278190614, 21
.6828360254628, 22
.6094864377429, 23
.3294552745182, 23
.8273423696585, 24
.0851743056398,\
24
.0979134140882, 23
.8680591804246, 23
.3954329981049, 22
.6979300314371, 21
.7946808320437, 20
.7048142903275,\
19
.4630603273459, 18
.1011332458409, 16
.6546373468543, 15
.1641312227858, 13
.6690456433255, 12
.2101604570310,\
10
.8256011255841, 9
.55468897394665, 8
.43058413047132, 7
.48147650023618, 6
.73850387747837, 6
.21667689392197,\
5
.92972783702215, 5
.89151242648978, 6
.09614442454558, 6
.54058093591135, 7
.21546395009521, 8
.09787282472529,\
9
.16744952227656, 10
.3947886499797, 11
.7450087033329, 13
.1840940648742, 14
.6717990386110, 15, 16
.4929784907258,\
17
.9458293799335, 19
.3198327455343, 20
.5757278190614, 21
.6828360254628, 22
.6094864377429, 23
.3294552745182,\
23
.8273423696585, 24
.0851743056398, 24
.0979134140882, 23
.8680591804246, 23
.3954329981049, 22
.6979300314371,\
21
.7946808320437, 20
.7048142903275, 19
.4630603273459, 18
.1011332458409, 16
.6546373468543, 15
.1641312227858,\
13
.6690456433255, 12
.2101604570310, 10
.8256011255841, 9
.55468897394665, 8
.43058413047132, 7
.48147650023618,\
6
.73850387747837, 6
.21667689392197, 5
.92972783702215, 5
.89151242648978, 6
.09614442454558, 6
.54058093591135,\
7
.21546395009521, 8
.09787282472529, 9
.16744952227656, 10
.3947886499797, 11
.7450087033329, 13
.1840940648742,\
14
.6717990386110, 15, 16
.4929784907258, 17
.9458293799335, 19
.3198327455343, 20
.5757278190614, 21
.6828360254628,\
22
.6094864377429, 23
.3294552745182, 23
.8273423696585, 24
.0851743056398, 24
.0979134140882, 23
.8680591804246,\
23
.3954329981049, 22
.6979300314371, 21
.7946808320437, 20
.7048142903275, 19
.4630603273459, 18
.1011332458409,\
16
.6546373468543, 15
.1641312227858, 13
.6690456433255, 12
.2101604570310, 10
.8256011255841, 9
.55468897394665,\
8
.43058413047132, 7
.48147650023618, 6
.73850387747837, 6
.21667689392197, 5
.92972783702215, 5
.89151242648978,\
6
.09614442454558, 6
.54058093591135, 7
.21546395009521, 8
.09787282472529, 9
.16744952227656, 10
.3947886499797,\
11
.7450087033329, 13
.1840940648742, 14
.6717990386110, 15, 16
.4929784907258, 17
.9458293799335, 19
.3198327455343,\
20
.5757278190614, 21
.6828360254628, 22
.6094864377429, 23
.3294552745182, 23
.8273423696585, 24
.0851743056398,\
24
.0979134140882, 23
.8680591804246, 23
.3954329981049, 22
.6979300314371, 21
.7946808320437, 20
.7048142903275,\
19
.4630603273459, 18
.1011332458409, 16
.6546373468543, 15
.1641312227858, 13
.6690456433255, 12
.2101604570310,\
10
.8256011255841, 9
.55468897394665, 8
.43058413047132, 7
.48147650023618, 6
.73850387747837, 6
.21667689392197,\
5
.92972783702215, 5
.89151242648978, 6
.09614442454558, 6
.54058093591135, 7
.21546395009521, 8
.09787282472529,\
9
.16744952227656, 10
.3947886499797, 11
.7450087033329, 13
.1840940648742, 14
.6717990386110, 15, 16
.5000000000000,\
19, 22, 25, 26, 27, 27
.5, 28, 30, 32, 34,36,37,38,39,40,41,42,43,44};
double relative_y_chushi[190] = {0,0, 0, 0, 0, 0, 0, 0, 0, 0, -0
.125512998485473, -0
.490926224144828,\
-1
.08816183366577, -1
.90503550632258, -2
.91432374260530, -4
.09153170739197, -5
.40532980676421, -6
.81832568457704,\
-8
.29414167929603, -9
.79219795955772, -11
.2726581312459, -12
.6943584686520, -14
.0201743080130, -15
.2155116851562,\
-16
.2435583500563, -17
.0816257944470, -17
.7058883021754, -18
.0959745771550, -18
.2468458811889, -18
.1508600590876,\
-17
.8102001737293, -17
.2379453204251, -16
.4446558358059, -15
.4543479571398, -14
.2951097755576, -12
.9940963464869,\
-11
.5898196518438, -10
.1193407357934, -8
.62161504175242, -7
.13755102147194, -5
.70681560192370, -4
.36927451073298,\
-3
.15859392661961, -2
.10954692453109, -1
.25047108785953, -0
.601302290877508, -0
.184672791857001, -0
.00847771368893407,\
0, -0
.125512998485473, -0
.490926224144828, -1
.08816183366577, -1
.90503550632258, -2
.91432374260530, -4
.09153170739197,\
-5
.40532980676421, -6
.81832568457704, -8
.29414167929603, -9
.79219795955772, -11
.2726581312459, -12
.6943584686520,\
-14
.0201743080130, -15
.2155116851562, -16
.2435583500563, -17
.0816257944470, -17
.7058883021754, -18
.0959745771550,\
-18
.2468458811889, -18
.1508600590876, -17
.8102001737293, -17
.2379453204251, -16
.4446558358059, -15
.4543479571398,\
-14
.2951097755576, -12
.9940963464869, -11
.5898196518438, -10
.1193407357934, -8
.62161504175242, -7
.13755102147194,\
-5
.70681560192370, -4
.36927451073298, -3
.15859392661961, -2
.10954692453109, -1
.25047108785953, -0
.601302290877508,\
-0
.184672791857001, -0
.00847771368893407, 0, 0
.125512998485473, 0
.490926224144828, 1
.08816183366577, 1
.90503550632258,\
2
.91432374260530, 4
.09153170739197, 5
.40532980676421, 6
.81832568457704, 8
.29414167929603, 9
.79219795955772, 11
.2726581312459,\
12
.6943584686520, 14
.0201743080130, 15
.2155116851562, 16
.2435583500563, 17
.0816257944470, 17
.7058883021754, 18
.0959745771550,\
18
.2468458811889, 18
.1508600590876, 17
.8102001737293, 17
.2379453204251, 16
.4446558358059, 15
.4543479571398, 14
.2951097755576,\
12
.9940963464869, 11
.5898196518438, 10
.1193407357934, 8
.62161504175242, 7
.13755102147194, 5
.70681560192370, 4
.36927451073298,\
3
.15859392661961, 2
.10954692453109, 1
.25047108785953, 0
.601302290877508, 0
.184672791857001, 0
.00847771368893407, 0,\
0
.125512998485473, 0
.490926224144828, 1
.08816183366577, 1
.90503550632258, 2
.91432374260530, 4
.09153170739197, 5
.40532980676421,\
6
.81832568457704, 8
.29414167929603, 9
.79219795955772, 11
.2726581312459, 12
.6943584686520, 14
.0201743080130, 15
.2155116851562,\
16
.2435583500563, 17
.0816257944470, 17
.7058883021754, 18
.0959745771550, 18
.2468458811889, 18
.1508600590876, 17
.8102001737293,\
17
.2379453204251, 16
.4446558358059, 15
.4543479571398, 14
.2951097755576, 12
.9940963464869, 11
.5898196518438, 10
.1193407357934,\
8
.62161504175242, 7
.13755102147194, 5
.70681560192370, 4
.36927451073298, 3
.15859392661961, 2
.10954692453109, 1
.25047108785953,\
0
.601302290877508, 0
.184672791857001, 0
.00847771368893407, 0, 0, 0,0,0,0,0,0,0,0,0,0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
double relative_x[950];
double relative_y[950];
interpolatePo
ints
(relative_x_chushi, relative_y_chushi, relative_x, relative_y,190,5
);
for
(int i=0;i<950;i++
) //12米
{
relative_x[i]=relative_x[i]+ correct_x;
relative_y[i]=relative_y[i]+ correct_y;
}
correct_angle = angleDegrees;
double po
int_center_x = 15
.0;
double po
int_center_y = 0
.0; //预设点集的八字中心点坐标
double cosAngle = cos
(-correct_angle* M_PI/180
.0
);
double sinAngle = sin
(-correct_angle* M_PI/180
.0
);
for
(int i=0;i<950;i++
) //12米
{
double temp_x = relative_x[i];
double temp_y = relative_y[i];
relative_x[i] = cosAngle *
(temp_x - po
int_center_x
) - sinAngle *
(temp_y - po
int_center_y
) + po
int_center_x;
relative_y[i] = sinAngle *
(temp_x - po
int_center_x
) + cosAngle *
(temp_y - po
int_center_y
) + po
int_center_y;
Po
int temp;
temp
.x = relative_x[i];
temp
.y = relative_y[i];
state_all
.push_back
(temp
);
}
Po
int trans_po
int;
for
(int i=0;i<950;i++
)//转换为世界坐标 改长注释22222222222222222222222222222
{
trans_po
int.x = state
.x +
(relative_x[i]+2
.38
)*cos
(state
.yaw*M_PI/180
) + relative_y[i]*sin
(state
.yaw*M_PI/180
);
trans_po
int.y = state
.y - relative_y[i]*cos
(state
.yaw*M_PI/180
) +
(relative_x[i]+2
.38
)*sin
(state
.yaw*M_PI/180
);
path_global_location
.push_back
(trans_po
int);
}
}
Po
int trans_local_xy
(Po
int global_xy
)//把相对于世界坐标的点转到激光雷达坐标系下
{
Po
int xy_local;
double trans_x =
(global_xy
.x-state
.x
)*cos
(-state
.yaw*M_PI/180
) -
(global_xy
.y-state
.y
)*sin
(-state
.yaw*M_PI/180
) - 2
.38;
double trans_y = -
(global_xy
.y-state
.y
)*cos
(-state
.yaw*M_PI/180
) -
(global_xy
.x-state
.x
)*sin
(-state
.yaw*M_PI/180
);
xy_local
.x = trans_x;
xy_local
.y = trans_y;
return xy_local;
}
void start_sign
(const std_msgs::Float64& msg
)
{
load_go = msg
.data;
cout<<"load_go= %d!!!"<<load_go<<endl;
if
(load_go==1
)
{
cout<<"start go!!!"<<endl;
start_go=true;
}else{
cout<<"wait for go"<<endl;
}
}
void zhuitongCallback
(const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& msg
)
{
obstruct
.clear
();
obstruct_
left.clear
();
obstruct_
right.clear
();
obstruct_
left_nei
.clear
();
obstruct_
left_wai
.clear
();
obstruct_
right_nei
.clear
();
obstruct_
right_wai
.clear
();
obstruct_
left_nei_new
.clear
();
obstruct_
left_wai_new
.clear
();
obstruct_
right_nei_new
.clear
();
obstruct_
right_wai_new
.clear
();
Po
int temp;
for
(int i = 0;i<msg->boxes
.size
();i++
)
{
temp
.x= msg->boxes[i]
.pose
.position
.x;
temp
.y= msg->boxes[i]
.pose
.position
.y;
float dis = distance
(0,0,temp
.x,temp
.y
);
if
(dis<1
.5||dis>29
)
{
continue;
}
obstruct
.push_back
(temp
);
if
(temp
.y>0
)
{
obstruct_
left.push_back
(temp
);
}else if
(temp
.y<0
)
{
obstruct_
right.push_back
(temp
);
}
}
if
(correct_data_get == false
)
{
if
(obstruct
.size
()>0
)
{
po
int_cloud_get = true;
}
if
(obstruct_
left.size
()>2
)
{
Po
int test;
test = findCircleCenter
(obstruct_
left,9
.125
);
for
(auto &obs_temp : obstruct_
left)
{
double dis_temp = distance
(obs_temp
.x,obs_temp
.y,test
.x,test
.y
);
if
(dis_temp>=9
.125
)
{
obstruct_
left_wai
.push_back
(obs_temp
);
}else
{
obstruct_
left_nei
.push_back
(obs_temp
);
}
}
}
// cout<<"x_
left:"<<test
.x<<endl;
// cout<<"y_
left:"<<test
.y<<endl;
if
(obstruct_
right.size
()>2
)
{
Po
int test2;
test2 = findCircleCenter
(obstruct_
right,9
.125
);
for
(auto &obs_temp2 : obstruct_
right)
{
double dis_temp2 = distance
(obs_temp2
.x,obs_temp2
.y,test2
.x,test2
.y
);
if
(dis_temp2>=9
.125
)
{
obstruct_
right_wai
.push_back
(obs_temp2
);
}else
{
obstruct_
right_nei
.push_back
(obs_temp2
);
}
}
}
// cout<<"x_
right:"<<test2
.x<<endl;
// cout<<"y_
right:"<<test2
.y<<endl;
if
(obstruct_
left_nei
.size
()>2
)
{
temp = findCircleCenter
(obstruct_
left_nei,7
.625
);
left_center_sum
.push_back
(temp
);
}
if
(obstruct_
left_wai
.size
()>2
)
{
temp = findCircleCenter
(obstruct_
left_wai,10
.625
);
left_center_sum
.push_back
(temp
);
}
if
(obstruct_
right_nei
.size
()>2
)
{
temp = findCircleCenter
(obstruct_
right_nei,7
.625
);
right_center_sum
.push_back
(temp
);
}
if
(obstruct_
right_wai
.size
()>2
)
{
temp = findCircleCenter
(obstruct_
right_wai,10
.625
);
right_center_sum
.push_back
(temp
);
}
// cout<<"
left:"<<
left_center_sum
.size
()<<endl;
// cout<<"
right:"<<
right_center_sum
.size
()<<endl;
if
(right_center_sum
.size
()>20&&
left_center_sum
.size
()>20
)
{
double
left_x_sum = 0;
double
left_y_sum = 0;
for
(int i = 0;i<
left_center_sum
.size
();i++
)
{
left_x_sum +=
left_center_sum[i]
.x;
left_y_sum +=
left_center_sum[i]
.y;
}
center1
.x =
left_x_sum/
(left_center_sum
.size
());
center1
.y =
left_y_sum/
(left_center_sum
.size
());
double
right_x_sum = 0;
double
right_y_sum = 0;
for
(int i = 0;i<
right_center_sum
.size
();i++
)
{
right_x_sum +=
right_center_sum[i]
.x;
right_y_sum +=
right_center_sum[i]
.y;
}
center2
.x =
right_x_sum/
(right_center_sum
.size
());
center2
.y =
right_y_sum/
(right_center_sum
.size
());
}
if
(center1
.x!=0&¢er1
.y!=0&¢er2
.x!=0&¢er2
.y!=0&&obstruct_
left.size
()>2&&obstruct_
right.size
()>2
)
{
for
(auto &obs_temp : obstruct_
left)
{
double dis_temp = distance
(obs_temp
.x,obs_temp
.y,center1
.x,center1
.y
);
if
(dis_temp>=9
.125
)
{
obstruct_
left_wai_new
.push_back
(obs_temp
);
}else
{
obstruct_
left_nei_new
.push_back
(obs_temp
);
}
}
for
(auto &obs_temp2 : obstruct_
right)
{
double dis_temp2 = distance
(obs_temp2
.x,obs_temp2
.y,center2
.x,center2
.y
);
if
(dis_temp2>=9
.125
)
{
obstruct_
right_wai_new
.push_back
(obs_temp2
);
}else
{
obstruct_
right_nei_new
.push_back
(obs_temp2
);
}
}
Po
int center;
if
(obstruct_
left_nei_new
.size
()>2
)
{
center = findCircleCenter
(obstruct_
left_nei_new,7
.625
);
center_
left.push_back
(center
);
}
if
(obstruct_
left_wai_new
.size
()>2
)
{
center = findCircleCenter
(obstruct_
left_wai_new,10
.625
);
center_
left.push_back
(center
);
}
if
(obstruct_
right_nei_new
.size
()>2
)
{
center = findCircleCenter
(obstruct_
right_nei_new,7
.625
);
center_
right.push_back
(center
);
}
if
(obstruct_
right_wai_new
.size
()>2
)
{
center = findCircleCenter
(obstruct_
right_wai_new,10
.625
);
center_
right.push_back
(center
);
}
Po
int left_sum;
Po
int right_sum;
if
(center_
left.size
()>20&¢er_
right.size
()>20
)
{
for
(int i = 0;i<center_
left.size
();i++
)
{
left_sum
.x += center_
left[i]
.x;
left_sum
.y += center_
left[i]
.y;
right_sum
.x += center_
right[i]
.x;
right_sum
.y += center_
right[i]
.y;
}
center_
left_final
.x =
left_sum
.x / center_
left.size
();
center_
left_final
.y =
left_sum
.y / center_
left.size
();
center_
right_final
.x =
right_sum
.x / center_
right.size
();
center_
right_final
.y =
right_sum
.y / center_
right.size
();
}
if
(center_
left_final
.x!=0&¢er_
left_final
.y!=0&¢er_
right_final
.x!=0&¢er_
right_final
.y!=0
)
{
center_
left_final
.x = center_
left_final
.x ;
center_
left_final
.y = center_
left_final
.y ;
center_
right_final
.x = center_
right_final
.x ;
center_
right_final
.y = center_
right_final
.y ;
correct_x =
(center_
left_final
.x + center_
right_final
.x
)/2 - 15;
correct_y =
(center_
left_final
.y + center_
right_final
.y
)/2 - 0;
double angle = atan2
(center_
right_final
.x - center_
left_final
.x, center_
right_final
.y - center_
left_final
.y
);
angleDegrees = angle * 180
.0 / M_PI-180;
if
(angleDegrees<-180
)
{
angleDegrees = angleDegrees+360
.0;
}
correct_data_get = true;
// cout<<"x:"<<correct_x+15<<endl;
// cout<<"y:"<<correct_y<<endl;
}
}
}
// visualization_msgs::Marker line_strip_all;
// line_strip_all
.header
.frame_id = "hesai40";
// line_strip_all
.header
.stamp = ros::Time::now
();
// line_strip_all
.ns = "trajectory_all";
// line_strip_all
.action = visualization_msgs::Marker::ADD;
// line_strip_all
.pose
.orientation
.w = 0
.0;
// line_strip_all
.id = 0;
// line_strip_all
.type = visualization_msgs::Marker::PO
INTS;
// // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
// line_strip_all
.scale
.x = 0
.5;
// // Line strip 是绿色
// line_strip_all
.color
.g = 1
.0f;
// line_strip_all
.color
.a = 1
.0;
// for
(int i = 0; i < obstruct_
left_nei_new
.size
(); ++i
)
// {
// geometry_msgs::Po
int p2;
// p2
.x = obstruct_
left_nei_new[i]
.x;
// p2
.y = obstruct_
left_nei_new[i]
.y;//
(int32_t
)
// p2
.z = 0;
// line_strip_all
.po
ints
.push_back
(p2
);
// }
// //发布各个markers
// marker_all
.publish
(line_strip_all
);
visualization_msgs::Marker line_strip_all2;
line_strip_all2
.header
.frame_id = "hesai40";
line_strip_all2
.header
.stamp = ros::Time::now
();
line_strip_all2
.ns = "trajectory_all2";
line_strip_all2
.action = visualization_msgs::Marker::ADD;
line_strip_all2
.pose
.orientation
.w = 0
.0;
line_strip_all2
.id = 0;
line_strip_all2
.type = visualization_msgs::Marker::PO
INTS;
// LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
line_strip_all2
.scale
.x = 0
.5;
// Line strip 是绿色
line_strip_all2
.color
.r = 1
.0f;
line_strip_all2
.color
.g = 0;
line_strip_all2
.color
.a = 1
.0;
geometry_msgs::Po
int p3;
p3
.x = center_
left_final
.x;
p3
.y = center_
left_final
.y;//
(int32_t
)
p3
.z = 0;
line_strip_all2
.po
ints
.push_back
(p3
);
p3
.x = center_
right_final
.x;
p3
.y = center_
right_final
.y;//
(int32_t
)
p3
.z = 0;
line_strip_all2
.po
ints
.push_back
(p3
);
//发布各个markers
marker_all2
.publish
(line_strip_all2
);
}
void update_yaw_location
(const ins::ASENSING::ConstPtr& msg
)
{
state
.yaw = msg->azimuth;
load_yaw = true;
state
.x = msg->location_x;
state
.y = msg->location_y;
load_location = true;
imu_speed=msg->absolute_velocity;
}
vector<Vehicle_Trajectory> calc_frenet_paths
(double x_array[],double y_array[],
int array_size
)
{
vector<Vehicle_Trajectory> frenet_paths;
vector<double> x_a,y_a;
vector<double> angle;
vector<Po
int> sp;
for
(int i = 0;i<array_size;++i
)
{
double at_angle;
if
(x_array[i] == 0
)
{
x_array[i] = 0
.000001;
}
at_angle = -atan
(y_array[i]/x_array[i]
);
angle
.push_back
(at_angle
);
}
for
(int i=-9;i<=+9;++i
)
{
Vehicle_Trajectory fp;
vector<double> s_x;
vector<double> s_y;
s_x
.push_back
(0
.0
);
s_y
.push_back
(0
.0
);
for
(int j = 0;j<array_size;++j
)
{
Po
int sp_temp;
sp_temp
.x = x_array[j]+i*D_ROAD_W*sin
(angle[j]
);
sp_temp
.y = y_array[j]+i*D_ROAD_W*cos
(angle[j]
);
s_x
.push_back
(sp_temp
.x
);
s_y
.push_back
(sp_temp
.y
);
}
for
(int i=0;i<10;++i
)
{
double h_x =
(s_x[i+1]-s_x[i]
)/9;
double h_y =
(s_y[i+1]-s_y[i]
)/9;
for
(int j=0;j<10;j++
)
{
fp
.x
.push_back
(h_x*j+s_x[i]
);
fp
.y
.push_back
(h_y*j+s_y[i]
);
x_a
.push_back
(h_x*j+s_x[i]
);
y_a
.push_back
(h_y*j+s_y[i]
);
}
}
frenet_paths
.push_back
(fp
);
}
return frenet_paths;
}
Vehicle_Trajectory calc_cost
(Vehicle_Trajectory tr_p,vector<Po
int> ob
)///计算路径点中离锥桶的最小距离
{
tr_p
.cf = 100;
for
(int i=0;i<ob
.size
();i++
)
{
for
(int j=0;j<tr_p
.x
.size
();j++
)
{
double dist = distance
(ob[i]
.x, ob[i]
.y,tr_p
.x[j], tr_p
.y[j]
);
if
(dist<tr_p
.cf
)
{
tr_p
.cf = dist;
}
}
}
return tr_p;
}
vector<Vehicle_Trajectory> check_paths
(vector<Vehicle_Trajectory> fplist, vector<Po
int> ob
)
{
vector<Vehicle_Trajectory> fpl;
Vehicle_Trajectory calc;
cout<<"ob
.size
()="<<ob
.size
()<<endl;
for
(int i=0;i<fplist
.size
();i++
)
{
calc = calc_cost
(fplist[i],ob
);// 碰撞检查 //计算每条路径点中离锥桶的最小距离
fpl
.push_back
(calc
);
}
return fpl;
}
Vehicle_Trajectory frenet_optimal_planning_nopo
int(double x_array[],double y_array[],
int array_size
)
{
vector<Vehicle_Trajectory> fplist;
fplist = calc_frenet_paths
(x_array,y_array,array_size
);
Vehicle_Trajectory bestpath = fplist[last_tem_index];
return bestpath;
}
Vehicle_Trajectory frenet_optimal_planning
(vector<Po
int> ob,double x_array[],double y_array[],
int &last_tem_index,
int array_size
) //最佳路径计算函数
{
vector<Vehicle_Trajectory> fplist;
fplist = calc_frenet_paths
(x_array,y_array,array_size
);
//cout<<"fplist
.size
()="<<fplist
.size
()<<endl;
vector<Vehicle_Trajectory> fpl_1;
fpl_1 = check_paths
(fplist, ob
);/////////计算每条路径与锥桶的最小距离
// cout<<"fpl_1
.size
()="<<fpl_1
.size
()<<endl;
int tem_index =last_tem_index;
int size=fpl_1
.size
();
if
(location_index<10
)
{
size=fpl_1
.size
()/2;
}
int start=0;
if
(location_index>84&&location_index<100
)
{
start=0;
size=fpl_1
.size
()*1/2;
}
if
(location_index>164&&location_index<180
)
{
start=fpl_1
.size
()/2;
size=fpl_1
.size
()*2/3;
cout<<"!!!!!!!!!!!!!!!!!!!!!!!"<<endl;
cout<<"33333333333333333333333"<<endl;
}
if
(location_index>=0&&location_index<10
)
{
for
(start;start<size;start++
)
{
if
(abs
(start-last_tem_index
)<3
)
{
if
(fpl_1[tem_index]
.cf < fpl_1[start]
.cf
)//找到与锥桶最小距离最小的路径序号
{
tem_index = start;
}
}
}
}
if
(location_index>=10&&location_index<84
)
{
for
(start;start<size;start++
)
{
if
(abs
(start-last_tem_index
)<3
)
{
if
(fpl_1[tem_index]
.cf < fpl_1[start]
.cf
)//找到与锥桶最小距离最小的路径序号
{
for
(start;start<16;start++
)
{
double t=fpl_1[start]
.cf;
if
(t>1
.0
)
{
tem_index = start;
break;}
}
if
(tem_index>13
){tem_index=13;}
if
(tem_index<4
){tem_index=4;}
}
}
}
}
if
(location_index>=84&&location_index<90
)
{
for
(start;start<size;start++
)
{
if
(abs
(start-last_tem_index
)<4
)
{
if
(fpl_1[tem_index]
.cf < fpl_1[start]
.cf
)//找到与锥桶最小距离最小的路径序号
{
double t=fpl_1[start]
.cf;
if
(t>1
.0
)
{
tem_index = start;
if
(tem_index>13
){tem_index=13;}
if
(tem_index<4
){tem_index=4;}
break;}
}
}
}
}
if
(location_index>=90&&location_index<180
)
{
for
(start;start<size;start++
)
{
if
(abs
(start-last_tem_index
)<4
)
{
if
(fpl_1[tem_index]
.cf < fpl_1[start]
.cf
)//找到与锥桶最小距离最小的路径序号
{
double t=fpl_1[start]
.cf;
if
(t>1
.0&&t<2
.0
)
{
tem_index = start;
if
(tem_index>13
){tem_index=13;}
if
(tem_index<4
){tem_index=4;}
break;}
}
}
}
}
cout<<"tem_index="<<tem_index<<endl;
Vehicle_Trajectory bestpath = fpl_1[tem_index];
last_tem_index =tem_index;
return bestpath;
}
int search_index
(Vehicle_State po
int_search,vector<Po
int> po
int_all
)
{
Vehicle_State current_po
int;
Vehicle_State po
int;
std::map<double,Vehicle_State>min_distance;
for
(auto &iter_p : po
int_all
)
{
double distance2 = std::sqrt
((po
int_search
.x - iter_p
.x
)*
(po
int_search
.x - iter_p
.x
)+
(po
int_search
.y - iter_p
.y
)*
(po
int_search
.y - iter_p
.y
));
current_po
int.x=iter_p
.x;
current_po
int.y=iter_p
.y;
min_distance
.insert
(std::make_pair
(distance2,current_po
int));
}
auto min = min_distance
.begin
();
po
int = min->second;
for
(int i=0;i<po
int_all
.size
();i++
)
{
if
(po
int.x==po
int_all[i]
.x&&po
int.y==po
int_all[i]
.y
)
{
if
(po
int.x<po
int_search
.x
)
{
return i+1;
}
return i;
}
}
return 0;
};
void path_plan
()
{
if
(load_yaw ==true && load_location==true && correct_data_get==true
)
{
sensor_msgs::Po
intCloud2 output;
pcl::Po
intCloud<pcl::Po
intXYZRGB>::Ptr cloud1
(new pcl::Po
intCloud<pcl::Po
intXYZRGB>
);
if
(load == 0
)
{
set_path_global_location
();
load++;
location_index = search_index
(state,path_global_location
);
cout<<"location_index:"<<location_index<<endl;
if
(location_index>50
)
{
location_index = 50;
}
location_index = 0;
}
double distance;//车前路径点与车的距离
distance = hypot
((path_global_location[location_index]
.x - state
.x
),
(path_global_location[location_index]
.y - state
.y
));
ROS_INFO
("distance=%f",distance
);
if
(distance <4
)//1111111111111111111111111111111111111111111111111小心!!!!!!!!!!!!!!
{
for
(int i=0;i<26;i++
)//车前路径点和顺下点 ///改点注释3333333333333333333333333333333333333
{
way_x[i] = path_global_location[location_index+i]
.x;
way_y[i] = path_global_location[location_index+i]
.y;
cout<<"x:"<<way_x[i]<<setprecision
(8
)<<endl;
cout<<"y:"<<way_y[i]<<setprecision
(8
)<<endl;
}
location_index++;
}
break_flag
.data=0; //2023
.8
.11
if
(location_index==435
)//87
{
break_flag
.data=35;
std::cout<<break_flag
.data<<"yyyyyyyyyyyyyyyyyyyyy"<<std::endl;
}
if
(location_index==430
)//86
{
break_flag
.data=45;
std::cout<<break_flag
.data<<"yyyyyyyyyyyyyyyyyyyyy"<<std::endl;
}
pub_brake
.publish
(break_flag
); //2023
.8
.11
for
(int i=0;i<26;i++
)//将路径点转换为激光雷达坐标下的点///改点注释444444444444444444444444444444
{
Po
int tr_xy;
Po
int lo_xy;
tr_xy
.x = way_x[i];
tr_xy
.y = way_y[i];
lo_xy = trans_local_xy
(tr_xy
);
x_array[i] = lo_xy
.x;
y_array[i] = lo_xy
.y;
}
int array_size = sizeof
(x_array
)/sizeof
(x_array[0]
);
if
(po
int_cloud_get==true
)
{
tra_best = frenet_optimal_planning
(obstruct,x_array,y_array,last_tem_index,array_size
);
po
int_cloud_get = false;
}
else
{
tra_best = frenet_optimal_planning_nopo
int(x_array,y_array,array_size
);
}
if
(location_index >= 850
)
{
stop_sign
.data = 1;
cout<<" stop_sign
.data = "<<stop_sign
.data<<endl;
ROS_INFO
("Vehicle has reached the target po
int! Node stop!"
);
pub_stop
.publish
(stop_sign
);
}
(*cloud1
).width=tra_best
.x
.size
();//zuiyou lujing //无序点云width为点云大小，heigh为１
(*cloud1
).height=1;
(*cloud1
).is_dense=false; //默认为false
(*cloud1
).po
ints
.resize
(tra_best
.x
.size
()); //点云大小
for
(int i=0;i<tra_best
.x
.size
();i++
)
{
(*cloud1
)[i]
.x=tra_best
.x[i];
(*cloud1
)[i]
.y=tra_best
.y[i];
// cout<<"
(*cloud1
)[i]
.x="<<
(*cloud1
)[i]
.x<<endl;
// cout<<"
(*cloud1
)[i]
.y="<<
(*cloud1
)[i]
.y<<endl;
(*cloud1
)[i]
.z=0;
(*cloud1
)[i]
.r=0;
(*cloud1
)[i]
.g=255;
(*cloud1
)[i]
.b=0;
}
pcl::toROSMsg
(*cloud1,output
); //第一个参数是输入，后面的是输出
output
.header
.frame_id = "hesai40";//输出的坐标
pub
.publish
(output
);///修改注释11111111111111111111111111111111111111111111111111111
visualization_msgs::Marker line_strip;
line_strip
.header
.frame_id = "hesai40";
line_strip
.header
.stamp = ros::Time::now
();
line_strip
.ns = "trajectory";
line_strip
.action = visualization_msgs::Marker::ADD;
line_strip
.pose
.orientation
.w = 0
.0;
line_strip
.id = 0;
line_strip
.type = visualization_msgs::Marker::LINE_STRIP;
// LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
line_strip
.scale
.x = 0
.1;
// Line strip 是绿色
line_strip
.color
.g = 1
.0f;
line_strip
.color
.a = 1
.0;
// cout<<"size:"<<tra_best
.x
.size
()<<endl;
for
(int i = 0; i < tra_best
.x
.size
(); ++i
)
{
geometry_msgs::Po
int p;
p
.x = tra_best
.x[i];
p
.y = tra_best
.y[i];//
(int32_t
)
p
.z = 0;
line_strip
.po
ints
.push_back
(p
);
}
//发布各个markers
marker_pub
.publish
(line_strip
);
visualization_msgs::Marker line_strip_all;
line_strip_all
.header
.frame_id = "hesai40";
line_strip_all
.header
.stamp = ros::Time::now
();
line_strip_all
.ns = "trajectory_all";
line_strip_all
.action = visualization_msgs::Marker::ADD;
line_strip_all
.pose
.orientation
.w = 0
.0;
line_strip_all
.id = 0;
line_strip_all
.type = visualization_msgs::Marker::LINE_STRIP;
// LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
line_strip_all
.scale
.x = 0
.1;
// Line strip 是绿色
line_strip_all
.color
.g = 1
.0f;
line_strip_all
.color
.a = 1
.0;
for
(int i = 0; i < path_global_location
.size
(); ++i
)
{
geometry_msgs::Po
int p2;
Po
int global_po
int;
global_po
int = trans_local_xy
(path_global_location[i]
);
p2
.x = global_po
int.x;
p2
.y = global_po
int.y;//
(int32_t
)
p2
.z = 0;
line_strip_all
.po
ints
.push_back
(p2
);
}
//发布各个markers
marker_all
.publish
(line_strip_all
);
// visualization_msgs::Marker line_strip_all;
// line_strip_all
.header
.frame_id = "hesai40";
// line_strip_all
.header
.stamp = ros::Time::now
();
// line_strip_all
.ns = "trajectory_all";
// line_strip_all
.action = visualization_msgs::Marker::ADD;
// line_strip_all
.pose
.orientation
.w = 0
.0;
// line_strip_all
.id = 0;
// line_strip_all
.type = visualization_msgs::Marker::PO
INTS;
// // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
// line_strip_all
.scale
.x = 0
.5;
// // Line strip 是绿色
// line_strip_all
.color
.g = 1
.0f;
// line_strip_all
.color
.a = 1
.0;
// for
(int i = 0; i < obstruct_
left_nei
.size
(); ++i
)
// {
// geometry_msgs::Po
int p2;
// p2
.x = obstruct_
left_nei[i]
.x;
// p2
.y = obstruct_
left_nei[i]
.y;//
(int32_t
)
// p2
.z = 0;
// line_strip_all
.po
ints
.push_back
(p2
);
// }
// //发布各个markers
// marker_all
.publish
(line_strip_all
);
}
}
int main
(int argc,char** argv
)
{
ros::init
(argc,argv,"trajectory"
);
ros::NodeHandle n;
stop_sign
.data = 0;
ros::Rate r
(100
);
ros::Subscriber sub_ecu = n
.subscribe
("/ECU_Data", 1, start_sign
);
ros::Subscriber sub_yaw_location = n
.subscribe
("/INS/ASENSING_INS", 1, update_yaw_location
);
ros::Subscriber pub_bbox = n
.subscribe<jsk_recognition_msgs::BoundingBoxArray>
("/detections", 10, zhuitongCallback
);
marker_pub = n
.advertise<visualization_msgs::Marker>
("visualization_marker", 1
);
marker_all = n
.advertise<visualization_msgs::Marker>
("visualization_all", 1
);
marker_all2 = n
.advertise<visualization_msgs::Marker>
("visualization_all2", 1
);
pub = n
.advertise<sensor_msgs::Po
intCloud2>
("/trajectory", 1
);//定义一个发布者，发布最后的轨迹////修改注释222222222222
pub_stop = n
.advertise<std_msgs::Float64>
("/stop_sign",1
);
pub_brake = n
.advertise<std_msgs::Float64>
("/brake",1
);
while
(ros::ok
())
{
path_plan
();
ros::spinOnce
(); // Keeps node alive basically
r
.sleep
(); // Sleep for loop_rate
}
}
上述代码是一个八字绕环路径规划的代码
路径是什么样的呢？你能画出来吗
本文介绍了Java中的Arrays.copyOfRange()方法，该方法用于从原始数组中复制一段范围到新的数组，包括起始下标但不包括结束上标。理解此方法对于高效处理数组至关重要。
扫一扫
3212
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
