解决决策ＭＰＣ算法个点间隔时执行的ｂｕｇ

src/decision/decision.cpp

@@ -25,11 +25,11 @@ int Decision::Init(){
     LOG(INFO)<<Name()<<": init() start...";
     node_handle_.reset(new ros::NodeHandle());
     DecisionDatabase::InitDB();
-    iCommandCmd = CMD_UNDEFIED;// set undefied mode
-    iRulesTotalNumbers = ilocalTotalNumbers =fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
+    iCommandCmd = CMD_AUTODRIE_START;// set undefied mode
+    iAbandon = iRulesTotalNumbers = ilocalTotalNumbers =fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
     iPredictionPointsPerNumber = 5;
     uiCarMode =6; // undefied
-
+    fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
     PlanRouteName = "siasun";
 
     LOG(INFO)<<Name()<<" init() end.";
@@ -43,12 +43,12 @@ int Decision::Start(){
     ros::ServiceServer CommandService = node_handle_->advertiseService(robot::common::CommandModeCmdService,
                                                                 &Decision::CommandRequestService, this);
     // under ros subscriber
-    ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 2000,
+    ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 200,
                                                           &Decision::RTKLocationCallback, this);
 
-    ros::Subscriber OBDSub = node_handle_->subscribe(robot::common::ObdData, 2000, &Decision::OBDCallback, this);
+    ros::Subscriber OBDSub = node_handle_->subscribe(robot::common::ObdData, 200, &Decision::OBDCallback, this);
 
-    ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::Si2Data, 2000, &Decision::RCUModeCallback, this);
+    ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::Si2Data, 200, &Decision::RCUModeCallback, this);
 
     // under ros publish
     Rules_pub = node_handle_->advertise<decision::rulesMsg>(robot::common::RuleInfo, 200);
@@ -103,13 +103,19 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
             ros::spinOnce();
             loop_rate.sleep();
         }
+        ptsx.push_back(0);
+        ptsy.push_back(0);
+        ptsx.push_back(0);
+        ptsy.push_back(0);
+        fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
     }
     else if (req.Cmd == CMD_PLAN_START)
     {
         DecisionDatabase::DeleteActionDirective(PlanRouteName.c_str());
-        DecisionDatabase::DeleteLocationRuleInfo(PlanRouteName.c_str());       
+        DecisionDatabase::DeleteLocationRuleInfo(PlanRouteName.c_str());
+        iAbandon = fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
     }
-    fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
+
     res.CmdResult = iCommandCmd = req.Cmd;
     LOG(INFO)<<Name()<<"CommandRequestService end."<< res.CmdResult;
     return true;
@@ -182,7 +188,14 @@ void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
 //自动生成测试用例
 void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
 {
-    double secsStart =ros::Time::now().toSec();
+    if (iAbandon< 30){
+        iAbandon ++;
+        return;
+    }
+    else{
+        iAbandon = 0;
+    }
+
     try {
         iLocalNowNumber ++;
         LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;         
@@ -223,13 +236,12 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
            Geometry_pub.publish(localMsg);
        }
        fCarLastSpeed = fCarNowSpeed;
+
     }
     catch (ros::Exception ex)
     {
         LOG(ERROR)<<Name()<<":ExcutePlanRoute error."<< ex.what();
     }
-    double secsEnd =ros::Time::now().toSec();
-    ros::Duration(2 - secsEnd + secsStart).sleep();
 }
 
 // 自动生成测试用例停止
