代码拉取完成，页面将自动刷新
#include"Formation.h"
/**
* @brief Formation::Formation
* 构造函数
* @param carid
* @param role
* @param carformationtype
*/
Formation::Formation(int carid,int role,int carformationtype){
this->carid=carid;
this->role=role;
this->currentformationtype=carformationtype;
mps = new MultipointSmooth;
back = new Back;
attrition = new Attrition;
reconstruction = new Reconstruction;
}
/**
* @brief Formation::~Formation
* 析构函数
*/
Formation::~Formation(){
if (mps != NULL)
delete mps;
if (back != NULL)
delete back;
if (attrition != NULL)
delete attrition;
if (reconstruction != NULL)
delete reconstruction;
}
/**
* @brief Formation::Reset
* 编队类重置，主要包括路径清楚和标志位归零
*/
void Formation::Reset(){
this->carid=CAR_ID;
this->currentformationtype=1;
globalVTP.clear();
localVTP.clear();
pilotcar_path.clear();
pilotcar_smoothpath.clear();
left_smoothpath.clear();
right_smoothpath.clear();
nearest_path.clear();
join_path.clear();
joinpath.clear();
join_path1.clear();
join_path2.clear();
linear_path.clear();
arc_path.clear();
atrrition_frontPoint_num=0;
pilotcar_path_num=0;
attrition_delaytimes=0;
cft2onlyonceflag=true;
cft4onlyonceflag=true;
cft4process1flag=true;
cft4process2flag=false;
getpilotcarposflag=false;
getfrontcarposflag=false;
getnearestfrontpath_flag=false;
getnearestbackpath_flag=false;
runningflag=false;
trajectory_flag=false;
cftchange_flag=false;
attritiononlyonceflag=true;
formationchangerole4only=true;
formationchangeholdonly=true;
getformationspeed=false;
trackmode=false;
getpointtracktarget_flag=false;
getpointtrackpath_flag=false;
mps->Reset();
back->Reset();
attrition->Reset();
reconstruction->Reset();
}
/**
* @brief Formation::singleVehicleInfo_callback
* SingleVehicleInfo消息回调函数，更新领航车路径以及其他车辆信息getPilotCarPath
* 接受到SingleVehicleInfo消息后，get_singlevehicleinfo_flag标志位置为true
* @param singlevehicleinfo
*/
void Formation::singleVehicleInfo_callback(const hmi_msgs::SingleVehicleInfo singlevehicleinfo){
// targetvehiclesdata=targetvehicles;
getSingleVehicleInfo(singlevehicleinfo);
get_singlevehicleinfo_flag=true;
}
/**
* @brief Formation::formationctlmode_callback
* FormationCtlMode消息回调函数，更新formationctlmode数据
* 接受到FormationCtlMode消息后，get_formationctlmode_flag标志位置为true
* @param FormationCtlMode
*/
void Formation::taskcontrol_callback(const hmi_msgs::TaskControl taskControl){
if((taskControl.vehicle_id==this->getCarID() && taskControl.module_id==0x05) || (taskControl.vehicle_id==0xFF && taskControl.module_id==0x05))
this->formationTaskControl=taskControl;
get_taskControl_flag=true;
}
/**
* @brief Formation::localposdata_callback
* Local_Pos_Data消息回调函数，更新本车全局位置信息setThiscarGlobalPos，更新倒车路径getLocalPosPath
* 接受到Local_Pos_Data消息后，get_Local_Pos_Data_flag标志位置为true
* @param InsMsg
*/
void Formation::localposdata_callback(const self_state::LocalPose Local_Pos_Data){
setThiscarLocalPos(Local_Pos_Data);
get_Local_Pos_Data_flag=true;
}
/**
* @brief Formation::globalposdata_callback
* Global_Pos_Data消息回调函数，更新thiscarglobalpos数据
* 接受到Global_Pos_Data消息后，get_Global_Pos_Data_flag标志位置为true
* @param LOCAL_POS_DATA
*/
void Formation::globalposdata_callback(const self_state::GlobalPose Global_Pos_Data){
setThiscarGlobalPos(Global_Pos_Data);
VehicleState backpoint;
backpoint.dr_x=Global_Pos_Data.gaussX;
backpoint.dr_y=Global_Pos_Data.gaussY;
backpoint.dr_heading=Global_Pos_Data.azimuth;
back->getLocalPosPath(backpoint);
get_Global_Pos_Data_flag=true;
}
/**
* @brief Formation::receivedTopicCheck
* 订阅消息核验，用于核验所有订阅消息是否正常接收，若没有正常接收则打印相关信息
*/
void Formation::receivedTopicCheck(){
if(!get_singlevehicleinfo_flag)
ROS_ERROR("hmi_msgs/SingleVehicleInfo topic miss");
if(!get_taskControl_flag)
ROS_ERROR("hmi_msgs/TaskControl topic miss");
if(!get_Global_Pos_Data_flag)
ROS_ERROR("self_state/GlobalPose topic miss");
if(!get_Local_Pos_Data_flag)
ROS_ERROR("self_state/LocalPose topic miss");
}
/**
* @brief Formation::initIO
* 初始化IO，主要包括订阅融合编队消息TARGETVEHICLES，订阅局部坐标消息LocalPosData
* 订阅本车全局消息insMsg_fused,订阅编队控制指令消息FormationCtlMode
* 发布局部编队结果LocalPlanTest
* @param nh
*/
void Formation::initIO(ros::NodeHandle nh){
subSingleVehicleInfo = nh.subscribe<hmi_msgs::SingleVehicleInfo>("hmi_msgs/SingleVehicleInfo", 1, &Formation::singleVehicleInfo_callback, this);
subGolbalPos = nh.subscribe<self_state::GlobalPose>("self_state/GlobalPose", 1, &Formation::globalposdata_callback, this);
subLocalPos = nh.subscribe<self_state::LocalPose>("self_state/LocalPose", 1, &Formation::localposdata_callback, this);
subTaskControl = nh.subscribe<hmi_msgs::TaskControl>("hmi_msgs/TaskControl", 1, &Formation::taskcontrol_callback, this);
pubLocalPlanResult = nh.advertise<behavior::LocalPlanResult>("behavior/LocalPlanResult", 1);
TrajectoryPub = nh.advertise<behavior::Trajectory>("behavior/Trajectorytogo", 1);
}
int Formation::getCarID(){
return this->carid;
}
int Formation::getrole(){
return this->role;
}
void Formation::setRole(int role){
this->role=role;
}
int Formation::getCFT(){
return this->currentformationtype;
}
void Formation::setCFT(int currentformationtype){
this->currentformationtype=currentformationtype;
}
/**
* @brief Formation::setThiscarGlobalPos
* 更新本车全局位置信息和朝向
* @param Global_Pos_Data
*/
void Formation::setThiscarGlobalPos(const self_state::GlobalPose Global_Pos_Data){
thiscarglobalpos.dr_x=Global_Pos_Data.gaussX;
thiscarglobalpos.dr_y=Global_Pos_Data.gaussY;
thiscarglobalpos.dr_heading=Global_Pos_Data.azimuth;
//TODO:更新本车全局速度
//cout<<"insmsg x:"<<thiscarglobalpos.dr_x<<" y:"<<thiscarglobalpos.dr_y<<" heading:"<<thiscarglobalpos.dr_heading<<endl;
ROS_INFO("Global_Pos_Data UPDATE!!!");
}
/**
* @brief Formation::setThiscarLocalPos
* 更新本车局部位置信息和朝向
* @param Local_Pos_Data
*/
void Formation::setThiscarLocalPos(const self_state::LocalPose Local_Pos_Data){
thiscarlocalpos.dr_x=Local_Pos_Data.dr_x;
thiscarlocalpos.dr_y=Local_Pos_Data.dr_y;
thiscarlocalpos.dr_z=Local_Pos_Data.dr_z;
thiscarlocalpos.dr_heading=Local_Pos_Data.dr_heading;
thiscarlocalpos.dr_roll=Local_Pos_Data.dr_roll;
thiscarlocalpos.dr_pitch=Local_Pos_Data.dr_pitch;
ROS_INFO("Local_Pos_Data UPDATE!!!");
}
/**
* @brief Formation::getPilotCarPath
* 根据编队融合信息，更新领航车位置、朝向和速度信息，并且每隔90cm记录领航车位置汇成领航车路径，领航车路径限制100个点
* 此外还更新编队其他车辆角色的位置、朝向和速度信息。
* @param targetvehicles
*/
void Formation::getSingleVehicleInfo(const hmi_msgs::SingleVehicleInfo singlevehicleinfo){
if(singlevehicleinfo.vehicle_role==1){
// pilotcarpos.dr_x=0;
// pilotcarpos.dr_y=6000;
// pilotcarpos.dr_heading=9000;
// pilotcarpos=last_path_point;
pilotcarpos.dr_x=singlevehicleinfo.gauss_x;
pilotcarpos.dr_y=singlevehicleinfo.gauss_y;
pilotcarpos.dr_heading=singlevehicleinfo.heading;
pilotcarspeed=singlevehicleinfo.cur_speed*3.6;
vehicleInfo_Role1=singlevehicleinfo;
if (pilotcar_path_num==0){
last_path_point.dr_x=singlevehicleinfo.gauss_x;
last_path_point.dr_y=singlevehicleinfo.gauss_y;
last_path_point.dr_heading=singlevehicleinfo.heading;
pilotcar_path.push_front(last_path_point);
ROS_INFO("TARGETVEHICLES UPDATE!!!");
pilotcar_path_num++;
}
else{
if (((pow((last_path_point.dr_x-singlevehicleinfo.gauss_x),2)+pow((last_path_point.dr_y-singlevehicleinfo.gauss_y),2))>=8000)&&((pow((last_path_point.dr_x-singlevehicleinfo.gauss_x),2)+pow((last_path_point.dr_y-singlevehicleinfo.gauss_y),2))<=800000)){
last_path_point.dr_x=singlevehicleinfo.gauss_x;
last_path_point.dr_y=singlevehicleinfo.gauss_y;
last_path_point.dr_heading=singlevehicleinfo.heading;
if (pilotcar_path.size()>100){
pilotcar_path.pop_back();
pilotcar_path.push_front(last_path_point);
}
else{
pilotcar_path.push_front(last_path_point);
}
// cout<<pilotcar_path.size()<<endl;
ROS_INFO("TARGETVEHICLES UPDATE!!!");
pilotcar_path_num++;
}
}
}
else if(singlevehicleinfo.vehicle_role==2){
role2carspeed=singlevehicleinfo.cur_speed*3.6;
role2carpos.dr_x=singlevehicleinfo.gauss_x;
role2carpos.dr_y=singlevehicleinfo.gauss_y;
role2carpos.dr_heading=singlevehicleinfo.heading;
vehicleInfo_Role2=singlevehicleinfo;
}
else if(singlevehicleinfo.vehicle_role==3){
role3carspeed=singlevehicleinfo.cur_speed*3.6;
role3carpos.dr_x=singlevehicleinfo.gauss_x;
role3carpos.dr_y=singlevehicleinfo.gauss_y;
role3carpos.dr_heading=singlevehicleinfo.heading;
vehicleInfo_Role3=singlevehicleinfo;
}
else if(singlevehicleinfo.vehicle_role==4){
role4carspeed=singlevehicleinfo.cur_speed*3.6;
role4carpos.dr_x=singlevehicleinfo.gauss_x;
role4carpos.dr_y=singlevehicleinfo.gauss_y;
role4carpos.dr_heading=singlevehicleinfo.heading;
vehicleInfo_Role4=singlevehicleinfo;
}
if(this->getCarID()==singlevehicleinfo.vehicle_id){
vehicleInfo_This=singlevehicleinfo;
}
}
/**
* @brief Formation::getNearstFrontPath
* 根据路径path，截取起点startcarpos和终点endcarpos间的路径,push_front压入
* @param path
* @param startcarpos
* @param endcarpos
*/
void Formation::getNearstFrontPath(const deque<VehicleState> path,const VehicleState startcarpos,const VehicleState endcarpos){
int startnum=0;
int endnum=0;
if (path.empty()){
ROS_WARN("WARNING:PILOT PATH IS EMPTY!!!");
}
else{
startnum=HMath::getNearestPathPointByDistance(path,startcarpos);
endnum=HMath::getNearestPathPointByDistance(path,endcarpos);
if (endnum>=startnum){
for(int i=startnum;i<=endnum;i++){
nearest_path.push_front(path[i]);
}
getnearestfrontpath_flag=true;
ROS_INFO("GET PATH TO GO!!!");
}
else{
getnearestfrontpath_flag=false;
ROS_WARN("WARNING:THIS CAR PASS OVER PILOT CAR!!!");
}
}
}
/**
* @brief Formation::getNearstBackPath
* 根据路径path，截取起点startcarpos和终点endcarpos间的路径,push_back压入
* @param path
* @param startcarpos
* @param endcarpos
*/
void Formation::getNearstBackPath(const deque<VehicleState> path,const VehicleState startcarpos,const VehicleState endcarpos){
int startnum=0;
int endnum=0;
if (path.empty()){
ROS_WARN("WARNING:PILOT PATH IS EMPTY!!!");
}
else{
startnum=HMath::getNearestPathPointByDistance(path,startcarpos);
endnum=HMath::getNearestPathPointByDistance(path,endcarpos);
if (endnum>=startnum){
for(int i=startnum;i<=endnum;i++){
nearest_path.push_back(path[i]);
}
getnearestbackpath_flag=true;
ROS_INFO("GET PATH TO GO!!!");
}
else{
getnearestbackpath_flag=false;
ROS_WARN("WARNING:THIS CAR PASS OVER PILOT CAR!!!");
}
}
}
/**
* @brief Formation::generateSidePath
* 根据中心路径smoothpath和路径宽度pathdist生成两侧的路径left_smoothpath和right_smoothpath
* @param smoothpath
*/
void Formation::generateSidePath(const deque<VehicleState> smoothpath){
left_smoothpath.clear();
right_smoothpath.clear();
VehicleState templeft;
VehicleState tempright;
for(int i=0;i<smoothpath.size()-1;i++){
double dx = smoothpath[i].dr_x - smoothpath[i+1].dr_x;
double dy = smoothpath[i].dr_y - smoothpath[i+1].dr_y;
double thelta = atan2(dy ,dx);
templeft.dr_x = smoothpath[i+1].dr_x - pathdist * sin(thelta);
templeft.dr_y = smoothpath[i+1].dr_y + pathdist * cos(thelta);
left_smoothpath.push_back(templeft);
tempright.dr_x = smoothpath[i+1].dr_x + pathdist * sin(thelta);
tempright.dr_y = smoothpath[i+1].dr_y - pathdist * cos(thelta);
right_smoothpath.push_back(tempright);
}
}
/**
* @brief Formation::getTrajectory
* 离线调试时输出规划结果
* @param nearpath
*/
void Formation::getTrajectory(const deque<VehicleState> nearpath){
if (nearpath.size()!=0){
if (nearpath.size()<=100){
for(int i=0;i<nearpath.size();i++){ //globalpos 2 thiscar coordinaate
path.center_line[i].x = nearpath[i].dr_x ;
path.center_line[i].y = nearpath[i].dr_y ;
}
path.effective_pointNum=nearest_path.size();
trajectory_flag=true;
}
else{
for(int i=0;i<=100;i++){ //globalpos 2 thiscar coordinaate
path.center_line[i].x = nearpath[i].dr_x ;
path.center_line[i].y = nearpath[i].dr_y ;
}
path.effective_pointNum=100;
trajectory_flag=true;
}
}
}
/**
* @brief Formation::updatePath
* 封装规划结果
* @param nearpath
*/
void Formation::updatePath(const deque<VehicleState> nearpath){
if (nearpath.size()>100){ //if pathpoints num larger than waypoints 100
geometry_msgs::Pose2D left_boundary;
geometry_msgs::Pose2D right_boundary;
for(int i=0;i<100;i++){ //globalpos to thiscar coordinaate
double dx = nearpath[i].dr_x - thiscarglobalpos.dr_x;
double dy = nearpath[i].dr_y - thiscarglobalpos.dr_y;
plan_result.center_line[i].x = dx * cos(thiscarglobalpos.dr_heading *0.01 * degree2rad) + dy * sin(thiscarglobalpos.dr_heading *0.01 * degree2rad);
plan_result.center_line[i].y = -dx * sin(thiscarglobalpos.dr_heading *0.01 * degree2rad) + dy * cos(thiscarglobalpos.dr_heading *0.01 * degree2rad);
}
for(int i=0;i<100;i++){ //fill path bounary information
double B_dx = nearpath[i+1].dr_x - nearpath[i].dr_x;
double B_dy = nearpath[i+1].dr_y - nearpath[i].dr_y;
double thelta = atan2(B_dy ,B_dx);
left_boundary.x = plan_result.center_line[i].x - roadWith * sin(thelta);
left_boundary.y = plan_result.center_line[i].y + roadWith * cos(thelta);
right_boundary.x = plan_result.center_line[i].x + roadWith * sin(thelta);
right_boundary.y = plan_result.center_line[i].y - roadWith * cos(thelta);
plan_result.left_boundary[i] = left_boundary;
plan_result.right_boundary[i] = right_boundary;
}
plan_result.effective_pointNum=100;
runningflag=true;
}
else{
ROS_INFO("--------test8-------");
geometry_msgs::Pose2D left_boundary;
geometry_msgs::Pose2D right_boundary;
for(int i=0;i<nearpath.size();i++){ //globalpos 2 thiscar coordinaate
double dx = nearpath[i].dr_x - thiscarglobalpos.dr_x;
double dy = nearpath[i].dr_y - thiscarglobalpos.dr_y;
plan_result.center_line[i].x = dx * cos(thiscarglobalpos.dr_heading *0.01 * degree2rad) + dy * sin(thiscarglobalpos.dr_heading *0.01 * degree2rad);//wo suan de shi cos*dx+sin*dy
plan_result.center_line[i].y = -dx * sin(thiscarglobalpos.dr_heading *0.01 * degree2rad) + dy * cos(thiscarglobalpos.dr_heading *0.01 * degree2rad);//-sin * dx + cos*dy
}
for(int i=0;i<nearpath.size()-1;i++){ //fill path bounary information
double B_dx = nearpath[i+1].dr_x - nearpath[i].dr_x;
double B_dy = nearpath[i+1].dr_y - nearpath[i].dr_y;
double thelta = atan2(B_dy ,B_dx);
left_boundary.x = plan_result.center_line[i].x - roadWith * sin(thelta);
left_boundary.y = plan_result.center_line[i].y + roadWith * cos(thelta);
right_boundary.x = plan_result.center_line[i].x + roadWith * sin(thelta);
right_boundary.y = plan_result.center_line[i].y - roadWith * cos(thelta);
plan_result.left_boundary[i] = left_boundary;
plan_result.right_boundary[i] = right_boundary;
}
plan_result.left_boundary[nearpath.size()-1] = left_boundary;
plan_result.right_boundary[nearpath.size()-1] = right_boundary;
plan_result.effective_pointNum=nearpath.size();
runningflag=true;
}
}
/**
* @brief Formation::updateLocalPath
* 将全局规划路径nearpath转换到当前车的局部坐标系下plan_result.center_line
* 更新前进规划标志位runningflag
* @param nearpath
*/
void Formation::updateLocalPath(const deque<VehicleState> nearpath){
if (nearpath.size()>100){ //if pathpoints num larger than waypoints 100
geometry_msgs::Pose2D left_boundary;
geometry_msgs::Pose2D right_boundary;
for(int i=0;i<100;i++){ //globalpos to thiscar coordinaate
plan_result.center_line[i].x = nearpath[i].dr_x;
plan_result.center_line[i].y = nearpath[i].dr_y;
}
for(int i=0;i<100;i++){ //fill path bounary information
double B_dx = nearpath[i+1].dr_x - nearpath[i].dr_x;
double B_dy = nearpath[i+1].dr_y - nearpath[i].dr_y;
double thelta = atan2(B_dy ,B_dx);
left_boundary.x = plan_result.center_line[i].x - roadWith * sin(thelta);
left_boundary.y = plan_result.center_line[i].y + roadWith * cos(thelta);
right_boundary.x = plan_result.center_line[i].x + roadWith * sin(thelta);
right_boundary.y = plan_result.center_line[i].y - roadWith * cos(thelta);
plan_result.left_boundary[i] = left_boundary;
plan_result.right_boundary[i] = right_boundary;
}
plan_result.effective_pointNum=100;
runningflag=true;
}
else{
geometry_msgs::Pose2D left_boundary;
geometry_msgs::Pose2D right_boundary;
for(int i=0;i<nearpath.size();i++){ //globalpos 2 thiscar coordinaate
plan_result.center_line[i].x = nearpath[i].dr_x;
plan_result.center_line[i].y = nearpath[i].dr_y;
}
for(int i=0;i<nearpath.size()-1;i++){ //fill path bounary information
double B_dx = nearpath[i+1].dr_x - nearpath[i].dr_x;
double B_dy = nearpath[i+1].dr_y - nearpath[i].dr_y;
double thelta = atan2(B_dy ,B_dx);
left_boundary.x = plan_result.center_line[i].x - roadWith * sin(thelta);
left_boundary.y = plan_result.center_line[i].y + roadWith * cos(thelta);
right_boundary.x = plan_result.center_line[i].x + roadWith * sin(thelta);
right_boundary.y = plan_result.center_line[i].y - roadWith * cos(thelta);
plan_result.left_boundary[i] = left_boundary;
plan_result.right_boundary[i] = right_boundary;
}
plan_result.left_boundary[nearpath.size()-1] = left_boundary;
plan_result.right_boundary[nearpath.size()-1] = right_boundary;
plan_result.effective_pointNum=nearpath.size();
runningflag=true;
}
}
/**
* @brief Formation::generateRunningPath
* 前进任务下根据currentformationtype生成对应路径
* currentformationtype=1为直线形保持:
* currentformationtype=2为直线形变菱形:
* currentformationtype=3为菱形保持:
* currentformationtype=4为菱形变直线形:
* currentformationtype=5为直线形变田形:
* currentformationtype=6为田形保持:
* currentformationtype=7为田形变直线:
*/
void Formation::generateRunningPath(){
globalVTP.clear();
VehicleState global_vtp_startpoint;
VehicleState global_vtp_midpoint;
VehicleState global_vtp_endpoint;
VehicleState global_vtp_waypoint;
switch (getCFT()) {
case 1: //直线形保持
cft4onlyonceflag=true;
cft4process1flag=true;
cft4process2flag=false;
//跟点模式
if (!trackmode){
ROS_INFO("POINT TRACKING!!!");
if(getpointtracktarget_flag){
VehicleState start;
start.dr_x=0;
start.dr_y=0;
start.dr_heading=0;
getpointtrackpath_flag=HMath::halfSmoothPahGenerate(start,pointtrackpoint,nearest_path);
getpointtracktarget_flag=false;
}
else{
ROS_WARN("WARNING:GET TRACK POINT FAILED!!!");
}
//跟点路径规划成功
if(getpointtrackpath_flag){
ROS_INFO("GET PATH TO GO!!!");
updateLocalPath(nearest_path);
//getTrajectory(nearest_path);
nearest_path.clear();
getpointtrackpath_flag=false;
}
}
//跟路模式
else{
//领航车路径平滑
pilotcar_smoothpath=mps->SmoothBSpline(pilotcar_path,smoothlength);
ROS_INFO("PATH TRACKING!!!");
//平滑后的路径不为空
if (!pilotcar_smoothpath.empty()){
//截取领航车位置和当前车位置间的领航车路径
getNearstFrontPath(pilotcar_smoothpath,pilotcarpos,thiscarglobalpos);
//如果当前位置离领航车路径过远则要连接当前位置和领航车路径
if (HMath::IsFarAwayFromPath(nearest_path,thiscarglobalpos)){
//计算当前位置离领航车路径最近的路径点
int num=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,thiscarglobalpos);
int size = pilotcar_smoothpath.size();
//最近的路径点为领航车路径的末端
if (num==(size-1)){
//根据具体role确定汇合路径
if (getrole()==2){
HMath::smoothPahGenerate(thiscarglobalpos,pilotcarpos,nearest_path);
}
else if (getrole()==3){
HMath::smoothPahGenerate(thiscarglobalpos,role2carpos,nearest_path);
}
else if (getrole()==4){
HMath::smoothPahGenerate(thiscarglobalpos,role3carpos,nearest_path);
}
// HMath::linearInterpolation(*nearest_path.begin(),thiscarglobalpos,linear_path);
// HMath::linkFrontPath(nearest_path,linear_path);
}
//最近的路径点不为领航车路径的末端
else{
if (!nearest_path.empty()){
//则选择距离当前位置第七个路径点作为汇合路径点
if (nearest_path.size()>=8){
//生成汇合路径
HMath::smoothPahGenerate(thiscarglobalpos,nearest_path[7],arc_path);
//连接汇合路径和领航车路径
HMath::linkArcPath(nearest_path,arc_path);
}
}
}
}
//生成了路径则进行路径封装
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
nearest_path.clear();
getnearestfrontpath_flag=false;
}
}
else{
ROS_WARN("WARNING:PILOT PATH IS EMPTY!!!");
}
}
break;
case 2://直线形变菱形
//领航车路径平滑
pilotcar_smoothpath=mps->SmoothBSpline(pilotcar_path,smoothlength);
//生成领航车两侧的路径用于菱形过渡规划
generateSidePath(pilotcar_smoothpath);
formationchangeholdonly=true;
//根据具体role对应执行
if(getrole() == 2){
//领航车左侧路径平滑
left_smoothpath=mps->SmoothBSpline(left_smoothpath,smoothlength);
//左侧路径延长
HMath::pathFrontExtend(left_smoothpath,25);
//规划左侧路径的汇合过渡路径，只执行一次
if (cft2onlyonceflag){
//汇合路径起点为此刻当前位置
startpoint=thiscarglobalpos;
//目标点朝向优化减小误差
double heading=(thiscarglobalpos.dr_heading+pilotcarpos.dr_heading)/2;
//汇合路径点位置为当前车辆左侧ydist,前方xdist+ydist
global_vtp_startpoint.dr_x = thiscarglobalpos.dr_x + (ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = thiscarglobalpos.dr_y + (ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_startpoint.dr_x - ydist * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_startpoint.dr_y + ydist * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_endpoint.dr_x + (xdist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_endpoint.dr_y + (xdist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_heading=heading;
//计算汇合点在左侧路径left_smoothpath的位置num
int num=HMath::getNearestPathPointByDistance(left_smoothpath,global_vtp_endpoint);
//获得汇合点位置jointpoint
jointpoint=left_smoothpath[num];
//规划global_vtp_startpoint到汇合点jointpoint的路径join_path1
HMath::linearInterpolation(global_vtp_startpoint,jointpoint,join_path1);
//过渡点位置为汇合点jointpoint前方2*ydist处
global_vtp_startpoint.dr_x = jointpoint.dr_x + (2*ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = jointpoint.dr_y + (2*ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_heading=heading;
//计算过渡点在左侧路径left_smoothpath的位置num1
int num1=HMath::getNearestPathPointByDistance(left_smoothpath,global_vtp_startpoint);
//获得过渡点位置global_vtp_startpoint
global_vtp_startpoint=left_smoothpath[num1];
//规划汇合点jointpoint到过渡点global_vtp_startpoint的路径join_path2
HMath::linearInterpolation(jointpoint,global_vtp_startpoint,join_path2);
//目标点即为过渡点
targetpoint=global_vtp_startpoint;
//将汇合路径join_path1和过渡路径join_path2连接为最终的过渡路径join_path
HMath::linkBackPath(thiscarglobalpos,join_path1,join_path2,join_path);
cft2onlyonceflag=false;
}
//连接过渡路径join_path和左侧路径left_smoothpath为最终的role2直线变菱形路径joinpath
HMath::linkPath(join_path,left_smoothpath,targetpoint,joinpath);
//截取菱形变换路径joinpath中当前位置thiscarglobalpos到领航车位置pilotcarpos的部分
getNearstBackPath(joinpath,thiscarglobalpos,pilotcarpos);
if (getnearestbackpath_flag){
//当前车辆已经到达菱形变道目标点targetpoint，则完成直线型变菱形
if (HMath::CalcDist(thiscarglobalpos,targetpoint)<100){
cftchange_flag=true;
nearest_path.clear();
}
//直线型变菱形未完成则进行结果封装
else{
updatePath(nearest_path);
getTrajectory(nearest_path);
nearest_path.clear();
}
getnearestbackpath_flag=false;
}
}
else if (getrole()==3) {
//领航车右侧路径平滑
right_smoothpath=mps->SmoothBSpline(right_smoothpath,smoothlength);
//右侧路径延长
HMath::pathFrontExtend(right_smoothpath,15);
//规划右侧路径的汇合过渡路径，只执行一次
if (cft2onlyonceflag){
//汇合路径起点为此刻当前位置
startpoint=thiscarglobalpos;
//目标点朝向优化减小误差
double heading=(thiscarglobalpos.dr_heading+pilotcarpos.dr_heading)/2;
//汇合路径点位置为当前车辆右侧ydist,前方xdist+ydist
global_vtp_startpoint.dr_x = thiscarglobalpos.dr_x + (ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = thiscarglobalpos.dr_y + (ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_startpoint.dr_x + ydist * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_startpoint.dr_y - ydist * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_endpoint.dr_x + (xdist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_endpoint.dr_y + (xdist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_heading=heading;
//计算汇合点在右侧路径right_smoothpath的位置num
int num=HMath::getNearestPathPointByDistance(right_smoothpath,global_vtp_endpoint);
//获得汇合点位置jointpoint
jointpoint=right_smoothpath[num];
//规划global_vtp_startpoint到汇合点jointpoint的路径join_path1
HMath::linearInterpolation(global_vtp_startpoint,jointpoint,join_path1);
//过渡点位置为汇合点jointpoint前方2*ydist处
global_vtp_startpoint.dr_x = jointpoint.dr_x + (2*ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = jointpoint.dr_y + (2*ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_heading=heading;
//计算过渡点在右侧路径right_smoothpath的位置num1
int num1=HMath::getNearestPathPointByDistance(right_smoothpath,global_vtp_startpoint);
//获得过渡点位置global_vtp_startpoint
global_vtp_startpoint=right_smoothpath[num1];
//规划汇合点jointpoint到过渡点global_vtp_startpoint的路径join_path2
HMath::linearInterpolation(jointpoint,global_vtp_startpoint,join_path2);
//目标点即为过渡点
targetpoint=global_vtp_startpoint;
//将汇合路径join_path1和过渡路径join_path2连接为最终的过渡路径join_path
HMath::linkBackPath(thiscarglobalpos,join_path1,join_path2,join_path);
cft2onlyonceflag=false;
}
//连接过渡路径join_path和左侧路径right_smoothpath为最终的role3菱形变换路径joinpath
HMath::linkPath(join_path,right_smoothpath,targetpoint,joinpath);
//截取菱形变换路径joinpath中当前位置thiscarglobalpos到领航车位置pilotcarpos的部分
getNearstBackPath(joinpath,thiscarglobalpos,pilotcarpos);
if (getnearestbackpath_flag){
//当前车辆已经到达菱形变道目标点targetpoint，则完成直线型变菱形
if (HMath::CalcDist(thiscarglobalpos,targetpoint)<100){
cftchange_flag=true;
nearest_path.clear();
}
//直线型变菱形未完成则进行结果封装
else{
updatePath(nearest_path);
getTrajectory(nearest_path);
nearest_path.clear();
}
getnearestbackpath_flag=false;
}
}
else if(getrole()==4){
globalVTP.clear();
if (formationchangerole4only){
//计算领航车位置pilotcarpos和前车位置role3carpos间的距离
formationchangerole4dist=HMath::CalcDist(pilotcarpos,role3carpos);
formationchangerole4only=false;
}
//过渡起点位置为领航车位置后方formationchangerole4dist
global_vtp_startpoint.dr_x = pilotcarpos.dr_x - formationchangerole4dist * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = pilotcarpos.dr_y - formationchangerole4dist * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//过渡终点位置为领航车位置后方rxdist
global_vtp_endpoint.dr_x = pilotcarpos.dr_x - (rxdist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = pilotcarpos.dr_y - (rxdist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//生成从过渡起点位置到过渡终点位置间的非均匀线性插值虚拟点globalVTP
HMath::generateUnEvenTargetPointUp(global_vtp_startpoint,global_vtp_endpoint,100,globalVTP);
static int currentnum=0;
//静态变量currentnum遍历完毕
if (currentnum >= globalVTP.size()){
currentnum=0;
}
else{
//过渡路径即为领航车路径pilotcar_smoothpath中当前位置thiscarglobalpos和领航车位置pilotcarpos间的路径
getNearstFrontPath(pilotcar_smoothpath,pilotcarpos,thiscarglobalpos);
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
nearest_path.clear();
getnearestfrontpath_flag=false;
}
currentnum++;
}
}
break;
case 3://菱形保持
//领航车路径平滑
pilotcar_smoothpath=mps->SmoothBSpline(pilotcar_path,smoothlength);
//生成领航车两侧的路径用于菱形保持规划
generateSidePath(pilotcar_smoothpath);
//左侧路径平滑
left_smoothpath=mps->SmoothBSpline(left_smoothpath,smoothlength);
//右侧路径平滑
right_smoothpath=mps->SmoothBSpline(right_smoothpath,smoothlength);
if(getrole() == 2){
cft2onlyonceflag=true;
//左侧目标点为领航车位置pilotcarpos左侧ydist
sidestartpoint.dr_x = pilotcarpos.dr_x - ydist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
sidestartpoint.dr_y = pilotcarpos.dr_y + ydist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//截取左侧路径left_smoothpath中当前位置thiscarglobalpos到目标点位置sidestartpoint
getNearstFrontPath(left_smoothpath,sidestartpoint,thiscarglobalpos);
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
getnearestfrontpath_flag=false;
}
}
else if (getrole()==3) {
cft2onlyonceflag=true;
//右侧目标点为领航车位置pilotcarpos右侧ydist
sidestartpoint.dr_x = pilotcarpos.dr_x + ydist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
sidestartpoint.dr_y = pilotcarpos.dr_y - ydist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//截取右侧路径right_smoothpath中当前位置thiscarglobalpos到目标点位置sidestartpoint
getNearstFrontPath(right_smoothpath,sidestartpoint,thiscarglobalpos);
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
getnearestfrontpath_flag=false;
}
}
else if(getrole()==4){
//截取领航车路径pilotcar_smoothpath中当前位置thiscarglobalpos到领航车位置pilotcarpos
getNearstFrontPath(pilotcar_smoothpath,pilotcarpos,thiscarglobalpos);
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
getnearestfrontpath_flag=false;
}
}
break;
case 4://菱形变直线形
//领航车路径平滑
pilotcar_smoothpath=mps->SmoothBSpline(pilotcar_path,smoothlength);
//生成领航车两侧的路径用于直线过渡规划
generateSidePath(pilotcar_smoothpath);
//左侧路径平滑
left_smoothpath=mps->SmoothBSpline(left_smoothpath,smoothlength);
//右侧路径平滑
right_smoothpath=mps->SmoothBSpline(right_smoothpath,smoothlength);
if(getrole() == 2){
//菱形过渡直线分为两个阶段，第一个阶段用于拉开距离
if (cft4process1flag){
//阶段一目标点设为领航车位置左侧ydist
sidestartpoint.dr_x = pilotcarpos.dr_x - ydist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
sidestartpoint.dr_y = pilotcarpos.dr_y + ydist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//截取左侧路径left_smoothpath中当前位置thiscarglobalpos到阶段一目标点位置sidestartpoint
getNearstFrontPath(left_smoothpath,sidestartpoint,thiscarglobalpos);
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
getnearestfrontpath_flag=false;
}
}
//菱形过渡直线分为两个阶段，第二个阶段用于入队
if (cft4process2flag){
//领航车路径延长
HMath::pathFrontExtend(pilotcar_smoothpath,25);
//规划领航车路径的汇合过渡路径，只执行一次
if (cft4onlyonceflag){
//汇合路径起点为此刻当前位置
startpoint=thiscarglobalpos;
//目标点朝向优化减小误差
double heading=(thiscarglobalpos.dr_heading+pilotcarpos.dr_heading)/2;
//汇合路径点位置为当前车辆右侧ydist,前方xdist+ydist
global_vtp_startpoint.dr_x = thiscarglobalpos.dr_x + (ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = thiscarglobalpos.dr_y + (ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_startpoint.dr_x + ydist * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_startpoint.dr_y - ydist * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_endpoint.dr_x + (xdist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_endpoint.dr_y + (xdist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_heading=heading;
//计算汇合点在领航车路径pilotcar_smoothpath的位置num
int num=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,global_vtp_endpoint);
//获得汇合点位置jointpoint
jointpoint=pilotcar_smoothpath[num];
//规划global_vtp_startpoint到汇合点jointpoint的路径join_path1
HMath::linearInterpolation(global_vtp_startpoint,jointpoint,join_path1);
//过渡点位置为汇合点jointpoint前方2*ydist处
global_vtp_startpoint.dr_x = jointpoint.dr_x + (2*ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = jointpoint.dr_y + (2*ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_heading=heading;
//计算过渡点在领航车路径pilotcar_smoothpath的位置num1
int num1=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,global_vtp_startpoint);
//获得过渡点位置global_vtp_startpoint
global_vtp_startpoint=pilotcar_smoothpath[num1];
//规划汇合点jointpoint到过渡点global_vtp_startpoint的路径join_path2
HMath::linearInterpolation(jointpoint,global_vtp_startpoint,join_path2);
//目标点即为过渡点
targetpoint=global_vtp_startpoint;
//将汇合路径join_path1和过渡路径join_path2连接为最终的过渡路径join_path
HMath::linkBackPath(thiscarglobalpos,join_path1,join_path2,join_path);
cft4onlyonceflag=false;
}
//连接过渡路径join_path和领航车路径pilotcar_smoothpath为最终的role2菱形变直线路径joinpath
HMath::linkPath(join_path,pilotcar_smoothpath,targetpoint,joinpath);
//截取菱形变换路径joinpath中当前位置thiscarglobalpos到领航车位置pilotcarpos的部分
getNearstBackPath(joinpath,thiscarglobalpos,pilotcarpos);
if (getnearestbackpath_flag){
//当前车辆已经到达菱形变直线目标点targetpoint，则完成菱形变直线
if (HMath::CalcDist(thiscarglobalpos,targetpoint)<100){
cftchange_flag=true;
nearest_path.clear();
}
//菱形变直线未完成则进行结果封装
else{
updatePath(nearest_path);
getTrajectory(nearest_path);
nearest_path.clear();
}
getnearestbackpath_flag=false;
}
}
}
else if (getrole()==3) {
//菱形过渡直线分为两个阶段，第一个阶段用于拉开距离
if (cft4process1flag){
//阶段一目标点设为领航车位置右侧ydist
sidestartpoint.dr_x = pilotcarpos.dr_x + ydist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
sidestartpoint.dr_y = pilotcarpos.dr_y - ydist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//截取左侧路径right_smoothpath中当前位置thiscarglobalpos到阶段一目标点位置sidestartpoint
getNearstFrontPath(right_smoothpath,sidestartpoint,thiscarglobalpos);
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
getnearestfrontpath_flag=false;
}
}
//菱形过渡直线分为两个阶段，第二个阶段用于入队
if (cft4process2flag){
//领航车路径延长
HMath::pathFrontExtend(pilotcar_smoothpath,15);
if (cft4onlyonceflag){
//汇合路径起点为此刻当前位置
startpoint=thiscarglobalpos;
//目标点朝向优化减小误差
double heading=(thiscarglobalpos.dr_heading+pilotcarpos.dr_heading)/2;
//汇合路径点位置为当前车辆左侧ydist,前方xdist+ydist
global_vtp_startpoint.dr_x = thiscarglobalpos.dr_x + (ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = thiscarglobalpos.dr_y + (ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_startpoint.dr_x - ydist * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_startpoint.dr_y + ydist * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_endpoint.dr_x + (xdist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_endpoint.dr_y + (xdist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_heading=heading;
//计算汇合点在领航车路径pilotcar_smoothpath的位置num
int num=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,global_vtp_endpoint);
//获得汇合点位置jointpoint
jointpoint=pilotcar_smoothpath[num];
//规划global_vtp_startpoint到汇合点jointpoint的路径join_path1
HMath::linearInterpolation(global_vtp_startpoint,jointpoint,join_path1);
//过渡点位置为汇合点jointpoint前方2*ydist处
global_vtp_startpoint.dr_x = jointpoint.dr_x + (2*ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = jointpoint.dr_y + (2*ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_heading=heading;
//计算过渡点在领航车路径pilotcar_smoothpath的位置num1
int num1=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,global_vtp_startpoint);
//获得过渡点位置global_vtp_startpoint
global_vtp_startpoint=pilotcar_smoothpath[num1];
//规划汇合点jointpoint到过渡点global_vtp_startpoint的路径join_path2
HMath::linearInterpolation(jointpoint,global_vtp_startpoint,join_path2);
//目标点即为过渡点
targetpoint=global_vtp_startpoint;
//将汇合路径join_path1和过渡路径join_path2连接为最终的过渡路径join_path
HMath::linkBackPath(thiscarglobalpos,join_path1,join_path2,join_path);
cft4onlyonceflag=false;
}
//连接过渡路径join_path和领航车路径pilotcar_smoothpath为最终的role3菱形变直线路径joinpath
HMath::linkPath(join_path,pilotcar_smoothpath,targetpoint,joinpath);
//截取菱形变换路径joinpath中当前位置thiscarglobalpos到领航车位置pilotcarpos的部分
getNearstBackPath(joinpath,thiscarglobalpos,pilotcarpos);
if (getnearestbackpath_flag){
//当前车辆已经到达菱形变直线目标点targetpoint，则完成菱形变直线
if (HMath::CalcDist(thiscarglobalpos,targetpoint)<100){
cftchange_flag=true;
nearest_path.clear();
}
//菱形变直线未完成则进行结果封装
else{
updatePath(nearest_path);
getTrajectory(nearest_path);
nearest_path.clear();
}
getnearestbackpath_flag=false;
}
}
}
else if(getrole()==4){
//截取领航车路径pilotcar_smoothpath中当前位置thiscarglobalpos到领航车位置pilotcarpos
getNearstFrontPath(pilotcar_smoothpath,pilotcarpos,thiscarglobalpos);
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
nearest_path.clear();
getnearestfrontpath_flag=false;
}
}
break;
case 5://直线形变田形
//领航车路径平滑
pilotcar_smoothpath=mps->SmoothBSpline(pilotcar_path,smoothlength);
//生成领航车两侧的路径用于直线形变田形规划
generateSidePath(pilotcar_smoothpath);
//根据具体role对应执行
if(getrole() == 2){
//截取领航车位置和当前车位置间的领航车路径
getNearstFrontPath(pilotcar_smoothpath,pilotcarpos,thiscarglobalpos);
if (getnearestbackpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
nearest_path.clear();
getnearestfrontpath_flag=false;
}
}
else if (getrole()==3) {
//领航车右侧路径平滑
right_smoothpath=mps->SmoothBSpline(right_smoothpath,smoothlength);
//右侧路径延长
HMath::pathFrontExtend(right_smoothpath,15);
//规划右侧路径的汇合过渡路径，只执行一次
if (cft2onlyonceflag){
//汇合路径起点为此刻当前位置
startpoint=thiscarglobalpos;
//目标点朝向优化减小误差
double heading=(thiscarglobalpos.dr_heading+pilotcarpos.dr_heading)/2;
//汇合路径点位置为当前车辆右侧ydist,前方xdist+ydist
global_vtp_startpoint.dr_x = thiscarglobalpos.dr_x + (ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = thiscarglobalpos.dr_y + (ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_startpoint.dr_x + ydist * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_startpoint.dr_y - ydist * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_endpoint.dr_x + (xdist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_endpoint.dr_y + (xdist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_heading=heading;
//计算汇合点在右侧路径right_smoothpath的位置num
int num=HMath::getNearestPathPointByDistance(right_smoothpath,global_vtp_endpoint);
//获得汇合点位置jointpoint
jointpoint=right_smoothpath[num];
//规划global_vtp_startpoint到汇合点jointpoint的路径join_path1
HMath::linearInterpolation(global_vtp_startpoint,jointpoint,join_path1);
//过渡点位置为汇合点jointpoint前方2*ydist处
global_vtp_startpoint.dr_x = jointpoint.dr_x + (2*ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = jointpoint.dr_y + (2*ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_heading=heading;
//计算过渡点在右侧路径right_smoothpath的位置num1
int num1=HMath::getNearestPathPointByDistance(right_smoothpath,global_vtp_startpoint);
//获得过渡点位置global_vtp_startpoint
global_vtp_startpoint=right_smoothpath[num1];
//规划汇合点jointpoint到过渡点global_vtp_startpoint的路径join_path2
HMath::linearInterpolation(jointpoint,global_vtp_startpoint,join_path2);
//目标点即为过渡点
targetpoint=global_vtp_startpoint;
//将汇合路径join_path1和过渡路径join_path2连接为最终的过渡路径join_path
HMath::linkBackPath(thiscarglobalpos,join_path1,join_path2,join_path);
cft2onlyonceflag=false;
}
//连接过渡路径join_path和右侧路径right_smoothpath为最终的role3直线形变田形路径joinpath
HMath::linkPath(join_path,right_smoothpath,targetpoint,joinpath);
//截取直线形变田形变换路径joinpath中当前位置thiscarglobalpos到领航车位置pilotcarpos的部分
getNearstBackPath(joinpath,thiscarglobalpos,pilotcarpos);
if (getnearestbackpath_flag){
//当前车辆已经到达田形变道目标点targetpoint，则完成直线形变田形
if (HMath::CalcDist(thiscarglobalpos,targetpoint)<100){
cftchange_flag=true;
nearest_path.clear();
}
//直线形变田形未完成则进行结果封装
else{
updatePath(nearest_path);
getTrajectory(nearest_path);
nearest_path.clear();
}
getnearestbackpath_flag=false;
}
}
else if(getrole()==4){
//领航车右侧路径平滑
right_smoothpath=mps->SmoothBSpline(right_smoothpath,smoothlength);
//规划右侧路径的汇合过渡路径，只执行一次
if (cft2onlyonceflag){
//汇合路径起点为此刻当前位置
startpoint=thiscarglobalpos;
//目标点朝向优化减小误差
double heading=(thiscarglobalpos.dr_heading+pilotcarpos.dr_heading)/2;
//汇合路径点位置为当前车辆右侧ydist,前方xdist+ydist
global_vtp_startpoint.dr_x = thiscarglobalpos.dr_x + (ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = thiscarglobalpos.dr_y + (ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_startpoint.dr_x + ydist * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_startpoint.dr_y - ydist * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_endpoint.dr_x + (xdist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_endpoint.dr_y + (xdist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_heading=heading;
//计算汇合点在右侧路径right_smoothpath的位置num
int num=HMath::getNearestPathPointByDistance(right_smoothpath,global_vtp_endpoint);
//获得汇合点位置jointpoint
jointpoint=right_smoothpath[num];
//规划global_vtp_startpoint到汇合点jointpoint的路径join_path1
HMath::linearInterpolation(global_vtp_startpoint,jointpoint,join_path1);
//过渡点位置为汇合点jointpoint前方2*ydist处
global_vtp_startpoint.dr_x = jointpoint.dr_x + (2*ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = jointpoint.dr_y + (2*ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_heading=heading;
//计算过渡点在右侧路径right_smoothpath的位置num1
int num1=HMath::getNearestPathPointByDistance(right_smoothpath,global_vtp_startpoint);
//获得过渡点位置global_vtp_startpoint
global_vtp_startpoint=right_smoothpath[num1];
//规划汇合点jointpoint到过渡点global_vtp_startpoint的路径join_path2
HMath::linearInterpolation(jointpoint,global_vtp_startpoint,join_path2);
//目标点即为过渡点
targetpoint=global_vtp_startpoint;
//将汇合路径join_path1和过渡路径join_path2连接为最终的过渡路径join_path
HMath::linkBackPath(thiscarglobalpos,join_path1,join_path2,join_path);
cft2onlyonceflag=false;
}
//连接过渡路径join_path和右侧路径right_smoothpath为最终的role4直线形变田形路径joinpath
HMath::linkPath(join_path,right_smoothpath,targetpoint,joinpath);
//截取直线形变田形变换路径joinpath中当前位置thiscarglobalpos到领航车位置pilotcarpos的部分
getNearstBackPath(joinpath,thiscarglobalpos,pilotcarpos);
if (getnearestbackpath_flag){
//当前车辆已经到达田形变道目标点targetpoint，则完成直线形变田形
if (HMath::CalcDist(thiscarglobalpos,targetpoint)<100){
cftchange_flag=true;
nearest_path.clear();
}
//直线形变田形未完成则进行结果封装
else{
updatePath(nearest_path);
getTrajectory(nearest_path);
nearest_path.clear();
}
getnearestbackpath_flag=false;
}
}
break;
case 6://田形保持
cft2onlyonceflag=true;
//领航车路径平滑
pilotcar_smoothpath=mps->SmoothBSpline(pilotcar_path,smoothlength);
//生成领航车两侧的路径用于菱形保持规划
generateSidePath(pilotcar_smoothpath);
if(getrole() == 2){
//截取领航车路径pilotcar_smoothpath中当前位置thiscarglobalpos到领航车位置pilotcarpos
getNearstFrontPath(pilotcar_smoothpath,pilotcarpos,thiscarglobalpos);
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
getnearestfrontpath_flag=false;
}
}
else if (getrole()==3) {
//右侧路径延长
HMath::pathFrontExtend(right_smoothpath,25);
//右侧路径平滑
right_smoothpath=mps->SmoothBSpline(right_smoothpath,smoothlength);
cft2onlyonceflag=true;
//右侧目标点为领航车位置pilotcarpos右侧ydist,前方xdist
sidestartpoint.dr_x = pilotcarpos.dr_x + ydist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
sidestartpoint.dr_y = pilotcarpos.dr_y - ydist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
sidestartpoint.dr_x = sidestartpoint.dr_x + xdist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
sidestartpoint.dr_y = sidestartpoint.dr_y + xdist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//截取右侧路径right_smoothpath中当前位置thiscarglobalpos到目标点位置sidestartpoint
getNearstFrontPath(right_smoothpath,sidestartpoint,thiscarglobalpos);
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
getnearestfrontpath_flag=false;
}
}
else if(getrole()==4){
//右侧路径平滑
right_smoothpath=mps->SmoothBSpline(right_smoothpath,smoothlength);
cft2onlyonceflag=true;
//右侧目标点为领航车位置pilotcarpos右侧ydist,前方xdist
sidestartpoint.dr_x = pilotcarpos.dr_x + ydist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
sidestartpoint.dr_y = pilotcarpos.dr_y - ydist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//截取右侧路径right_smoothpath中当前位置thiscarglobalpos到目标点位置sidestartpoint
getNearstFrontPath(right_smoothpath,sidestartpoint,thiscarglobalpos);
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
getnearestfrontpath_flag=false;
}
}
break;
case 7://田形变直线形
//领航车路径平滑
pilotcar_smoothpath=mps->SmoothBSpline(pilotcar_path,smoothlength);
//生成领航车两侧的路径用于直线过渡规划
generateSidePath(pilotcar_smoothpath);
if(getrole() == 2){
//截取领航车路径pilotcar_smoothpath中当前位置thiscarglobalpos到领航车位置pilotcarpos
getNearstFrontPath(pilotcar_smoothpath,pilotcarpos,thiscarglobalpos);
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
getnearestfrontpath_flag=false;
}
}
else if (getrole()==3) {
//右侧路径延长
HMath::pathFrontExtend(right_smoothpath,25);
//右侧路径平滑
right_smoothpath=mps->SmoothBSpline(right_smoothpath,smoothlength);
//田形变直线形分为两个阶段，第一个阶段用于拉开距离
if (cft4process1flag){
//阶段一目标点设为领航车位置右侧ydist,前方xdist
sidestartpoint.dr_x = pilotcarpos.dr_x + ydist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
sidestartpoint.dr_y = pilotcarpos.dr_y - ydist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
sidestartpoint.dr_x = sidestartpoint.dr_x + xdist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
sidestartpoint.dr_y = sidestartpoint.dr_y + xdist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//截取左侧路径right_smoothpath中当前位置thiscarglobalpos到阶段一目标点位置sidestartpoint
getNearstFrontPath(right_smoothpath,sidestartpoint,thiscarglobalpos);
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
getnearestfrontpath_flag=false;
}
}
//田形变直线形分为两个阶段，第二个阶段用于入队
if (cft4process2flag){
if (cft4onlyonceflag){
//汇合路径起点为此刻当前位置
startpoint=thiscarglobalpos;
//目标点朝向优化减小误差
double heading=(thiscarglobalpos.dr_heading+pilotcarpos.dr_heading)/2;
//汇合路径点位置为当前车辆左侧ydist,前方xdist+ydist
global_vtp_startpoint.dr_x = thiscarglobalpos.dr_x + (ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = thiscarglobalpos.dr_y + (ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_startpoint.dr_x - ydist * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_startpoint.dr_y + ydist * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_endpoint.dr_x + (xdist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_endpoint.dr_y + (xdist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_heading=heading;
//计算汇合点在领航车路径pilotcar_smoothpath的位置num
int num=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,global_vtp_endpoint);
//获得汇合点位置jointpoint
jointpoint=pilotcar_smoothpath[num];
//规划global_vtp_startpoint到汇合点jointpoint的路径join_path1
HMath::linearInterpolation(global_vtp_startpoint,jointpoint,join_path1);
//过渡点位置为汇合点jointpoint前方2*ydist处
global_vtp_startpoint.dr_x = jointpoint.dr_x + (2*ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = jointpoint.dr_y + (2*ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_heading=heading;
//计算过渡点在领航车路径pilotcar_smoothpath的位置num1
int num1=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,global_vtp_startpoint);
//获得过渡点位置global_vtp_startpoint
global_vtp_startpoint=pilotcar_smoothpath[num1];
//规划汇合点jointpoint到过渡点global_vtp_startpoint的路径join_path2
HMath::linearInterpolation(jointpoint,global_vtp_startpoint,join_path2);
//目标点即为过渡点
targetpoint=global_vtp_startpoint;
//将汇合路径join_path1和过渡路径join_path2连接为最终的过渡路径join_path
HMath::linkBackPath(thiscarglobalpos,join_path1,join_path2,join_path);
cft4onlyonceflag=false;
}
//连接过渡路径join_path和领航车路径pilotcar_smoothpath为最终的role3田形变直线形路径joinpath
HMath::linkPath(join_path,pilotcar_smoothpath,targetpoint,joinpath);
//截取田形变直线形路径joinpath中当前位置thiscarglobalpos到领航车位置pilotcarpos的部分
getNearstBackPath(joinpath,thiscarglobalpos,pilotcarpos);
if (getnearestbackpath_flag){
//当前车辆已经到达田形变直线形目标点targetpoint，则完成田形变直线形
if (HMath::CalcDist(thiscarglobalpos,targetpoint)<100){
cftchange_flag=true;
nearest_path.clear();
}
//田形变直线形未完成则进行结果封装
else{
updatePath(nearest_path);
getTrajectory(nearest_path);
nearest_path.clear();
}
getnearestbackpath_flag=false;
}
}
}
else if(getrole()==4){
//右侧路径延长
HMath::pathFrontExtend(right_smoothpath,15);
//右侧路径平滑
right_smoothpath=mps->SmoothBSpline(right_smoothpath,smoothlength);
//田形变直线形分为两个阶段，第一个阶段用于拉开距离
if (cft4process1flag){
//阶段一目标点设为领航车位置右侧ydist
sidestartpoint.dr_x = pilotcarpos.dr_x + ydist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
sidestartpoint.dr_y = pilotcarpos.dr_y - ydist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//截取左侧路径right_smoothpath中当前位置thiscarglobalpos到阶段一目标点位置sidestartpoint
getNearstFrontPath(right_smoothpath,sidestartpoint,thiscarglobalpos);
if (getnearestfrontpath_flag){
updatePath(nearest_path);
getTrajectory(nearest_path);
getnearestfrontpath_flag=false;
}
}
//田形变直线形分为两个阶段，第二个阶段用于入队
if (cft4process2flag){
if (cft4onlyonceflag){
//汇合路径起点为此刻当前位置
startpoint=thiscarglobalpos;
//目标点朝向优化减小误差
double heading=(thiscarglobalpos.dr_heading+pilotcarpos.dr_heading)/2;
//汇合路径点位置为当前车辆左侧ydist,前方xdist+ydist
global_vtp_startpoint.dr_x = thiscarglobalpos.dr_x + (ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = thiscarglobalpos.dr_y + (ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_startpoint.dr_x - ydist * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_startpoint.dr_y + ydist * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_x = global_vtp_endpoint.dr_x + (xdist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_y = global_vtp_endpoint.dr_y + (xdist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_endpoint.dr_heading=heading;
//计算汇合点在领航车路径pilotcar_smoothpath的位置num
int num=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,global_vtp_endpoint);
//获得汇合点位置jointpoint
jointpoint=pilotcar_smoothpath[num];
//规划global_vtp_startpoint到汇合点jointpoint的路径join_path1
HMath::linearInterpolation(global_vtp_startpoint,jointpoint,join_path1);
//过渡点位置为汇合点jointpoint前方2*ydist处
global_vtp_startpoint.dr_x = jointpoint.dr_x + (2*ydist) * cos((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_y = jointpoint.dr_y + (2*ydist) * sin((int(heading*0.01) % 360)*degree2rad);
global_vtp_startpoint.dr_heading=heading;
//计算过渡点在领航车路径pilotcar_smoothpath的位置num1
int num1=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,global_vtp_startpoint);
//获得过渡点位置global_vtp_startpoint
global_vtp_startpoint=pilotcar_smoothpath[num1];
//规划汇合点jointpoint到过渡点global_vtp_startpoint的路径join_path2
HMath::linearInterpolation(jointpoint,global_vtp_startpoint,join_path2);
//目标点即为过渡点
targetpoint=global_vtp_startpoint;
//将汇合路径join_path1和过渡路径join_path2连接为最终的过渡路径join_path
HMath::linkBackPath(thiscarglobalpos,join_path1,join_path2,join_path);
cft4onlyonceflag=false;
}
//连接过渡路径join_path和领航车路径pilotcar_smoothpath为最终的role3田形变直线形路径joinpath
HMath::linkPath(join_path,pilotcar_smoothpath,targetpoint,joinpath);
//截取田形变直线形路径joinpath中当前位置thiscarglobalpos到领航车位置pilotcarpos的部分
getNearstBackPath(joinpath,thiscarglobalpos,pilotcarpos);
if (getnearestbackpath_flag){
//当前车辆已经到达田形变直线形目标点targetpoint，则完成田形变直线形
if (HMath::CalcDist(thiscarglobalpos,targetpoint)<100){
cftchange_flag=true;
nearest_path.clear();
}
//田形变直线形未完成则进行结果封装
else{
updatePath(nearest_path);
getTrajectory(nearest_path);
nearest_path.clear();
}
getnearestbackpath_flag=false;
}
}
}
break;
}
}
/**
* @brief Formation::getFrontCarPos
* 正常行驶包括队形变换下的目标车辆位置确定
*/
void Formation::getFrontCarPos(){
double dx=0;
double dy=0;
VehicleState frontcar;
//跟点模式下，目标前车位置即为跟点位置
if(!trackmode){
frontcarlocalpos=pointtrackpoint;
getfrontcarposflag=true;
}
else{
switch (getCFT()) {
//直线保持
case 1:
localplannum=0;
//根据当前车辆role确定对应的目标前车位置
if (getrole() == 2){
frontcar=pilotcarpos;
getfrontcarposflag=true;
}
else if (getrole()==3){
frontcar=role2carpos;
getfrontcarposflag=true;
}
else if (getrole()==4) {
frontcar=role3carpos;
getfrontcarposflag=true;
}
break;
//直线变菱形
case 2:
if (getrole() == 2){
//目标前车位置即为领航车左侧ydist
frontcar.dr_x = pilotcarpos.dr_x - ydist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
frontcar.dr_y = pilotcarpos.dr_y + ydist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
nearest_path.clear();
left_smoothpath.clear();
getfrontcarposflag=true;
//如果直线变菱形已完成，则自动将当前状态设为3，即为菱形保持
if (cftchange_flag){
setCFT(3);
cftchange_flag=false;
}
}
else if (getrole()==3){
//目标前车位置即为领航车后方rxdist，右侧ydist
frontcar.dr_x = pilotcarpos.dr_x - rxdist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
frontcar.dr_y = pilotcarpos.dr_y - rxdist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
int num=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,frontcar);
frontcar=pilotcar_smoothpath[num];
frontcar.dr_x = frontcar.dr_x + ydist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
frontcar.dr_y = frontcar.dr_y - ydist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
getfrontcarposflag=true;
//如果直线变菱形已完成，则自动将当前状态设为3，即为菱形保持
if (cftchange_flag){
setCFT(3);
cftchange_flag=false;
}
}
else if (getrole()==4) {
static int currentnum2=0;
//静态变量遍历完毕，则代表直线变菱形已完成，自动将当前状态设为3，即为菱形保持
if (currentnum2 >= globalVTP.size()){
currentnum2=0;
setCFT(3);
}
//否则一次更新目标点位置
else{
// int num=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,globalVTP[currentnum2]);
// frontcar=pilotcar_smoothpath[num];
frontcar=globalVTP[currentnum2];
currentnum2++;
getfrontcarposflag=true;
}
}
break;
case 3://菱形保持
if (getrole() == 2){
//计算左侧目标点sidestartpoint在左侧路径left_smoothpath的位置num
int num=HMath::getNearestPathPointByDistance(left_smoothpath,sidestartpoint);
//获得目标前车位置
frontcar=left_smoothpath[num];
getfrontcarposflag=true;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
}
else if (getrole()==3){
localVTP.clear();
VehicleState local_vtp_startpoint;
VehicleState local_vtp_endpoint;
VehicleState local_vtp_waypoint;
//目标前车起点为领航车位置pilotcarpos后方rxdist，右侧ydist
local_vtp_startpoint.dr_x = pilotcarpos.dr_x - (rxdist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_startpoint.dr_y = pilotcarpos.dr_y - (rxdist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_startpoint.dr_x = local_vtp_startpoint.dr_x + (ydist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_startpoint.dr_y = local_vtp_startpoint.dr_y - (ydist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//计算目标前车起点local_vtp_startpoint在右侧路径right_smoothpath的位置num1
int num1=HMath::getNearestPathPointByDistance(right_smoothpath,local_vtp_startpoint);
//获得目标前车起点位置
local_vtp_startpoint=right_smoothpath[num1];
//目标前车终点为领航车位置pilotcarpos右侧ydist
local_vtp_endpoint.dr_x = pilotcarpos.dr_x + (rxdist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_endpoint.dr_y = pilotcarpos.dr_y - (rxdist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//计算目标前车终点local_vtp_endpoint在右侧路径right_smoothpath的位置num2
int num2=HMath::getNearestPathPointByDistance(right_smoothpath,local_vtp_endpoint);
//获得目标前车终点位置
local_vtp_endpoint=right_smoothpath[num2];
// local_vtp_waypoint=local_vtp_startpoint;
// for(int i=100;i>=1;i--){
// local_vtp_waypoint.dr_x = local_vtp_waypoint.dr_x + i * (local_vtp_endpoint.dr_x-local_vtp_startpoint.dr_x)/5050;
// local_vtp_waypoint.dr_y = local_vtp_waypoint.dr_y + i * (local_vtp_endpoint.dr_y-local_vtp_startpoint.dr_y)/5050;
// local_vtp_waypoint.dr_heading=pilotcarpos.dr_heading;
// localVTP.push_back(local_vtp_waypoint);
// }
//生成从目标前车起点位置local_vtp_startpoint到目标前车终点位置local_vtp_endpoint间的非均匀线性插值虚拟点localVTP
HMath::generateUnEvenTargetPointUp(local_vtp_startpoint,local_vtp_endpoint,100,localVTP);
//静态变量遍历完毕,则目标前车位置为领航车位置右侧ydist
if (localplannum >= localVTP.size()){
frontcar.dr_x = pilotcarpos.dr_x + (ydist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
frontcar.dr_y = pilotcarpos.dr_y - (ydist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
getfrontcarposflag=true;
}
//静态变量遍历未完毕，则进行遍历
else{
frontcar=localVTP[localplannum];
localplannum++;
getfrontcarposflag=true;
}
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
}
else if (getrole()==4) {
//目标前车位置为领航车位置pilotcarpos后方rxdist
frontcar.dr_x=pilotcarpos.dr_x-rxdist*cos(pilotcarpos.dr_heading*0.01 * degree2rad);
frontcar.dr_y=pilotcarpos.dr_y-rxdist*sin(pilotcarpos.dr_heading*0.01 * degree2rad);
//计算目标前车位置frontcar在领航车路径pilotcar_smoothpath的位置num
int num=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,frontcar);
//获得目标前车位置
frontcar=pilotcar_smoothpath[num];
getfrontcarposflag=true;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
}
break;
case 4://菱形变直线形
if (getrole() == 2){
//阶段一目标前车位置
if (cft4process1flag){
//目标前车位置为领航车位置pilotcarpos左侧ydist
sidestartpoint.dr_x = pilotcarpos.dr_x - ydist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
sidestartpoint.dr_y = pilotcarpos.dr_y + ydist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//获得目标前车位置
frontcar=sidestartpoint;
getfrontcarposflag=true;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
// cout<<thiscarglobalpos.dr_x<<" "<<thiscarglobalpos.dr_y<<endl;
// cout<<role3carpos.dr_x<<" "<<role3carpos.dr_y<<endl;
// cout<<HMath::CalcDist(thiscarglobalpos,role3carpos)<<endl;
//判断如果当前车辆位置thiscarglobalpos和角色3车辆位置role3carpos距离足够大，则进行阶段二
if (HMath::CalcDist(thiscarglobalpos,role3carpos)>=2200){
cft4process1flag=false;
cft4process2flag=true;
}
}
//阶段二目标前车位置
if (cft4process2flag){
//目标前车位置为领航车位置pilotcarpos
frontcar.dr_x = pilotcarpos.dr_x;
frontcar.dr_y = pilotcarpos.dr_y;
getfrontcarposflag=true;
//如果菱形变直线已完成，则自动将当前状态设为1，即为直线保持
if (cftchange_flag){
setCFT(1);
cftchange_flag=false;
}
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
}
}
else if (getrole()==3){
//阶段一目标前车位置
if (cft4process1flag){
//目标速度为当前时刻速度并保持
if (formationchangeholdonly){
formation_speed=pilotcarspeed;
formationchangeholdonly=false;
}
getformationspeed=true;
getfrontcarposflag=false;
frontcar.dr_x=0;
frontcar.dr_y=0;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
// cout<<thiscarglobalpos.dr_x<<" "<<thiscarglobalpos.dr_y<<endl;
// cout<<role2carpos.dr_x<<" "<<role2carpos.dr_y<<endl;
// cout<<HMath::CalcDist(role2carpos,thiscarglobalpos)<<endl;
//判断如果当前车辆位置thiscarglobalpos和角色2车辆位置role2carpos距离足够大，则进行阶段二
if (HMath::CalcDist(role2carpos,thiscarglobalpos)>=2200){
cft4process1flag=false;
cft4process2flag=true;
}
}
//阶段二目标前车位置
if (cft4process2flag){
//目标前车位置为领航车位置pilotcarpos后方rxdist
frontcar.dr_x = pilotcarpos.dr_x-rxdist*cos(pilotcarpos.dr_heading*0.01 * degree2rad);
frontcar.dr_y = pilotcarpos.dr_y-rxdist*sin(pilotcarpos.dr_heading*0.01 * degree2rad);
getfrontcarposflag=true;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
//如果菱形变直线已完成，则自动将当前状态设为1，即为直线保持
if (cftchange_flag){
setCFT(1);
cftchange_flag=false;
}
}
}
else if (getrole()==4) {
//阶段一目标前车位置
if (cft4process1flag){
//目标速度为当前时刻速度并保持
if (formationchangeholdonly){
formation_speed=pilotcarspeed;
formationchangeholdonly=false;
}
getformationspeed=true;
getfrontcarposflag=false;
frontcar.dr_x=0;
frontcar.dr_y=0;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
// cout<<role2carpos.dr_x<<" "<<role2carpos.dr_y<<endl;
// cout<<role3carpos.dr_x<<" "<<role3carpos.dr_y<<endl;
// cout<<HMath::CalcDist(role2carpos,role3carpos)<<endl;
//判断如果角色2车辆位置role2carpos和角色3车辆位置role2carpos距离足够大，则进行阶段二
if (HMath::CalcDist(role2carpos,role3carpos)>=2200){
cft4process1flag=false;
cft4process2flag=true;
}
}
//阶段二目标前车位置
if (cft4process2flag){
//计算角色3车辆位置role3carpos在领航车路径pilotcar_smoothpath的位置num
int num3=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,role3carpos);
//获得目标前车位置
frontcar=pilotcar_smoothpath[num3];
getfrontcarposflag=true;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
//判断如果角色3车辆位置role3carpos已经入队，则菱形变直线已完成，则自动将当前状态设为1，即为直线保持
if (HMath::CalcDist(frontcar,role3carpos)<=200){
setCFT(1);
}
}
}
break;
case 5://直线变田形
//根据当前车辆role确定对应的目标前车位置
if (getrole() == 2){
frontcar=pilotcarpos;
getfrontcarposflag=true;
nearest_path.clear();
}
else if (getrole()==3){
//目标前车位置即为领航车后方rxdist，右侧ydist
frontcar.dr_x = pilotcarpos.dr_x - rxdist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
frontcar.dr_y = pilotcarpos.dr_y - rxdist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
int num=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,frontcar);
frontcar=pilotcar_smoothpath[num];
frontcar.dr_x = frontcar.dr_x + ydist*sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
frontcar.dr_y = frontcar.dr_y - ydist*cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
getfrontcarposflag=true;
//如果直线变田形已完成，则自动将当前状态设为6，即为田形保持
if (cftchange_flag){
setCFT(6);
cftchange_flag=false;
}
}
else if (getrole()==4) {
frontcar=role3carpos;
getfrontcarposflag=true;
//如果直线变田形已完成，则自动将当前状态设为6，即为田形保持
if (cftchange_flag){
setCFT(6);
cftchange_flag=false;
}
}
break;
case 6://田形保持
if (getrole() == 2){
frontcar=pilotcarpos;
getfrontcarposflag=true;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
}
else if (getrole()==3){
localVTP.clear();
VehicleState local_vtp_startpoint;
VehicleState local_vtp_endpoint;
VehicleState local_vtp_waypoint;
//目标前车起点为领航车位置pilotcarpos后方rxdist，右侧ydist
local_vtp_startpoint.dr_x = pilotcarpos.dr_x - (rxdist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_startpoint.dr_y = pilotcarpos.dr_y - (rxdist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_startpoint.dr_x = local_vtp_startpoint.dr_x + (ydist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_startpoint.dr_y = local_vtp_startpoint.dr_y - (ydist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//计算目标前车起点local_vtp_startpoint在右侧路径right_smoothpath的位置num1
int num1=HMath::getNearestPathPointByDistance(right_smoothpath,local_vtp_startpoint);
//获得目标前车起点位置
local_vtp_startpoint=right_smoothpath[num1];
//目标前车终点为领航车位置pilotcarpos前方rxdist,右侧ydist
local_vtp_endpoint.dr_x = pilotcarpos.dr_x + (rxdist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_endpoint.dr_y = pilotcarpos.dr_y + (rxdist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_endpoint.dr_x = local_vtp_endpoint.dr_x + (rxdist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_endpoint.dr_y = local_vtp_endpoint.dr_y - (rxdist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
//计算目标前车终点local_vtp_endpoint在右侧路径right_smoothpath的位置num2
int num2=HMath::getNearestPathPointByDistance(right_smoothpath,local_vtp_endpoint);
//获得目标前车终点位置
local_vtp_endpoint=right_smoothpath[num2];
// local_vtp_waypoint=local_vtp_startpoint;
// for(int i=100;i>=1;i--){
// local_vtp_waypoint.dr_x = local_vtp_waypoint.dr_x + i * (local_vtp_endpoint.dr_x-local_vtp_startpoint.dr_x)/5050;
// local_vtp_waypoint.dr_y = local_vtp_waypoint.dr_y + i * (local_vtp_endpoint.dr_y-local_vtp_startpoint.dr_y)/5050;
// local_vtp_waypoint.dr_heading=pilotcarpos.dr_heading;
// localVTP.push_back(local_vtp_waypoint);
// }
//生成从目标前车起点位置local_vtp_startpoint到目标前车终点位置local_vtp_endpoint间的非均匀线性插值虚拟点localVTP
HMath::generateUnEvenTargetPointUp(local_vtp_startpoint,local_vtp_endpoint,150,localVTP);
//静态变量遍历完毕,则目标前车位置为领航车位置右侧ydist
if (localplannum >= localVTP.size()){
frontcar.dr_x = pilotcarpos.dr_x + (ydist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
frontcar.dr_y = pilotcarpos.dr_y - (ydist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
frontcar.dr_x = frontcar.dr_x + (rxdist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
frontcar.dr_y = frontcar.dr_y + (rxdist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
getfrontcarposflag=true;
}
//静态变量遍历未完毕，则进行遍历
else{
frontcar=localVTP[localplannum];
localplannum++;
getfrontcarposflag=true;
}
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
}
else if (getrole()==4) {
frontcar=role3carpos;
getfrontcarposflag=true;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
}
break;
case 7://田形变直线形
if (getrole() == 2){
frontcar=pilotcarpos;
getfrontcarposflag=true;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
}
else if (getrole()==3){
//阶段一目标前车位置
if (cft4process1flag){
//目标速度为当前时刻速度并保持
if (formationchangeholdonly){
formation_speed=pilotcarspeed;
formationchangeholdonly=false;
}
getformationspeed=true;
getfrontcarposflag=false;
frontcar.dr_x=0;
frontcar.dr_y=0;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
// cout<<thiscarglobalpos.dr_x<<" "<<thiscarglobalpos.dr_y<<endl;
// cout<<role2carpos.dr_x<<" "<<role2carpos.dr_y<<endl;
// cout<<HMath::CalcDist(role2carpos,thiscarglobalpos)<<endl;
//判断如果当前车辆位置thiscarglobalpos和角色2车辆位置role2carpos距离足够大，则进行阶段二
if (HMath::CalcDist(role2carpos,thiscarglobalpos)>=2100){
cft4process1flag=false;
cft4process2flag=true;
}
}
//阶段二目标前车位置
if (cft4process2flag){
//目标前车位置为领航车位置pilotcarpos后方rxdist
frontcar.dr_x = pilotcarpos.dr_x-rxdist*cos(pilotcarpos.dr_heading*0.01 * degree2rad);
frontcar.dr_y = pilotcarpos.dr_y-rxdist*sin(pilotcarpos.dr_heading*0.01 * degree2rad);
getfrontcarposflag=true;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
//如果菱形变直线已完成，则自动将当前状态设为1，即为直线保持
if (cftchange_flag){
setCFT(1);
cftchange_flag=false;
}
}
}
else if (getrole()==4) {
//阶段一目标前车位置
if (cft4process1flag){
frontcar=role3carpos;
getformationspeed=true;
getfrontcarposflag=true;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
// cout<<role2carpos.dr_x<<" "<<role2carpos.dr_y<<endl;
// cout<<role3carpos.dr_x<<" "<<role3carpos.dr_y<<endl;
// cout<<HMath::CalcDist(role2carpos,role3carpos)<<endl;
//判断如果角色2车辆位置role2carpos和角色3车辆位置role2carpos距离足够大，则进行阶段二
if (HMath::CalcDist(role2carpos,role3carpos)>=2100){
cft4process1flag=false;
cft4process2flag=true;
}
}
//阶段二目标前车位置
if (cft4process2flag){
//计算角色3车辆位置role3carpos在领航车路径pilotcar_smoothpath的位置num
int num3=HMath::getNearestPathPointByDistance(pilotcar_smoothpath,role3carpos);
//获得目标前车位置
frontcar=pilotcar_smoothpath[num3];
getfrontcarposflag=true;
nearest_path.clear();
left_smoothpath.clear();
right_smoothpath.clear();
//判断如果角色3车辆位置role3carpos已经入队，则菱形变直线已完成，则自动将当前状态设为1，即为直线保持
if (HMath::CalcDist(frontcar,role3carpos)<=200){
setCFT(1);
}
}
}
break;
}
// frontcarlocalpos.dr_x=frontcar.dr_x;
// frontcarlocalpos.dr_y=frontcar.dr_y;
//目标前车绝对位置转换到当前车辆下的局部位置
dx = frontcar.dr_x - thiscarglobalpos.dr_x;
dy = frontcar.dr_y - thiscarglobalpos.dr_y;
frontcarlocalpos.dr_x = dx * cos(thiscarglobalpos.dr_heading *0.01 * degree2rad) + dy * sin(thiscarglobalpos.dr_heading *0.01 * degree2rad);
frontcarlocalpos.dr_y = -dx * sin(thiscarglobalpos.dr_heading *0.01 * degree2rad) + dy * cos(thiscarglobalpos.dr_heading *0.01 * degree2rad);
}
}
/**
* @brief Formation::getAttritionfrontCarPos
* 减员任务下，根据当前车辆role和减员车辆role确定目标车辆位置
* @param role
*/
void Formation::getAttritionfrontCarPos(int role){
double dx=0;
double dy=0;
VehicleState frontcar;
VehicleState local_vtp_startpoint;
VehicleState local_vtp_endpoint;
VehicleState local_vtp_waypoint;
switch (role){
case 2:
if (getrole()==3){
localVTP.clear();
if (attritiononlyonceflag){
attritioncar_dist=HMath::CalcDist(pilotcarpos,role2carpos);
attritiononlyonceflag=false;
}
local_vtp_startpoint.dr_x = pilotcarpos.dr_x - (attritioncar_dist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_startpoint.dr_y = pilotcarpos.dr_y - (attritioncar_dist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_endpoint=pilotcarpos;
for(int i=0; i<attritionnum+1; i++){
local_vtp_waypoint.dr_x = local_vtp_startpoint.dr_x + i*(local_vtp_endpoint.dr_x-local_vtp_startpoint.dr_x)/attritionnum;
local_vtp_waypoint.dr_y = local_vtp_startpoint.dr_y + i*(local_vtp_endpoint.dr_y-local_vtp_startpoint.dr_y)/attritionnum;
local_vtp_waypoint.dr_heading=pilotcarpos.dr_heading;
localVTP.push_back(local_vtp_waypoint);
}
// cout<<localVTP.size()<<" "<<atrrition_frontPoint_num<<endl;
// local_vtp_startpoint.dr_x = pilotcarpos.dr_x - (attritioncar_dist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
// local_vtp_startpoint.dr_y = pilotcarpos.dr_y - (attritioncar_dist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
// local_vtp_endpoint=pilotcarpos;
// HMath::generateUnEvenTargetPointUp(local_vtp_startpoint,local_vtp_endpoint,80,localVTP);
if (attrition_delaytimes>20){
if (atrrition_frontPoint_num>=localVTP.size()){
frontcar=pilotcarpos;
}
else{
frontcar=localVTP[atrrition_frontPoint_num];
atrrition_frontPoint_num++;
}
}
else{
frontcar=local_vtp_startpoint;
attrition_delaytimes++;
}
getfrontcarposflag=true;
}
else if (getrole()==4){
frontcar=role3carpos;
getfrontcarposflag=true;
}
break;
case 3:
if (getrole()==2){
frontcar=pilotcarpos;
getfrontcarposflag=true;
}
else if (getrole()==4){
localVTP.clear();
if (attritiononlyonceflag){
attritioncar_dist=HMath::CalcDist(role2carpos,role3carpos);
attritiononlyonceflag=false;
}
local_vtp_startpoint.dr_x = role2carpos.dr_x - (attritioncar_dist) * cos((int(role2carpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_startpoint.dr_y = role2carpos.dr_y - (attritioncar_dist) * sin((int(role2carpos.dr_heading*0.01) % 360)*degree2rad);
local_vtp_endpoint=role2carpos;
for(int i=0; i<attritionnum+1; i++){
local_vtp_waypoint.dr_x = local_vtp_startpoint.dr_x + i*(local_vtp_endpoint.dr_x-local_vtp_startpoint.dr_x)/attritionnum;
local_vtp_waypoint.dr_y = local_vtp_startpoint.dr_y + i*(local_vtp_endpoint.dr_y-local_vtp_startpoint.dr_y)/attritionnum;
local_vtp_waypoint.dr_heading=pilotcarpos.dr_heading;
localVTP.push_back(local_vtp_waypoint);
}
// local_vtp_startpoint.dr_x = role2carpos.dr_x - (attritioncar_dist) * cos((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
// local_vtp_startpoint.dr_y = role2carpos.dr_y - (attritioncar_dist) * sin((int(pilotcarpos.dr_heading*0.01) % 360)*degree2rad);
// local_vtp_endpoint=role2carpos;
// HMath::generateUnEvenTargetPointUp(local_vtp_startpoint,local_vtp_endpoint,80,localVTP);
// cout<<localVTP.size()<<" "<<atrrition_frontPoint_num<<endl;
if (attrition_delaytimes>20){
if (atrrition_frontPoint_num>=localVTP.size()){
frontcar=role2carpos;
}
else{
frontcar=localVTP[atrrition_frontPoint_num];
atrrition_frontPoint_num++;
}
}
else{
frontcar=local_vtp_startpoint;
attrition_delaytimes++;
}
getfrontcarposflag=true;
}
break;
case 4:
if (getrole()==2){
frontcar=pilotcarpos;
getfrontcarposflag=true;
}
else if (getrole()==3){
frontcar=role2carpos;
getfrontcarposflag=true;
}
break;
}
// frontcarlocalpos.dr_x=frontcar.dr_x;
// frontcarlocalpos.dr_y=frontcar.dr_y;
dx = frontcar.dr_x - thiscarglobalpos.dr_x;
dy = frontcar.dr_y - thiscarglobalpos.dr_y;
frontcarlocalpos.dr_x = dx * cos(thiscarglobalpos.dr_heading *0.01 * degree2rad) + dy * sin(thiscarglobalpos.dr_heading *0.01 * degree2rad);
frontcarlocalpos.dr_y = -dx * sin(thiscarglobalpos.dr_heading *0.01 * degree2rad) + dy * cos(thiscarglobalpos.dr_heading *0.01 * degree2rad);
}
/**
* @brief Formation::getTrackMode
* 确定跟踪模式，根据singlevehicleinfo.control_mode标志位确定车辆跟踪模式为跟点false还是跟路径true
* @param targetvehicles
*/
void Formation::getTrackMode(const hmi_msgs::SingleVehicleInfo singlevehicleinfo){
//TODO:确定跟点和跟路模式是否需要，怎么定义
if(singlevehicleinfo.control_mode==1) {
trackmode=true;
}
else if(singlevehicleinfo.control_mode==2){
trackmode=false;
pilotcar_path.clear();
}
}
/**
* @brief Formation::getPointTrackTarget
* 获取跟点模式下的目标点位置pointtrackpoint，和速度pointtrackspeed
* 获得后更新标志位getpointtracktarget
* @param targetvehicles
*/
void Formation::getPointTrackTarget(const hmi_msgs::SingleVehicleInfo singlevehicleinfo){
//TODO:确定跟单模式下目标点的位置和速度
// pointtrackpoint.dr_x=targetvehicles.otherVehicle[i].y;
// pointtrackpoint.dr_y=-targetvehicles.otherVehicle[i].x;
// pointtrackspeed=(targetvehicles.otherVehicle[i].speed)*0.01*3.6;
getpointtracktarget_flag=true;
}
/**
* @brief Formation::updatePlanResult
* 编队分配结果更新，根据车辆行驶状态nVehicleCommond和目标速度nSpeed更新分配结果
* @param nVehicleCommond
* @param nSpeed
*/
void Formation::updatePlanResult(int nVehicleCommond,int nSpeed){
//TODO:对接编队结果封装
plan_result.local_time = ros::Time::now().toNSec();
plan_result.message_num = plan_id;
plan_result.coorType=1;
plan_result.freezeCoor.x = thiscarlocalpos.dr_x;//localpos
plan_result.freezeCoor.y = thiscarlocalpos.dr_y;
//plan_result.plan_frame.g_x = thiscarlocalpos.dr_x;
//plan_result.plan_frame.g_y = thiscarlocalpos.dr_y;
plan_result.freezeCoor.theta = thiscarlocalpos.dr_heading;
// plan_result.plan_frame.pitch = thiscarlocalpos.dr_pitch;
// plan_result.plan_frame.roll = thiscarlocalpos.dr_roll;
// plan_result.is_ok = 1;
//ES=200,ST,AD_SPEED,AD_DISTANCE,AD_POINT,BK_SPEED,BK_POINT,IG,FO
plan_result.vehicle_command = nVehicleCommond;
if (trackmode){
if(getrole()==1){
plan_result.exp_speed = 0;
}
else if(getrole()==2){
plan_result.exp_speed = int(pilotcarspeed);
}
else if(getrole()==3){
plan_result.exp_speed = int(HMath::getMinimum(nSpeed,role2carspeed));
}
else if(getrole()==4){
plan_result.exp_speed = int(HMath::getMinimum(nSpeed,role3carspeed));
}
}
else{
plan_result.exp_speed = int(pointtrackspeed);
}
ROS_INFO("Speed :%d",plan_result.exp_speed);
if (getfrontcarposflag){
plan_result.target_pose.x=frontcarlocalpos.dr_x;
plan_result.target_pose.y=frontcarlocalpos.dr_y;
getfrontcarposflag=false;
}
else{
plan_result.target_pose.x=-1;
plan_result.target_pose.y=-1;
if (getformationspeed){
plan_result.exp_speed =int(formation_speed);
getformationspeed=false;
}
}
// plan_result.expVelocity=2000;
// for(int i=0;i<50;i++)
// {
// plan_result.center_line_property[i].direction = thiscarlocalpos.dr_heading ;
// }
plan_id ++;
}
/**
* @brief Formation::updateAttritionPlanResult
* 减员任务下，编队结果封装
* @param nVehicleCommond
* @param role
*/
void Formation::updateAttritionPlanResult(int nVehicleCommond,int role){
//TODO:对接减员编队结果封装
plan_result.local_time = ros::Time::now().toNSec();
plan_result.message_num = plan_id;
plan_result.coorType=1;
plan_result.freezeCoor.x = thiscarlocalpos.dr_x;//localpos
plan_result.freezeCoor.y = thiscarlocalpos.dr_y;
// plan_result.plan_frame.g_x = thiscarlocalpos.dr_x;
// plan_result.plan_frame.g_y = thiscarlocalpos.dr_y;
plan_result.freezeCoor.theta = thiscarlocalpos.dr_heading;
// plan_result.plan_frame.pitch = thiscarlocalpos.dr_pitch;
// plan_result.plan_frame.roll = thiscarlocalpos.dr_roll;
// plan_result.is_ok = 1;
//ES=200,ST,AD_SPEED,AD_DISTANCE,AD_POINT,BK_SPEED,BK_POINT,IG,FO
plan_result.vehicle_command = nVehicleCommond;
if(getrole()==1){
plan_result.exp_speed = 0;
}
else if(getrole()==2){
plan_result.exp_speed = int(pilotcarspeed);
}
else if(getrole()==3){
plan_result.exp_speed = int(pilotcarspeed);
if (role==2){
plan_result.exp_speed = int(pilotcarspeed);
}
else if(role==4){
plan_result.exp_speed = int(HMath::getMinimum(pilotcarspeed,role2carspeed));
}
}
else if(getrole()==4){
plan_result.exp_speed = int(pilotcarspeed);
if (role==2){
plan_result.exp_speed = int(HMath::getMinimum(pilotcarspeed,role3carspeed));
}
else if (role==3){
plan_result.exp_speed = int(HMath::getMinimum(pilotcarspeed,role2carspeed));
}
}
ROS_INFO("Speed :%d",plan_result.exp_speed);
if (getfrontcarposflag){
plan_result.target_pose.x=frontcarlocalpos.dr_x;
plan_result.target_pose.y=frontcarlocalpos.dr_y;
getfrontcarposflag=false;
}
else{
plan_result.target_pose.x=-1;
plan_result.target_pose.y=-1;
}
// plan_result.expVelocity=2000;
// for(int i=0;i<50;i++)
// {
// plan_result.center_line_property[i].direction = thiscarlocalpos.dr_heading ;
// }
plan_id ++;
}
/**
* @brief Formation::formationReady
* 等待任务分配，包括路径规划generateRunningPath和目标位置规划getFrontCarPos
* 更新分配结果updatePlanResult，其中车辆行驶状态为AD_SPEED，速度为0
*/
void Formation::formationReady(){
generateRunningPath();
getFrontCarPos();
updatePlanResult(AD_SPEED,0);
}
/**
* @brief Formation::formationRunning
* 前进任务分配，包括路径规划generateRunningPath和目标位置规划getFrontCarPos
* 更新分配结果updatePlanResult，其中车辆行驶状态为AD_SPEED，速度为pilotcarspeed
* 以及倒车和相关标志位
*/
//TODO:倒车逻辑
void Formation::formationRunning(){
generateRunningPath();
getFrontCarPos();
updatePlanResult(AD_SPEED,pilotcarspeed);
if (!back->storeflag){
back->waypoint.clear();
back->storeflag=true;
}
attrition->parkpoint_onlyonceflag=true;
}
/**
* @brief Formation::formationStop
* 停车任务分配，包括路径规划generateRunningPath和目标位置规划getFrontCarPos
* 更新分配结果updatePlanResult，其中车辆行驶状态为AD_SPEED，速度为0
*/
void Formation::formationStop(){
generateRunningPath();
getFrontCarPos();
updatePlanResult(AD_SPEED,0);
}
/**
* @brief Formation::formationBackWard
* 倒车任务分配，包括执行倒车逻辑backRun()
*/
void Formation::formationBackWard(){
vector<VehicleState> allcargpos={thiscarglobalpos,pilotcarpos,role2carpos,role3carpos,role4carpos,thiscarlocalpos};
back->backRun(BK_SPEED,6,plan_id,allcargpos,getrole());
plan_id=back->plan_id;
pilotcar_path.clear();
// pathheading=0;
}
/**
* @brief Formation::formationAttition
* 减员任务分配，当前车俩不为减员角色则正常行驶，否则进行减员任务分配
* @param attritionrole
*/
void Formation::formationAttition(int attritionrole){
reconstruction->onlyonceflag=true;
//本车role和减员车role不匹配
if (getrole()!=attritionrole){
//正常行驶，获取路径
generateRunningPath();
//减员情况下获取前车位置
getAttritionfrontCarPos(attritionrole);
updateAttritionPlanResult(AD_SPEED,attritionrole);
// updatePlanResult(AD_SPEED,pilotcarspeed);
}
//本车role和减员车role匹配
else{
//减员车辆编队未完成
if (!attrition->attritioncompleteflag){
double heading=(thiscarglobalpos.dr_heading+pilotcarpos.dr_heading)/2;
attrition->run(AD_POINT,plan_id,thiscarglobalpos,thiscarlocalpos,heading);
plan_id=attrition->plan_id;
// pathheading=0;
}
}
}
/**
* @brief Formation::formationReConstructin
* 重组任务分配，当前车俩不为重组角色则正常行驶，否则进行重组任务分配
* @param reconstructionGoal
*/
void Formation::formationReConstructin(int reconstructionGoal){
atrrition_frontPoint_num=0;
attrition_delaytimes=0;
attrition->parkpoint_onlyonceflag=true;
attrition->attritioncompleteflag=false;
if (getrole()!=4){
generateRunningPath();
getFrontCarPos();
updatePlanResult(AD_SPEED,pilotcarspeed);
}
else{
reconstruction->run(AD_SPEED,plan_id,pilotcarspeed,pilotcar_path,role3carpos,thiscarglobalpos,thiscarlocalpos);
plan_id=reconstruction->plan_id;
// pathheading=0;
}
}
此处可能存在不合适展示的内容，页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容，可点击提交进行申诉，我们将尽快为您处理。
马建仓 AI 助手