完善自动驾驶业务

src/common/message.h

@@ -21,6 +21,12 @@ namespace common{
 // -----------------topics ----------------------------
 const std::string Geometry = "geometry"; // utm coords
 
+const std::string GeometryPlan = "geometryplan";
+
+const std::string ControlInfo = "controlinfo";
+
+const std::string RuleInfo = "ruleinfo";
+
 const std::string CanDataAnalysis = "canDataAnalysis";
 
 const std::string ObdData = "obddata";

src/decision/CMakeLists.txt

@@ -48,6 +48,7 @@ find_package(catkin REQUIRED COMPONENTS
  add_message_files(
    FILES
    rulesMsg.msg
+   controlMsg.msg
    localizationMsg.msg
  )
 

src/decision/decision.cpp

@@ -6,7 +6,11 @@
 #include <canbus/PedalActCtrReq.h>
 #include <canbus/SftActCtrReq.h>
 #include <canbus/StrActCtrReq.h>
+#include <decision/localizationMsg.h>
+#include <decision/rulesMsg.h>
+#include <decision/controlMsg.h>
 #include <stdlib.h>
+#include <math.h>
 
 
 namespace robot {
@@ -22,8 +26,8 @@ int Decision::Init(){
     node_handle_.reset(new ros::NodeHandle());
     DecisionDatabase::InitDB();
     iCommandCmd = 0;// set undefied mode
-    fRealCarSpeed = iLocalizationNumbers = iRulesNumbers = 0;
-    iPredictionPoints = 20;
+    iRulesTotalNumbers = ilocalTotalNumbers =fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
+    iPredictionPointsPerNumber = 20;
     uiCarMode =6; // undefied
 
     PlanRouteName = "siasun";
@@ -43,7 +47,13 @@ int Decision::Start(){
 
     ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::Si2Data, 2000, &Decision::RCUModeCallback, this);
 
-    ros::AsyncSpinner spinner(4); // Use 4 threads
+    Rules_pub = node_handle_->advertise<decision::rulesMsg>(robot::common::RuleInfo, 200);
+
+    Geometry_pub = node_handle_->advertise<decision::localizationMsg>(robot::common::GeometryPlan, 200);
+
+    Control_pub = node_handle_->advertise<decision::controlMsg>(robot::common::ControlInfo, 200);
+
+    ros::AsyncSpinner spinner(6); // Use 6 threads
     spinner.start();
     spinner.stop();
     ros::waitForShutdown();
@@ -63,20 +73,20 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
     LOG(INFO)<<Name()<<"CommandRequest get command ID:."<< req.Cmd;
     if (req.Cmd == CMD_AUTODRIE_START)
     {
-        double breakCtr = -5;
+        double breakCtr = -5; // 踩刹车到50%
         ros::Rate loop_rate(1);
         while(ros::ok())
         {
             if (SpeedControl(breakCtr) == 1)
             {
-                // TODO send location and rule
-                if (BreakCtrControl() == 1)
+                LoadPlanRoute();
+                if (BreakCtrControl(SFT_PSNG_LRN_REQ_FORWARD_1) == 1) // 前进挡
                     break;
             }
             ros::spinOnce();
             loop_rate.sleep();
         }
-        fRealCarSpeed = iLocalizationNumbers = iRulesNumbers = 0;
+        fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
     }
     else if (req.Cmd == CMD_PLAN_START)
     {
@@ -89,11 +99,29 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
 
 void Decision::LoadPlanRoute()
 {
-    iLocalizationNumbers = DecisionDatabase::countOfLocationRuleInfo();
-    iRulesNumbers = DecisionDatabase::countOfActionDirective();
-    for (int i = 0; i< iLocalizationNumbers; i++)
+    ilocalTotalNumbers = DecisionDatabase::countOfLocationRuleInfo();
+    iRulesTotalNumbers = DecisionDatabase::countOfActionDirective();
+
+    for (int i = 0; i< ilocalTotalNumbers; i++)
     {
         std::vector<LOCATION_RULE_INFO> tempInfo = DecisionDatabase::QueryLocationRuleInfo(i, 1);
+        decision::localizationMsg localMsg;
+        localMsg.Locationid = tempInfo[i].LocationID;
+        localMsg.Longitude = tempInfo[i].LocationLon;
+        localMsg.Latitude = tempInfo[i].LocationLat;
+        localMsg.Height = tempInfo[i].LocationHeight;
+        localMsg.LocationSpeed = tempInfo[i].LocationSpeed;
+        Geometry_pub.publish(localMsg);
+    }
+
+    for (int i = 0; i< iRulesTotalNumbers; i++)
+    {
+        std::vector<ACTION_DIRECTIVE> tempInfo = DecisionDatabase::QueryActionDirective(i, 1);
+        decision::rulesMsg ruleMsg;
+        ruleMsg.DirectiveID = tempInfo[i].DirectiveID;
+        ruleMsg.DirectiveInfo = tempInfo[i].DirectiveInfo;
+        ruleMsg.DirectiveResult = tempInfo[i].DirectiveResult;
+        Rules_pub.publish(ruleMsg);
     }
 
 }
@@ -121,8 +149,8 @@ void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
 
 void Decision::OBDCallback(const canbus::obdMsg::ConstPtr& msg)
 {
-    fRealCarSpeed = msg->VehSpd;
-    LOG(INFO)<<Name()<< "car real speed:"<<fRealCarSpeed;
+    fCarNowSpeed = msg->VehSpd;
+    LOG(INFO)<<Name()<< "car real speed:"<<fCarNowSpeed;
 }
 
 void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
@@ -135,41 +163,41 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
 {
     try
     {
-        iLocalizationNumbers ++;
-        if ((iLocalizationNumbers % iPredictionPoints)  == 0)
+        iLocalNowNumber ++;
+        if ((iLocalNowNumber % iPredictionPointsPerNumber)  == 0)
         {          
-            iRulesNumbers++;
+            iRulesNowNumber++;
             std::stringstream strRuleContent;
-            if (iRulesNumbers == 1)
-                strRuleContent<<"动作"<<iRulesNumbers<<":起步请加速到"<< fRealCarSpeed<<"km/h";
+            if (iRulesNowNumber == 1)
+                strRuleContent<<"动作"<<iRulesNowNumber<<":起步请加速到"<< fCarNowSpeed<<"km/h";
             else{
-                if (fRealCarSpeed > fLastCarSpeed)
+                if (fCarNowSpeed > fCarLastSpeed)
                 {
-                    strRuleContent<<"动作"<<iRulesNumbers<<":加速到"<< fRealCarSpeed<<"km/h";
+                    strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
                 }
-                else if (fRealCarSpeed < fLastCarSpeed)
+                else if (fCarNowSpeed < fCarLastSpeed)
                 {
-                    strRuleContent<<"动作"<<iRulesNumbers<<":制动至"<< fRealCarSpeed<<"km/h";
+                    strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
                 }
-                else if (fRealCarSpeed == fLastCarSpeed)
+                else if (fCarNowSpeed == fCarLastSpeed)
                 {
-                    strRuleContent<<"动作"<<iRulesNumbers<<":保持"<< fRealCarSpeed<<"km/h";
+                    strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
                 }
            }
 
-           int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNumbers,
-                            strRuleContent.str().c_str(), 0, iLocalizationNumbers, msg->Longitude, msg->Latitude,
-                            msg->Height, fRealCarSpeed);
+           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) {
-                iLocalizationNumbers--;
-                iRulesNumbers--;
+                iLocalNowNumber--;
+                iRulesNowNumber--;
            }
-           fLastCarSpeed = fRealCarSpeed;
+           fCarLastSpeed = fCarNowSpeed;
         }
         else
         {
-           DecisionDatabase::InsertLocationRuleInfo(PlanRouteName.c_str(),iLocalizationNumbers, msg->Longitude,
-                                             msg->Latitude, msg->Height, fRealCarSpeed);
+           DecisionDatabase::InsertLocationRuleInfo(PlanRouteName.c_str(),iLocalNowNumber, msg->Longitude,
+                                             msg->Latitude, msg->Height, fCarNowSpeed);
         }
     }
     catch (ros::Exception ex)
@@ -181,33 +209,33 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
 
 void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
 {
-     if ((iLocalizationNumbers % iPredictionPoints)  == 0)
+     if ((iLocalNowNumber % iPredictionPointsPerNumber)  == 0)
      {
          return;
      }
      else
      {
-         iLocalizationNumbers++;
-         iRulesNumbers++;
+         iLocalNowNumber++;
+         iRulesNowNumber++;
          std::stringstream strRuleContent;
-         if (fRealCarSpeed > fLastCarSpeed)
+         if (fCarNowSpeed > fCarLastSpeed)
          {
-             strRuleContent<<"动作"<<iRulesNumbers<<":加速到"<< fRealCarSpeed<<"km/h";
+             strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
          }
-         else if (fRealCarSpeed < fLastCarSpeed)
+         else if (fCarNowSpeed < fCarLastSpeed)
          {
-             strRuleContent<<"动作"<<iRulesNumbers<<":制动至"<< fRealCarSpeed<<"km/h";
+             strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
          }
-         else if (fRealCarSpeed == fLastCarSpeed)
+         else if (fCarNowSpeed == fCarLastSpeed)
          {
-             strRuleContent<<"动作"<<iRulesNumbers<<":保持"<< fRealCarSpeed<<"km/h";
+             strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
          }
-        int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNumbers,
-                         strRuleContent.str().c_str(), 0, iLocalizationNumbers, msg->Longitude, msg->Latitude,
-                         msg->Height, fRealCarSpeed);
+        int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
+                         strRuleContent.str().c_str(), 0, iLocalNowNumber, msg->Longitude, msg->Latitude,
+                         msg->Height, fCarNowSpeed);
         if (iRet < 0) {
-             iLocalizationNumbers--;
-             iRulesNumbers--;
+             iLocalNowNumber--;
+             iRulesNowNumber--;
         }
      }
 }
@@ -217,7 +245,109 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
 {
    // 判断当前RCU是否为自动驾驶模式
     if (uiCarMode != CTR_MD_REQ_AUTO_DRIVE)
-        return ;
+        return;
+    if (CarAutoDriveStop())
+        return;
+    int iPredictPoints;
+    if (ilocalTotalNumbers < (iLocalNowNumber + iPredictionPointsPerNumber))
+    {
+        iPredictPoints = ilocalTotalNumbers - iLocalNowNumber;
+    }
+    else
+    {
+        iPredictPoints = iPredictionPointsPerNumber;
+    }
+    std::vector<LOCATION_RULE_INFO> tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
+    std::vector<double> ptsy(iPredictPoints);
+    std::vector<double> ptsx(iPredictPoints);
+    for (int i= 0; i< iPredictPoints; i++)
+    {
+       ptsx.push_back( tempInfo[i].LocationLon);
+       ptsy.push_back( tempInfo[i].LocationLat);
+    }
+    mpc.ref_speed = tempInfo[iPredictPoints-1].LocationSpeed;
+    double px = msg->Longitude;
+    double py = msg->Latitude;
+    double psi = deg2rad(msg->AngleDeviated + 90);
+    double v =  (double)fCarNowSpeed;
+    MPC_Controller(ptsx, ptsy, px, py, psi, v);
+    UpdataCarRuleResults();
+    iLocalNowNumber ++;
+}
+
+bool  Decision::CarAutoDriveStop()
+{
+    try
+    {
+        if (ilocalTotalNumbers = iLocalNowNumber)
+        {
+            uiCarMode = CMD_AUTODRIE_END;
+            double breakCtr = -5; // 踩刹车到50%
+            ros::Rate loop_rate(1);
+            while(ros::ok())
+            {
+                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
+            {
+                 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;
+        }
+        else
+            return false;
+    }
+    catch (ros::Exception ex)
+    {
+        LOG(ERROR)<<Name()<<":CarAutoDriveStop error."<< ex.what();
+        return false;
+    }
+}
+
+
+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 ++;
+        }
+        return true;
+    }
+    catch (ros::Exception ex)
+    {
+        LOG(ERROR)<<Name()<<":UpdataCarRuleResults error."<< ex.what();
+        return false;
+    }
 }
 
 void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &ptsy,
@@ -243,6 +373,12 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
      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_pub.publish(control);
 }
 
 unsigned int Decision::SpeedControl(double &speed)