@@ -266,32 +278,24 @@ void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
 void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
 {
    // 判断当前RCU是否为自动驾驶模式
-//    if (uiCarMode != CTR_MD_REQ_AUTO_DRIVE)
-//    {
-//        LOG(INFO)<<Name()<<" ExcuteSelfDriving uiCarMode is not auto drive.";
-//        return;
-//    }
+    if (uiCarMode != CTR_MD_REQ_AUTO_DRIVE)
+    {
+        LOG(INFO)<<Name()<<" ExcuteSelfDriving uiCarMode is not auto drive.";
+        return;
+    }
     LOG(INFO)<<Name()<<" ilocalTotalNumbers:"<<ilocalTotalNumbers << "iLocalNowNumber:"<<iLocalNowNumber;
     if (CarAutoDriveStop())
         return;
-    int iPredictPoints;
-    if (ilocalTotalNumbers < (iLocalNowNumber + iPredictionPointsPerNumber)){
-        iPredictPoints = ilocalTotalNumbers - iLocalNowNumber;
-    }
-    else{
-        iPredictPoints = iPredictionPointsPerNumber;
-    }
-    if ((iLocalNowNumber > 0)&&((((msg->Longitude - ptsx[0])>0) && ((msg->Longitude - ptsx[1])< 0) ||
-            ((msg->Longitude - ptsx[0])<0) && ((msg->Longitude - ptsx[1])> 0))&&
-            (((msg->Latitude - ptsy[0])>0) && ((msg->Latitude - ptsy[1])< 0) ||
-                        ((msg->Latitude - ptsy[0])<0) && ((msg->Latitude - ptsy[1])> 0))))
-    {
-        LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route does not alter.";
-    }
-    else
-    {
+    int iPredictPoints = iPredictionPointsPerNumber;
+    double px = msg->Longitude;
+    double py = msg->Latitude;
+    bool bRetLon = (((ptsx[1] - ptsx[0])>= 0)&&((px - ptsx[1])> 0))||
+            (((ptsx[1] - ptsx[0])< 0)&&((px - ptsx[1])< 0));
+    bool bRetLat = (((ptsy[1] - ptsy[0])>= 0)&&((py - ptsy[1])> 0))||
+            (((ptsy[1] - ptsy[0])< 0)&&((py - ptsy[1])< 0));
+    if (bRetLon|| bRetLat)
+    {       
         std::vector<LOCATION_RULE_INFO> tempInfo;
-        LOG(INFO)<<Name()<<" ExcuteSelfDriving QueryLocationRuleInfo start.";
         tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
         ptsx.clear();
         ptsy.clear();
@@ -301,15 +305,18 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
            ptsy.push_back( tempInfo[i].LocationLat);
         }
         mpc.ref_speed = tempInfo[1].LocationSpeed;
+        iLocalNowNumber ++;
+        UpdataCarRuleResults();       
         LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route  altered.";
     }
-    double px = msg->Longitude;
-    double py = msg->Latitude;
+    else
+    {
+        LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route does not alter.";
+    }
     double psi = deg2rad(msg->AngleDeviated + 90);
     double v =  (double)fCarNowSpeed;
     MPC_Controller(ptsx, ptsy, px, py, psi, v);
-    UpdataCarRuleResults();
-    iLocalNowNumber ++;
+
 }
 
 void Decision::TestMPC()
