修改自动驾驶刹车命令ｂｕｇ

src/canbus/canbus.cpp

@@ -282,7 +282,7 @@ int Canbus::read_data(TPCANMsg *pMsgBuff)
     {
         T_OBD_VEH_SPD tOBDVehSpd = *((T_OBD_VEH_SPD*)MessageCoder::CanMsgAnalyse(robot::common::ObdSpeed,pMsgBuff->DATA));
         status2.uiVehSpd = tOBDVehSpd.obdVehSpd;
-        obd.VehSpd = status2.uiVehSpd;
+        obd.VehSpd = tOBDVehSpd.obdVehSpd;
     }
     else if((pMsgBuff->ID >= robot::common::PowerOnReq && pMsgBuff->ID <= robot::common::BrakeForceCtrStp) ||
             (pMsgBuff->ID >= robot::common::SysMonitorBack && pMsgBuff->ID <= robot::common::SteerPosTimeReq) ||

src/canbus/candatatype.h

@@ -2173,22 +2173,39 @@ typedef struct
 /************OBD实际车辆速度*******/
 typedef struct
 {
+     unsigned int obdVehSpdH : 8;		//实际车辆速度
+     unsigned int obdVehSpdL : 8;		//实际车辆速度
+     unsigned int : 16;
+     unsigned int : 32;
+//       unsigned int : 32;
+//       unsigned int : 16;
+//       unsigned int obdVehSpdH : 8;		//实际车辆速度
+//       unsigned int obdVehSpdL : 8;		//实际车辆速度
+}T_OBD_3E9;
 
-//    unsigned int obdVehSpd : 16;		//实际车辆速度
-//     unsigned int : 16;
-//     unsigned int : 32;
-       unsigned int : 32;
-       unsigned int : 16;
-       unsigned int obdVehSpd : 16;		//实际车辆速度
-
+typedef struct
+{
+    unsigned obdVehSpdL : 8;		//实际车辆速度
+    unsigned obdVehSpdH : 8;		//实际车辆速度
+    unsigned int : 16;
+}T_OBD_3E9_SRC;
 
+typedef struct
+{
+    unsigned obdVehSpd : 16;		//实际车辆速度
+    unsigned int : 16;
+}T_OBD_3E9_DEST;
 
-}T_OBD_3E9;
+typedef union
+{
+    T_OBD_3E9_SRC tobd3e9Src;		//实际车辆速度
+    T_OBD_3E9_DEST  tobd3e9Dest;
+}T_OBD_3E9_UNION;
 
 //实际车辆速度
 typedef struct
 {
-    unsigned int obdVehSpd;			//实际车辆速度
+    float obdVehSpd;			//实际车辆速度
 }T_OBD_VEH_SPD;
 
 #define OBD_VEH_SPD_RATIO		64	//实际车辆速度比例

src/canbus/messagecoder.cpp