@@ -290,11 +426,11 @@ unsigned int Decision::StreeControl(double &streer)
      return srv.response.uiReturn;
 }
 
-unsigned int Decision::BreakCtrControl()
+unsigned int Decision::BreakCtrControl(unsigned int breakmode)
 {
      ros::ServiceClient clientSftActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::ShiftActCtrService);
      canbus::SftActCtrReq srv;
-     srv.request.uiSftPsngCtrReq = SFT_PSNG_LRN_REQ_FORWARD_1;
+     srv.request.uiSftPsngCtrReq = breakmode;
      srv.request.uiSftMdReq = SFT_MD_REQ_2_SECOND;
      srv.request.uiClchPsngCtrReq = 1;
      srv.request.iClchSpdCtrReq = 0;

src/decision/decision.h

@@ -96,10 +96,15 @@ private:
 
     unsigned int StreeControl(double &streer);
 
-    unsigned int BreakCtrControl();
+    unsigned int BreakCtrControl(unsigned int breakmode);
 
     void LoadPlanRoute();
 
+
+    bool CarAutoDriveStop();
+
+    bool UpdataCarRuleResults();
+
     void MPC_Controller(std::vector<double> &ptsx, std::vector<double> &ptsy,
                         double &px, double &py,  double &psi, double &v);
 