@@ -362,18 +369,16 @@ bool  Decision::CarAutoDriveStop()
 {
     try{
 
-        if (ilocalTotalNumbers == iLocalNowNumber){
-            uiCarMode = CMD_AUTODRIE_END;
+        if (ilocalTotalNumbers == iLocalNowNumber + iPredictionPointsPerNumber){
+            iCommandCmd = CMD_AUTODRIE_END;
             double breakCtr = -5; // 踩刹车到50%
-            ros::Rate loop_rate(1);
-            while(ros::ok()){
+            iLocalNowNumber ++;
+            while(true){
                 if (SpeedControl(breakCtr) == 1)
                 {
                     if (BreakCtrControl(SFT_PSNG_LRN_REQ_PARK) == 1) // P挡
                         break;
                 }
-                ros::spinOnce();
-                loop_rate.sleep();
             }
             decision::rulesMsg ruleMsg;
             if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速（ <10km/h）：± 0.25km/h  高速（ >10km/h）：± 1km/h
@@ -435,9 +440,9 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
      Eigen::MatrixXd waypoints = transformGlobal2Vehicle(px, py, psi, ptsx, ptsy);
      Eigen::VectorXd Ptsx = waypoints.row(0);
      Eigen::VectorXd Ptsy = waypoints.row(1);
-     for (unsigned i=0; i < ptsx.size(); ++i) {
-         LOG(INFO)<<Name()<< " lon: "<< Ptsx(i)<< " lat: "<<Ptsy(i);
-     }
+//     for (unsigned i=0; i < ptsx.size(); ++i) {
+//         LOG(INFO)<<Name()<< " lon: "<< Ptsx(i)<< " lat: "<<Ptsy(i);
+//     }
      // 获取状态值
      auto coeffs = polyfit(Ptsx, Ptsy, 3);
      double cte = evaluateCte(coeffs);
@@ -448,18 +453,18 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
      // 这个地方有延迟，取第三个
      vector<vector<double>> result = mpc.Solve(state, coeffs);
      // 展示MPC预测的路径
-     vector<double> mpc_x_vals=result[0];
-     vector<double> mpc_y_vals=result[1];
-     std::vector<double>::iterator iter_start = mpc_x_vals.begin();
-     std::vector<double>::iterator iter_end = mpc_x_vals.end();
-     std::vector<double>::iterator iter;
-     int i = 0;
-     for ( iter = iter_start; iter != iter_end; iter++)
-     {
-       LOG(INFO)<<Name()<<" prediction LocationLon = "<<std::setprecision(15)<<mpc_x_vals[i]
-             <<"prediction LocationLat = "<< std::setprecision(14)<<mpc_y_vals[i];
-       i++;
-     }
+//     vector<double> mpc_x_vals=result[0];
+//     vector<double> mpc_y_vals=result[1];
+//     std::vector<double>::iterator iter_start = mpc_x_vals.begin();
+//     std::vector<double>::iterator iter_end = mpc_x_vals.end();
+//     std::vector<double>::iterator iter;
+//     int i = 0;
+//     for ( iter = iter_start; iter != iter_end; iter++)
+//     {
+//       LOG(INFO)<<Name()<<" prediction LocationLon = "<<std::setprecision(15)<<mpc_x_vals[i]
+//             <<"prediction LocationLat = "<< std::setprecision(14)<<mpc_y_vals[i];
+//       i++;
+//     }
      double steer_value = result[2][mpc.latency];
      double throttle_value = result[3][mpc.latency];
      mpc.delta_prev = steer_value;

src/decision/decision.h

@@ -166,6 +166,8 @@ private:
 
 private:
 
+    int iAbandon;
+
     std::string PlanRouteName;
 
     int32_t iCommandCmd; // excute mode 0: undefined 1: plan start 2: auto-drive start  3: plan stop 4: auto-drive stop

src/decision/decisiondatabase.cpp

@@ -6,7 +6,7 @@ namespace decisionSpace {
 
 std::string DecisionDatabase::Name() { return "Database"; }
 
-std::string to_string_with_precision(double a_value,int n){
+std::string to_string_with_precision(double a_value, int n){
     std::ostringstream out;
     out<<std::setprecision(n)<<a_value;
     return out.str();
@@ -218,10 +218,10 @@ int DecisionDatabase::InsertLocationRuleInfo(const char *ruleName,
     /********************向表中插入一条记录 start *****************************/
     std::string sRuleName=ruleName;
     std::string sLocationID=std::to_string(iLocationID);
-    std::string sLocationLon=to_string_with_precision(dLocationLon,15);
-    std::string sLocationLat=to_string_with_precision(dLocationLat,14);
-    std::string sLocationHeight=to_string_with_precision(dLocationHeight,5);
-    std::string sLocationSpeed=to_string_with_precision(fLocationSpeed, 5);
+    std::string sLocationLon=to_string_with_precision(dLocationLon,12);
+    std::string sLocationLat=to_string_with_precision(dLocationLat,11);
+    std::string sLocationHeight=to_string_with_precision(dLocationHeight,4);
+    std::string sLocationSpeed=to_string_with_precision(fLocationSpeed, 4);
     std::string s_sql="INSERT INTO location_rule_info ( \
             rule_name,                                  \
             location_id,                                \
@@ -273,10 +273,10 @@ int DecisionDatabase::InsertLocationRuleInfoAndActionDirective(const char* ruleN
         /********************向表中插入一条记录 start *****************************/
         std::string sRuleName=ruleName;
         std::string sLocationID=std::to_string(iLocationID);
-        std::string sLocationLon=to_string_with_precision(dLocationLon,15);
-        std::string sLocationLat=to_string_with_precision(dLocationLat,14);
-        std::string sLocationHeight=to_string_with_precision(dLocationHeight,5);
-        std::string sLocationSpeed=to_string_with_precision(fLocationSpeed, 5);
+        std::string sLocationLon=to_string_with_precision(dLocationLon,12);
+        std::string sLocationLat=to_string_with_precision(dLocationLat,11);
+        std::string sLocationHeight=to_string_with_precision(dLocationHeight,4);
+        std::string sLocationSpeed=to_string_with_precision(fLocationSpeed, 4);
         std::string s_sqlsqlLocation="INSERT INTO location_rule_info ( \
                 rule_name,                                  \
                 location_id,                                \

src/localization/localization.cpp

@@ -216,10 +216,10 @@ int Localization::PublishData(NMEA0183 &nmea)
 //        std::string zone;
 //        CoordsConvertion::LLtoUTM(nmea.gprmc.Latitude, nmea.gprmc.Longitude, northing, easting, zone);
         localization::localMsg localization;
-        localization.Longitude =  to_string_with_precision(nmea.gprmc.Longitude, 15);
-        localization.Latitude = to_string_with_precision(nmea.gprmc.Latitude, 15);
-        localization.Height = to_string_with_precision(nmea.gpgga.Height, 5);
-        localization.AngleDeviated = to_string_with_precision(nmea.gprmc.Deviated, 6);
+        localization.Longitude =  to_string_with_precision(nmea.gprmc.Longitude, 12);
+        localization.Latitude = to_string_with_precision(nmea.gprmc.Latitude, 11);
+        localization.Height = to_string_with_precision(nmea.gpgga.Height, 4);
+        localization.AngleDeviated = to_string_with_precision(nmea.gprmc.Deviated, 4);
         geometry_pub.publish(localization);
     }
     catch (ros::Exception ex)