src/decision/decision.cpp

@@ -31,34 +31,6 @@ int Decision::Init(){
     uiCarMode =6; // undefied
 
     PlanRouteName = "siasun";
-    ilocalTotalNumbers = 100;
-    fCarNowSpeed = 0;
-    vPx.push_back(127.162345678);
-    vPy.push_back(41.843340742);
-    vPx.push_back(127.162345678);
-    vPy.push_back(41.843340742);
-    vPx.push_back(127.162345678);
-    vPy.push_back(41.843340742);
-    vPx.push_back(127.162345678);
-    vPy.push_back(41.843340742);
-    vPx.push_back(127.162345678);
-    vPy.push_back(41.843340742);
-    vPx.push_back(127.162345678);
-    vPy.push_back(41.843340742);
-    vPx.push_back(127.160345678);
-    vPy.push_back(41.843340742);
-    vPx.push_back(127.154345678);
-    vPy.push_back(41.851940742);
-    vPx.push_back(127.154345678);
-    vPy.push_back(41.851940742);
-    vPx.push_back(127.154345678);
-    vPy.push_back(41.851940742);
-    vPx.push_back(127.154345678);
-    vPy.push_back(41.851940742);
-    for (int i =0; i< 11; i++)
-    {
-       TestMPC();
-    }
 
     LOG(INFO)<<Name()<<" init() end.";
     return ROBOT_SUCCESS;
@@ -101,7 +73,7 @@ int Decision::Start(){
 
 void Decision::Stop(){
     LOG(INFO)<<Name()<<": stop start...";
-      LOG(INFO)<<Name()<<": stop end.";
+    LOG(INFO)<<Name()<<": stop end.";
     return ;
 }
 
@@ -131,13 +103,13 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
             ros::spinOnce();
             loop_rate.sleep();
         }
-        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());       
     }
+    fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
     res.CmdResult = iCommandCmd = req.Cmd;
     LOG(INFO)<<Name()<<"CommandRequestService end."<< res.CmdResult;
     return true;
@@ -176,7 +148,6 @@ void Decision::LoadPlanRoute()
 // 接收ＲＴＫ数据分为自动驾驶模式开始和结束和路径规划模式开始和结束
 void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
 {
-//     double secsStart =ros::Time::now().toSec();
      switch (iCommandCmd) {
      case CMD_PLAN_START:
          ExcutePlanRoute(msg);
@@ -193,8 +164,6 @@ void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
      default:
          break;
      }
-//    double secsEnd =ros::Time::now().toSec();
-//    ros::Duration(2 - secsEnd + secsStart).sleep();
 }
 
 
