修改注释，优化导航定位代码

src/canbus/canbus.cpp

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2019 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: limengqi
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 
@@ -77,10 +77,10 @@ int Canbus::Start() {
       if (unBaudrate) {
               errno = funCAN_Init(pcan_handle, unBaudrate, nExtended);
               if (errno) {
-                  LOG(ERROR)<<Name()<<": CAN_Init()";
+                  LOG(ERROR)<<Name()<<": CAN_Init() error.";
               }
               else
-                  LOG(INFO)<<" Device Info: " << szDevNode <<"; CAN_BAUD_1M; CAN_INIT_TYPE_ST";
+                  LOG(INFO)<<" Device Info: " << szDevNode <<"Baudrate:"<<unBaudrate;
           }
   }
   else
@@ -94,8 +94,8 @@ int Canbus::Start() {
 
   // this location is necessary
 
-  canRCUData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::CanDataAnalysis, 2000);
-  canObdData_pub = node_handle_->advertise<canbus::obdMsg>(robot::common::ObdData, 2000);
+  canRCUData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::CanDataAnalysis, 1);
+  canObdData_pub = node_handle_->advertise<canbus::obdMsg>(robot::common::ObdData, 1);
 
   ros::ServiceServer ActuatorEnableService = node_handle_->advertiseService(robot::common::ActuatorEnableService,
                                                                 &Canbus::ActuatorEnableRequest, this);
@@ -416,9 +416,9 @@ bool Canbus::PedalActCtrRequest(canbus::PedalActCtrReq::Request &req,
         request.uiBrkPsngCtrReq = req.uiBrkPsngCtrReq;
 
         MessageCoder::CanMsgPackage((void*)&request, robot::common::PedalActCtrReq, &canPedalActCtrReqMsg);
-        if (write_data(&canPedalActCtrReqMsg) != ROBOT_SUCCESS)
+        if (write_data(&canPedalActCtrReqMsg)!=ROBOT_SUCCESS)
         {
-            res.uiReturn = 0; // =0
+            res.uiReturn = 0;  //= 0
             return true;
         }
         res.uiReturn = 1;

src/canbus/canbus.h

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: limengqi
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/canbus/canbus_app.cpp

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: limengqi
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #include "../common/robotapp.h"

src/canbus/candatatype.h

@@ -1,11 +1,8 @@
-#pragma once
-//============================================================================
-// Name        : canDataType.h
-// Author      :
-// Version     :
-// Copyright   : SIASUN
-// Description : can2.0协议数据结构
-//============================================================================
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: limengqi
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 
 #ifndef CAN_DATA_TYPE_H_
 #define CAN_DATA_TYPE_H_

src/canbus/messagecoder.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: limengqi
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "messagecoder.h"
 #include"candatatype.h"
 #include"string.h"
@@ -23,10 +28,10 @@ void MessageCoder::CharReverse(unsigned char * s, int n)
 void MessageCoder::CanMsgAnalyseID_5F2(unsigned char * ucData, T_ACTUATOR_ENABLE_REQUEST *tActEnableReq)
 {
     T_CONTROL_CMD_5F2 tCtrCmd5F2 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tCtrCmd5F2, ucCanMsgSrc, CAN_MSG_LEN);
+//    unsigned char ucCanMsgSrc[8] = { 0 };
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tCtrCmd5F2, ucData, CAN_MSG_LEN);
     tActEnableReq->uiAccClchCtrReq = tCtrCmd5F2.uiAccClchCtrReq;
     tActEnableReq->uiAccMotEnReq = tCtrCmd5F2.uiAccMotEnReq;
     tActEnableReq->uiBrkMotEnReq = tCtrCmd5F2.uiBrkMotEnReq;
@@ -46,10 +51,10 @@ void MessageCoder::CanMsgAnalyseID_5F5(unsigned char * ucData, T_STR_ACT_CTR_REQ
     T_CONTROL_CMD_5F5 tCtrCmd5F5 = { 0 };
     T_CONTROL_CMD_5F5_UNION tCtrCmd5F5Union;
 
-    unsigned char ucCanMsgSrc[8] = { 0 };
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tCtrCmd5F5, ucCanMsgSrc, CAN_MSG_LEN);
+//    unsigned char ucCanMsgSrc[8] = { 0 };
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tCtrCmd5F5, ucData, CAN_MSG_LEN);
 
     memset(&tCtrCmd5F5Union, 0, sizeof(T_CONTROL_CMD_5F5_UNION));
     tCtrCmd5F5Union.tCtrCmd5F5Src.iStrSpdCtrReqL = tCtrCmd5F5.iStrSpdCtrReqL;
