修改canbus模块和感知模块的ｂｕｇ

src/canbus/canbus.cpp

@@ -112,7 +112,7 @@ int Canbus::Start() {
   //data receive
   while(ros::ok())
   {
-      read_loop();     
+      read_loop();
       ros::spinOnce();
       loop_rate.sleep();
    }
@@ -129,7 +129,6 @@ void Canbus::OnTimer(const ros::TimerEvent &) {
       heatbeat = 0;
   }
   icuMonitor.uiICUAlive = heatbeat++;
-  heatbeat ++;
 
   try
   {
@@ -259,7 +258,7 @@ int Canbus::read_data(TPCANMsg *pMsgBuff)
     }
     else if(pMsgBuff->ID == robot::common::ObdVehHBrkStat)
     {
-        T_OBD_VEH_BRK_STAT tOBDVehBrkStat = *((T_OBD_VEH_BRK_STAT*)MessageCoder::CanMsgAnalyse(robot::common::ObdVehHBrkStat,pMsgBuff->DATA));
+       T_OBD_VEH_BRK_STAT tOBDVehBrkStat = *((T_OBD_VEH_BRK_STAT*)MessageCoder::CanMsgAnalyse(robot::common::ObdVehHBrkStat,pMsgBuff->DATA));
        obd.VehHBrkStat = status1.uiVehHBrkStat = tOBDVehBrkStat.obdVehHBrkStat;
     }
     else if(pMsgBuff->ID == robot::common::ObdVehSwStat)
@@ -275,8 +274,15 @@ int Canbus::read_data(TPCANMsg *pMsgBuff)
     }
     else if(pMsgBuff->ID == robot::common::ObdEngSpd)
     {
-        //T_OBD_ENG_SPD tOBDEngSpd = *((T_OBD_ENG_SPD*)MessageCoder::CanMsgAnalyse(robot::common::ObdEngSpd,pMsgBuff->DATA));
-        //status2.uiEngSpd = tOBDEngSpd.obdEngSpd;
+        T_OBD_ENG_SPD tOBDEngSpd = *((T_OBD_ENG_SPD*)MessageCoder::CanMsgAnalyse(robot::common::ObdEngSpd,pMsgBuff->DATA));
+        status2.uiEngSpd = tOBDEngSpd.obdEngSpd;
+        obd.EngSpd = status2.uiEngSpd;
+    }
+    else if(pMsgBuff->ID == robot::common::ObdSpeed)
+    {
+        T_OBD_VEH_SPD tOBDVehSpd = *((T_OBD_VEH_SPD*)MessageCoder::CanMsgAnalyse(robot::common::ObdSpeed,pMsgBuff->DATA));
+        status2.uiVehSpd = tOBDVehSpd.obdVehSpd;
+        obd.VehSpd = status2.uiVehSpd;
     }
     else if((pMsgBuff->ID >= robot::common::PowerOnReq && pMsgBuff->ID <= robot::common::BrakeForceCtrStp) ||
             (pMsgBuff->ID >= robot::common::SysMonitorBack && pMsgBuff->ID <= robot::common::SteerPosTimeReq) ||
@@ -293,16 +299,19 @@ void Canbus::publish_data(TPCANMsg *pMsgBuff)
 {
     canbus::canMsg rosMsg;
     rosMsg.id = pMsgBuff->ID;
-    std::stringstream ss;
-    ss << pMsgBuff->DATA;
-    rosMsg.data = ss.str();
-    canRCUData_pub.publish(rosMsg);
+    //std::stringstream ss;
+    //ss << pMsgBuff->DATA;
+    //rosMsg.data = ss.str();
+
 
     char strId[4] = {0};
     sprintf(strId,"%3xh",pMsgBuff->ID);
-    char strData[16]={0};
-    MessageCoder::ConvertUnCharToStr(strData,pMsgBuff->DATA,8);
-    LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << strId << ",Data:"<<strData;
+    char chData[16]={0};
+    MessageCoder::ConvertUnCharToStr(chData,pMsgBuff->DATA,8);
+    //std::string strData = chData;
+    rosMsg.data = chData;
+    canRCUData_pub.publish(rosMsg);
+    LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << strId << ",Data:"<<std::string(chData);
 }
 
 int Canbus::write_data(TPCANMsg *pMsgBuff)
@@ -375,7 +384,7 @@ bool Canbus::ExcuteLearnModeRequest(canbus::LearnModeRequest::Request & req,
         MessageCoder::CanMsgPackage((void*)&request, robot::common::LearnModeReq, &canLearnModeReqMsg);
         if (write_data(&canLearnModeReqMsg)!=ROBOT_SUCCESS)
         {
-            res.uiReturn = 1;  //= 0
+            res.uiReturn = 0;  //= 0
             return true;
         }
         res.uiReturn = 1;
@@ -409,7 +418,7 @@ bool Canbus::PedalActCtrRequest(canbus::PedalActCtrReq::Request &req,
         MessageCoder::CanMsgPackage((void*)&request, robot::common::PedalActCtrReq, &canPedalActCtrReqMsg);
         if (write_data(&canPedalActCtrReqMsg) != ROBOT_SUCCESS)
         {
-            res.uiReturn = 1; // =0
+            res.uiReturn = 0; // =0
             return true;
         }
         res.uiReturn = 1;
@@ -439,7 +448,7 @@ bool Canbus::SftActCtrRequest(canbus::SftActCtrReq::Request &req,
         MessageCoder::CanMsgPackage((void*)&request, robot::common::ShiftActCtrReq, &canSftActCtrReqMsg);
         if (write_data(&canSftActCtrReqMsg) != ROBOT_SUCCESS)
         {
-            res.uiReturn = 1; // =0
+            res.uiReturn = 0; // =0
             return true;
         }
         res.uiReturn = 1;
@@ -469,7 +478,7 @@ bool Canbus::StrActCtrRequest(canbus::StrActCtrReq::Request &req,
         MessageCoder::CanMsgPackage((void*)&request, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
         if (write_data(&canStrActCtrReqMsg) !=ROBOT_SUCCESS)
         {
-            res.uiReturn = 1; // =0
+            res.uiReturn = 0; // =0
             return true;
         }
         res.uiReturn = 1;

src/canbus/candatatype.h

@@ -2137,11 +2137,31 @@ typedef struct
 typedef struct
 {
     unsigned int : 8;
-    unsigned int obdEngSpd : 16;		//实际发动机转速
+    unsigned int obdEngSpdH : 8;		//实际发动机转速
+    unsigned int obdEngSpdL : 8;		//实际发动机转速
     unsigned int : 8;
     unsigned int : 32;
 }T_OBD_0C9;
 
+typedef struct
+{
+    unsigned obdEngSpdL : 8;		//实际发动机转速
+    unsigned obdEngSpdH : 8;		//实际发动机转速
+    unsigned int : 16;
+}T_OBD_0C9_SRC;
+
+typedef struct
+{
+    unsigned obdEngSpd : 16;		//实际发动机转速
+    unsigned int : 16;
+}T_OBD_0C9_DEST;
+
+typedef union
+{
+    T_OBD_0C9_SRC tobd0c9Src;		//实际发动机转速
+    T_OBD_0C9_DEST  tobd0c9Dest;
+}T_OBD_0C9_UNION;
+
 //实际发动机转速
 typedef struct
 {
@@ -2150,6 +2170,31 @@ typedef struct
 
 #define OBD_ENG_SPD_RATIO		4	//实际发动机转速比例
 
+/************OBD实际车辆速度*******/
+typedef struct
+{
+
+//    unsigned int obdVehSpd : 16;		//实际车辆速度
+//     unsigned int : 16;
+//     unsigned int : 32;
+       unsigned int : 32;
+       unsigned int : 16;
+       unsigned int obdVehSpd : 16;		//实际车辆速度
+
+
+
+}T_OBD_3E9;
+
+//实际车辆速度
+typedef struct
+{
+    unsigned int obdVehSpd;			//实际车辆速度
+}T_OBD_VEH_SPD;
+
+#define OBD_VEH_SPD_RATIO		64	//实际车辆速度比例
+
+
+
 } // namespace canbus
 
 } // namespace robot

src/canbus/messagecoder.cpp

@@ -17,17 +17,6 @@ void MessageCoder::CharReverse(unsigned char * s, int n)
         s[i] = s[j];
         s[j] = c;
     }
-
-    //unsigned char ucMsgOld[100];
-    //unsigned char ucMsgNew[100];
-    //memset(ucMsgOld,0,n);
-    //memset(ucMsgNew, 0, n);
-    //memcpy(ucMsgOld, s, 8);
-    //for (int i = 7; i >= 0; i--)
-    //{
-    //	ucMsgNew[7 - i] = ucMsgOld[i];
-    //}
-    //memcpy(s, ucMsgNew, n);
 }
 
 //执行机构使能请求--数据解析
@@ -741,13 +730,38 @@ void MessageCoder::OBDAnalyseID_1E5(unsigned char *ucData, T_OBD_STR_ANG *tOBDSt
 //OBD实际发动机转速--数据解析
 void MessageCoder::OBDAnalyseID_0C9(unsigned char *ucData, T_OBD_ENG_SPD *tOBDEngSpd)
 {
+//    T_OBD_0C9 tOBD0C9 = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
+
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+//    memcpy(&tOBD0C9, ucCanMsgSrc, CAN_MSG_LEN);
+
+//    tOBDEngSpd->obdEngSpd = tOBD0C9.obdEngSpd / OBD_ENG_SPD_RATIO;
+
     T_OBD_0C9 tOBD0C9 = { 0 };
     unsigned char ucCanMsgSrc[8] = { 0 };
 
     memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
     memcpy(&tOBD0C9, ucCanMsgSrc, CAN_MSG_LEN);
 
-    tOBDEngSpd->obdEngSpd = tOBD0C9.obdEngSpd / OBD_ENG_SPD_RATIO;
+    T_OBD_0C9_UNION tObd0c9Union;
+    memset(&tObd0c9Union,0,sizeof(T_OBD_0C9_UNION));
+    tObd0c9Union.tobd0c9Src.obdEngSpdH = tOBD0C9.obdEngSpdH;
+    tObd0c9Union.tobd0c9Src.obdEngSpdL = tOBD0C9.obdEngSpdL;
+
+    tOBDEngSpd->obdEngSpd = tObd0c9Union.tobd0c9Dest.obdEngSpd / OBD_ENG_SPD_RATIO;
+}
+
+//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;
 }
 
 //控制器上电命令--数据组包
@@ -1409,6 +1423,11 @@ void * MessageCoder::CanMsgAnalyse(int id,unsigned char * ucData)
         OBDAnalyseID_0C9(ucData,&tOBDEngSpd);
         p = &tOBDEngSpd;
         break;
+    case robot::common::ObdSpeed://OBD实际车辆速度
+        T_OBD_VEH_SPD tOBDVehSpd;
+        OBDAnalyseID_3E9(ucData,&tOBDVehSpd);
+        p = &tOBDVehSpd;
+        break;
     default:
         break;
     }

src/canbus/messagecoder.h

@@ -115,6 +115,9 @@ private:
     //OBD实际发动机转速--数据解析
     static void OBDAnalyseID_0C9(unsigned char *ucData, T_OBD_ENG_SPD *tOBDEngSpd);
 
+    //OBD实际车辆速度--数据解析
+    static void OBDAnalyseID_3E9(unsigned char *ucData, T_OBD_VEH_SPD *tOBDVehSpd);
+
     //控制器上电命令--数据组包
     static void CanMsgPackageID_5F1(T_POWER_ON_REQUEST *tPowOnReq, TPCANMsg *tCanMsg);
 

src/canbus/msg/obdMsg.msg

@@ -5,3 +5,4 @@ float32 VehSpd
 int32 SftPsng
 int32 VehHBrkStat
 int32 VehSwStat
+float32 EngSpd

src/common/canmsgobdmsgid.h

@@ -118,6 +118,8 @@ const int ObdVehSwStat = 0x121;
 const int ObdStrAng = 0x1E5;
 //OBD实际发动机转速
 const int ObdEngSpd = 0x0C9;
+//OBD实际车辆速度
+const int ObdSpeed = 0x3E9;
 
 } // common
 } // robot