@@ -216,40 +185,44 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
     double secsStart =ros::Time::now().toSec();
     try {
         iLocalNowNumber ++;
-        LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;
-        if ((iLocalNowNumber % iPredictionPointsPerNumber)  == 0) {          
-            iRulesNowNumber++;
-            std::stringstream strRuleContent;
-            if (iRulesNowNumber == 1)
-                strRuleContent<<"动作"<<iRulesNowNumber<<":起步请加速到"<< fCarNowSpeed<<"km/h";
-            else{
-                if (fCarNowSpeed > fCarLastSpeed)
-                {
-                    strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
-                }
-                else if (fCarNowSpeed < fCarLastSpeed)
-                {
-                    strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
-                }
-                else if (fCarNowSpeed == fCarLastSpeed)
-                {
-                    strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
-                }
-           }
-
-           int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
-                            strRuleContent.str().c_str(), RULE_EXCUTE_RESULT_INIT, iLocalNowNumber, msg->Longitude,
-                            msg->Latitude, msg->Height, fCarNowSpeed);
-           if (iRet < 0) {
-                iLocalNowNumber--;
-                iRulesNowNumber--;
-           }
-           fCarLastSpeed = fCarNowSpeed;
-        }
+        LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;         
+        iRulesNowNumber++;
+        std::stringstream strRuleContent;
+        if (iRulesNowNumber == 1)
+            strRuleContent<<"动作"<<iRulesNowNumber<<":起步请加速到"<< fCarNowSpeed<<"km/h";
         else{
-           DecisionDatabase::InsertLocationRuleInfo(PlanRouteName.c_str(),iLocalNowNumber, msg->Longitude,
-                                             msg->Latitude, msg->Height, fCarNowSpeed);
-        }
+            if (fCarNowSpeed > fCarLastSpeed)
+            {
+                strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
+            }
+            else if (fCarNowSpeed < fCarLastSpeed)
+            {
+                strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
+            }
+            else if (fCarNowSpeed == fCarLastSpeed)
+            {
+                strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
+            }
+       }
+       int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
+                        strRuleContent.str().c_str(), RULE_EXCUTE_RESULT_INIT, iLocalNowNumber, msg->Longitude,
+                        msg->Latitude, msg->Height, fCarNowSpeed);
+       if (iRet < 0) {
+            iLocalNowNumber--;
+            iRulesNowNumber--;
+       }
+       else
+       {
+           decision::localizationMsg localMsg;
+           localMsg.Locationid = iLocalNowNumber;
+           localMsg.Longitude = msg->Longitude;
+           localMsg.Latitude = msg->Latitude;
+           localMsg.Height = msg->Height;
+           localMsg.LocationSpeed = fCarNowSpeed;
+           localMsg.AngleDeviated = msg->AngleDeviated;
+           Geometry_pub.publish(localMsg);
+       }
+       fCarLastSpeed = fCarNowSpeed;
     }
     catch (ros::Exception ex)
     {
@@ -264,36 +237,29 @@ void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
 {
      LOG(INFO)<<Name()<<"StopPlanRoute last record iLocalNowNumber = "<< iLocalNowNumber;
      iCommandCmd = CMD_UNDEFIED;// set undefied mode
-     if ((iLocalNowNumber % iPredictionPointsPerNumber)  == 0){
-          LOG(INFO)<<Name()<<"StopPlanRoute last record iLocalNowNumber = "<< iLocalNowNumber
-                  << " iLocalNowNumber % iPredictionPointsPerNumber  = 0";
-         return;
+     iLocalNowNumber++;
+     iRulesNowNumber++;
+     std::stringstream strRuleContent;
+     if (fCarNowSpeed > fCarLastSpeed)
+     {
+         strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
      }
-     else{
-         iLocalNowNumber++;
-         iRulesNowNumber++;
-         std::stringstream strRuleContent;
-         if (fCarNowSpeed > fCarLastSpeed)
-         {
-             strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
-         }
-         else if (fCarNowSpeed < fCarLastSpeed)
-         {
-             strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
-         }
-         else if (fCarNowSpeed == fCarLastSpeed)
-         {
-             strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
-         }
-        int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
-                         strRuleContent.str().c_str(), 0, iLocalNowNumber, msg->Longitude, msg->Latitude,
-                         msg->Height, fCarNowSpeed);
-        if (iRet < 0) {
-             LOG(ERROR)<<Name()<<"StopPlanRoute insert iLocalNowNumber = "<< iLocalNowNumber<< "  last record error";
-             iLocalNowNumber--;
-             iRulesNowNumber--;
-        }
+     else if (fCarNowSpeed < fCarLastSpeed)
+     {
+         strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
      }
+     else if (fCarNowSpeed == fCarLastSpeed)
+     {
+         strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
+     }
+    int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
+                     strRuleContent.str().c_str(), 0, iLocalNowNumber, msg->Longitude, msg->Latitude,
+                     msg->Height, fCarNowSpeed);
+    if (iRet < 0) {
+         LOG(ERROR)<<Name()<<"StopPlanRoute insert iLocalNowNumber = "<< iLocalNowNumber<< "  last record error";
+         iLocalNowNumber--;
+         iRulesNowNumber--;
+    }
 }
 
 // 执行自动驾驶ＭＰＣ算法
@@ -305,7 +271,7 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
 //        LOG(INFO)<<Name()<<" ExcuteSelfDriving uiCarMode is not auto drive.";
 //        return;
 //    }
-    LOG(INFO)<<Name()<<" ilocalTotalNumbers::."<<ilocalTotalNumbers << "iLocalNowNumber:"<<iLocalNowNumber;
+    LOG(INFO)<<Name()<<" ilocalTotalNumbers:"<<ilocalTotalNumbers << "iLocalNowNumber:"<<iLocalNowNumber;
     if (CarAutoDriveStop())
         return;
     int iPredictPoints;
@@ -438,27 +404,25 @@ bool  Decision::CarAutoDriveStop()
 bool Decision::UpdataCarRuleResults()
 {
     try{
-        if (iLocalNowNumber >0 && ((iLocalNowNumber % iPredictionPointsPerNumber) == 0)){
-            decision::rulesMsg ruleMsg;
-            if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速（ <10km/h）：± 0.25km/h  高速（ >10km/h）：± 1km/h
-            {
-                 ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_SUCCESS;
-            }
-            else
-            {
-                ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_FIAL;
-            }
-            int iResult = ruleMsg.DirectiveResult;
-            DecisionDatabase::UpdateActionDirective(PlanRouteName.c_str(), iRulesNowNumber, iResult);
-            ruleMsg.DirectiveID = iRulesNowNumber;
-            Rules_pub.publish(ruleMsg);
-            iRulesNowNumber ++;
+        iRulesNowNumber ++;
+        decision::rulesMsg ruleMsg;
+        if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速（ <10km/h）：± 0.25km/h  高速（ >10km/h）：± 1km/h
+        {
+             ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_SUCCESS;
         }
+        else
+        {
+            ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_FIAL;
+        }
+        int iResult = ruleMsg.DirectiveResult;
+        DecisionDatabase::UpdateActionDirective(PlanRouteName.c_str(), iRulesNowNumber, iResult);
+        ruleMsg.DirectiveID = iRulesNowNumber;
+        Rules_pub.publish(ruleMsg);
         return true;
     }
     catch (ros::Exception ex)
     {
-        LOG(ERROR)<<Name()<<":UpdataCarRuleResults error."<< ex.what();
+        LOG(ERROR)<<Name()<<"  UpdataCarRuleResults error."<< ex.what();
         return false;
     }
 }
@@ -492,8 +456,8 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
      int i = 0;
      for ( iter = iter_start; iter != iter_end; iter++)
      {
-       LOG(INFO)<<Name()<<" MPC_Controller prediction LocationLon = "<<std::setprecision(16)<<mpc_x_vals[i]
-             <<" LocationLat = "<< std::setprecision(16)<<mpc_y_vals[i];
+       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];
@@ -501,20 +465,19 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
      mpc.delta_prev = steer_value;
      mpc.a_prev = throttle_value;
      LOG(INFO)<<Name()<< " stree alter value:"<<steer_value<<" throttle alter value:"<<throttle_value;
-//     SpeedControl(steer_value);
-//     StreeControl(throttle_value);
-//     decision::controlMsg control;
-//     control.nowSpeed = fCarNowSpeed;
-//     control.refSpeed = mpc.ref_speed;
-//     control.steer =  mpc.delta_prev;
-//     control.throttle = mpc.a_prev;
-//     control.cte = cte;
-//     Control_pub.publish(control);
+     SpeedControl(steer_value);
+     StreeControl(throttle_value);
+     decision::controlMsg control;
+     control.nowSpeed = fCarNowSpeed;
+     control.refSpeed = mpc.ref_speed;
+     control.steer =  mpc.delta_prev;
+     control.throttle = mpc.a_prev;
+     control.cte = cte;
+     Control_pub.publish(control);
 }
 
 unsigned int Decision::SpeedControl(double &speed)
 {
-
      canbus::PedalActCtrReq srv;
      if (mpc.a_prev >0){
          srv.request.uiAccPsngCtrReq =(unsigned int)(speed *10);

src/decision/msg/localizationMsg.msg

@@ -3,4 +3,4 @@ float64 Longitude
 float64 Latitude
 float64 Height
 float64 LocationSpeed
-int32 type
+float64 AngleDeviated

src/localization/localization.cpp

@@ -212,14 +212,14 @@ int Localization::PublishData(NMEA0183 &nmea)
     LOG(INFO)<<Name()<<" PublishData start\n";
     try{
 
-        double northing, easting;
-        std::string zone;
-        //CoordsConvertion::LLtoUTM(nmea.gprmc.Latitude, nmea.gprmc.Longitude, northing, easting, zone);
+//        double northing, easting;
+//        std::string zone;
+//        CoordsConvertion::LLtoUTM(nmea.gprmc.Latitude, nmea.gprmc.Longitude, northing, easting, zone);
         localization::localMsg localization;
-        localization.Longitude =  nmea.gprmc.Longitude;
-        localization.Latitude = nmea.gprmc.Latitude;
-        localization.Height = nmea.gpgga.Height;
-        localization.AngleDeviated = nmea.gprmc.Deviated;
+        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);
         geometry_pub.publish(localization);
     }
     catch (ros::Exception ex)

src/localization/localization.h

@@ -86,7 +86,12 @@ class Localization : public robot::common::RobotApp{
       return atof(strResult.substr(0, pos).c_str()) + atof(strResult.substr(pos, strResult.length() - pos).c_str())/60.0;
   }
 
-  //void Nmea0138Analyse( std::string &serial_data, nmea_msgs::Gpgga &gpgga_msg, nmea_msgs::Gprmc);
+
+  inline double to_string_with_precision(double a_value, int n){
+      std::ostringstream out;
+      out<<std::setprecision(n)<<a_value;
+      return atof(out.str().c_str());
+  }
 
   /**
    * @brief publish data message to other nodes

src/localization/msg/localMsg.msg

@@ -1,4 +1,4 @@
 float64 Longitude
 float64 Latitude
-float32 Height
-float32 AngleDeviated
+float64 Height
+float64 AngleDeviated