代码拉取完成，页面将自动刷新
#include "Back.h"
void Back::Reset(){
backflag=false;
getfrontcarposflag=false;
trajectoryflag=false;
storeflag=true;
waypointnum=0;
}
/**
* @brief Back::getLocalPosPath
* 每隔90cm记录倒车路径，倒车路径共350个路径点
* @param localpos
*/
void Back::getLocalPosPath(VehicleState localpos){
thiscarlocalpos=localpos;
if (waypointnum==0){
waypoint.push_front(localpos);
ROS_INFO("THIS CAR BACKPOINT UPDATE!!!");
lastwaypoint=localpos;
waypointnum++;
}
else{
if (((pow((lastwaypoint.dr_x-localpos.dr_x),2)+pow((lastwaypoint.dr_y-localpos.dr_y),2))>=8000)&&((pow((lastwaypoint.dr_x-localpos.dr_x),2)+pow((lastwaypoint.dr_y-localpos.dr_y),2))<=800000)){
if (waypoint.size()>350){
waypoint.pop_back();
waypoint.push_front(localpos);
}
else{
waypoint.push_front(localpos);
}
// cout<<waypoint.size()<<endl;
ROS_INFO("THIS CAR BACKPOINT UPDATE!!!");
lastwaypoint=localpos;
waypointnum++;
}
}
}
/**
* @brief Back::getAllCarGpos
* 更新所有角色车辆全局信息，以及本车局部信息
* @param allcargpos
*/
void Back::getAllCarGpos(vector<VehicleState> &allcargpos){
thiscarglobalpos=allcargpos[0];
pilotglobalpos=allcargpos[1];
role2globalpos=allcargpos[2];
role3globalpos=allcargpos[3];
role4globalpos=allcargpos[4];
thiscarlpos=allcargpos[5];
}
/**
* @brief Back::storeBackPathData
* 存储当前时刻倒车路径，只有从正常行驶切换到倒车才会执行，也就是正常来说只执行一次
* 标志位storeflag存储后即置为false，只有当车辆运行时才会再次被置为true
*/
void Back::storeBackPathData(){
if (storeflag){
backway=waypoint;
backendpoint=waypoint[waypoint.size()-1];
storeflag=false;
}
}
/**
* @brief Back::generataBackPath
* 更新倒车路径backpath
*/
void Back::generataBackPath(){
int startnum=0;
int endnum=0;
if (backway.empty()){
backflag=false;
ROS_WARN("WARNING:BACK WAY IS EMPTY!!!");
}
else{
//计算距离当前位置的最近路径点序号
startnum=HMath::getNearestPathPointByIncrease(backway,thiscarlocalpos);
//计算距离倒车末端的最近路径点序号
endnum=HMath::getNearestPathPointByIncrease(backway,backendpoint);
if (endnum>=startnum){
for(int i=startnum;i<=endnum;i++){
backpath.push_back(backway[i]);
}
backflag=true;
ROS_INFO("GET BACK PATH TO GO!!!");
}
else{
backflag=false;
ROS_WARN("WARNING:GET BACK PATH FAILED!!!");
}
}
}
/**
* @brief Back::updataBackPath
* 根据backpath更新规划结果plan_result，具体包括倒车路径的坐标转换和边界确定
*/
void Back::updataBackPath(){
if (backpath.size()>100){ //if pathpoints num larger than waypoints 100
// multiagent::PATH_DATA _PROPERTY path_boundary;
geometry_msgs::Pose2D left_boundary;
geometry_msgs::Pose2D right_boundary;
for(int i=0;i<100;i++){ //globalpos to thiscar coordinaate
double dx = (backpath[i].dr_x - thiscarlocalpos.dr_x);
double dy = (backpath[i].dr_y - thiscarlocalpos.dr_y);
plan_result.center_line[i].x = dx * cos(thiscarlocalpos.dr_heading *0.01 * degree2rad) + dy * sin(thiscarlocalpos.dr_heading *0.01 * degree2rad);
plan_result.center_line[i].y =( -dx * sin(thiscarlocalpos.dr_heading *0.01 * degree2rad) + dy * cos(thiscarlocalpos.dr_heading *0.01 * degree2rad));
}
for(int i=0;i<100;i++){ //fill path bounary information
double B_dx = backpath[i+1].dr_x - backpath[i].dr_x;
double B_dy = backpath[i+1].dr_y - backpath[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;
}
else{
geometry_msgs::Pose2D left_boundary;
geometry_msgs::Pose2D right_boundary;
for(int i=0;i<backpath.size();i++){ //globalpos 2 thiscar coordinaate
double dx = (backpath[i].dr_x - thiscarlocalpos.dr_x);
double dy = (backpath[i].dr_y - thiscarlocalpos.dr_y);
plan_result.center_line[i].x = dx * cos(thiscarlocalpos.dr_heading *0.01 * degree2rad) + dy * sin(thiscarlocalpos.dr_heading *0.01 * degree2rad);//wo suan de shi cos*dx+sin*dy
plan_result.center_line[i].y = (-dx * sin(thiscarlocalpos.dr_heading *0.01 * degree2rad) + dy * cos(thiscarlocalpos.dr_heading *0.01 * degree2rad));//-sin * dx + cos*dy
}
for(int i=0;i<backpath.size()-1;i++){ //fill path bounary information
double B_dx = backpath[i+1].dr_x - backpath[i].dr_x;
double B_dy = backpath[i+1].dr_y - backpath[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[backpath.size()-1] = left_boundary;
plan_result.right_boundary[backpath.size()-1] = right_boundary;
plan_result.effective_pointNum=backpath.size();
}
}
void Back::getTrajectory(){
if (backpath.size()!=0){
if (backpath.size()<=100){
for(int i=0;i<backpath.size();i++){ //globalpos 2 thiscar coordinaate
backtrajectory.center_line[i].x = backpath[i].dr_x ;
backtrajectory.center_line[i].y = backpath[i].dr_y ;
}
backtrajectory.effective_pointNum=backpath.size();
trajectoryflag=true;
}
else{
for(int i=0;i<=100;i++){ //globalpos 2 thiscar coordinaate
backtrajectory.center_line[i].x = backpath[i].dr_x ;
backtrajectory.center_line[i].y = backpath[i].dr_y ;
}
backtrajectory.effective_pointNum=100;
trajectoryflag=true;
}
}
}
/**
* @brief Back::getFrontCarPos
* 根据当前车辆角色获取目标点位置位置，并转换到局部坐标系下
*/
void Back::getFrontCarPos(){
double dx=0;
double dy=0;
VehicleState frontcar;
if (role==1){
frontcar=role2globalpos;
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);
}
else if (role==2){
frontcar=role3globalpos;
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);
}
else if (role==3) {
frontcar=role4globalpos;
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);
}
else if (role==4) {
frontcarlocalpos.dr_x=-1;
frontcarlocalpos.dr_y=-1;
}
getfrontcarposflag=true;
}
/**
* @brief Back::updataBackPlanResult
* 倒车编队结果封装
* @param nVehicleCommond
* @param nSpeed
* @param nPlanId
*/
void Back::updataBackPlanResult(int nVehicleCommond,int nSpeed,int nPlanId){
//TODO:对接倒车编队结果封装
plan_result.local_time = ros::Time::now().toNSec();
plan_result.message_num = nPlanId;
plan_result.coorType=1;
plan_result.freezeCoor.x = thiscarlpos.dr_x;//localpos
plan_result.freezeCoor.y = thiscarlpos.dr_y;
// plan_result.plan_frame.g_x = thiscarlpos.dr_x;
// plan_result.plan_frame.g_y = thiscarlpos.dr_y;
plan_result.freezeCoor.theta = thiscarlpos.dr_heading;
// plan_result.plan_frame.pitch = thiscarlpos.dr_pitch;
// plan_result.plan_frame.roll = thiscarlpos.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;
plan_result.exp_speed = nSpeed;//3.6km/h
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<100;i++) {
// plan_result.path_property[i].direction = thiscarlpos.dr_heading ;
// }
nPlanId ++;
plan_id=nPlanId;
}
/**
* @brief Back::backRun
* 倒车运行逻辑，包括存储当前时刻倒车路径storeBackPathData(),获取所有车辆信息getAllCarGpos()，生成当前位置到倒车末端间的路径generataBackPath()
* @param nVehicleCommond
* @param nSpeed
* @param nPlanId
* @param allcargpos
* @param role
*/
void Back::backRun(int nVehicleCommond, int nSpeed, int nPlanId, vector<VehicleState> &allcargpos,int role){
this->role=role;
storeBackPathData();
getAllCarGpos(allcargpos);
generataBackPath();
//倒车路径生成成功
if (backflag){
updataBackPath();
getTrajectory();
backpath.clear();
getFrontCarPos();
updataBackPlanResult(nVehicleCommond,nSpeed,nPlanId);
}
}
此处可能存在不合适展示的内容，页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容，可点击提交进行申诉，我们将尽快为您处理。
马建仓 AI 助手