|
@@ -25,13 +25,14 @@ int Decision::Init(){
|
|
|
LOG(INFO)<<Name()<<": init() start...";
|
|
|
node_handle_.reset(new ros::NodeHandle());
|
|
|
DecisionDatabase::InitDB();
|
|
|
- iCommandCmd = CMD_AUTODRIE_START;// set undefied mode
|
|
|
+ iCommandCmd = CMD_UNDEFIED;// set undefied mode
|
|
|
iAbandon = iRulesTotalNumbers = ilocalTotalNumbers =fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
|
iPredictionPointsPerNumber = 5;
|
|
|
uiCarMode =6; // undefied
|
|
|
fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
|
PlanRouteName = "siasun";
|
|
|
-
|
|
|
+// for (int i =0; i< 10; i++)
|
|
|
+// TestMPC();
|
|
|
LOG(INFO)<<Name()<<" init() end.";
|
|
|
return ROBOT_SUCCESS;
|
|
|
|
|
@@ -85,7 +86,7 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
|
|
|
if (req.Cmd == CMD_AUTODRIE_START)
|
|
|
{
|
|
|
LoadPlanRoute();
|
|
|
- double breakCtr = -5; // 踩刹车到50%
|
|
|
+ double breakCtr = -50; // 踩刹车到50%
|
|
|
ros::Rate loop_rate(1);
|
|
|
while(ros::ok())
|
|
|
{
|
|
@@ -321,47 +322,19 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
|
|
|
|
void Decision::TestMPC()
|
|
|
{
|
|
|
- if (CarAutoDriveStop())
|
|
|
- return;
|
|
|
- int iPredictPoints;
|
|
|
- if (ilocalTotalNumbers < (iLocalNowNumber + iPredictionPointsPerNumber)){
|
|
|
- iPredictPoints = ilocalTotalNumbers - iLocalNowNumber;
|
|
|
- }
|
|
|
- else{
|
|
|
- iPredictPoints = iPredictionPointsPerNumber;
|
|
|
- }
|
|
|
- if ((iLocalNowNumber > 0)&&((((vPx[iLocalNowNumber] - ptsx[0])>0) && ((vPx[iLocalNowNumber] - ptsx[1])< 0) ||
|
|
|
- ((vPx[iLocalNowNumber] - ptsx[0])<0) && ((vPx[iLocalNowNumber] - ptsx[1])> 0))&&
|
|
|
- (((vPy[iLocalNowNumber] - ptsy[0])>0) && ((vPy[iLocalNowNumber] - ptsy[1])< 0) ||
|
|
|
- ((vPy[iLocalNowNumber] - ptsy[0])<0) && ((vPy[iLocalNowNumber]- ptsy[1])> 0))))
|
|
|
+ std::vector<LOCATION_RULE_INFO> tempInfo;
|
|
|
+ tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, 5);
|
|
|
+ ptsx.clear();
|
|
|
+ ptsy.clear();
|
|
|
+ for (int i= 0; i< 5; i++)
|
|
|
{
|
|
|
- LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route does not alter.";
|
|
|
+ ptsx.push_back( tempInfo[i].LocationLon);
|
|
|
+ ptsy.push_back( tempInfo[i].LocationLat);
|
|
|
}
|
|
|
- else
|
|
|
- {
|
|
|
- std::vector<LOCATION_RULE_INFO> tempInfo;
|
|
|
- LOG(INFO)<<Name()<<" ExcuteSelfDriving QueryLocationRuleInfo start.";
|
|
|
- tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
|
|
|
- ptsx.clear();
|
|
|
- ptsy.clear();
|
|
|
- for (int i= 0; i< iPredictPoints; i++)
|
|
|
- {
|
|
|
- ptsx.push_back( tempInfo[i].LocationLon);
|
|
|
- ptsy.push_back( tempInfo[i].LocationLat);
|
|
|
- }
|
|
|
- mpc.ref_speed = tempInfo[1].LocationSpeed;
|
|
|
- LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route altered.";
|
|
|
- }
|
|
|
-
|
|
|
- double px = vPx[iLocalNowNumber];
|
|
|
- double py = vPy[iLocalNowNumber];
|
|
|
- double psi = deg2rad(40.45 + 90);
|
|
|
+ mpc.ref_speed = tempInfo[1].LocationSpeed;
|
|
|
+ double psi = deg2rad(230 + 90);
|
|
|
double v = (double)fCarNowSpeed;
|
|
|
- LOG(INFO)<<Name()<< " MPC start";
|
|
|
- MPC_Controller(ptsx, ptsy, px, py, psi, v);
|
|
|
- LOG(INFO)<<Name()<< " MPC end";
|
|
|
- //UpdataCarRuleResults();
|
|
|
- iLocalNowNumber ++;
|
|
|
+ MPC_Controller(ptsx, ptsy, ptsx[1], ptsy[1], psi, v);
|
|
|
}
|
|
|
|
|
|
// 自动驾驶停止
|
|
@@ -371,7 +344,7 @@ bool Decision::CarAutoDriveStop()
|
|
|
|
|
|
if (ilocalTotalNumbers == iLocalNowNumber + iPredictionPointsPerNumber){
|
|
|
iCommandCmd = CMD_AUTODRIE_END;
|
|
|
- double breakCtr = -5; // 踩刹车到50%
|
|
|
+ double breakCtr = -50; // 踩刹车到50%
|
|
|
iLocalNowNumber ++;
|
|
|
while(true){
|
|
|
if (SpeedControl(breakCtr) == 1)
|
|
@@ -447,6 +420,7 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
|
|
|
auto coeffs = polyfit(Ptsx, Ptsy, 3);
|
|
|
double cte = evaluateCte(coeffs);
|
|
|
double epsi = evaluateEpsi(coeffs);
|
|
|
+ LOG(INFO)<<Name()<< " cte: "<< cte<< " epsi: "<<epsi;
|
|
|
Eigen::VectorXd state(6);
|
|
|
state << 0, 0, 0, v, cte, epsi;
|
|
|
// 计算方向和油门,范围都在[-1, 1]之间
|
|
@@ -468,11 +442,9 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
|
|
|
double Lf = 2.67;
|
|
|
double steer_value = (vars[0]*1300/25/(deg2rad(25)*Lf));
|
|
|
double throttle_value = vars[1]*100;
|
|
|
- mpc.delta_prev = vars[0];
|
|
|
- mpc.a_prev = throttle_value;
|
|
|
LOG(INFO)<<Name()<< " stree alter value:"<<steer_value<<" throttle alter value:"<<throttle_value;
|
|
|
- SpeedControl(steer_value);
|
|
|
- StreeControl(throttle_value);
|
|
|
+ StreeControl(steer_value);
|
|
|
+ SpeedControl(throttle_value);
|
|
|
decision::controlMsg control;
|
|
|
control.nowSpeed = fCarNowSpeed;
|
|
|
control.refSpeed = mpc.ref_speed;
|
|
@@ -491,7 +463,7 @@ unsigned int Decision::SpeedControl(double &speed)
|
|
|
}
|
|
|
else{
|
|
|
srv.request.uiAccPsngCtrReq = 0;
|
|
|
- srv.request.uiBrkPsngCtrReq =(unsigned int)(speed *10);
|
|
|
+ srv.request.uiBrkPsngCtrReq =(unsigned int)(-speed *10);
|
|
|
}
|
|
|
srv.request.iAccSpdCtrReq = 20;
|
|
|
srv.request.iBrkSpdCtrReq = 50;
|