@@ -140,20 +145,30 @@ private:
 
     int32_t iCommandCmd; // excute mode 0: undefined 1: plan start 2: auto-drive start  3: plan stop 4: auto-drive stop
 
-    float_t fRealCarSpeed;
+    float_t fCarNowSpeed;
+
+    float_t fCarLastSpeed; //plan
 
-    float_t fLastCarSpeed; //plan
+    int64_t iLocalNowNumber; // plan or auto drive localization numbers
 
-    int64_t iLocalizationNumbers; // plan or auto drive localization numbers
+    int64_t ilocalTotalNumbers;
 
-    int64_t iRulesNumbers; // plan or auto drive rule numbers
+    int64_t iRulesNowNumber; // plan or auto drive rule numbers
 
-    int32_t iPredictionPoints;
+    int64_t iRulesTotalNumbers;
+
+    int32_t iPredictionPointsPerNumber;
 
     unsigned int uiCarMode;
 
     robot::common::MPC mpc;
 
+    ros::Publisher Rules_pub;
+
+    ros::Publisher Geometry_pub;
+
+    ros::Publisher Control_pub;
+
 };
 
 

src/decision/decisiondatabase.h

@@ -138,6 +138,7 @@ public :
     */
     static std::vector<LOCATION_RULE_INFO> QueryLocationRuleInfo(int iStart, int iLength);
 
+
     /**
     * @brief 查询总数的action_Directive的记录
     * @return 0失败

src/decision/decisiondatatype.h

@@ -32,6 +32,10 @@ namespace decisionSpace {
 #define	CTR_MD_REQ_ROBOT_CHECK		4	//控制模式请求-机器人自检模式
 #define	CTR_MD_REQ_ROBOT_DEBUG		5	//控制模式请求-机器人调试模式
 
+#define RULE_EXCUTE_RESULT_INIT           0
+#define RULE_EXCUTE_RESULT_FIAL           1
+#define RULE_EXCUTE_RESULT_SUCCESS        2
+
 typedef struct
 {
   std::string RuleName;

src/decision/msg/controlMsg.msg

@@ -0,0 +1,4 @@
+float64 steer
+float64 throttle
+float64 refSpeed
+float64 nowSpeed