@@ -755,13 +755,26 @@ void MessageCoder::OBDAnalyseID_0C9(unsigned char *ucData, T_OBD_ENG_SPD *tOBDEn
 //OBD实际车辆速度--数据解析
 void MessageCoder::OBDAnalyseID_3E9(unsigned char *ucData, T_OBD_VEH_SPD *tOBDVehSpd)
 {
+//    T_OBD_3E9 tOBD3E9 = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
+
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+//    memcpy(&tOBD3E9, ucCanMsgSrc, CAN_MSG_LEN);
+
+//    tOBDVehSpd->obdVehSpd = tOBD3E9.obdVehSpd / OBD_VEH_SPD_RATIO;
+
     T_OBD_3E9 tOBD3E9 = { 0 };
     unsigned char ucCanMsgSrc[8] = { 0 };
 
     memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
     memcpy(&tOBD3E9, ucCanMsgSrc, CAN_MSG_LEN);
 
-    tOBDVehSpd->obdVehSpd = tOBD3E9.obdVehSpd / OBD_VEH_SPD_RATIO;
+    T_OBD_3E9_UNION tObd3e9Union;
+    memset(&tObd3e9Union,0,sizeof(T_OBD_3E9_UNION));
+    tObd3e9Union.tobd3e9Src.obdVehSpdH = tOBD3E9.obdVehSpdH;
+    tObd3e9Union.tobd3e9Src.obdVehSpdL = tOBD3E9.obdVehSpdL;
+
+    tOBDVehSpd->obdVehSpd = (float)tObd3e9Union.tobd3e9Dest.obdVehSpd / (float)OBD_VEH_SPD_RATIO;
 }
 
 //控制器上电命令--数据组包

src/common/mpc.cpp

@@ -158,12 +158,16 @@ vector<double> MPC::Solve(Eigen::VectorXd x0, Eigen::VectorXd coeffs) {
        vars_upperbound[i] = 1.0e19;
      }
    for (int i = delta_start; i < a_start; i++) {
-     vars_lowerbound[i] = (delta_prev < -0.32) ? -0.436332 : delta_prev - 0.1;
-     vars_upperbound[i] = (delta_prev > 0.32) ? 0.436332 : delta_prev + 0.1;
+       vars_lowerbound[i] = -0.436332;
+       vars_upperbound[i] = 0.436332;
+//     vars_lowerbound[i] = (delta_prev < -0.32) ? -0.436332 : delta_prev - 0.1;
+//     vars_upperbound[i] = (delta_prev > 0.32) ? 0.436332 : delta_prev + 0.1;
    }
    for (int i = a_start; i < n_vars; i++) {
-     vars_lowerbound[i] = (a_prev < -0.31) ? -0.36 : a_prev - 0.05;; // -0.36
-     vars_upperbound[i] = (a_prev > 0.31) ? 0.36 : a_prev + 0.05;;  // 0.36
+       vars_lowerbound[i] = -0.5; // -0.36
+       vars_upperbound[i] = 0.5;
+//     vars_lowerbound[i] = (a_prev < -0.31) ? -0.36 : a_prev - 0.05;; // -0.36
+//     vars_upperbound[i] = (a_prev > 0.31) ? 0.36 : a_prev + 0.05;;  // 0.36
    }
    // 设置约束条件的的最大和最小值，除了初始状态其他约束都为0
    Dvector constraints_lowerbound(n_constraints);
@@ -207,7 +211,7 @@ vector<double> MPC::Solve(Eigen::VectorXd x0, Eigen::VectorXd coeffs) {
     bool ok = true;
     ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
 
-    auto cost = solution.obj_value;
+    //auto cost = solution.obj_value;
     //std::cout << "Cost " << cost << std::endl;
 
     vector<double> result;

src/decision/decision.cpp

@@ -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;

src/decision/decision.h

@@ -208,6 +208,9 @@ private:
     std::vector<double> vPx;
     std::vector<double> vPy;
 
+    double throttle_Last = 0;
+    double steer_Last = 0;
+
 };
 
 

src/localization/localization.cpp

@@ -21,7 +21,7 @@ std::string Localization::Name() const { return "localization"; }
 int Localization::Init() {
     try
     {
-        LOG(INFO)<<Name()<<" init start ...";       
+        LOG(INFO)<<Name()<<" init start ...";
         serial_port = "/dev/ttyUSB0";
         baudrate = 115200;
         timeout = 1000;

src/perception/perception.cpp

@@ -77,65 +77,43 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
     CharReverse(ucCanMsgSrc,8);
     switch (msg->id) {
     case robot::common::ActuatorEnableStatus:
-    {
         PublishState_6E5(ucCanMsgSrc);
-    }
         break;
     case robot::common::RSysInfoBack2:
-    {
         PublishState_6ED(ucCanMsgSrc);
-    }
         break;
     case robot::common::SysLearnBack:
-    {
         PublishState_6EE(ucCanMsgSrc);
-    }
         break;
     case robot::common::SysWarnErr:
-    {
         //SoftStopControl();
         PublishState_6D1(ucCanMsgSrc);
-    }
         break;
     case robot::common::AccErr:
-    {
         //SoftStopControl();
         PublishState_6D2_6(ucCanMsgSrc, 2, bus_pub_6D2);
-    }
         break;
     case robot::common::BrkErr:
-    {
         //SoftStopControl();
         PublishState_6D2_6(ucCanMsgSrc, 3, bus_pub_6D3);
-    }
         break;
     case robot::common::XsftErr:
-    {
         //SoftStopControl();
         PublishState_6D2_6(ucCanMsgSrc, 4, bus_pub_6D4);
-    }
         break;
     case robot::common::YsftErr:
-    {
         //SoftStopControl();
         PublishState_6D2_6(ucCanMsgSrc, 5, bus_pub_6D5);
-    }
         break;
     case robot::common::StrErr:
-    {
         //SoftStopControl();
         PublishState_6D2_6(ucCanMsgSrc, 6, bus_pub_6D6);
-    }
         break;
     case robot::common::EstopInfo:
-    {
         PublishState_2E0(ucCanMsgSrc);
-    }
         break;
     case robot::common::EstopErr:
-    {
         PublishState_2E1(ucCanMsgSrc);
-    }
         break;
     default:
         break;