src/common/mpc.cpp

@@ -10,7 +10,7 @@ namespace common {
 
 
 // 设置预测状态点的个数和两个相邻状态点之间的时间间隔
-size_t N = 20;
+size_t N = 10;
 double dt = 0.2; // 200ms
 
 // 设置汽车头到车辆重心之间的距离
@@ -18,6 +18,8 @@ const double Lf = 2.67;
 
 // 参考速度，为了避免车辆在行驶中停止
 //double ref_v = 65;
+double ref_cte = 0;
+double ref_epsi = 0;
 
 // solver使用的是一个向量存储所有的状态值，所以需要确定每种状态在向量中的开始位置
 size_t x_start = 0;
@@ -27,7 +29,7 @@ size_t v_start = psi_start + N;
 size_t cte_start = v_start + N;
 size_t epsi_start = cte_start + N;
 size_t delta_start = epsi_start + N;
-size_t a_start = delta_start + N - 1;
+size_t a_start = delta_start + N -1;
 
 class FG_eval {
 public:
@@ -43,30 +45,26 @@ public:
     typedef CPPAD_TESTVECTOR(AD<double>)ADvector;
 
     // 该函数的目的是定义约束，fg向量包含的是总损失和约束，vars向量包含的是状态值和驱动器的输入
-    void operator()(ADvector& fg, const ADvector& vars) {
+     void operator()(ADvector& fg, const ADvector& vars) {
         // 任何cost都会加到fg[0]
         fg[0] = 0;
         // 使车辆轨迹和参考路径的误差最小，且使车辆的速度尽量接近参考速度
-        for (int t = 0; t < N; t++) {
-            fg[0] += CppAD::pow(vars[cte_start + t], 2);
-            fg[0] += CppAD::pow(vars[epsi_start + t], 2);
-            fg[0] += CppAD::pow(vars[v_start + t] - ref_v, 2);
-        }
+        for (int i = 0; i < N; i++) {
+             fg[0] += 2000*CppAD::pow(vars[cte_start + i] - ref_cte, 2);
+             fg[0] += 2000*CppAD::pow(vars[epsi_start + i] - ref_epsi, 2);
+             fg[0] += CppAD::pow(vars[v_start + i] - ref_v, 2);
+           }
         // 使车辆行驶更平稳，尽量减少每一次驱动器的输入大小
         // 注意：驱动器的输入是N-1个.最后一个点没有驱动器输入
-        for (int t = 0; t < N - 1; t++) {
-            fg[0] += CppAD::pow(vars[delta_start + t], 2);
-            fg[0] += CppAD::pow(vars[a_start + t], 2);
+        for (int i = 0; i < N - 1; i++) {
+          fg[0] += 5*CppAD::pow(vars[delta_start + i], 2);
+          fg[0] += 5*CppAD::pow(vars[a_start + i], 2);
         }
         // 为了使车辆运动更平滑，尽量减少相邻两次驱动器输入的差距
         // 注意：这个地方是N-2个
-        for (int t = 0; t < N - 2; t++) {
-            fg[0] += 600*CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
-            fg[0] += CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 2);
-        }
-        for (int t = 0; t < N - 2; t++) {
-            fg[0] += 600*CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
-            fg[0] += CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 2);
+        for (int i = 0; i < N - 2; i++) {
+          fg[0] += 200*CppAD::pow(vars[delta_start + i + 1] - vars[delta_start + i], 2);
+          fg[0] += 10*CppAD::pow(vars[a_start + i + 1] - vars[a_start + i], 2);
         }
 
         // 设置fg的初始值为状态的初始值，这个地方为初始条件约束
@@ -78,37 +76,39 @@ public:
         fg[1 + epsi_start] = vars[epsi_start];
 
         // 因为t=0初始条件约束已经有了，计算其他约束条件
-        for (int t = 1; t < N; t++) {
+        for (int t = 0; t < N -1; t++) {
             // t+1时刻的状态
-            AD<double> x1 = vars[x_start + t];
-            AD<double> y1 = vars[y_start + t];
-            AD<double> psi1 = vars[psi_start + t];
-            AD<double> v1 = vars[v_start + t];
-            AD<double> cte1 = vars[cte_start + t];
-            AD<double> epsi1 = vars[epsi_start + t];
+            AD<double> x1 = vars[x_start + t + 1];
+            AD<double> y1 = vars[y_start + t + 1];
+            AD<double> psi1 = vars[psi_start + t + 1];
+            AD<double> v1 = vars[v_start + t + 1];
+            AD<double> cte1 = vars[cte_start + t + 1];
+            AD<double> epsi1 = vars[epsi_start + t + 1];
 
             // t时刻的状态
-            AD<double> x0 = vars[x_start + t - 1];
-            AD<double> y0 = vars[y_start + t - 1];
-            AD<double> psi0 = vars[psi_start + t - 1];
-            AD<double> v0 = vars[v_start + t - 1];
-            AD<double> cte0 = vars[cte_start + t - 1];
-            AD<double> epsi0 = vars[epsi_start + t - 1];
+            AD<double> x0 = vars[x_start + t ];
+            AD<double> y0 = vars[y_start + t ];
+            AD<double> psi0 = vars[psi_start + t ];
+            AD<double> v0 = vars[v_start + t ];
+            AD<double> cte0 = vars[cte_start + t ];
+            AD<double> epsi0 = vars[epsi_start + t ];
             // t时刻的驱动器输入
-            AD<double> delta0 = vars[delta_start + t - 1];
-            AD<double> a0 = vars[a_start + t - 1];
+            AD<double> delta0 = vars[delta_start + t ];
+            AD<double> a0 = vars[a_start + t ];
 
             // t时刻参考路径的距离和角度值
-            AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2]*x0*x0 + coeffs[3]*x0*x0*x0;
-            AD<double> psides0 = CppAD::atan(coeffs[1]+2*coeffs[2]*x0 + 3 * coeffs[3]*x0*x0);
+            AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * x0 * x0 + coeffs[3] * x0 * x0 * x0;
+            AD<double> psides0 = CppAD::atan(3*coeffs[3]* x0* x0 + 2*coeffs[2]* x0 + coeffs[1]);
 
             // 根据如上的状态值计算约束条件
-            fg[1 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
-            fg[1 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
-            fg[1 + psi_start + t] = psi1 - (psi0 + v0 * delta0 / Lf * dt);
-            fg[1 + v_start + t] = v1 - (v0 + a0 * dt);
-            fg[1 + cte_start + t] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
-            fg[1 + epsi_start + t] = epsi1 - ((psi0 - psides0) + v0 * delta0 / Lf * dt);
+            fg[2 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
+            fg[2 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
+            fg[2 + psi_start + t] = psi1 - (psi0 - v0 * delta0 / Lf * dt);
+            fg[2 + v_start + t] = v1 - (v0 + a0 * dt);
+            fg[2 + cte_start + t] =
+                cte1 - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
+            fg[2 + epsi_start + t] =
+                epsi1 - ((psi0 - psides0) - v0 * delta0 / Lf * dt);
         }
     }
 };
@@ -119,78 +119,72 @@ MPC::MPC() {
 MPC::~MPC() {
 }
 
-vector<vector<double>> MPC::Solve(Eigen::VectorXd x0, Eigen::VectorXd coeffs) {
-    size_t i;
-    typedef CPPAD_TESTVECTOR(double)Dvector;
+vector<double> MPC::Solve(Eigen::VectorXd x0, Eigen::VectorXd coeffs) {
+  size_t i;
+  typedef CPPAD_TESTVECTOR(double) Dvector;
 
-    double x = x0[0];
-    double y = x0[1];
-    double psi = x0[2];
-    double v = x0[3];
-    double cte = x0[4];
-    double epsi = x0[5];
+  double x = x0[0];
+  double y = x0[1];
+  double psi = x0[2];
+  double v = x0[3];
+  double cte = x0[4];
+  double epsi = x0[5];
 
-    // 独立状态的个数，注意：驱动器的输入（N - 1）* 2
-    size_t n_vars = N * 6 + (N - 1) * 2;
-    // 约束条件的个数
-    size_t n_constraints = N * 6;
+  // 独立状态的个数，注意：驱动器的输入（N - 1）* 2
+  size_t n_vars = N * 6 + (N - 1) * 2;
+  // 约束条件的个数
+  size_t n_constraints = N * 6;
 
     // 除了初始值，初始化每一个状态为0
-    Dvector vars(n_vars);
-    for (int i = 0; i < n_vars; i++) {
-        vars[i] = 0.0;
-    }
-    vars[x_start] = x;
-    vars[y_start] = y;
-    vars[psi_start] = psi;
-    vars[v_start] = v;
-    vars[cte_start] = cte;
-    vars[epsi_start] = epsi;
+  Dvector vars(n_vars);
+   for (int i = 0; i < n_vars; i++) {
+     vars[i] = 0;
+   }
+   vars[x_start] = x;
+   vars[y_start] = y;
+   vars[psi_start] = psi;
+   vars[v_start] = v;
+   vars[cte_start] = cte;
+   vars[epsi_start] = epsi;
 
     // 设置每一个状态变量的最大和最小值
     // 【1】设置非驱动输入的最大和最小值
     // 【2】设置方向盘转动角度范围-25—25度
-    // 【3】加速度的范围-1—1
-    Dvector vars_lowerbound(n_vars);
-    Dvector vars_upperbound(n_vars);
-    for (int i = 0; i < delta_start; i++) {
-        vars_lowerbound[i] = -1.0e19;
-        vars_upperbound[i] = 1.0e19;
-    }
-    for (int i = delta_start; i < a_start; i++) {
-        vars_lowerbound[i] = -0.436332;
-        vars_upperbound[i] = 0.436332;
-    }
-    for (int i = a_start; i < n_vars; i++) {
-        vars_lowerbound[i] = -1.0;
-        vars_upperbound[i] = 1.0;
-    }
-    // 前一次计算的驱动器输入作为约束
-    vars_lowerbound[a_start] = a_prev;
-    vars_upperbound[a_start] = a_prev;
-    vars_lowerbound[delta_start] = delta_prev;
-    vars_upperbound[delta_start] = delta_prev;
-
-    // 设置约束条件的的最大和最小值，除了初始状态其他约束都为0
-    Dvector constraints_lowerbound(n_constraints);
-    Dvector constraints_upperbound(n_constraints);
-    for (int i = 0; i < n_constraints; i++) {
-        constraints_lowerbound[i] = 0;
-        constraints_upperbound[i] = 0;
-    }
-    constraints_lowerbound[x_start] = x;
-    constraints_lowerbound[y_start] = y;
-    constraints_lowerbound[psi_start] = psi;
-    constraints_lowerbound[v_start] = v;
-    constraints_lowerbound[cte_start] = cte;
-    constraints_lowerbound[epsi_start] = epsi;
-
-    constraints_upperbound[x_start] = x;
-    constraints_upperbound[y_start] = y;
-    constraints_upperbound[psi_start] = psi;
-    constraints_upperbound[v_start] = v;
-    constraints_upperbound[cte_start] = cte;
-    constraints_upperbound[epsi_start] = epsi;
+    // 【3】加速度的范围-—100%
+   Dvector vars_lowerbound(n_vars);
+   Dvector vars_upperbound(n_vars);
+   for (int i = 0; i < delta_start; i++) {
+       vars_lowerbound[i] = -1.0e19;
+       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;
+   }
+   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
+   }
+   // 设置约束条件的的最大和最小值，除了初始状态其他约束都为0
+   Dvector constraints_lowerbound(n_constraints);
+   Dvector constraints_upperbound(n_constraints);
+   for (int i = 0; i < n_constraints; i++) {
+     constraints_lowerbound[i] = 0;
+     constraints_upperbound[i] = 0;
+   }
+   constraints_lowerbound[x_start] = x;
+   constraints_lowerbound[y_start] = y;
+   constraints_lowerbound[psi_start] = psi;
+   constraints_lowerbound[v_start] = v;
+   constraints_lowerbound[cte_start] = cte;
+   constraints_lowerbound[epsi_start] = epsi;
+
+   constraints_upperbound[x_start] = x;
+   constraints_upperbound[y_start] = y;
+   constraints_upperbound[psi_start] = psi;
+   constraints_upperbound[v_start] = v;
+   constraints_upperbound[cte_start] = cte;
+   constraints_upperbound[epsi_start] = epsi;
 
     // Object that computes objective and constraints
     FG_eval fg_eval(coeffs, ref_speed);
@@ -203,9 +197,9 @@ vector<vector<double>> MPC::Solve(Eigen::VectorXd x0, Eigen::VectorXd coeffs) {
 
     // 计算这个问题
     CppAD::ipopt::solve_result<Dvector> solution;
-    CppAD::ipopt::solve<Dvector, FG_eval>(options, vars, vars_lowerbound,
-            vars_upperbound, constraints_lowerbound, constraints_upperbound,
-            fg_eval, solution);
+    CppAD::ipopt::solve<Dvector, FG_eval>(
+        options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
+        constraints_upperbound, fg_eval, solution);
 
     //
     // Check some of the solution values
@@ -216,22 +210,16 @@ vector<vector<double>> MPC::Solve(Eigen::VectorXd x0, Eigen::VectorXd coeffs) {
     auto cost = solution.obj_value;
     //std::cout << "Cost " << cost << std::endl;
 
-    // 返回预测出的轨迹点状态
-    vector<double> X;
-    vector<double> Y;
-    vector<double> Delta;
-    vector<double> A;
-    for (auto i = 0; i < N - 1; i++) {
-        X.push_back(solution.x[x_start + i]);
-        Y.push_back(solution.x[y_start + i]);
-        Delta.push_back(solution.x[delta_start + i]);
-        A.push_back(solution.x[a_start + i]);
+    vector<double> result;
+
+    result.push_back(solution.x[delta_start]);
+    result.push_back(solution.x[a_start]);
+
+    for (int i = 0; i < N-1; i++)
+    {
+       result.push_back(solution.x[x_start + i + 1]);
+       result.push_back(solution.x[y_start + i + 1]);
     }
-    vector<vector<double>> result;
-    result.push_back(X);
-    result.push_back(Y);
-    result.push_back(Delta);
-    result.push_back(A);
     return result;
 }
 

src/common/mpc.h

@@ -27,12 +27,11 @@ class MPC {
 
   double delta_prev {0};
   double a_prev {0.1};
-  int latency = 2;
   double ref_speed = 0;
 
   // Solve the model given an initial state and polynomial coefficients.
   // Return the first actuatotions.
-  vector<vector<double>> Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs);
+ vector<double> Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs);
 };
 
 

src/decision/decision.cpp

@@ -188,7 +188,7 @@ void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
 //自动生成测试用例
 void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
 {
-    if (iAbandon< 30){
+    if (iAbandon< 20){
         iAbandon ++;
         return;
     }
@@ -451,7 +451,7 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
      state << 0, 0, 0, v, cte, epsi;
      // 计算方向和油门，范围都在[-1, 1]之间
      // 这个地方有延迟，取第三个
-     vector<vector<double>> result = mpc.Solve(state, coeffs);
+     auto vars = mpc.Solve(state, coeffs);
      // 展示MPC预测的路径
 //     vector<double> mpc_x_vals=result[0];
 //     vector<double> mpc_y_vals=result[1];
@@ -465,9 +465,10 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
 //             <<"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;
+     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);
@@ -475,8 +476,8 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
      decision::controlMsg control;
      control.nowSpeed = fCarNowSpeed;
      control.refSpeed = mpc.ref_speed;
-     control.steer =  mpc.delta_prev;
-     control.throttle = mpc.a_prev;
+     control.steer =  steer_value;
+     control.throttle = throttle_value;
      control.cte = cte;
      Control_pub.publish(control);
 }
@@ -484,7 +485,7 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
 unsigned int Decision::SpeedControl(double &speed)
 {
      canbus::PedalActCtrReq srv;
-     if (mpc.a_prev >0){
+     if (speed >0){
          srv.request.uiAccPsngCtrReq =(unsigned int)(speed *10);
          srv.request.uiBrkPsngCtrReq =0;
      }

src/launch/robot.launch

@@ -11,7 +11,7 @@ ns = "NAME_SPACE" 这个属性可以让你在自定义的命名空间里运行
     <arg name="port" value="9090"/>
  </include>
 <node pkg="decision" name="decision" type="decision" output="screen" required="true"/>
-<!--<node pkg="perception" name="perception" type="perception" output="screen" respawn="true"/>-->
+<node pkg="perception" name="perception" type="perception" output="screen" respawn="true"/>
 <node pkg="localization" name="localization" type="localization" output="screen" respawn="true"/>
 <node pkg="canbus" name="canbus" type="canbus" output="screen" respawn="true"/>
 </launch>

src/perception/perception.cpp

@@ -1,7 +1,6 @@
 #include "perception.h"
 #include "../common/message.h"
 #include "../common/canmsgobdmsgid.h"
-#include "../canbus/candatatype.h"
 #include "perception/aesMsg.h"
 #include "perception/slMsg.h"
 #include "perception/seMsg.h"
@@ -14,6 +13,8 @@
 namespace  robot {
 namespace perception {
 
+#define CAN_MSG_LEN 8
+
 
 std::string Perception::Name() const { return "perception"; }
 
@@ -71,69 +72,69 @@ void Perception::Stop()
 
 void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
 {
-    LOG(INFO) << " Msg CanData :" << "ID:" << msg->id << ",Data:" << msg->data;
-
+    unsigned char ucCanMsgSrc[8] = { 0 };
+    HexStrToByte(msg->data.c_str(), ucCanMsgSrc, 16);
+    CharReverse(ucCanMsgSrc,8);
     switch (msg->id) {
-
     case robot::common::ActuatorEnableStatus:
     {
-        PublishState_6E5(msg);
+        PublishState_6E5(ucCanMsgSrc);
     }
         break;
     case robot::common::RSysInfoBack2:
     {
-        PublishState_6ED(msg);
+        PublishState_6ED(ucCanMsgSrc);
     }
         break;
     case robot::common::SysLearnBack:
     {
-        PublishState_6EE(msg);
+        PublishState_6EE(ucCanMsgSrc);
     }
         break;
     case robot::common::SysWarnErr:
     {
-        SoftStopControl();
-        PublishState_6D1(msg);
+        //SoftStopControl();
+        PublishState_6D1(ucCanMsgSrc);
     }
         break;
     case robot::common::AccErr:
     {
-        SoftStopControl();
-        PublishState_6D2_6(msg, 2, bus_pub_6D2);
+        //SoftStopControl();
+        PublishState_6D2_6(ucCanMsgSrc, 2, bus_pub_6D2);
     }
         break;
     case robot::common::BrkErr:
     {
-        SoftStopControl();
-        PublishState_6D2_6(msg, 3, bus_pub_6D3);
+        //SoftStopControl();
+        PublishState_6D2_6(ucCanMsgSrc, 3, bus_pub_6D3);
     }
         break;
     case robot::common::XsftErr:
     {
-        SoftStopControl();
-        PublishState_6D2_6(msg, 4, bus_pub_6D4);
+        //SoftStopControl();
+        PublishState_6D2_6(ucCanMsgSrc, 4, bus_pub_6D4);
     }
         break;
     case robot::common::YsftErr:
     {
-        SoftStopControl();
-        PublishState_6D2_6(msg, 5, bus_pub_6D5);
+        //SoftStopControl();
+        PublishState_6D2_6(ucCanMsgSrc, 5, bus_pub_6D5);
     }
         break;
     case robot::common::StrErr:
     {
-        SoftStopControl();
-        PublishState_6D2_6(msg, 6, bus_pub_6D6);
+        //SoftStopControl();
+        PublishState_6D2_6(ucCanMsgSrc, 6, bus_pub_6D6);
     }
         break;
     case robot::common::EstopInfo:
     {
-        PublishState_2E0(msg);
+        PublishState_2E0(ucCanMsgSrc);
     }
         break;
     case robot::common::EstopErr:
     {
-        PublishState_2E1(msg);
+        PublishState_2E1(ucCanMsgSrc);
     }
         break;
     default:
@@ -141,19 +142,17 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
     }
 }
 
-bool Perception::PublishState_6E5(const canbus::canMsg::ConstPtr &msg)
+bool Perception::PublishState_6E5(unsigned char* msg)
 {
-    static robot::canbusSpace::T_STATE_OUT_6E5 s_state_6E5;
-
-    int copySize;
-    if(msg->data.size() < sizeof(s_state_6E5))
-        copySize = msg->data.size();
-    else
-        copySize = sizeof(s_state_6E5);
+//    int copySize;
+//    if(msg.size() < sizeof(s_state_6E5))
+//        copySize = msg->data.size();
+//    else
+//        copySize = sizeof(s_state_6E5);
 
     robot::canbusSpace::T_STATE_OUT_6E5 state_6E5;
     memset(&state_6E5, 0 ,sizeof(state_6E5));
-    memcpy(&state_6E5, msg->data.c_str(), copySize);
+    memcpy(&state_6E5, msg, CAN_MSG_LEN);
 
     if(state_6E5.uiAccClchCtrStat != s_state_6E5.uiAccClchCtrStat || state_6E5.uiAccMotEnStat != s_state_6E5.uiAccMotEnStat ||
             state_6E5.uiBrkHVFFStat != s_state_6E5.uiBrkHVFFStat || state_6E5.uiBrkMotEnStat != s_state_6E5.uiBrkMotEnStat ||
@@ -162,9 +161,9 @@ bool Perception::PublishState_6E5(const canbus::canMsg::ConstPtr &msg)
             state_6E5.uiYSftMotEnStat != s_state_6E5.uiYSftMotEnStat)
     {
 
-        memcpy(&s_state_6E5, msg->data.c_str(), copySize);
-        if(msg->data.size() < sizeof(s_state_6E5))
-            memset((char*)&s_state_6E5 + msg->data.size(), 0, sizeof(s_state_6E5) - copySize);
+        memcpy(&s_state_6E5, msg, CAN_MSG_LEN);
+//        if(msg->data.size() < sizeof(s_state_6E5))
+//            memset((char*)&s_state_6E5 + msg->data.size(), 0, sizeof(s_state_6E5) - copySize);
         if(bus_pub_6E5)
         {
             ::perception::aesMsg msg;
@@ -193,19 +192,19 @@ bool Perception::PublishState_6E5(const canbus::canMsg::ConstPtr &msg)
     }
 }
 
-bool Perception::PublishState_6EE(const canbus::canMsg::ConstPtr &msg)
+bool Perception::PublishState_6EE(unsigned char* msg)
 {
-    static robot::canbusSpace::T_STATE_OUT_6EE s_state_6EE;
 
-    int copySize;
-    if(msg->data.size() < sizeof(s_state_6EE))
-        copySize = msg->data.size();
-    else
-        copySize = sizeof(s_state_6EE);
+
+//    int copySize;
+//    if(msg->data.size() < sizeof(s_state_6EE))
+//        copySize = msg->data.size();
+//    else
+//        copySize = sizeof(s_state_6EE);
 
     robot::canbusSpace::T_STATE_OUT_6EE state_6EE;
     memset(&state_6EE, 0 ,sizeof(state_6EE));
-    memcpy(&state_6EE, msg->data.c_str(), copySize);
+    memcpy(&state_6EE,  msg, CAN_MSG_LEN);
 
     if(state_6EE.uiAcc0LrnStat != s_state_6EE.uiAcc0LrnStat || state_6EE.uiAccLrnStat != s_state_6EE.uiAccLrnStat ||
             state_6EE.uiAccMaxLrnStat != s_state_6EE.uiAccMaxLrnStat || state_6EE.uiBrk0LrnStat != s_state_6EE.uiBrk0LrnStat ||
@@ -220,9 +219,9 @@ bool Perception::PublishState_6EE(const canbus::canMsg::ConstPtr &msg)
             state_6EE.uiUpLrnStat != s_state_6EE.uiUpLrnStat)
     {
 
-        memcpy(&s_state_6EE, msg->data.c_str(), copySize);
-        if(msg->data.size() < sizeof(s_state_6EE))
-            memset((char*)&s_state_6EE + msg->data.size(), 0, sizeof(s_state_6EE) - copySize);
+        memcpy(&s_state_6EE,  msg, CAN_MSG_LEN);
+//        if(msg->data.size() < sizeof(s_state_6EE))
+//            memset((char*)&s_state_6EE + msg->data.size(), 0, sizeof(s_state_6EE) - copySize);
         if(bus_pub_6EE)
         {
             ::perception::slMsg msg;
@@ -263,19 +262,19 @@ bool Perception::PublishState_6EE(const canbus::canMsg::ConstPtr &msg)
     }
 }
 
-bool Perception::PublishState_6ED(const canbus::canMsg::ConstPtr &msg)
+bool Perception::PublishState_6ED(unsigned char* msg)
 {
-    static robot::canbusSpace::T_STATE_OUT_6ED s_state_6ED;
 
-    int copySize;
-    if(msg->data.size() < sizeof(s_state_6ED))
-        copySize = msg->data.size();
-    else
-        copySize = sizeof(s_state_6ED);
+
+//    int copySize;
+//    if(msg->data.size() < sizeof(s_state_6ED))
+//        copySize = msg->data.size();
+//    else
+//        copySize = sizeof(s_state_6ED);
 
     robot::canbusSpace::T_STATE_OUT_6ED state_6ED;
     memset(&state_6ED, 0 ,sizeof(state_6ED));
-    memcpy(&state_6ED, msg->data.c_str(), copySize);
+    memcpy(&state_6ED,  msg, CAN_MSG_LEN);
 
     if(state_6ED.uiAccCtrAlive != s_state_6ED.uiAccCtrAlive || state_6ED.uiAccMotClbr != s_state_6ED.uiAccMotClbr ||
             state_6ED.uiBrkCtrAlive != s_state_6ED.uiBrkCtrAlive || state_6ED.uiBrkMotClbr != s_state_6ED.uiBrkMotClbr ||
@@ -286,9 +285,9 @@ bool Perception::PublishState_6ED(const canbus::canMsg::ConstPtr &msg)
             state_6ED.uiYSftArmCtrAlive != s_state_6ED.uiYSftArmCtrAlive || state_6ED.uiYSftMotClbr != s_state_6ED.uiYSftMotClbr)
     {
 
-        memcpy(&s_state_6ED, msg->data.c_str(), copySize);
-        if(msg->data.size() < sizeof(s_state_6ED))
-            memset((char*)&s_state_6ED + msg->data.size(), 0, sizeof(s_state_6ED) - copySize);
+        memcpy(&s_state_6ED,  msg, CAN_MSG_LEN);
+//        if(msg->data.size() < sizeof(s_state_6ED))
+//            memset((char*)&s_state_6ED + msg->data.size(), 0, sizeof(s_state_6ED) - copySize);
         if(bus_pub_6ED)
         {
             ::perception::si2Msg msg;
@@ -322,19 +321,19 @@ bool Perception::PublishState_6ED(const canbus::canMsg::ConstPtr &msg)
     }
 }
 
-bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
+bool Perception::PublishState_6D1(unsigned char* msg)
 {
-    static robot::canbusSpace::T_STATE_OUT_6D1 s_state_6D1;
 
-    int copySize;
-    if(msg->data.size() < sizeof(s_state_6D1))
-        copySize = msg->data.size();
-    else
-        copySize = sizeof(s_state_6D1);
+
+//    int copySize;
+//    if(msg->data.size() < sizeof(s_state_6D1))
+//        copySize = msg->data.size();
+//    else
+//        copySize = sizeof(s_state_6D1);
 
     robot::canbusSpace::T_STATE_OUT_6D1 state_6D1;
     memset(&state_6D1, 0 ,sizeof(state_6D1));
-    memcpy(&state_6D1, msg->data.c_str(), copySize);
+    memcpy(&state_6D1, msg, CAN_MSG_LEN);
 
     if(state_6D1.uiSysErra != s_state_6D1.uiSysErra || state_6D1.uiSysErrb != s_state_6D1.uiSysErrb || state_6D1.uiSysErrc != s_state_6D1.uiSysErrc ||
             state_6D1.uiSysErrd != s_state_6D1.uiSysErrd || state_6D1.uiSysErre != s_state_6D1.uiSysErre || state_6D1.uiSysErrf != s_state_6D1.uiSysErrf ||
@@ -347,9 +346,9 @@ bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
             state_6D1.uiSysErry != s_state_6D1.uiSysErry || state_6D1.uiSysErrz != s_state_6D1.uiSysErrz)
     {
 
-        memcpy(&s_state_6D1, msg->data.c_str(), copySize);
-        if(msg->data.size() < sizeof(s_state_6D1))
-            memset((char*)&s_state_6D1 + msg->data.size(), 0, sizeof(s_state_6D1) - copySize);
+        memcpy(&s_state_6D1, msg, CAN_MSG_LEN);
+//        if(msg->data.size() < sizeof(s_state_6D1))
+//            memset((char*)&s_state_6D1 + msg->data.size(), 0, sizeof(s_state_6D1) - copySize);
         if(bus_pub_6D1)
         {
             ::perception::seMsg msg;
@@ -395,13 +394,9 @@ bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
     }
 }
 
-bool Perception::PublishState_6D2_6(const canbus::canMsg::ConstPtr &msg, int type, const ros::Publisher &pub)
+bool Perception::PublishState_6D2_6(unsigned char* msg, int type, const ros::Publisher &pub)
 {
-    static robot::canbusSpace::T_ACC_ERR s_state_6D2;
-    static robot::canbusSpace::T_BRK_ERR s_state_6D3;
-    static robot::canbusSpace::T_XSFT_ERR s_state_6D4;
-    static robot::canbusSpace::T_YSFT_ERR s_state_6D5;
-    static robot::canbusSpace::T_STR_ERR s_state_6D6;
+
 
     robot::canbusSpace::T_ACC_ERR *pErr;
     switch (type) {
@@ -426,16 +421,16 @@ bool Perception::PublishState_6D2_6(const canbus::canMsg::ConstPtr &msg, int typ
         break;
     }
 
-    int copySize;
-    if(msg->data.size() < sizeof(s_state_6D2))
-        copySize = msg->data.size();
-    else
-        copySize = sizeof(s_state_6D2);
+//    int copySize;
+//    if(msg->data.size() < sizeof(s_state_6D2))
+//        copySize = msg->data.size();
+//    else
+//        copySize = sizeof(s_state_6D2);
 
     //6D2_2 6D2_3 6D2_4 6D2_5 6D2_6 数据结构相同
     robot::canbusSpace::T_ACC_ERR state_6D2;
     memset(&state_6D2, 0 ,sizeof(state_6D2));
-    memcpy(&state_6D2, msg->data.c_str(), copySize);
+    memcpy(&state_6D2, msg, CAN_MSG_LEN);
 
     if(state_6D2.uiAccErra != pErr->uiAccErra || state_6D2.uiAccErraa != pErr->uiAccErraa || state_6D2.uiAccErrab != pErr->uiAccErrab ||
             state_6D2.uiAccErrac != pErr->uiAccErrac || state_6D2.uiAccErrad != pErr->uiAccErrad || state_6D2.uiAccErrae != pErr->uiAccErrae ||
@@ -451,9 +446,28 @@ bool Perception::PublishState_6D2_6(const canbus::canMsg::ConstPtr &msg, int typ
             state_6D2.uiAccErry != pErr->uiAccErry || state_6D2.uiAccErrz != pErr->uiAccErrz)
     {
 
-        memcpy(pErr, msg->data.c_str(), copySize);
-        if(msg->data.size() < sizeof(s_state_6D2))
-            memset((char*)&s_state_6D2 + msg->data.size(), 0, sizeof(s_state_6D2) - copySize);
+
+        switch (type) {
+        case 2:
+             memcpy(&s_state_6D2, msg, CAN_MSG_LEN);
+            break;
+        case 3:
+             memcpy(&s_state_6D3, msg, CAN_MSG_LEN);
+            break;
+        case 4:
+             memcpy(&s_state_6D4, msg, CAN_MSG_LEN);
+            break;
+        case 5:
+             memcpy(&s_state_6D5, msg, CAN_MSG_LEN);
+            break;
+        case 6:
+             memcpy(&s_state_6D6, msg, CAN_MSG_LEN);
+            break;
+        default:
+            LOG(INFO) << Name() << " PublishState_6D2_6 " << type << " type is unvalid";
+            return false;
+            break;
+        }
         if(pub)
         {
             ::perception::cecMsg msg;
@@ -508,29 +522,27 @@ bool Perception::PublishState_6D2_6(const canbus::canMsg::ConstPtr &msg, int typ
     }
 }
 
-bool Perception::PublishState_2E0(const canbus::canMsg::ConstPtr &msg)
+bool Perception::PublishState_2E0(unsigned char* msg)
 {
-    static robot::canbusSpace::T_ESTOP_2E0 s_state_2E0;
-
-    int copySize;
-    if(msg->data.size() < sizeof(s_state_2E0))
-    {
-        copySize = msg->data.size();
-    }
-    else
-        copySize = sizeof(s_state_2E0);
+//    int copySize;
+//    if(msg->data.size() < sizeof(s_state_2E0))
+//    {
+//        copySize = msg->data.size();
+//    }
+//    else
+//        copySize = sizeof(s_state_2E0);
 
     robot::canbusSpace::T_ESTOP_2E0 state_2E0;
     memset(&state_2E0, 0 ,sizeof(state_2E0));
-    memcpy(&state_2E0, msg->data.c_str(), copySize);
+    memcpy(&state_2E0, msg, CAN_MSG_LEN);
 
     if(state_2E0.uiEstpAbnStat != s_state_2E0.uiEstpAbnStat || state_2E0.uiEstpAlive != state_2E0.uiEstpAlive ||
             state_2E0.uiEstpEnStat != s_state_2E0.uiEstpEnStat || state_2E0.uiPmpEnStat != s_state_2E0.uiPmpEnStat )
     {
 
-        memcpy(&s_state_2E0, msg->data.c_str(), copySize);
-        if(msg->data.size() < sizeof(s_state_2E0))
-            memset((char*)&s_state_2E0 + msg->data.size(), 0, sizeof(s_state_2E0) - copySize);
+        memcpy(&s_state_2E0,msg, CAN_MSG_LEN);
+//        if(msg->data.size() < sizeof(s_state_2E0))
+//            memset((char*)&s_state_2E0 + msg->data.size(), 0, sizeof(s_state_2E0) - copySize);
         if(bus_pub_2E0)
         {
             ::perception::eiMsg msg;
@@ -554,19 +566,17 @@ bool Perception::PublishState_2E0(const canbus::canMsg::ConstPtr &msg)
     }
 }
 
-bool Perception::PublishState_2E1(const canbus::canMsg::ConstPtr &msg)
+bool Perception::PublishState_2E1(unsigned char* msg)
 {
-    static robot::canbusSpace::T_ESTOP_2E1 s_state_2E1;
-
-    int copySize;
-    if(msg->data.size() < sizeof(s_state_2E1))
-        copySize = msg->data.size();
-    else
-        copySize = sizeof(s_state_2E1);
+//    int copySize;
+//    if(msg->data.size() < sizeof(s_state_2E1))
+//        copySize = msg->data.size();
+//    else
+//        copySize = sizeof(s_state_2E1);
 
     robot::canbusSpace::T_ESTOP_2E1 state_2E1;
     memset(&state_2E1, 0 ,sizeof(state_2E1));
-    memcpy(&state_2E1, msg->data.c_str(), copySize);
+    memcpy(&state_2E1, msg, CAN_MSG_LEN);
 
     if(state_2E1.uiEspErra != s_state_2E1.uiEspErra || state_2E1.uiEspErrb != s_state_2E1.uiEspErrb || state_2E1.uiEspErrc != s_state_2E1.uiEspErrc ||
             state_2E1.uiEspErrd != s_state_2E1.uiEspErrd || state_2E1.uiEspErre != s_state_2E1.uiEspErre || state_2E1.uiEspErrf != s_state_2E1.uiEspErrf ||
@@ -574,9 +584,9 @@ bool Perception::PublishState_2E1(const canbus::canMsg::ConstPtr &msg)
             state_2E1.uiEspErrj != s_state_2E1.uiEspErrj)
     {
 
-        memcpy(&s_state_2E1, msg->data.c_str(), copySize);
-        if(msg->data.size() < sizeof(s_state_2E1))
-            memset((char*)&s_state_2E1 + msg->data.size(), 0, sizeof(s_state_2E1) - copySize);
+        memcpy(&s_state_2E1,  msg, CAN_MSG_LEN);
+//        if(msg->data.size() < sizeof(s_state_2E1))
+//            memset((char*)&s_state_2E1 + msg->data.size(), 0, sizeof(s_state_2E1) - copySize);
         if(bus_pub_2E1)
         {
             ::perception::eeMsg msg;

src/perception/perception.h

@@ -5,6 +5,7 @@
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 #include <canbus/canMsg.h>
+#include "../canbus/candatatype.h"
 
 namespace robot {
 namespace perception {
@@ -39,6 +40,10 @@ public:
     */
     void Stop() override;
 
+
+
+private:
+
     /**
     * @brief Can topic data callback function
     */
@@ -49,47 +54,101 @@ public:
     * @brief Publish State_6E5 topic
     */
 
-    bool PublishState_6E5(const canbus::canMsg::ConstPtr& msg);
+    bool PublishState_6E5(unsigned char* msg);
 
     /**
     * @brief Publish State_6ED topic
     */
 
-    bool PublishState_6ED(const canbus::canMsg::ConstPtr& msg);
+    bool PublishState_6ED(unsigned char* msg);
 
     /**
     * @brief Publish State_6EE topic
     */
 
-    bool PublishState_6EE(const canbus::canMsg::ConstPtr& msg);
+    bool PublishState_6EE(unsigned char* msg);
 
     /**
     * @brief Publish State_6D1 topic
     */
 
-    bool PublishState_6D1(const canbus::canMsg::ConstPtr& msg);
+    bool PublishState_6D1(unsigned char* msg);
 
     /**
     * @brief Publish State_6D2~State_6D6 topic
     */
 
-    bool PublishState_6D2_6(const canbus::canMsg::ConstPtr& msg, int type, const ros::Publisher &pub);
+    bool PublishState_6D2_6(unsigned char* msg, int type, const ros::Publisher &pub);
 
     /**
     * @brief Publish State_2E0 topic
     */
 
-    bool PublishState_2E0(const canbus::canMsg::ConstPtr& msg);
+    bool PublishState_2E0(unsigned char* msg);
 
     /**
     * @brief Publish State_2E1 topic
     */
 
-    bool PublishState_2E1(const canbus::canMsg::ConstPtr& msg);
+    bool PublishState_2E1(unsigned char* msg);
 
     bool SoftStopControl();
 
+    //字符串反转
+    inline void CharReverse(unsigned char * s, int n)
+    {
+        for (int i = 0,j=n-1; i < j; i++,j--)
+        {
+            char c = s[i];
+            s[i] = s[j];
+            s[j] = c;
+        }
+
+    }
+
+
+    //十六进制字符串转换成字节数组
+    inline void HexStrToByte(const char* source, unsigned char* dest, int sourceLen)
+    {
+        short i;
+        unsigned char highByte, lowByte;
+        for (i = 0; i < sourceLen; i += 2)
+        {
+            highByte = toupper(source[i]);
+            lowByte = toupper(source[i + 1]);
+
+            if (highByte > 0x39)
+                highByte -= 0x37;
+            else
+                highByte -= 0x30;
+
+            if (lowByte > 0x39)
+                lowByte -= 0x37;
+            else
+                lowByte -= 0x30;
+
+            dest[i / 2] = (highByte << 4) | lowByte;
+        }
+    }
+
 private:
+     robot::canbusSpace::T_STATE_OUT_6E5 s_state_6E5;
+
+     robot::canbusSpace::T_ESTOP_2E1 s_state_2E1;
+
+     robot::canbusSpace::T_ESTOP_2E0 s_state_2E0;
+
+     robot::canbusSpace::T_STATE_OUT_6EE s_state_6EE;
+
+     robot::canbusSpace::T_STATE_OUT_6ED s_state_6ED;
+
+     robot::canbusSpace::T_STATE_OUT_6D1 s_state_6D1;
+
+     robot::canbusSpace::T_ACC_ERR s_state_6D2;
+     robot::canbusSpace::T_BRK_ERR s_state_6D3;
+     robot::canbusSpace::T_XSFT_ERR s_state_6D4;
+     robot::canbusSpace::T_YSFT_ERR s_state_6D5;
+     robot::canbusSpace::T_STR_ERR s_state_6D6;
 
      ros::Publisher bus_pub_6E5;
      ros::Publisher bus_pub_6ED;