@@ -65,11 +70,11 @@ void MessageCoder::CanMsgAnalyseID_5F5(unsigned char * ucData, T_STR_ACT_CTR_REQ
 void MessageCoder::CanMsgAnalyseID_6F1(unsigned char * ucData, T_VEHICLE_STATUS_1 * tVehicleStat1)
 {
     T_STATE_IN_6F1 tStatIn6F1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatIn6F1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatIn6F1, ucData, CAN_MSG_LEN);
 
     tVehicleStat1->uiAccPsng = tStatIn6F1.uiAccPsng;
     tVehicleStat1->uiBrkPsng = tStatIn6F1.uiBrkPsng;
@@ -86,11 +91,11 @@ void MessageCoder::CanMsgAnalyseID_6F2(unsigned char * ucData, T_VEHICLE_STATUS_
 {
     T_STATE_IN_6F2 tStatIn6F2 = { 0 };
     T_STATE_IN_6F2_UNION tStatIn6F2Union;
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatIn6F2, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatIn6F2, ucData, CAN_MSG_LEN);
 
     memset(&tStatIn6F2Union, 0, sizeof(T_STATE_IN_6F2_UNION));
     tStatIn6F2Union.tStateIn6F2Src.iStrAngGrdL = tStatIn6F2.iStrAngGrdL;
@@ -106,11 +111,11 @@ void MessageCoder::CanMsgAnalyseID_6F2(unsigned char * ucData, T_VEHICLE_STATUS_
 void MessageCoder::CanMsgAnalyseID_6F3(unsigned char * ucData, T_VEHICLE_STATUS_3 * tVehicleStat3)
 {
     T_STATE_IN_6F3 tStatIn6F3 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatIn6F3, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatIn6F3, ucData, CAN_MSG_LEN);
 
     tVehicleStat3->uiVehBatQut = tStatIn6F3.uiVehBatQut;
     tVehicleStat3->uiVehBatCur = tStatIn6F3.uiVehBatCur;
@@ -121,11 +126,11 @@ void MessageCoder::CanMsgAnalyseID_6F3(unsigned char * ucData, T_VEHICLE_STATUS_
 void MessageCoder::CanMsgAnalyseID_6F4(unsigned char * ucData, T_COMPUTER_STATUS * tComputerStat)
 {
     T_STATE_IN_6F4 tStatIn6F4 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatIn6F4, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatIn6F4, ucData, CAN_MSG_LEN);
 
     tComputerStat->uiCmpStat = tStatIn6F4.uiCmpStat;
 }
@@ -134,11 +139,11 @@ void MessageCoder::CanMsgAnalyseID_6F4(unsigned char * ucData, T_COMPUTER_STATUS
 void MessageCoder::CanMsgAnalyseID_6E0(unsigned char * ucData, T_RBT_ACT_PSNG * tRbtActPsng)
 {
     T_STATE_OUT_6E0 tStatout6E0 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E0, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E0, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6E0_UNION tStateOut6E0Union;
     memset(&tStateOut6E0Union, 0, sizeof(T_STATE_OUT_6E0_UNION));
@@ -155,11 +160,11 @@ void MessageCoder::CanMsgAnalyseID_6E0(unsigned char * ucData, T_RBT_ACT_PSNG *
 void MessageCoder::CanMsgAnalyseID_6E1(unsigned char * ucData, T_ACC_BRK_PSNG * tAccBrkPsng)
 {
     T_STATE_OUT_6E1 tStatout6E1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E1, ucData, CAN_MSG_LEN);
 
     tAccBrkPsng->uiClbtAccPsng0 = tStatout6E1.uiClbtAccPsng0;
     tAccBrkPsng->uiClbtAccPsngMax = tStatout6E1.uiClbtAccPsngMax;
@@ -171,11 +176,11 @@ void MessageCoder::CanMsgAnalyseID_6E1(unsigned char * ucData, T_ACC_BRK_PSNG *
 void MessageCoder::CanMsgAnalyseID_6E2(unsigned char * ucData, T_STR_LRN_PSNG * tStrLrnPsng)
 {
     T_STATE_OUT_6E2 tStatout6E2 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E2, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E2, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6E2_UNION tStateOut6E2Union;
     memset(&tStateOut6E2Union,0,sizeof(T_STATE_OUT_6E2_UNION));
@@ -191,11 +196,11 @@ void MessageCoder::CanMsgAnalyseID_6E2(unsigned char * ucData, T_STR_LRN_PSNG *
 void MessageCoder::CanMsgAnalyseID_6E3(unsigned char * ucData, T_SFT_LRN_PST_1 * tSftLrnPst1)
 {
     T_STATE_OUT_6E3 tStatout6E3 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E3, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E3, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6E3_UNION tStateOut6E3Union;
     memset(&tStateOut6E3Union,0,sizeof(T_STATE_OUT_6E3_UNION));
@@ -215,11 +220,11 @@ void MessageCoder::CanMsgAnalyseID_6E3(unsigned char * ucData, T_SFT_LRN_PST_1 *
 void MessageCoder::CanMsgAnalyseID_6E4(unsigned char * ucData, T_CTR_POW_STATUS * tCtrPowStatus)
 {
     T_STATE_OUT_6E4 tStatout6E4 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E4, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E4, ucData, CAN_MSG_LEN);
 
     tCtrPowStatus->uiAccCtrPowStat = tStatout6E4.uiAccCtrPowStat;
     tCtrPowStatus->uiBrkCtrPowStat = tStatout6E4.uiBrkCtrPowStat;
@@ -234,11 +239,11 @@ void MessageCoder::CanMsgAnalyseID_6E4(unsigned char * ucData, T_CTR_POW_STATUS
 void MessageCoder::CanMsgAnalyseID_6E5(unsigned char * ucData, T_ACT_ENABLE_STATUS * tActEnableStatus)
 {
     T_STATE_OUT_6E5 tStatout6E5 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E5, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E5, ucData, CAN_MSG_LEN);
 
     tActEnableStatus->uiAccClchCtrStat = tStatout6E5.uiAccClchCtrStat;
     tActEnableStatus->uiAccMotEnStat = tStatout6E5.uiAccMotEnStat;
@@ -255,11 +260,11 @@ void MessageCoder::CanMsgAnalyseID_6E5(unsigned char * ucData, T_ACT_ENABLE_STAT
 void MessageCoder::CanMsgAnalyseID_6E6(unsigned char * ucData, T_ACC_CTR_STATUS * tAccCtrStatus)
 {
     T_STATE_OUT_6E6 tStatout6E6 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E6, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E6, ucData, CAN_MSG_LEN);
 
     tAccCtrStatus->iAccDspSpd = tStatout6E6.iAccDspSpd;
     tAccCtrStatus->iAccMotSpd = tStatout6E6.iAccMotSpd;
@@ -270,11 +275,11 @@ void MessageCoder::CanMsgAnalyseID_6E6(unsigned char * ucData, T_ACC_CTR_STATUS
 void MessageCoder::CanMsgAnalyseID_6E7(unsigned char * ucData, T_BRK_CTR_STATUS * tBrkCtrStatus)
 {
     T_STATE_OUT_6E7 tStatout6E7 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E7, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E7, ucData, CAN_MSG_LEN);
 
     tBrkCtrStatus->iBrkDspSpd = tStatout6E7.iBrkDspSpd;
     tBrkCtrStatus->iBrkFoc = tStatout6E7.iBrkFoc;
@@ -288,11 +293,11 @@ void MessageCoder::CanMsgAnalyseID_6E7(unsigned char * ucData, T_BRK_CTR_STATUS
 void MessageCoder::CanMsgAnalyseID_6E8(unsigned char * ucData, T_CLCH_CTR_STATUS * tClchCtrStatus)
 {
     T_STATE_OUT_6E8 tStatout6E8 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E8, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E8, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6E8_UNION tStateOut6E8Union;
     memset(&tStateOut6E8Union,0,sizeof(T_STATE_OUT_6E8_UNION));
@@ -309,11 +314,11 @@ void MessageCoder::CanMsgAnalyseID_6E8(unsigned char * ucData, T_CLCH_CTR_STATUS
 void MessageCoder::CanMsgAnalyseID_6E9(unsigned char * ucData, T_STR_STATUS * tStrStatus)
 {
     T_STATE_OUT_6E9 tStatout6E9 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6E9, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6E9, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6E9_UNION tStateOut6E9Union;
     memset(&tStateOut6E9Union,0,sizeof(T_STATE_OUT_6E9_UNION));
@@ -331,11 +336,11 @@ void MessageCoder::CanMsgAnalyseID_6E9(unsigned char * ucData, T_STR_STATUS * tS
 void MessageCoder::CanMsgAnalyseID_6EA(unsigned char * ucData, T_SFT_STATUS * tSftStatus)
 {
     T_STATE_OUT_6EA tStatout6EA = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6EA, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6EA, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6EA_UNION tStateOut6EAUnion;
     memset(&tStateOut6EAUnion, 0, sizeof(T_STATE_OUT_6EA_UNION));
@@ -357,11 +362,11 @@ void MessageCoder::CanMsgAnalyseID_6EA(unsigned char * ucData, T_SFT_STATUS * tS
 void MessageCoder::CanMsgAnalyseID_6EB(unsigned char * ucData, T_SYS_INFO_1 * tSysInfo1)
 {
     T_STATE_OUT_6EB tStatout6EB = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6EB, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6EB, ucData, CAN_MSG_LEN);
 
     tSysInfo1->uiAccMotCur = tStatout6EB.uiAccMotCur;
     tSysInfo1->uiBrkMotCur = tStatout6EB.uiBrkMotCur;
@@ -376,11 +381,11 @@ void MessageCoder::CanMsgAnalyseID_6EB(unsigned char * ucData, T_SYS_INFO_1 * tS
 void MessageCoder::CanMsgAnalyseID_6ED(unsigned char * ucData, T_SYS_INFO_2 * tSysInfo2)
 {
     T_STATE_OUT_6ED tStatout6ED = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6ED, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6ED, ucData, CAN_MSG_LEN);
 
     tSysInfo2->uiAccCtrAlive = tStatout6ED.uiAccCtrAlive;
     tSysInfo2->uiAccMotClbr = tStatout6ED.uiAccMotClbr;
@@ -403,11 +408,11 @@ void MessageCoder::CanMsgAnalyseID_6ED(unsigned char * ucData, T_SYS_INFO_2 * tS
 void MessageCoder::CanMsgAnalyseID_6EE(unsigned char * ucData, T_SYS_LRN * tSysLrn)
 {
     T_STATE_OUT_6EE tStatout6EE = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6EE, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6EE, ucData, CAN_MSG_LEN);
 
     tSysLrn->uiAcc0LrnStat = tStatout6EE.uiAcc0LrnStat;
     tSysLrn->uiAccLrnStat = tStatout6EE.uiAccLrnStat;
@@ -428,11 +433,11 @@ void MessageCoder::CanMsgAnalyseID_6EE(unsigned char * ucData, T_SYS_LRN * tSysL
 void MessageCoder::CanMsgAnalyseID_6EF(unsigned char * ucData, T_SFT_LRN_PST_2 * tSFtLrnPst2)
 {
     T_STATE_OUT_6EF tStatout6EF = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6EF, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6EF, ucData, CAN_MSG_LEN);
 
     T_STATE_OUT_6EF_UNION tStateOut6EFUnion;
     memset(&tStateOut6EFUnion, 0, sizeof(T_STATE_OUT_6EF_UNION));
@@ -452,11 +457,11 @@ void MessageCoder::CanMsgAnalyseID_6EF(unsigned char * ucData, T_SFT_LRN_PST_2 *
 void MessageCoder::CanMsgAnalyseID_6D0(unsigned char * ucData, T_SYS_MONITOR * tSysMonitor)
 {
     T_STATE_OUT_6D0 tStatout6D0 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatout6D0, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatout6D0, ucData, CAN_MSG_LEN);
 
     tSysMonitor->iCtrTmp = tStatout6D0.iCtrTmp;
     tSysMonitor->uiAccClchAbnStat = tStatout6D0.uiAccClchAbnStat;
@@ -483,11 +488,11 @@ void MessageCoder::CanMsgAnalyseID_6D0(unsigned char * ucData, T_SYS_MONITOR * t
 void MessageCoder::CanMsgAnalyseID_6D1(unsigned char * ucData, T_SYS_ERR * tSysErr)
 {
     T_STATE_OUT_6D1 tStatOut6D1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tStatOut6D1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tStatOut6D1, ucData, CAN_MSG_LEN);
 
     tSysErr->uiSysErra = tStatOut6D1.uiSysErra;
     tSysErr->uiSysErrb = tStatOut6D1.uiSysErrb;
@@ -521,11 +526,11 @@ void MessageCoder::CanMsgAnalyseID_6D1(unsigned char * ucData, T_SYS_ERR * tSysE
 void MessageCoder::CanMsgAnalyseID_6D7(unsigned char * ucData, T_STEER_PST_TIME_REQ * tSteerPstTimeReq)
 {
     T_CONTROL_CMD_6D7 tCtrCmd6D7 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tCtrCmd6D7, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tCtrCmd6D7, ucData, CAN_MSG_LEN);
 
     tSteerPstTimeReq->uiNumReq = tCtrCmd6D7.uiNumReq;
     tSteerPstTimeReq->uiSeqIdReq = tCtrCmd6D7.uiSeqIdReq;
@@ -535,11 +540,11 @@ void MessageCoder::CanMsgAnalyseID_6D7(unsigned char * ucData, T_STEER_PST_TIME_
 void MessageCoder::CanMsgAnalyseID_2E0(unsigned char * ucData, T_ESTOP_INFO * tEstopInfo)
 {
     T_ESTOP_2E0 tEstop2E0 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tEstop2E0, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tEstop2E0, ucData, CAN_MSG_LEN);
 
     tEstopInfo->uiEstpAbnStat = tEstop2E0.uiEstpAbnStat;
     tEstopInfo->uiEstpAlive = tEstop2E0.uiEstpAlive;
@@ -551,11 +556,11 @@ void MessageCoder::CanMsgAnalyseID_2E0(unsigned char * ucData, T_ESTOP_INFO * tE
 void MessageCoder::CanMsgAnalyseID_2E1(unsigned char * ucData, T_ESTOP_ERR * tEstopErr)
 {
     T_ESTOP_2E1 tEstop2E1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tEstop2E1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tEstop2E1, ucData, CAN_MSG_LEN);
 
     tEstopErr->uiEspErra = tEstop2E1.uiEspErra;
     tEstopErr->uiEspErrb = tEstop2E1.uiEspErrb;
@@ -573,11 +578,11 @@ void MessageCoder::CanMsgAnalyseID_2E1(unsigned char * ucData, T_ESTOP_ERR * tEs
 void MessageCoder::CanMsgAnalyseErr(unsigned char * ucData, T_MOT_CTR_ERR * tMotCtrErr)
 {
     T_CTR_ERR_CODE tCtrErrCode = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    CharReverse(ucCanMsgSrc,8);
-    memcpy(&tCtrErrCode, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    CharReverse(ucData,8);
+    memcpy(&tCtrErrCode, ucData, CAN_MSG_LEN);
 
     tMotCtrErr->uiErra = tCtrErrCode.a;
     tMotCtrErr->uiErraa = tCtrErrCode.aa;
@@ -620,10 +625,10 @@ void MessageCoder::CanMsgAnalyseErr(unsigned char * ucData, T_MOT_CTR_ERR * tMot
 void MessageCoder::OBDAnalyseID_1A1(unsigned char *ucData, T_OBD_ACC_PSNG *tOBDAccPsng)
 {
     T_OBD_1A1 tOBD1A1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD1A1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD1A1, ucData, CAN_MSG_LEN);
 
     tOBDAccPsng->obdAccPsng = (tOBD1A1.obdAccPsng *1000) / OBD_ACC_PSNG_RATIO;
 
@@ -633,10 +638,10 @@ void MessageCoder::OBDAnalyseID_1A1(unsigned char *ucData, T_OBD_ACC_PSNG *tOBDA
 void MessageCoder::OBDAnalyseID_0F1(unsigned char *ucData, T_OBD_BRK_PSNG *tOBDBrkPsng)
 {
     T_OBD_0F1 tOBD0F1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD0F1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD0F1, ucData, CAN_MSG_LEN);
 
     tOBDBrkPsng->obdBrkPsng = (tOBD0F1.obdBrkPsng *1000) / OBD_BRK_PSNG_RATIO;
 }
@@ -645,10 +650,10 @@ void MessageCoder::OBDAnalyseID_0F1(unsigned char *ucData, T_OBD_BRK_PSNG *tOBDB
 void MessageCoder::OBDAnalyseID_135(unsigned char *ucData, T_OBD_SFT_PSNG *tOBDSftPsng)
 {
     T_OBD_135 tOBD135 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD135, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD135, ucData, CAN_MSG_LEN);
 
     switch(tOBD135.obdSftPsng)
     {
@@ -674,10 +679,10 @@ void MessageCoder::OBDAnalyseID_135(unsigned char *ucData, T_OBD_SFT_PSNG *tOBDS
 void MessageCoder::OBDAnalyseID_1F1(unsigned char *ucData, T_OBD_VEH_BRK_STAT *tOBDVehBrkStat)
 {
     T_OBD_1F1 tOBD1F1 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD1F1, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD1F1, ucData, CAN_MSG_LEN);
 
     tOBDVehBrkStat->obdVehHBrkStat = tOBD1F1.obdVehHBrkStat;
 }
@@ -686,10 +691,10 @@ void MessageCoder::OBDAnalyseID_1F1(unsigned char *ucData, T_OBD_VEH_BRK_STAT *t
 void MessageCoder::OBDAnalyseID_121(unsigned char *ucData, T_OBD_VEH_SW_STAT *tOBDVehSwStat)
 {
     T_OBD_121 tOBD121 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD121, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD121, ucData, CAN_MSG_LEN);
 
     switch (tOBD121.obdVehSwStat) {
     case 4:
@@ -714,10 +719,10 @@ void MessageCoder::OBDAnalyseID_121(unsigned char *ucData, T_OBD_VEH_SW_STAT *tO
 void MessageCoder::OBDAnalyseID_1E5(unsigned char *ucData, T_OBD_STR_ANG *tOBDStrAng)
 {
     T_OBD_1E5 tOBD1E5 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD1E5, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD1E5, ucData, CAN_MSG_LEN);
 
     T_OBD_1E5_UNION tObd1e5Union;
     memset(&tObd1e5Union,0,sizeof(T_OBD_1E5_UNION));
@@ -739,10 +744,10 @@ void MessageCoder::OBDAnalyseID_0C9(unsigned char *ucData, T_OBD_ENG_SPD *tOBDEn
 //    tOBDEngSpd->obdEngSpd = tOBD0C9.obdEngSpd / OBD_ENG_SPD_RATIO;
 
     T_OBD_0C9 tOBD0C9 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD0C9, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD0C9, ucData, CAN_MSG_LEN);
 
     T_OBD_0C9_UNION tObd0c9Union;
     memset(&tObd0c9Union,0,sizeof(T_OBD_0C9_UNION));
@@ -764,10 +769,10 @@ void MessageCoder::OBDAnalyseID_3E9(unsigned char *ucData, T_OBD_VEH_SPD *tOBDVe
 //    tOBDVehSpd->obdVehSpd = tOBD3E9.obdVehSpd / OBD_VEH_SPD_RATIO;
 
     T_OBD_3E9 tOBD3E9 = { 0 };
-    unsigned char ucCanMsgSrc[8] = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
 
-    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
-    memcpy(&tOBD3E9, ucCanMsgSrc, CAN_MSG_LEN);
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+    memcpy(&tOBD3E9, ucData, CAN_MSG_LEN);
 
     T_OBD_3E9_UNION tObd3e9Union;
     memset(&tObd3e9Union,0,sizeof(T_OBD_3E9_UNION));

src/canbus/messagecoder.h

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: limengqi
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #ifndef MESSAGECODER_H
 #define MESSAGECODER_H
 

src/common/canmsgobdmsgid.h

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #ifndef CANMSGOBDMSGID_H
 #define CANMSGOBDMSGID_H
 

src/common/commonmain.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include <ros/ros.h>
 #include <iostream>
 #include "gloghelper.h"

src/common/file.cpp

@@ -1,7 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/common/file.h

@@ -1,10 +1,8 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * 。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
-
 /**
  * @file
  */

src/common/gloghelper.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include <stdlib.h>
 #include "gloghelper.h"
 
@@ -39,16 +44,14 @@ GLogHelper::GLogHelper(char* program, std::string appName)
     FLAGS_colorlogtostderr=true;
 
     //设置 google::ERROR 级别的日志存储路径和文件名称前缀
-    //google::SetLogDestination(google::ERROR,"log/error_");
+    google::SetLogDestination(google::ERROR,"log/error_");
 
     //设置 google::INFO 级别的日志存储路径和文件名称前缀
     google::SetLogDestination(google::INFO,LOGDIR"/log_");
 
     //设置 google::WARNING 级别的日志存储路径和文件名称前缀
-    // google::SetLogDestination(google::WARNING,LOGDIR"/WARNING_");
+    google::SetLogDestination(google::WARNING,LOGDIR"/WARNING_");
 
-    //设置 google::ERROR 级别的日志存储路径和文件名称前缀
-    // google::SetLogDestination(google::ERROR,LOGDIR"/ERROR_");
     //缓冲日志输出，默觉得30秒。此处改为马上输出
     FLAGS_logbufsecs =0;
     //最大日志大小为 100MB

src/common/message.h

@@ -1,7 +1,6 @@
-
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * Author:mds
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/common/mpc.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "mpc.h"
 #include <cppad/cppad.hpp>
 #include <cppad/ipopt/solve.hpp>
@@ -11,7 +16,7 @@ namespace common {
 
 // 设置预测状态点的个数和两个相邻状态点之间的时间间隔
 size_t N = 10;
-double dt = 0.2; // 200ms
+double dt = 0.05; // 50ms
 
 // 设置汽车头到车辆重心之间的距离
 const double Lf = 2.67;

src/common/mpc.h

@@ -1,7 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * Author:mds。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #ifndef ROBOT_MPC_H

src/common/robotapp.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "robotapp.h"
 
 namespace robot {

src/common/robotapp.h

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/common/sqlite3helper.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "sqlite3helper.h"
 #include <string.h>
 #include <stdio.h>

src/common/sqlite3helper.h

@@ -1,7 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * Author:mds。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/common/string_tokenizer.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "string_tokenizer.h"
 #include "std_msgs/String.h"
 

src/common/string_tokenizer.h

@@ -1,7 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * Author:mds。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #ifndef COMMON_STRING_TOKENIZER_H

src/common/time.h

@@ -1,7 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * Author:mds。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/common/util.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "util.h"
 
 namespace robot {

src/common/util.h

@@ -1,7 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * Author:mds。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/decision/decision.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "decision.h"
 #include "../common/message.h"
 #include "decisiondatatype.h"
@@ -28,7 +33,7 @@ int Decision::Init(){
     iCommandCmd = CMD_UNDEFIED;// set undefied mode
     iAbandon = iRulesTotalNumbers = ilocalTotalNumbers =fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
     iPredictionPointsPerNumber = 5;
-    uiCarMode =6; // undefied
+    uiCarMode = CTR_MD_REQ_ROBOT_UNDIFED; // undefied
     fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
     PlanRouteName = "siasun";
 //    for (int i =0; i< 10; i++)
@@ -44,19 +49,19 @@ 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, 200,
+    ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 1,
                                                           &Decision::RTKLocationCallback, this);
 
-    ros::Subscriber OBDSub = node_handle_->subscribe(robot::common::ObdData, 200, &Decision::OBDCallback, this);
+    ros::Subscriber OBDSub = node_handle_->subscribe(robot::common::ObdData, 1, &Decision::OBDCallback, this);
 
-    ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::Si2Data, 200, &Decision::RCUModeCallback, this);
+    ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::Si2Data, 1, &Decision::RCUModeCallback, this);
 
     // under ros publish
-    Rules_pub = node_handle_->advertise<decision::rulesMsg>(robot::common::RuleInfo, 200);
+    Rules_pub = node_handle_->advertise<decision::rulesMsg>(robot::common::RuleInfo, 100);
 
-    Geometry_pub = node_handle_->advertise<decision::localizationMsg>(robot::common::GeometryPlan, 200);
+    Geometry_pub = node_handle_->advertise<decision::localizationMsg>(robot::common::GeometryPlan, 100);
 
-    Control_pub = node_handle_->advertise<decision::controlMsg>(robot::common::ControlInfo, 200);
+    Control_pub = node_handle_->advertise<decision::controlMsg>(robot::common::ControlInfo, 100);
 
     // under ros service of client
     clientSftActCtrReq = node_handle_->serviceClient<canbus::SftActCtrReq>(robot::common::ShiftActCtrService);
@@ -65,7 +70,7 @@ int Decision::Start(){
 
     clientStrActCtrReq = node_handle_->serviceClient<canbus::StrActCtrReq>(robot::common::SteerActCtrService);
 
-    ros::AsyncSpinner spinner(2); // thread_count The number of threads to use.  A value of 0 means to use the number of processor cores
+    ros::AsyncSpinner spinner(3); // thread_count The number of threads to use.  A value of 0 means to use the number of processor cores
     spinner.start();
     ros::waitForShutdown();
     LOG(INFO)<<Name()<<"  start() end.";
@@ -86,24 +91,27 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
     if (req.Cmd == CMD_AUTODRIE_START)
     {
         LoadPlanRoute();
-        double breakCtr = -50; // 踩刹车到50%
+        throttle_Last = -70; // 踩刹车到70%
         ros::Rate loop_rate(1);
         while(ros::ok())
         {
-            if (SpeedControl(breakCtr) == 1)
+            if (SpeedControl(throttle_Last) == 1)
             {
-                LOG(INFO)<<Name()<<"CommandRequestService break control 50% excute success.";
+                ros::Duration(2).sleep();  // sleep for two second
+                LOG(INFO)<<Name()<<"CommandRequestService break control 70% excute success.";
                 if (BreakCtrControl(SFT_PSNG_LRN_REQ_FORWARD_1) == 1) // 前进挡
                     break;
             }
             else
             {
-                LOG(ERROR)<<Name()<<"CommandRequestService break control 50% excute error.";
+                LOG(ERROR)<<Name()<<"CommandRequestService break control 70% excute error.";
                 return false;
             }
             ros::spinOnce();
             loop_rate.sleep();
         }
+        ptsx.clear();
+        ptsy.clear();
         ptsx.push_back(0);
         ptsy.push_back(0);
         ptsx.push_back(0);
@@ -155,22 +163,22 @@ void Decision::LoadPlanRoute()
 // 接收ＲＴＫ数据分为自动驾驶模式开始和结束和路径规划模式开始和结束
 void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
 {
-     switch (iCommandCmd) {
-     case CMD_PLAN_START:
-         ExcutePlanRoute(msg);
-         break;
-     case CMD_AUTODRIE_START:
-         ExcuteSelfDriving(msg);
-         break;
-     case CMD_PLAN_END:
-         StopPlanRoute(msg);
-         break;
-     case CMD_AUTODRIE_END:
-         CarAutoDriveStop();
-         break;
-     default:
-         break;
-     }
+    switch (iCommandCmd) {
+    case CMD_PLAN_START:
+        ExcutePlanRoute(msg);
+        break;
+    case CMD_AUTODRIE_START:
+        ExcuteSelfDriving(msg);
+        break;
+    case CMD_PLAN_END:
+        StopPlanRoute(msg);
+        break;
+    case CMD_AUTODRIE_END:
+        CarAutoDriveStop();
+        break;
+    default:
+        break;
+    }
 }
 
 
@@ -189,7 +197,7 @@ void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
 //自动生成测试用例
 void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
 {
-    if (iAbandon< 20){
+    if (iAbandon< 40){ // 2 second excute
         iAbandon ++;
         return;
     }
@@ -205,16 +213,13 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
         if (iRulesNowNumber == 1)
             strRuleContent<<"动作"<<iRulesNowNumber<<":起步请加速到"<< fCarNowSpeed<<"km/h";
         else{
-            if (fCarNowSpeed > fCarLastSpeed)
-            {
+            if (fCarNowSpeed > fCarLastSpeed){
                 strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
             }
-            else if (fCarNowSpeed < fCarLastSpeed)
-            {
+            else if (fCarNowSpeed < fCarLastSpeed){
                 strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
             }
-            else if (fCarNowSpeed == fCarLastSpeed)
-            {
+            else if (fCarNowSpeed == fCarLastSpeed){
                 strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
             }
        }
@@ -225,8 +230,7 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
             iLocalNowNumber--;
             iRulesNowNumber--;
        }
-       else
-       {
+       else{
            decision::localizationMsg localMsg;
            localMsg.Locationid = iLocalNowNumber;
            localMsg.Longitude = msg->Longitude;
@@ -253,16 +257,13 @@ void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
      iLocalNowNumber++;
      iRulesNowNumber++;
      std::stringstream strRuleContent;
-     if (fCarNowSpeed > fCarLastSpeed)
-     {
+     if (fCarNowSpeed > fCarLastSpeed){
          strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
      }
-     else if (fCarNowSpeed < fCarLastSpeed)
-     {
+     else if (fCarNowSpeed < fCarLastSpeed){
          strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
      }
-     else if (fCarNowSpeed == fCarLastSpeed)
-     {
+     else if (fCarNowSpeed == fCarLastSpeed){
          strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
      }
     int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
@@ -279,8 +280,7 @@ void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
 void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
 {
    // 判断当前RCU是否为自动驾驶模式
-    if (uiCarMode != CTR_MD_REQ_AUTO_DRIVE)
-    {
+    if (uiCarMode != CTR_MD_REQ_AUTO_DRIVE){
         LOG(INFO)<<Name()<<" ExcuteSelfDriving uiCarMode is not auto drive.";
         return;
     }
@@ -294,8 +294,7 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
             (((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)
-    {       
+    if (bRetLon|| bRetLat){       
         std::vector<LOCATION_RULE_INFO> tempInfo;
         tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
         ptsx.clear();
@@ -310,11 +309,10 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
         UpdataCarRuleResults();       
         LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route  altered.";
     }
-    else
-    {
+    else{
         LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route does not alter.";
     }
-    double psi = deg2rad(msg->AngleDeviated + 90);
+    double psi = msg->AngleDeviated + 90;
     double v =  (double)fCarNowSpeed;
     MPC_Controller(ptsx, ptsy, px, py, psi, v);
 
@@ -344,15 +342,21 @@ bool  Decision::CarAutoDriveStop()
 
         if (ilocalTotalNumbers == iLocalNowNumber + iPredictionPointsPerNumber){
             iCommandCmd = CMD_AUTODRIE_END;
-            double breakCtr = -50; // 踩刹车到50%
-            iLocalNowNumber ++;
+            int breakCtr = (iRulesTotalNumbers -iRulesNowNumber) *10 -70; // 踩刹车到70%
+            //iLocalNowNumber ++;
             while(true){
-                if (SpeedControl(breakCtr) == 1)
-                {
+                if (SpeedControl(breakCtr) == 1){
+                    ros::Duration(2).sleep();  // sleep for two second
                     if (BreakCtrControl(SFT_PSNG_LRN_REQ_PARK) == 1) // P挡
                         break;
                 }
             }
+            if (iRulesNowNumber == iRulesTotalNumbers){
+               iCommandCmd = CMD_UNDEFIED;
+               return true;
+            }
+
+            iRulesNowNumber ++;
             decision::rulesMsg ruleMsg;
             if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速（ <10km/h）：± 0.25km/h  高速（ >10km/h）：± 1km/h
             {
@@ -427,21 +431,15 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
      // 这个地方有延迟，取第三个
      auto vars = 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++;
-//     }
-     double Lf = 2.67;
-     double steer_value = (vars[0]*1300/25/(deg2rad(25)*Lf));
-     double throttle_value = vars[1]*100;
+     int steer_value = rad2deg(vars[0]);
+
+     double acc = vars[1]; // 加速度
+     int throttle_value = throttle_Last + acc*0.1*3.6*100;
+//     if (throttle_Last>=0)
+//         throttle_value =  abs(throttle_Now - throttle_Last) >=15? throttle_Last -15: throttle_Now;
+//     else
+//         throttle_value =  abs(throttle_Now - throttle_Last) >=15? throttle_Last +15: throttle_Now;
+     throttle_Last = throttle_value;
      LOG(INFO)<<Name()<< " stree alter value:"<<steer_value<<" throttle alter value:"<<throttle_value;
      StreeControl(steer_value);
      SpeedControl(throttle_value);
@@ -454,20 +452,20 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
      Control_pub.publish(control);
 }
 
-unsigned int Decision::SpeedControl(double &speed)
+unsigned int Decision::SpeedControl(int &speed)
 {
      canbus::PedalActCtrReq srv;
      if (speed >0){
-         srv.request.uiAccPsngCtrReq =(unsigned int)(speed *10);
+         srv.request.uiAccPsngCtrReq = speed *10;
          srv.request.uiBrkPsngCtrReq =0;
      }
      else{
          srv.request.uiAccPsngCtrReq = 0;
-         srv.request.uiBrkPsngCtrReq =(unsigned int)(-speed *10);
+         srv.request.uiBrkPsngCtrReq = -speed *10;
      }
      srv.request.iAccSpdCtrReq = 20;
-     srv.request.iBrkSpdCtrReq = 50;
      srv.request.uiBrkCtrMdReq = 1;
+     srv.request.iBrkSpdCtrReq = 90;
      srv.request.uiBrkFocCtrReq = 350;
      if (clientPedalActCtrReq.call(srv))
      {
@@ -481,13 +479,13 @@ unsigned int Decision::SpeedControl(double &speed)
 }
 
 
-unsigned int Decision::StreeControl(double &streer)
+unsigned int Decision::StreeControl(int &streer)
 {
 
      canbus::StrActCtrReq srv;
-     srv.request.iStrAngCtrReq =(unsigned int)(streer *100);
+     srv.request.iStrAngCtrReq = streer *100;
      srv.request.uiStrCtrMdReq =1;
-     srv.request.iStrSpdCtrReq = 200;
+     srv.request.iStrSpdCtrReq = 290;
      srv.request.iStrTrqCtrReq = 50;
      if (clientStrActCtrReq.call(srv))
      {

src/decision/decision.h

@@ -1,7 +1,6 @@
-
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #ifndef ROBOT_ROBOT_DECISION_H
@@ -118,9 +117,9 @@ private:
 
     void TestMPC();
     //速度控制
-    unsigned int SpeedControl(double &speed);
+    unsigned int SpeedControl(int &speed);
     //方向控制
-    unsigned int StreeControl(double &streer);
+    unsigned int StreeControl(int &streer);
     //档位控制
     unsigned int BreakCtrControl(unsigned int breakmode);
     //加载规划路径
@@ -162,7 +161,10 @@ private:
     Eigen::MatrixXd transformGlobal2Vehicle(double x, double y, double psi,
            const vector<double> & ptsx, const vector<double> & ptsy);
 
-
+    /**
+    * @brief timer callback function
+    */
+    void OnTimer(const ros::TimerEvent &event);
 
 private:
 
@@ -208,8 +210,8 @@ private:
     std::vector<double> vPx;
     std::vector<double> vPy;
 
-    double throttle_Last = 0;
-    double steer_Last = 0;
+    int throttle_Last = 0;
+    int steer_Last = 0;
 
 };
 

src/decision/decision_app.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "../common/robotapp.h"
 #include "decision.h"
 

src/decision/decisiondatabase.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "decisiondatabase.h"
 #include <string>
 namespace robot {

src/decision/decisiondatabase.h

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #ifndef DECISIONDATABASE_H
 #define DECISIONDATABASE_H
 

src/decision/decisiondatatype.h

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #ifndef ROBOT_DECISIONDATATYPE_H
@@ -31,6 +31,7 @@ namespace decisionSpace {
 #define	CTR_MD_REQ_HAND_DRIVE		3	//控制模式请求-人工驾驶模式
 #define	CTR_MD_REQ_ROBOT_CHECK		4	//控制模式请求-机器人自检模式
 #define	CTR_MD_REQ_ROBOT_DEBUG		5	//控制模式请求-机器人调试模式
+#define	CTR_MD_REQ_ROBOT_UNDIFED		6	//控制模式请求-机器人调试模式
 
 #define RULE_EXCUTE_RESULT_INIT           0
 #define RULE_EXCUTE_RESULT_FIAL           1

src/localization/coordsconvertion.cpp

@@ -1,6 +1,6 @@
 /******************************************************************************
- * Copyright 2019 The siasun Transport Authors. All Rights Reserved.
- *
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #include "coordsconvertion.h"

src/localization/coordsconvertion.h

@@ -1,8 +1,8 @@
 /******************************************************************************
-* Copyright 2020 The siasun Transport Authors. All Rights Reserved.
-*
-* Licensed under the Apache License, Version 1.0 (the "License");
-*****************************************************************************/
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 
 /**
 * @file

src/localization/localization.cpp

@@ -1,6 +1,6 @@
 /******************************************************************************
- * Copyright 2019 The siasun Transport Authors. All Rights Reserved.
- *
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #include <iterator>
@@ -40,7 +40,7 @@ int Localization::Start() {
  LOG(INFO)<<Name()<<" start start...";
  try
  {
-     geometry_pub = node_handle_->advertise<localization::localMsg>(robot::common::Geometry, 200);
+     geometry_pub = node_handle_->advertise<localization::localMsg>(robot::common::Geometry, 1);
      //设置串口属性，并打开串口
      serial_gnss.setPort(serial_port);
      serial_gnss.setBaudrate(baudrate);
@@ -50,11 +50,11 @@ int Localization::Start() {
      if (IsOpen())
      {
          //指定循环的频率
-         ros::Rate loop_rate(2000);
+         ros::Rate loop_rate(rate);
          while(ros::ok())
          {
 
-           if(serial_gnss.available() >0){
+           if(serial_gnss.available() >200){
                LOG(INFO)<<Name()<<"  Reading from serial port. data size :"<<serial_gnss.available();
                std::string strSerialResult;
                strSerialResult= serial_gnss.read(serial_gnss.available());

src/localization/localization.h

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 
@@ -108,12 +108,14 @@ private:
 
   uint32_t baudrate = 0;
 
-  uint32_t timeout = 10;
+  uint32_t timeout = 50;
 
   serial::Serial serial_gnss; //声明串口对象
 
   ros::Publisher geometry_pub;
 
+  const double rate = 20;  //20HZ
+
 };
 
 

src/localization/localization_app.cpp

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #include "../common/robotapp.h"

src/localization/localizationdata.h

@@ -1,10 +1,8 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
-
-
 /**
  * @file
  */

src/perception/perception.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: zhangyu,Altered: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "perception.h"
 #include "../common/message.h"
 #include "../common/canmsgobdmsgid.h"
@@ -33,22 +38,22 @@ int Perception::Start()
     LOG(INFO) << Name() << " Start start...";
     try
     {
-        ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1000, &Perception::CanTopicCallback, this);
+        ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1, &Perception::CanTopicCallback, this);
 
-        bus_pub_6E5 = node_handle_->advertise<::perception::aesMsg>(robot::common::AesData, 2000);
-        bus_pub_6EE = node_handle_->advertise<::perception::slMsg>(robot::common::SlData, 2000);
-        bus_pub_6ED = node_handle_->advertise<::perception::si2Msg>(robot::common::Si2Data, 2000);
+        bus_pub_6E5 = node_handle_->advertise<::perception::aesMsg>(robot::common::AesData, 1);
+        bus_pub_6EE = node_handle_->advertise<::perception::slMsg>(robot::common::SlData, 1);
+        bus_pub_6ED = node_handle_->advertise<::perception::si2Msg>(robot::common::Si2Data, 1);
 
-        bus_pub_6D1 = node_handle_->advertise<::perception::seMsg>(robot::common::SeData, 2000);
+        bus_pub_6D1 = node_handle_->advertise<::perception::seMsg>(robot::common::SeData, 1);
 
-        bus_pub_6D2 = node_handle_->advertise<::perception::cecMsg>(robot::common::AcceData, 2000);
-        bus_pub_6D3 = node_handle_->advertise<::perception::cecMsg>(robot::common::BrkeData, 2000);
-        bus_pub_6D4 = node_handle_->advertise<::perception::cecMsg>(robot::common::XsfteData, 2000);
-        bus_pub_6D5 = node_handle_->advertise<::perception::cecMsg>(robot::common::YsfteData, 2000);
-        bus_pub_6D6 = node_handle_->advertise<::perception::cecMsg>(robot::common::StreData, 2000);
+        bus_pub_6D2 = node_handle_->advertise<::perception::cecMsg>(robot::common::AcceData, 1);
+        bus_pub_6D3 = node_handle_->advertise<::perception::cecMsg>(robot::common::BrkeData, 1);
+        bus_pub_6D4 = node_handle_->advertise<::perception::cecMsg>(robot::common::XsfteData, 1);
+        bus_pub_6D5 = node_handle_->advertise<::perception::cecMsg>(robot::common::YsfteData, 1);
+        bus_pub_6D6 = node_handle_->advertise<::perception::cecMsg>(robot::common::StreData, 1);
 
-        bus_pub_2E0 = node_handle_->advertise<::perception::eiMsg>(robot::common::EiData, 2000);
-        bus_pub_2E1 = node_handle_->advertise<::perception::eeMsg>(robot::common::EeData, 2000);
+        bus_pub_2E0 = node_handle_->advertise<::perception::eiMsg>(robot::common::EiData, 1);
+        bus_pub_2E1 = node_handle_->advertise<::perception::eeMsg>(robot::common::EeData, 1);
 
         clientActuatorEnableRequest = node_handle_->serviceClient<canbus::ActuatorEnableRequest>(robot::common::ActuatorEnableService);
 

src/perception/perception.h

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: zhangyu,Altered: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #ifndef ROBOT_ROBOT_PERCEPTION_H
 #define ROBOT_ROBOT_PERCEPTION_H
 

src/perception/perception_app.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: zhangyu
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "../common/robotapp.h"
 #include "perception.h"