1、添加数据库

2、添加TCPCAN通信
3、修改台架规范测试任务逻辑
4、添加modbusTCP模拟量通信
5修改油门制动报警为行动元报警信息

src/canbus/CMakeLists.txt

@@ -151,6 +151,7 @@ include_directories(
 # include
   ${catkin_INCLUDE_DIRS}
   /usr/local/include
+  /usr/include/mysql
 )
 
 ## Declare a C++ library
@@ -252,6 +253,8 @@ add_executable(canbus
     messagecoder.cpp
     pid.h
     pid.cpp
+    canbusmysqldatabase.h
+    canbusmysqldatabase.cpp
  )
 add_dependencies(canbus ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(canbus
@@ -260,4 +263,5 @@ target_link_libraries(canbus
     gflags
     pcan
     dl
-    protobuf)
+    protobuf
+    mysqlpp)

src/canbus/canbus.h

@@ -65,7 +65,6 @@
 #include <ctime>
 
 using namespace std;
-
 /**
  * @namespace robot::canbus
  * @brief robot::canbus
@@ -183,7 +182,7 @@ class Canbus : public robot::common::RobotApp{
 
   int read_txt(string &path,int flag);  //读取TXT文件
 
-  int get_datetime_mill(tm *datetime);  //获取当前时间和毫秒
+  std::string get_datetime_mill();  //获取当前时间和毫秒
 
   void publish_rack_speed(); //发布台架测试任务实际车速
 
@@ -378,14 +377,36 @@ private:
    localization::localMsg msg_RTK;
 
    int fuzzy_pid_number = 0;
-   double fuzzy_pid[6] = {0};
+   double fuzzy_pid[8] = {0};
    float set_speed_last = 0;
 
    const double duration_rack_reply_detail = 0.1;//100ms //台架测试任务细节图所用数据回复时间
    ros::Timer rack_reply_detail_timer_;  //台架测试任务细节图所用数据回复定时器
    float set_speed_reply = 0;
 
-
+   double P = 0;
+   double I = 0;
+   double D = 0;
+   double KP = 0;
+   double KI = 0;
+   double KD = 0;
+   int brk_factor = 0;
+   int brk_max = 0;
+   long task_id = 0;    //任务ID
+   std::string standard_name = "NEDC" ;  //规范名称
+   double over_threshold_value = 0.1;  //超差阈值
+   int task_num = 0;     //规范执行次数
+   int task_already_num= 0;   //当前任务已经执行规范次数
+   double task_time_stamp = 0;  //时间戳
+   float rack_set_speed = 0;   //设定速度
+   float rack_actual_speed = 0; //实际速度
+
+   double task_time_stamp_ui = 0;
+   struct timeval now_time_ui;//当前时间
+   struct timeval last_time_ui;//上一次时间
+
+   bool over_speed_flag = false; //超差标志
+   int over_speed_num = 0;  //超差次数
 
   const double duration = 0.01; //10ms
 
@@ -423,6 +444,13 @@ private:
 
   canbus::obdMsg obd; // obd data
 
+  //20240111 by zhaowei add tcp2can
+  string canbus_mode; //can总线模式: usb(默认)/tcp
+
+  int canbus_socket;
+
+  unsigned char s_g_pBufGet[13] , s_g_pBufSet[13];
+
 };
 
 }  // namespace canbus

src/canbus/canbusmysqldatabase.cpp

@@ -0,0 +1,419 @@
+#include "canbusmysqldatabase.h"
+#include <string>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string>
+#include <iostream>
+
+using namespace std;
+using namespace mysqlpp;
+
+namespace  robot{
+namespace canbusSpace {
+
+std::string CanbusMySqlDataBase::Name() { return "MySQL Database"; }
+
+int CanbusMySqlDataBase::QueryDB(){
+  try
+  {
+    /***************************打开数据库 start ****************************/
+    const char* db = 0, *server = 0, *user = 0, *password = "";
+    db = "dreamview_web";
+    server = "localhost";
+    user = "root";
+    password = "Aa123456";
+
+    mysqlpp::Connection conn(false);
+    if (conn.connect(db, server, user, password))
+    {
+      LOG(INFO)<<Name()<<":Canbus opendb dreamview_web succeed";
+      mysqlpp::Query query = conn.query("select * from Base_Department;");
+      mysqlpp::StoreQueryResult res = query.store();
+      cout << "ares.num_rows() = " << res.num_rows() << endl;
+        for (size_t i = 0; i < res.num_rows(); i++)
+        {
+          cout << "Id: " << res[i]["Id"] << "\t - Name: " << res[i]["Name"]
+          << "\t - ParentId: " << res[i]["ParentId"] <<endl;
+        }
+
+        /* Let's get a count of something */
+        query = conn.query("select count(*) as row_count from Base_Department;" ) ;
+        StoreQueryResult bres = query.store();
+        if(bres.num_rows() > 0)
+        {
+                  cout << "Total rows: " << bres[0]["row_count"] << endl;
+        }
+
+    }
+    else
+    {
+      LOG(INFO)<<Name()<<":Canbus opendb dreamview_web fail";
+    }
+    /***************************打开数据库 end ******************************/
+    conn.disconnect();
+    return DATABASE_SUCCESS;
+  }
+  catch (BadQuery er)
+  { // handle any connection or
+  // query errors that may come up
+    LOG(ERROR)<<Name()<< " Canbus database error. "<< er.what();
+    return DATABASE_FAILTURE;
+  }
+  catch (const BadConversion& er)
+  {
+  // Handle bad conversions
+    LOG(ERROR)<<Name()<< " Canbus database Conversion error: "<< er.what()
+             <<"\tretrieved data size: " << er.retrieved
+             <<", actual size: " << er.actual_size;
+    return DATABASE_FAILTURE;
+  }
+  catch (const Exception& er)
+  {
+    // Catch-all for any other MySQL++ exceptions
+    LOG(ERROR)<<Name()<< " Canbus database error. "<< er.what();
+    return DATABASE_FAILTURE;
+  }
+
+}
+
+int CanbusMySqlDataBase::InsertDB()
+{
+  try
+  {
+    /***************************打开数据库 start ****************************/
+    const char* db = 0, *server = 0, *user = 0, *password = "";
+    db = "dreamview_web";
+    server = "localhost";
+    user = "root";
+    password = "Aa123456";
+
+    mysqlpp::Connection conn(false);
+    if (conn.connect(db, server, user, password))
+    {
+      LOG(INFO)<<Name()<<":Canbus opendb dreamview_web succeed";
+      std::string sRuleName="123456";
+      std::string sDirectiveID = "7";
+      std::string id = "10002";
+      std::string s_sql="insert into Base_Department (Id,Name,ParentId)"
+                        "VALUES (\'"+id+" \',\'"+sRuleName+"\',\'"+sDirectiveID+"\');";
+      mysqlpp::Query query = conn.query(s_sql);
+      mysqlpp::SimpleResult  res = query.execute();
+     cout << "Inserted into Base_Department table, ID =" << res.insert_id() << endl;
+    }
+    else
+    {
+      LOG(INFO)<<Name()<<":Canbus opendb dreamview_web fail";
+    }
+    /***************************打开数据库 end ******************************/
+    conn.disconnect();
+    return DATABASE_SUCCESS;
+  }
+  catch (BadQuery er)
+  { // handle any connection or
+  // query errors that may come up
+    LOG(ERROR)<<Name()<< " Canbus database error. "<< er.what();
+    return DATABASE_FAILTURE;
+  }
+  catch (const BadConversion& er)
+  {
+  // Handle bad conversions
+    LOG(ERROR)<<Name()<< " Canbus database Conversion error: "<< er.what()
+             <<"\tretrieved data size: " << er.retrieved
+             <<", actual size: " << er.actual_size;
+    return DATABASE_FAILTURE;
+  }
+  catch (const Exception& er)
+  {
+    // Catch-all for any other MySQL++ exceptions
+    LOG(ERROR)<<Name()<< " Canbus database error. "<< er.what();
+    return DATABASE_FAILTURE;
+  }
+
+}
+
+int CanbusMySqlDataBase::QuerySysPara(long id,RACK_SYS_PARA *rackSysPara)
+{
+  try
+  {
+    /***************************打开数据库 start ****************************/
+    const char* db = 0, *server = 0, *user = 0, *password = "";
+    db = "dreamview_web";
+    server = "localhost";
+    user = "root";
+    password = "Aa123456";
+
+    mysqlpp::Connection conn(false);
+    if (conn.connect(db, server, user, password))
+    {
+      LOG(INFO)<<Name()<<":Canbus opendb dreamview_web succeed";
+      std::string carId=std::to_string(id);
+      std::string sql = "SELECT * FROM rack_syspara WHERE Id = "+carId+";";
+      mysqlpp::Query query = conn.query(sql);
+      mysqlpp::StoreQueryResult res = query.store();
+      cout << "ares.num_rows() = " << res.num_rows() << endl;
+      if(res.num_rows() <= 0)
+      {
+         return DATABASE_FAILTURE;
+      }
+      rackSysPara->Id = res[0]["Id"];
+      rackSysPara->CarName = res[0]["CarName"].c_str();
+      rackSysPara->P = res[0]["P"];
+      rackSysPara->I = res[0]["I"];
+      rackSysPara->D = res[0]["D"];
+      rackSysPara->KP = res[0]["KP"];
+      rackSysPara->KI = res[0]["KI"];
+      rackSysPara->KD = res[0]["KD"];
+      rackSysPara->BrkFactor = res[0]["BrkFactor"];
+      rackSysPara->BrkMax = res[0]["BrkMax"];
+
+    }
+    else
+    {
+      LOG(INFO)<<Name()<<":Canbus opendb dreamview_web fail";
+      return DATABASE_FAILTURE;
+    }
+    /***************************打开数据库 end ******************************/
+    conn.disconnect();
+    return DATABASE_SUCCESS;
+  }
+  catch (BadQuery er)
+  { // handle any connection or
+  // query errors that may come up
+    LOG(ERROR)<<Name()<< " Canbus database error. "<< er.what();
+    return DATABASE_FAILTURE;
+  }
+  catch (const BadConversion& er)
+  {
+  // Handle bad conversions
+    LOG(ERROR)<<Name()<< " Canbus database Conversion error: "<< er.what()
+             <<"\tretrieved data size: " << er.retrieved
+             <<", actual size: " << er.actual_size;
+    return DATABASE_FAILTURE;
+  }
+  catch (const Exception& er)
+  {
+    // Catch-all for any other MySQL++ exceptions
+    LOG(ERROR)<<Name()<< " Canbus database error. "<< er.what();
+    return DATABASE_FAILTURE;
+  }
+
+}
+
+long CanbusMySqlDataBase::InsertTaskInfo(int CarType,
+                                        std::string Standard,
+                                        std::string DateTime,
+                                        double OverThresholdValue,
+                                        int StandardNum)
+{
+  try
+  {
+    /***************************打开数据库 start ****************************/
+    const char* db = 0, *server = 0, *user = 0, *password = "";
+    db = "dreamview_web";
+    server = "localhost";
+    user = "root";
+    password = "Aa123456";
+
+    mysqlpp::Connection conn(false);
+    long insertId = -1;
+    if (conn.connect(db, server, user, password))
+    {
+      LOG(INFO)<<Name()<<":Canbus opendb dreamview_web succeed";
+      std::string dataCarType=std::to_string(CarType);
+      std::string dataStandard=Standard;
+      std::string dataDateTime=DateTime;
+      std::string dataOverThresholdValue=std::to_string(OverThresholdValue);
+      std::string dataStandardNum=std::to_string(StandardNum);
+      std::string s_sql="INSERT INTO rack_taskinfo (CarType,Standard,DateTime,OverThresholdValue,StandardNum)"
+                        "VALUES ("+dataCarType+",\'"+dataStandard+"\',\'"+dataDateTime+"\',"
+                        ""+dataOverThresholdValue+","+dataStandardNum+");";
+      mysqlpp::Query query = conn.query(s_sql);
+      mysqlpp::SimpleResult  res = query.execute();
+     LOG(INFO)<<Name()<<"Inserted into rack_taskinfo table, ID =" << res.insert_id();
+     insertId = res.insert_id();
+    }
+    else
+    {
+      LOG(INFO)<<Name()<<":Canbus opendb dreamview_web fail";
+      return DATABASE_FAILTURE;
+    }
+    /***************************打开数据库 end ******************************/
+    conn.disconnect();
+    return insertId;
+  }
+  catch (BadQuery er)
+  { // handle any connection or
+  // query errors that may come up
+    LOG(ERROR)<<Name()<< " Canbus database error. "<< er.what();
+    return DATABASE_FAILTURE;
+  }
+  catch (const BadConversion& er)
+  {
+  // Handle bad conversions
+    LOG(ERROR)<<Name()<< " Canbus database Conversion error: "<< er.what()
+             <<"\tretrieved data size: " << er.retrieved
+             <<", actual size: " << er.actual_size;
+    return DATABASE_FAILTURE;
+  }
+  catch (const Exception& er)
+  {
+    // Catch-all for any other MySQL++ exceptions
+    LOG(ERROR)<<Name()<< " Canbus database error. "<< er.what();
+    return DATABASE_FAILTURE;
+  }
+
+}
+
+int CanbusMySqlDataBase::InsertTaskData(long TaskId,
+                                        double SetSpeed,
+                                        double ActualSpeed,
+                                        long AccOrderPsng,
+                                        long BrkOrderPsng,
+                                        double StandardSpeedTimeStamp,
+                                        int OverSpeedNum,
+                                        int StandardTaskCount)
+{
+  try
+  {
+    /***************************打开数据库 start ****************************/
+    const char* db = 0, *server = 0, *user = 0, *password = "";
+    db = "dreamview_web";
+    server = "localhost";
+    user = "root";
+    password = "Aa123456";
+
+    mysqlpp::Connection conn(false);
+    int insertId = -1;
+    if (conn.connect(db, server, user, password))
+    {
+      LOG(INFO)<<Name()<<":Canbus opendb dreamview_web succeed";
+      std::string dataTaskId=std::to_string(TaskId);
+      std::string dataSetSpeed=std::to_string(SetSpeed);
+      std::string dataActualSpeed=std::to_string(ActualSpeed);
+      std::string dataAccOrderPsng=std::to_string(AccOrderPsng);
+      std::string dataBrkOrderPsng=std::to_string(BrkOrderPsng);
+      std::string dataStandardSpeedTimeStampe=std::to_string(StandardSpeedTimeStamp);
+      std::string dataOverSpeedNum=std::to_string(OverSpeedNum);
+      std::string dataStandardTaskCount = std::to_string(StandardTaskCount);
+      std::string s_sql="INSERT INTO rack_taskdata "
+                        "(TaskId,SetSpeed,ActualSpeed,AccOrderPsng,BrkOrderPsng,"
+                        "StandardSpeedTimeStamp,OverSpeedNum,StandardTaskCount)"
+                        "VALUES ("+dataTaskId+","+dataSetSpeed+","+dataActualSpeed+","
+                        ""+dataAccOrderPsng+","+dataBrkOrderPsng+","
+                        ""+dataStandardSpeedTimeStampe+","+dataOverSpeedNum+","+dataStandardTaskCount+");";
+      mysqlpp::Query query = conn.query(s_sql);
+      mysqlpp::SimpleResult  res = query.execute();
+      LOG(INFO)<<Name()<<"Inserted into rack_taskdata table, ID =" << res.insert_id();
+     insertId = res.insert_id();
+     if(insertId <= 0)
+     {
+       LOG(INFO)<<Name()<<"insert rack_taskdata error";
+     }
+    }
+    else
+    {
+      LOG(INFO)<<Name()<<":Canbus opendb dreamview_web fail";
+      return DATABASE_FAILTURE;
+    }
+    /***************************打开数据库 end ******************************/
+    conn.disconnect();
+    return insertId;
+  }
+  catch (BadQuery er)
+  { // handle any connection or
+  // query errors that may come up
+    LOG(ERROR)<<Name()<< " Canbus database error. "<< er.what();
+    return DATABASE_FAILTURE;
+  }
+  catch (const BadConversion& er)
+  {
+  // Handle bad conversions
+    LOG(ERROR)<<Name()<< " Canbus database Conversion error: "<< er.what()
+             <<"\tretrieved data size: " << er.retrieved
+             <<", actual size: " << er.actual_size;
+    return DATABASE_FAILTURE;
+  }
+  catch (const Exception& er)
+  {
+    // Catch-all for any other MySQL++ exceptions
+    LOG(ERROR)<<Name()<< " Canbus database error. "<< er.what();
+    return DATABASE_FAILTURE;
+  }
+
+}
+
+int CanbusMySqlDataBase::InsertTaskDataUI(long TaskId,
+                                        double SetSpeed,
+                                        double ActualSpeed,
+                                        double StandardSpeedTimeStamp,
+                                        int OverSpeedNum,
+                                        int StandardTaskCount)
+{
+  try
+  {
+    /***************************打开数据库 start ****************************/
+    const char* db = 0, *server = 0, *user = 0, *password = "";
+    db = "dreamview_web";
+    server = "localhost";
+    user = "root";
+    password = "Aa123456";
+
+    mysqlpp::Connection conn(false);
+    int insertId = -1;
+    if (conn.connect(db, server, user, password))
+    {
+      LOG(INFO)<<Name()<<":Canbus opendb dreamview_web succeed";
+      std::string dataTaskId=std::to_string(TaskId);
+      std::string dataSetSpeed=std::to_string(SetSpeed);
+      std::string dataActualSpeed=std::to_string(ActualSpeed);
+      std::string dataStandardSpeedTimeStampe=std::to_string(StandardSpeedTimeStamp);
+      std::string dataOverSpeedNum=std::to_string(OverSpeedNum);
+      std::string dataStandardTaskCount = std::to_string(StandardTaskCount);
+      std::string s_sql="INSERT INTO rack_taskdataui "
+                        "(TaskId,SetSpeed,ActualSpeed,"
+                        "StandardSpeedTimeStamp,OverSpeedNum,StandardTaskCount)"
+                        "VALUES ("+dataTaskId+","+dataSetSpeed+","+dataActualSpeed+","
+                        ""+dataStandardSpeedTimeStampe+","+dataOverSpeedNum+","+dataStandardTaskCount+");";
+      mysqlpp::Query query = conn.query(s_sql);
+      mysqlpp::SimpleResult  res = query.execute();
+     LOG(INFO)<<Name()<<"Inserted into rack_taskdataui table, ID =" << res.insert_id();
+     insertId = res.insert_id();
+     if(insertId <= 0)
+     {
+       LOG(INFO)<<Name()<<"insert rack_taskdataui error";
+     }
+    }
+    else
+    {
+      LOG(INFO)<<Name()<<":Canbus opendb dreamview_web fail";
+      return DATABASE_FAILTURE;
+    }
+    /***************************打开数据库 end ******************************/
+    conn.disconnect();
+    return insertId;
+  }
+  catch (BadQuery er)
+  { // handle any connection or
+  // query errors that may come up
+    LOG(ERROR)<<Name()<< " Canbus database error. "<< er.what();
+    return DATABASE_FAILTURE;
+  }
+  catch (const BadConversion& er)
+  {
+  // Handle bad conversions
+    LOG(ERROR)<<Name()<< " Canbus database Conversion error: "<< er.what()
+             <<"\tretrieved data size: " << er.retrieved
+             <<", actual size: " << er.actual_size;
+    return DATABASE_FAILTURE;
+  }
+  catch (const Exception& er)
+  {
+    // Catch-all for any other MySQL++ exceptions
+    LOG(ERROR)<<Name()<< " Canbus database error. "<< er.what();
+    return DATABASE_FAILTURE;
+  }
+
+}
+
+}// namespace canbusSpace
+}// namespace robot

src/canbus/canbusmysqldatabase.h

@@ -0,0 +1,80 @@
+#ifndef CANBUSMYSQLDATABASE_H
+#define CANBUSMYSQLDATABASE_H
+
+#include<mysql/mysql.h>
+#include<mysql++/mysql++.h>
+#include "canbus.h"
+#include "candatatype.h"
+
+namespace robot {
+namespace canbusSpace {
+
+#define DATABASE_SUCCESS     0
+#define DATABASE_FAILTURE   -1
+
+class CanbusMySqlDataBase
+{
+
+public:
+      CanbusMySqlDataBase(){}
+
+  static std::string Name();
+
+  /**
+  * @brief
+  * @brief
+  * @return
+  */
+  static int QueryDB();
+
+  static int InsertDB();
+
+  /**
+  * @brief 查询参数数据库
+  * @brief 输入参数为车型ID
+  * @return 返回是否查询成功 0表示成功，-1表示失败
+  */
+  static int QuerySysPara(long id,RACK_SYS_PARA *rackSysPara);
+
+  /**
+  * @brief 插入测试任务信息表
+  * @brief 输入参数
+  * @return 返回插入数据的Id ，-1表示失败
+  */
+  static long InsertTaskInfo(int CarType,
+                            std::string Standard,
+                            std::string DateTime,
+                            double OverThresholdValue,
+                            int StandardNum);
+
+  /**
+  * @brief 插入测试任务数据采集表
+  * @brief 输入参数
+  * @return 返回插入数据的Id ，-1表示失败
+  */
+  static int InsertTaskData(long TaskId,
+                            double SetSpeed,
+                            double ActualSpeed,
+                            long AccOrderPsng,
+                            long BrkOrderPsng,
+                            double StandardSpeedTimeStamp,
+                            int OverSpeedNum,
+                            int StandardTaskCount);
+
+  /**
+  * @brief 测试任务数据采集表界面显示
+  * @brief 输入参数
+  * @return 返回插入数据的Id ，-1表示失败
+  */
+  static int InsertTaskDataUI(long TaskId,
+                              double SetSpeed,
+                              double ActualSpeed,
+                              double StandardSpeedTimeStamp,
+                              int OverSpeedNum,
+                              int StandardTaskCount);
+private:
+
+};
+}// namespace canbusSpace
+}// namespace robot
+#endif // CANBUSMYSQLDATABASE_H

src/canbus/candatatype.h

@@ -6,6 +6,7 @@
 
 #ifndef CAN_DATA_TYPE_H_
 #define CAN_DATA_TYPE_H_
+#include <string>
 //version 4.5.1.X
 namespace robot {
 namespace canbusSpace {
@@ -2207,6 +2208,90 @@ typedef struct
 }T_OBD_ACC_PSNG;
 //最小值 005   最大值09DA
 
+/************MYSQL数据库 系统参数数据表*******/
+typedef struct
+{
+  long Id;
+  std::string CarName;
+  double P;
+  double I;
+  double D;
+  double KP;
+  double KI;
+  double KD;
+  int BrkFactor;
+  int BrkMax;
+} RACK_SYS_PARA;
+
+/************行动元油门制动报警信息*******/
+typedef struct
+{
+  unsigned int  be:1;
+  unsigned int  bf:1;
+  unsigned int  bg:1;
+  unsigned int  bh:1;
+  unsigned int  bi:1;
+  unsigned int  bj:1;
+  unsigned int  bk:1;
+  unsigned int  bl:1;
+  unsigned int  aw:1;
+  unsigned int  ax:1;
+  unsigned int  ay:1;
+  unsigned int  az:1;
+  unsigned int  ba:1;
+  unsigned int  bb:1;
+  unsigned int  bc:1;
+  unsigned int  bd:1;
+  unsigned int  ao:1;
+  unsigned int  ap:1;
+  unsigned int  aq:1;
+  unsigned int  ar:1;
+  unsigned int  as:1;
+  unsigned int  at:1;
+  unsigned int  au:1;
+  unsigned int  av:1;
+  unsigned int  ag:1;
+  unsigned int  ah:1;
+  unsigned int  ai:1;
+  unsigned int  aj:1;
+  unsigned int  ak:1;
+  unsigned int  al:1;
+  unsigned int  am:1;
+  unsigned int  an:1;
+  unsigned int  y:1;
+  unsigned int  z:1;
+  unsigned int  aa:1;
+  unsigned int  ab:1;
+  unsigned int  ac:1;
+  unsigned int  ad:1;
+  unsigned int  ae:1;
+  unsigned int  af:1;
+  unsigned int  q:1;
+  unsigned int  r:1;
+  unsigned int  s:1;
+  unsigned int  t:1;
+  unsigned int  u:1;
+  unsigned int  v:1;
+  unsigned int  w:1;
+  unsigned int  x:1;
+  unsigned int  i:1;
+  unsigned int  j:1;
+  unsigned int  k:1;
+  unsigned int  l:1;
+  unsigned int  m:1;
+  unsigned int  n:1;
+  unsigned int  o:1;
+  unsigned int  p:1;
+  unsigned int  a:1;
+  unsigned int  b:1;
+  unsigned int  c:1;
+  unsigned int  d:1;
+  unsigned int  e:1;
+  unsigned int  f:1;
+  unsigned int  g:1;
+  unsigned int  h:1;
+
+}T_MOT_CTR_ERR_1;
 
 } // namespace canbus
 

src/canbus/msg/rackTestReplyMsg.msg

@@ -1,2 +1,8 @@
 uint32 RackTestReplyState
+float64 SetSpeed
 float64 ActualSpeed
+uint32 TaskTotalTime
+uint32 TaskCurrentTime
+uint32 OverSpeedNum
+uint32 TaskId
+uint32 TaskAlreadyNum

src/canbus/srv/RackTestReq.srv

@@ -1,3 +1,5 @@
 uint32 uiStart                   #启动标志位
+uint32 uiCarId                   #车型ID
+int32 iTaskNum                   #规范文件执行次数
 ---
 uint32 uiReturn

src/launch/robot.launch

@@ -10,8 +10,14 @@ ns = "NAME_SPACE" 这个属性可以让你在自定义的命名空间里运行
 <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" >
     <arg name="port" value="9090"/>
  </include>
-<node pkg="decision" name="decision" type="decision" output="screen" required="true"/>
+<node pkg="decision" name="decision" type="decision" output="screen" required="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="localization" name="localization" type="localization" output="screen" respawn="true"/>-->
 <node pkg="canbus" name="canbus" type="canbus" output="screen" respawn="true"/>
+<param name = "rcu_canbus_mode" value = "tcp" /><!--can总线模式: usb(默认)/tcp-->
+<param name = "rcu_canbus_ip" value = "192.168.200.10" /><!--tcp转can模块IP地址-->
+<param name = "rcu_canbus_port" value = "1234"/><!--tcp转can模块端口-->
+<param name = "car_input_mode" value = "modbus" /><!--速度接入方式: rtk(gpfpd)/modbus(模拟量0~10V)-->
+<param name = "car_input_ip" value = "192.168.200.80" /><!--速度接入IP地址-->
+<param name = "car_input_port" value = "502"/><!--速度接入端口-->
 </launch>

src/localization/localization.cpp

@@ -11,7 +11,8 @@
 #include "../common/message.h"
 #include <iostream>
 #include <cmath>
-
+#include <arpa/inet.h>
+#include <sys/socket.h>
 
 
 namespace  robot{
@@ -19,78 +20,143 @@ namespace localizationspace {
 std::string Localization::Name() const { return "localization"; }
 
 int Localization::Init() {
-    try
-    {
-        LOG(INFO)<<Name()<<" init start ...";
+  LOG(INFO)<<Name()<<" init start ...";
+  if (!ros::param::get("car_input_mode", car_input_mode))
+    car_input_mode = "rtk";
+  LOG(INFO)<< Name()<<": read param car_input_mode = " << car_input_mode;
+
+  if (car_input_mode == "modbus") {
+    std::string car_input_ip;
+    int car_input_port;
+    if (!ros::param::get("car_input_ip", car_input_ip)) {
+        LOG(ERROR)<< Name()<<": read param car_input_ip error: not exist";
+        return ROBOT_FAILTURE;
+    }
+    if (!ros::param::get("car_input_port", car_input_port)) {
+        LOG(ERROR)<< Name()<<": read param car_input_port error: not exist";
+        return ROBOT_FAILTURE;
+    }
+    LOG(INFO)<< Name() <<": read param car_input_ip:port = " << car_input_ip << ":" << car_input_port;
+    modbus_socket = socket(AF_INET, SOCK_STREAM, 0);
+    struct sockaddr_in serv_addr;
+    memset(&serv_addr, 0, sizeof(serv_addr));  //每个字节都用0填充
+    serv_addr.sin_family = AF_INET;  //使用IPv4地址
+    serv_addr.sin_addr.s_addr = inet_addr(car_input_ip.c_str());
+    serv_addr.sin_port = htons(car_input_port);  //端口
+    int result = connect(modbus_socket, (struct sockaddr*)&serv_addr, sizeof(serv_addr));
+    if (result == 0) {
+      LOG(INFO) << Name() << ": " << car_input_ip << ":" << car_input_port << " connected";
+    }
+    else {
+      LOG(ERROR) << Name() << ": connecting " << car_input_ip << ":" << car_input_port << " failed";
+    }
+  }
+  else if (car_input_mode == "rtk") {
+    try {
         serial_port = "/dev/ttyUSB0";
 //        baudrate = 115200;
 //        serial_port = "/dev/tty0";
         baudrate = 460800;
         timeout = 1000;//ms
-        node_handle_.reset(new ros::NodeHandle());
-        LOG(INFO)<<Name()<<" init end.";
     }
     catch (ros::Exception e)
     {
         LOG(ERROR)<<Name()<< " Unable to Init " << e.what();
         return ROBOT_FAILTURE;
     }
-    return ROBOT_SUCCESS;
+  }
+  node_handle_.reset(new ros::NodeHandle());
+  LOG(INFO)<<Name()<<" init end.";
+  return ROBOT_SUCCESS;
 }
 
 int Localization::Start() {
- LOG(INFO)<<Name()<<" start start...";
- try
- {
-     geometry_pub = node_handle_->advertise<localization::localMsg>(robot::common::Geometry, 1);
-     //设置串口属性，并打开串口
-     serial_gnss.setPort(serial_port);
-     serial_gnss.setBaudrate(baudrate);
-     serial::Timeout to = serial::Timeout::simpleTimeout(timeout);
-     serial_gnss.setTimeout(to);
-     serial_gnss.open();
-     if (IsOpen())
-     {
-         //指定循环的频率
-         ros::Rate loop_rate(rate);
-         while(ros::ok())
-         {
-           while(serial_gnss.available() >0)
+  LOG(INFO)<<Name()<<" start start...";
+  geometry_pub = node_handle_->advertise<localization::localMsg>(robot::common::Geometry, 1);
+  if (car_input_mode == "modbus") {
+    ros::Rate loop_rate(rate);
+    while(ros::ok())
+    {
+      int send_length = write(modbus_socket, modbus_req, sizeof(modbus_req));
+      if (send_length != sizeof(modbus_req)){
+         LOG(ERROR) << Name() <<" modbus send error: " << send_length;
+      }
+      else {
+        int recv_len = read(modbus_socket, modbus_recv,  sizeof(modbus_recv));
+        if (recv_len != sizeof(modbus_recv)) {
+          LOG(ERROR) << Name() <<" canbus recv read error: " << recv_len;
+        }
+        else {
+          try{
+              localization::localMsg localization;
+              localization.Longitude =  0;
+              localization.Latitude = 0;
+              localization.Height = 0;
+              localization.AngleDeviated = 0;
+              int voltage = (unsigned int)modbus_recv[sizeof(modbus_recv)-2] * 256 + (unsigned int)modbus_recv[sizeof(modbus_recv)-1];
+              localization.Speed = voltage / 65535.0 * 180.0;
+              geometry_pub.publish(localization);
+              LOG(INFO) << Name() << " publish speed:" << std::setprecision(4) << localization.Speed;
+          }
+          catch (ros::Exception ex) {
+              LOG(ERROR)<<Name()<< " publish speed error: " << ex.what();
+          }
+        }
+      }
+      ros::spinOnce();
+      loop_rate.sleep();
+    }
+  }
+  else if (car_input_mode == "rtk") {
+    try {
+       //设置串口属性，并打开串口
+       serial_gnss.setPort(serial_port);
+       serial_gnss.setBaudrate(baudrate);
+       serial::Timeout to = serial::Timeout::simpleTimeout(timeout);
+       serial_gnss.setTimeout(to);
+       serial_gnss.open();
+       if (IsOpen())
+       {
+           //指定循环的频率
+           ros::Rate loop_rate(rate);
+           while(ros::ok())
            {
-             std::string strSerialResult;
-             strSerialResult = serial_gnss.readline();
-           //  LOG(INFO)<<Name()<<"  Read: " << strSerialResult;
-             NMEA0183 nmea;
-             if(Analyse_ReadLine(strSerialResult,nmea) == ROBOT_SUCCESS)
+             while(serial_gnss.available() >0)
              {
-               PublishData(nmea);
+               std::string strSerialResult;
+               strSerialResult = serial_gnss.readline();
+             //  LOG(INFO)<<Name()<<"  Read: " << strSerialResult;
+               NMEA0183 nmea;
+               if(Analyse_ReadLine(strSerialResult,nmea) == ROBOT_SUCCESS)
+               {
+                 PublishData(nmea);
+               }
              }
-           }
-           //TODO
-//           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());
-//               strSerialResult = serial_gnss.readline(serial_gnss.available());
-//               LOG(INFO)<<Name()<<"  Read: " << strSerialResult;
-//               NMEA0183 nmea;
-//               if (Analyse(strSerialResult, nmea) == ROBOT_SUCCESS)
-//                   PublishData(nmea);
-//           }
-           //TODO
+             //TODO
+  //           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());
+  //               strSerialResult = serial_gnss.readline(serial_gnss.available());
+  //               LOG(INFO)<<Name()<<"  Read: " << strSerialResult;
+  //               NMEA0183 nmea;
+  //               if (Analyse(strSerialResult, nmea) == ROBOT_SUCCESS)
+  //                   PublishData(nmea);
+  //           }
+             //TODO
 
-           ros::spinOnce();
-           loop_rate.sleep();
-         }
-     }
- }
- catch (serial::IOException& e)
- {
-     LOG(ERROR)<<Name()<< " Unable to open port " << e.what();
-     return ROBOT_FAILTURE;
- }
- LOG(INFO)<<Name()<<" start end.";
- return ROBOT_SUCCESS;
+             ros::spinOnce();
+             loop_rate.sleep();
+           }
+       }
+    }
+    catch (serial::IOException& e) {
+      LOG(ERROR)<<Name()<< " Unable to open port " << e.what();
+      return ROBOT_FAILTURE;
+    }
+  }
+  LOG(INFO)<<Name()<<" start end.";
+  return ROBOT_SUCCESS;
 }
 
 bool Localization::IsOpen()
@@ -108,10 +174,14 @@ bool Localization::IsOpen()
   }
 }
 
-
 void Localization::Stop() {
   LOG(INFO)<<Name()<<" Stop start...";
-  serial_gnss.close();
+  if (car_input_mode == "modbus") {
+     close(modbus_socket);
+  }
+  else if (car_input_mode == "rtk") {
+    serial_gnss.close();
+  }
   LOG(INFO)<<Name()<<" Stop end.";
 }
 

src/localization/localization.h

@@ -21,7 +21,6 @@
 #include "ros/ros.h"
 #include <stdlib.h>
 #include <math.h>
-
 #include "localizationdata.h"
 
 /**
@@ -122,6 +121,15 @@ private:
 
   const double rate = 100;  //100HZ
 
+  //20240111 by zhaowei add tcp2can
+  std::string car_input_mode; //速度接入方式: rtk(gpfpd)/modbus(模拟量0~10V)
+
+  int modbus_socket;
+
+  const unsigned char modbus_req[12] = {0x00, 0x01, 0x00, 0x00, 0x00, 0x06, 0x01, 0x04, 0x01, 0x01, 0x00, 0x01};
+
+  unsigned char modbus_recv[11];
+  //----------
 };
 
 

src/perception/CMakeLists.txt

@@ -62,6 +62,7 @@ add_message_files(
   si1Msg.msg
   smMsg.msg
   zsftStatsMsg.msg
+  cecMsg2.msg
   )
 
 ## Generate services in the 'srv' folder

src/perception/msg/cecMsg2.msg

@@ -0,0 +1,64 @@
+uint32  Erra
+uint32  Errb
+uint32  Errc
+uint32  Errd
+uint32  Erre
+uint32  Errf
+uint32  Errg
+uint32  Errh
+uint32  Erri
+uint32  Errj
+uint32  Errk
+uint32  Errl
+uint32  Errm
+uint32  Errn
+uint32  Erro
+uint32  Errp
+uint32  Errq
+uint32  Errr
+uint32  Errs
+uint32  Errt
+uint32  Erru
+uint32  Errv
+uint32  Errw
+uint32  Errx
+uint32  Erry
+uint32  Errz
+uint32  Erraa
+uint32  Errab
+uint32  Errac
+uint32  Errad
+uint32  Errae
+uint32  Erraf
+uint32  Errag
+uint32  Errah
+uint32  Errai
+uint32  Erraj
+uint32  Errak
+uint32  Erral
+uint32  Erram
+uint32  Erran
+uint32  Errao
+uint32  Errap
+uint32  Erraq
+uint32  Errar
+uint32  Erras
+uint32  Errat
+uint32  Errau
+uint32  Errav
+uint32  Erraw
+uint32  Errax
+uint32  Erray
+uint32  Erraz
+uint32  Errba
+uint32  Errbb
+uint32  Errbc
+uint32  Errbd
+uint32  Errbe
+uint32  Errbf
+uint32  Errbg
+uint32  Errbh
+uint32  Errbi
+uint32  Errbj
+uint32  Errbk
+uint32  Errbl

src/perception/perception.cpp

@@ -41,8 +41,8 @@ int Perception::Start()
 
         bus_pub_6D1 = node_handle_->advertise<::perception::seMsg>(robot::common::SeData, 1);
 
-        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_6D2 = node_handle_->advertise<::perception::cecMsg2>(robot::common::AcceData, 1);
+        bus_pub_6D3 = node_handle_->advertise<::perception::cecMsg2>(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);
@@ -107,23 +107,23 @@ LOG(INFO)<<Name()<<" Receive 6EE";
         break;
     case robot::common::AccErr:
         //SoftStopControl();
-        PublishState_6D2_6(ucCanMsgSrc, 2, bus_pub_6D2);
+        PublishState_6D2_3(ucCanMsgSrc, 2, bus_pub_6D2);
         break;
     case robot::common::BrkErr:
         //SoftStopControl();
-        PublishState_6D2_6(ucCanMsgSrc, 3, bus_pub_6D3);
+        PublishState_6D2_3(ucCanMsgSrc, 3, bus_pub_6D3);
         break;
     case robot::common::XsftErr:
         //SoftStopControl();
-        PublishState_6D2_6(ucCanMsgSrc, 4, bus_pub_6D4);
+        PublishState_6D4_6(ucCanMsgSrc, 4, bus_pub_6D4);
         break;
     case robot::common::YsftErr:
         //SoftStopControl();
-        PublishState_6D2_6(ucCanMsgSrc, 5, bus_pub_6D5);
+        PublishState_6D4_6(ucCanMsgSrc, 5, bus_pub_6D5);
         break;
     case robot::common::StrErr:
         //SoftStopControl();
-        PublishState_6D2_6(ucCanMsgSrc, 6, bus_pub_6D6);
+        PublishState_6D4_6(ucCanMsgSrc, 6, bus_pub_6D6);
         break;
     case robot::common::EstopInfo:
         PublishState_2E0(ucCanMsgSrc);
@@ -402,29 +402,20 @@ bool Perception::PublishState_6D1(unsigned char* msg)
     }
 }
 
-bool Perception::PublishState_6D2_6(unsigned char* msg, int type, const ros::Publisher &pub)
+bool Perception::PublishState_6D2_3(unsigned char* msg, int type, const ros::Publisher &pub)
 {
 
 
-    robot::canbusSpace::T_ACC_ERR *pErr;
+    robot::canbusSpace::T_MOT_CTR_ERR_1 *pErr;
     switch (type) {
     case 2:
         pErr = &s_state_6D2;
         break;
     case 3:
-        pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D3;
-        break;
-    case 4:
-        pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D4;
-        break;
-    case 5:
-        pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D5;
-        break;
-    case 6:
-        pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D6;
+        pErr = &s_state_6D3;
         break;
     default:
-        LOG(INFO) << Name() << " PublishState_6D2_6 " << type << " type is unvalid";
+        LOG(INFO) << Name() << " PublishState_6D2_3 " << type << " type is unvalid";
         return false;
         break;
     }
@@ -435,23 +426,27 @@ bool Perception::PublishState_6D2_6(unsigned char* msg, int type, const ros::Pub
 //    else
 //        copySize = sizeof(s_state_6D2);
 
-    //6D2_2 6D2_3 6D2_4 6D2_5 6D2_6 数据结构相同
-    robot::canbusSpace::T_ACC_ERR state_6D2;
+    //6D2_2 6D2_3 数据结构相同
+    robot::canbusSpace::T_MOT_CTR_ERR_1 state_6D2;
     memset(&state_6D2, 0 ,sizeof(state_6D2));
     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 ||
-            state_6D2.uiAccErraf != pErr->uiAccErraf || state_6D2.uiAccErrag != pErr->uiAccErrag || state_6D2.uiAccErrah != pErr->uiAccErrah ||
-            state_6D2.uiAccErrai != pErr->uiAccErrai || state_6D2.uiAccErrb != pErr->uiAccErrb || state_6D2.uiAccErrc != pErr->uiAccErrc ||
-            state_6D2.uiAccErrd != pErr->uiAccErrd || state_6D2.uiAccErre != pErr->uiAccErre || state_6D2.uiAccErrf != pErr->uiAccErrf ||
-            state_6D2.uiAccErrg != pErr->uiAccErrg || state_6D2.uiAccErrh != pErr->uiAccErrh || state_6D2.uiAccErrai != pErr->uiAccErrai ||
-            state_6D2.uiAccErrj != pErr->uiAccErrj || state_6D2.uiAccErrk != pErr->uiAccErrk || state_6D2.uiAccErrl != pErr->uiAccErrl ||
-            state_6D2.uiAccErrm != pErr->uiAccErrm || state_6D2.uiAccErrn != pErr->uiAccErrn || state_6D2.uiAccErro != pErr->uiAccErro ||
-            state_6D2.uiAccErrp != pErr->uiAccErrp || state_6D2.uiAccErrq != pErr->uiAccErrq || state_6D2.uiAccErrr != pErr->uiAccErrr ||
-            state_6D2.uiAccErrs != pErr->uiAccErrs || state_6D2.uiAccErrt != pErr->uiAccErrt || state_6D2.uiAccErru != pErr->uiAccErru ||
-            state_6D2.uiAccErrv != pErr->uiAccErrv || state_6D2.uiAccErrw != pErr->uiAccErrw || state_6D2.uiAccErrx != pErr->uiAccErrx ||
-            state_6D2.uiAccErry != pErr->uiAccErry || state_6D2.uiAccErrz != pErr->uiAccErrz)
+    if(state_6D2.a != pErr->a || state_6D2.aa != pErr->aa || state_6D2.ab != pErr->ab || state_6D2.ac != pErr->ac
+       || state_6D2.ad != pErr->ad || state_6D2.ae != pErr->ae || state_6D2.af != pErr->af || state_6D2.ag != pErr->ag
+       || state_6D2.ah != pErr->ah || state_6D2.ai != pErr->ai || state_6D2.aj != pErr->aj || state_6D2.ak != pErr->ak
+       || state_6D2.al != pErr->al || state_6D2.am != pErr->am || state_6D2.an != pErr->an || state_6D2.ao != pErr->ao
+       || state_6D2.ap != pErr->ap || state_6D2.aq != pErr->aq || state_6D2.ar != pErr->ar || state_6D2.as != pErr->as
+       || state_6D2.at != pErr->at || state_6D2.au != pErr->au || state_6D2.av != pErr->av || state_6D2.aw != pErr->aw
+       || state_6D2.ax != pErr->ax || state_6D2.ay != pErr->ay || state_6D2.az != pErr->az || state_6D2.b != pErr->b
+       || state_6D2.ba != pErr->ba || state_6D2.bb != pErr->bb || state_6D2.bc != pErr->bc || state_6D2.bd != pErr->be
+       || state_6D2.bf != pErr->bf || state_6D2.bg != pErr->bg || state_6D2.bh != pErr->bh || state_6D2.bi != pErr->bi
+       || state_6D2.bj != pErr->bj || state_6D2.bk != pErr->bk || state_6D2.bl != pErr->bl || state_6D2.c != pErr->c
+       || state_6D2.d != pErr->d || state_6D2.e != pErr->e || state_6D2.f != pErr->f || state_6D2.g != pErr->g
+       || state_6D2.h != pErr->h || state_6D2.i != pErr->i || state_6D2.j != pErr->j || state_6D2.k != pErr->k
+       || state_6D2.l != pErr->l || state_6D2.m != pErr->m || state_6D2.n != pErr->n || state_6D2.o != pErr->o
+       || state_6D2.p != pErr->p || state_6D2.q != pErr->q || state_6D2.r != pErr->r || state_6D2.s != pErr->s
+       || state_6D2.t != pErr->t || state_6D2.u != pErr->u || state_6D2.v != pErr->v || state_6D2.w != pErr->w
+       || state_6D2.x != pErr->x || state_6D2.y != pErr->y || state_6D2.z != pErr->z)
     {
 
 
@@ -462,6 +457,204 @@ bool Perception::PublishState_6D2_6(unsigned char* msg, int type, const ros::Pub
         case 3:
              memcpy(&s_state_6D3, msg, CAN_MSG_LEN);
             break;
+        default:
+            LOG(INFO) << Name() << " PublishState_6D2_3 " << type << " type is unvalid";
+            return false;
+            break;
+        }
+
+        if(type == 2)
+        {
+            cec_msg22.Erra = state_6D2.a;
+            cec_msg22.Erraa = state_6D2.aa;
+            cec_msg22.Errab = state_6D2.ab;
+            cec_msg22.Errac = state_6D2.ac;
+            cec_msg22.Errad = state_6D2.ad;
+            cec_msg22.Errae = state_6D2.ae;
+            cec_msg22.Erraf = state_6D2.af;
+            cec_msg22.Errag = state_6D2.ag;
+            cec_msg22.Errah = state_6D2.ah;
+            cec_msg22.Errai = state_6D2.ai;
+            cec_msg22.Erraj = state_6D2.aj;
+            cec_msg22.Errak = state_6D2.ak;
+            cec_msg22.Erral = state_6D2.al;
+            cec_msg22.Erram = state_6D2.am;
+            cec_msg22.Erran = state_6D2.an;
+            cec_msg22.Errao = state_6D2.ao;
+            cec_msg22.Errap = state_6D2.ap;
+            cec_msg22.Erraq = state_6D2.aq;
+            cec_msg22.Errar = state_6D2.ar;
+            cec_msg22.Erras = state_6D2.as;
+            cec_msg22.Errat = state_6D2.at;
+            cec_msg22.Errau = state_6D2.au;
+            cec_msg22.Errav = state_6D2.av;
+            cec_msg22.Erraw = state_6D2.aw;
+            cec_msg22.Errax = state_6D2.ax;
+            cec_msg22.Erray = state_6D2.ay;
+            cec_msg22.Erraz = state_6D2.az;
+            cec_msg22.Errb = state_6D2.b;
+            cec_msg22.Errba = state_6D2.ba;
+            cec_msg22.Errbb = state_6D2.bb;
+            cec_msg22.Errbc = state_6D2.bc;
+            cec_msg22.Errbd = state_6D2.bd;
+            cec_msg22.Errbe = state_6D2.be;
+            cec_msg22.Errbf = state_6D2.bf;
+            cec_msg22.Errbg = state_6D2.bg;
+            cec_msg22.Errbh = state_6D2.bh;
+            cec_msg22.Errbi = state_6D2.bi;
+            cec_msg22.Errbk = state_6D2.bk;
+            cec_msg22.Errbj = state_6D2.bj;
+            cec_msg22.Errbl = state_6D2.bl;
+            cec_msg22.Errc = state_6D2.c;
+            cec_msg22.Errd = state_6D2.d;
+            cec_msg22.Erre = state_6D2.e;
+            cec_msg22.Errf = state_6D2.f;
+            cec_msg22.Errg = state_6D2.g;
+            cec_msg22.Errh = state_6D2.h;
+            cec_msg22.Erri = state_6D2.i;
+            cec_msg22.Errj = state_6D2.j;
+            cec_msg22.Errk = state_6D2.k;
+            cec_msg22.Errl = state_6D2.l;
+            cec_msg22.Errm = state_6D2.m;
+            cec_msg22.Errn = state_6D2.n;
+            cec_msg22.Erro = state_6D2.o;
+            cec_msg22.Errp = state_6D2.p;
+            cec_msg22.Errq = state_6D2.q;
+            cec_msg22.Errr = state_6D2.r;
+            cec_msg22.Errs = state_6D2.s;
+            cec_msg22.Errt = state_6D2.t;
+            cec_msg22.Erru = state_6D2.u;
+            cec_msg22.Errv = state_6D2.v;
+            cec_msg22.Errw = state_6D2.w;
+            cec_msg22.Errx = state_6D2.x;
+            cec_msg22.Erry = state_6D2.y;
+            cec_msg22.Errz = state_6D2.z;
+        }
+        else if(type == 3)
+        {
+          cec_msg23.Erra = state_6D2.a;
+          cec_msg23.Erraa = state_6D2.aa;
+          cec_msg23.Errab = state_6D2.ab;
+          cec_msg23.Errac = state_6D2.ac;
+          cec_msg23.Errad = state_6D2.ad;
+          cec_msg23.Errae = state_6D2.ae;
+          cec_msg23.Erraf = state_6D2.af;
+          cec_msg23.Errag = state_6D2.ag;
+          cec_msg23.Errah = state_6D2.ah;
+          cec_msg23.Errai = state_6D2.ai;
+          cec_msg23.Erraj = state_6D2.aj;
+          cec_msg23.Errak = state_6D2.ak;
+          cec_msg23.Erral = state_6D2.al;
+          cec_msg23.Erram = state_6D2.am;
+          cec_msg23.Erran = state_6D2.an;
+          cec_msg23.Errao = state_6D2.ao;
+          cec_msg23.Errap = state_6D2.ap;
+          cec_msg23.Erraq = state_6D2.aq;
+          cec_msg23.Errar = state_6D2.ar;
+          cec_msg23.Erras = state_6D2.as;
+          cec_msg23.Errat = state_6D2.at;
+          cec_msg23.Errau = state_6D2.au;
+          cec_msg23.Errav = state_6D2.av;
+          cec_msg23.Erraw = state_6D2.aw;
+          cec_msg23.Errax = state_6D2.ax;
+          cec_msg23.Erray = state_6D2.ay;
+          cec_msg23.Erraz = state_6D2.az;
+          cec_msg23.Errb = state_6D2.b;
+          cec_msg23.Errba = state_6D2.ba;
+          cec_msg23.Errbb = state_6D2.bb;
+          cec_msg23.Errbc = state_6D2.bc;
+          cec_msg23.Errbd = state_6D2.bd;
+          cec_msg23.Errbe = state_6D2.be;
+          cec_msg23.Errbf = state_6D2.bf;
+          cec_msg23.Errbg = state_6D2.bg;
+          cec_msg23.Errbh = state_6D2.bh;
+          cec_msg23.Errbi = state_6D2.bi;
+          cec_msg23.Errbk = state_6D2.bk;
+          cec_msg23.Errbj = state_6D2.bj;
+          cec_msg23.Errbl = state_6D2.bl;
+          cec_msg23.Errc = state_6D2.c;
+          cec_msg23.Errd = state_6D2.d;
+          cec_msg23.Erre = state_6D2.e;
+          cec_msg23.Errf = state_6D2.f;
+          cec_msg23.Errg = state_6D2.g;
+          cec_msg23.Errh = state_6D2.h;
+          cec_msg23.Erri = state_6D2.i;
+          cec_msg23.Errj = state_6D2.j;
+          cec_msg23.Errk = state_6D2.k;
+          cec_msg23.Errl = state_6D2.l;
+          cec_msg23.Errm = state_6D2.m;
+          cec_msg23.Errn = state_6D2.n;
+          cec_msg23.Erro = state_6D2.o;
+          cec_msg23.Errp = state_6D2.p;
+          cec_msg23.Errq = state_6D2.q;
+          cec_msg23.Errr = state_6D2.r;
+          cec_msg23.Errs = state_6D2.s;
+          cec_msg23.Errt = state_6D2.t;
+          cec_msg23.Erru = state_6D2.u;
+          cec_msg23.Errv = state_6D2.v;
+          cec_msg23.Errw = state_6D2.w;
+          cec_msg23.Errx = state_6D2.x;
+          cec_msg23.Erry = state_6D2.y;
+          cec_msg23.Errz = state_6D2.z;
+        }
+
+        return true;
+    }
+    else
+    {
+    //    LOG(INFO) << Name() << " PublishState_6D2_3 " << type << " data is repeat";
+        return false;
+    }
+}
+
+bool Perception::PublishState_6D4_6(unsigned char* msg, int type, const ros::Publisher &pub)
+{
+
+
+    robot::canbusSpace::T_ACC_ERR *pErr;
+    switch (type) {
+    case 4:
+        pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D4;
+        break;
+    case 5:
+        pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D5;
+        break;
+    case 6:
+        pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D6;
+        break;
+    default:
+        LOG(INFO) << Name() << " PublishState_6D4_6 " << type << " type is unvalid";
+        return false;
+        break;
+    }
+
+//    int copySize;
+//    if(msg->data.size() < sizeof(s_state_6D2))
+//        copySize = msg->data.size();
+//    else
+//        copySize = sizeof(s_state_6D2);
+
+    // 6D2_4 6D2_5 6D2_6 数据结构相同
+    robot::canbusSpace::T_ACC_ERR state_6D4;
+    memset(&state_6D4, 0 ,sizeof(state_6D4));
+    memcpy(&state_6D4, msg, CAN_MSG_LEN);
+
+    if(state_6D4.uiAccErra != pErr->uiAccErra || state_6D4.uiAccErraa != pErr->uiAccErraa || state_6D4.uiAccErrab != pErr->uiAccErrab ||
+            state_6D4.uiAccErrac != pErr->uiAccErrac || state_6D4.uiAccErrad != pErr->uiAccErrad || state_6D4.uiAccErrae != pErr->uiAccErrae ||
+            state_6D4.uiAccErraf != pErr->uiAccErraf || state_6D4.uiAccErrag != pErr->uiAccErrag || state_6D4.uiAccErrah != pErr->uiAccErrah ||
+            state_6D4.uiAccErrai != pErr->uiAccErrai || state_6D4.uiAccErrb != pErr->uiAccErrb || state_6D4.uiAccErrc != pErr->uiAccErrc ||
+            state_6D4.uiAccErrd != pErr->uiAccErrd || state_6D4.uiAccErre != pErr->uiAccErre || state_6D4.uiAccErrf != pErr->uiAccErrf ||
+            state_6D4.uiAccErrg != pErr->uiAccErrg || state_6D4.uiAccErrh != pErr->uiAccErrh || state_6D4.uiAccErrai != pErr->uiAccErrai ||
+            state_6D4.uiAccErrj != pErr->uiAccErrj || state_6D4.uiAccErrk != pErr->uiAccErrk || state_6D4.uiAccErrl != pErr->uiAccErrl ||
+            state_6D4.uiAccErrm != pErr->uiAccErrm || state_6D4.uiAccErrn != pErr->uiAccErrn || state_6D4.uiAccErro != pErr->uiAccErro ||
+            state_6D4.uiAccErrp != pErr->uiAccErrp || state_6D4.uiAccErrq != pErr->uiAccErrq || state_6D4.uiAccErrr != pErr->uiAccErrr ||
+            state_6D4.uiAccErrs != pErr->uiAccErrs || state_6D4.uiAccErrt != pErr->uiAccErrt || state_6D4.uiAccErru != pErr->uiAccErru ||
+            state_6D4.uiAccErrv != pErr->uiAccErrv || state_6D4.uiAccErrw != pErr->uiAccErrw || state_6D4.uiAccErrx != pErr->uiAccErrx ||
+            state_6D4.uiAccErry != pErr->uiAccErry || state_6D4.uiAccErrz != pErr->uiAccErrz)
+    {
+
+
+        switch (type) {
         case 4:
              memcpy(&s_state_6D4, msg, CAN_MSG_LEN);
             break;
@@ -472,207 +665,131 @@ bool Perception::PublishState_6D2_6(unsigned char* msg, int type, const ros::Pub
              memcpy(&s_state_6D6, msg, CAN_MSG_LEN);
             break;
         default:
-            LOG(INFO) << Name() << " PublishState_6D2_6 " << type << " type is unvalid";
+            LOG(INFO) << Name() << " PublishState_6D4_6 " << type << " type is unvalid";
             return false;
             break;
         }
 
-        if(type == 2)
-        {
-            cec_msg2.Erra = state_6D2.uiAccErra;
-            cec_msg2.Erraa = state_6D2.uiAccErraa;
-            cec_msg2.Errab = state_6D2.uiAccErrab;
-            cec_msg2.Errac = state_6D2.uiAccErrac;
-            cec_msg2.Errad = state_6D2.uiAccErrad;
-            cec_msg2.Errae = state_6D2.uiAccErrae;
-            cec_msg2.Erraf = state_6D2.uiAccErraf;
-            cec_msg2.Errag = state_6D2.uiAccErrag;
-            cec_msg2.Errah = state_6D2.uiAccErrah;
-            cec_msg2.Errai = state_6D2.uiAccErrai;
-            cec_msg2.Errb = state_6D2.uiAccErrb;
-            cec_msg2.Errc = state_6D2.uiAccErrc;
-            cec_msg2.Errd = state_6D2.uiAccErrd;
-            cec_msg2.Erre = state_6D2.uiAccErre;
-            cec_msg2.Errf = state_6D2.uiAccErrf;
-            cec_msg2.Errg = state_6D2.uiAccErrg;
-            cec_msg2.Errh = state_6D2.uiAccErrh;
-            cec_msg2.Erri = state_6D2.uiAccErri;
-            cec_msg2.Errj = state_6D2.uiAccErrj;
-            cec_msg2.Errk = state_6D2.uiAccErrk;
-            cec_msg2.Errl = state_6D2.uiAccErrl;
-            cec_msg2.Errm = state_6D2.uiAccErrm;
-            cec_msg2.Errn = state_6D2.uiAccErrn;
-            cec_msg2.Erro = state_6D2.uiAccErro;
-            cec_msg2.Errp = state_6D2.uiAccErrp;
-            cec_msg2.Errq = state_6D2.uiAccErrq;
-            cec_msg2.Errr = state_6D2.uiAccErrr;
-            cec_msg2.Errs = state_6D2.uiAccErrs;
-            cec_msg2.Errt = state_6D2.uiAccErrt;
-            cec_msg2.Erru = state_6D2.uiAccErru;
-            cec_msg2.Errv = state_6D2.uiAccErrv;
-            cec_msg2.Errw = state_6D2.uiAccErrw;
-            cec_msg2.Errx = state_6D2.uiAccErrx;
-            cec_msg2.Erry = state_6D2.uiAccErry;
-            cec_msg2.Errz = state_6D2.uiAccErrz;
-        }
-        else if(type == 3)
-        {
-          cec_msg3.Erra = state_6D2.uiAccErra;
-          cec_msg3.Erraa = state_6D2.uiAccErraa;
-          cec_msg3.Errab = state_6D2.uiAccErrab;
-          cec_msg3.Errac = state_6D2.uiAccErrac;
-          cec_msg3.Errad = state_6D2.uiAccErrad;
-          cec_msg3.Errae = state_6D2.uiAccErrae;
-          cec_msg3.Erraf = state_6D2.uiAccErraf;
-          cec_msg3.Errag = state_6D2.uiAccErrag;
-          cec_msg3.Errah = state_6D2.uiAccErrah;
-          cec_msg3.Errai = state_6D2.uiAccErrai;
-          cec_msg3.Errb = state_6D2.uiAccErrb;
-          cec_msg3.Errc = state_6D2.uiAccErrc;
-          cec_msg3.Errd = state_6D2.uiAccErrd;
-          cec_msg3.Erre = state_6D2.uiAccErre;
-          cec_msg3.Errf = state_6D2.uiAccErrf;
-          cec_msg3.Errg = state_6D2.uiAccErrg;
-          cec_msg3.Errh = state_6D2.uiAccErrh;
-          cec_msg3.Erri = state_6D2.uiAccErri;
-          cec_msg3.Errj = state_6D2.uiAccErrj;
-          cec_msg3.Errk = state_6D2.uiAccErrk;
-          cec_msg3.Errl = state_6D2.uiAccErrl;
-          cec_msg3.Errm = state_6D2.uiAccErrm;
-          cec_msg3.Errn = state_6D2.uiAccErrn;
-          cec_msg3.Erro = state_6D2.uiAccErro;
-          cec_msg3.Errp = state_6D2.uiAccErrp;
-          cec_msg3.Errq = state_6D2.uiAccErrq;
-          cec_msg3.Errr = state_6D2.uiAccErrr;
-          cec_msg3.Errs = state_6D2.uiAccErrs;
-          cec_msg3.Errt = state_6D2.uiAccErrt;
-          cec_msg3.Erru = state_6D2.uiAccErru;
-          cec_msg3.Errv = state_6D2.uiAccErrv;
-          cec_msg3.Errw = state_6D2.uiAccErrw;
-          cec_msg3.Errx = state_6D2.uiAccErrx;
-          cec_msg3.Erry = state_6D2.uiAccErry;
-          cec_msg3.Errz = state_6D2.uiAccErrz;
-        }
-        else if(type ==4)
+        if(type ==4)
         {
-          cec_msg4.Erra = state_6D2.uiAccErra;
-          cec_msg4.Erraa = state_6D2.uiAccErraa;
-          cec_msg4.Errab = state_6D2.uiAccErrab;
-          cec_msg4.Errac = state_6D2.uiAccErrac;
-          cec_msg4.Errad = state_6D2.uiAccErrad;
-          cec_msg4.Errae = state_6D2.uiAccErrae;
-          cec_msg4.Erraf = state_6D2.uiAccErraf;
-          cec_msg4.Errag = state_6D2.uiAccErrag;
-          cec_msg4.Errah = state_6D2.uiAccErrah;
-          cec_msg4.Errai = state_6D2.uiAccErrai;
-          cec_msg4.Errb = state_6D2.uiAccErrb;
-          cec_msg4.Errc = state_6D2.uiAccErrc;
-          cec_msg4.Errd = state_6D2.uiAccErrd;
-          cec_msg4.Erre = state_6D2.uiAccErre;
-          cec_msg4.Errf = state_6D2.uiAccErrf;
-          cec_msg4.Errg = state_6D2.uiAccErrg;
-          cec_msg4.Errh = state_6D2.uiAccErrh;
-          cec_msg4.Erri = state_6D2.uiAccErri;
-          cec_msg4.Errj = state_6D2.uiAccErrj;
-          cec_msg4.Errk = state_6D2.uiAccErrk;
-          cec_msg4.Errl = state_6D2.uiAccErrl;
-          cec_msg4.Errm = state_6D2.uiAccErrm;
-          cec_msg4.Errn = state_6D2.uiAccErrn;
-          cec_msg4.Erro = state_6D2.uiAccErro;
-          cec_msg4.Errp = state_6D2.uiAccErrp;
-          cec_msg4.Errq = state_6D2.uiAccErrq;
-          cec_msg4.Errr = state_6D2.uiAccErrr;
-          cec_msg4.Errs = state_6D2.uiAccErrs;
-          cec_msg4.Errt = state_6D2.uiAccErrt;
-          cec_msg4.Erru = state_6D2.uiAccErru;
-          cec_msg4.Errv = state_6D2.uiAccErrv;
-          cec_msg4.Errw = state_6D2.uiAccErrw;
-          cec_msg4.Errx = state_6D2.uiAccErrx;
-          cec_msg4.Erry = state_6D2.uiAccErry;
-          cec_msg4.Errz = state_6D2.uiAccErrz;
+          cec_msg4.Erra = state_6D4.uiAccErra;
+          cec_msg4.Erraa = state_6D4.uiAccErraa;
+          cec_msg4.Errab = state_6D4.uiAccErrab;
+          cec_msg4.Errac = state_6D4.uiAccErrac;
+          cec_msg4.Errad = state_6D4.uiAccErrad;
+          cec_msg4.Errae = state_6D4.uiAccErrae;
+          cec_msg4.Erraf = state_6D4.uiAccErraf;
+          cec_msg4.Errag = state_6D4.uiAccErrag;
+          cec_msg4.Errah = state_6D4.uiAccErrah;
+          cec_msg4.Errai = state_6D4.uiAccErrai;
+          cec_msg4.Errb = state_6D4.uiAccErrb;
+          cec_msg4.Errc = state_6D4.uiAccErrc;
+          cec_msg4.Errd = state_6D4.uiAccErrd;
+          cec_msg4.Erre = state_6D4.uiAccErre;
+          cec_msg4.Errf = state_6D4.uiAccErrf;
+          cec_msg4.Errg = state_6D4.uiAccErrg;
+          cec_msg4.Errh = state_6D4.uiAccErrh;
+          cec_msg4.Erri = state_6D4.uiAccErri;
+          cec_msg4.Errj = state_6D4.uiAccErrj;
+          cec_msg4.Errk = state_6D4.uiAccErrk;
+          cec_msg4.Errl = state_6D4.uiAccErrl;
+          cec_msg4.Errm = state_6D4.uiAccErrm;
+          cec_msg4.Errn = state_6D4.uiAccErrn;
+          cec_msg4.Erro = state_6D4.uiAccErro;
+          cec_msg4.Errp = state_6D4.uiAccErrp;
+          cec_msg4.Errq = state_6D4.uiAccErrq;
+          cec_msg4.Errr = state_6D4.uiAccErrr;
+          cec_msg4.Errs = state_6D4.uiAccErrs;
+          cec_msg4.Errt = state_6D4.uiAccErrt;
+          cec_msg4.Erru = state_6D4.uiAccErru;
+          cec_msg4.Errv = state_6D4.uiAccErrv;
+          cec_msg4.Errw = state_6D4.uiAccErrw;
+          cec_msg4.Errx = state_6D4.uiAccErrx;
+          cec_msg4.Erry = state_6D4.uiAccErry;
+          cec_msg4.Errz = state_6D4.uiAccErrz;
         }
         else if(type == 5)
         {
-          cec_msg5.Erra = state_6D2.uiAccErra;
-          cec_msg5.Erraa = state_6D2.uiAccErraa;
-          cec_msg5.Errab = state_6D2.uiAccErrab;
-          cec_msg5.Errac = state_6D2.uiAccErrac;
-          cec_msg5.Errad = state_6D2.uiAccErrad;
-          cec_msg5.Errae = state_6D2.uiAccErrae;
-          cec_msg5.Erraf = state_6D2.uiAccErraf;
-          cec_msg5.Errag = state_6D2.uiAccErrag;
-          cec_msg5.Errah = state_6D2.uiAccErrah;
-          cec_msg5.Errai = state_6D2.uiAccErrai;
-          cec_msg5.Errb = state_6D2.uiAccErrb;
-          cec_msg5.Errc = state_6D2.uiAccErrc;
-          cec_msg5.Errd = state_6D2.uiAccErrd;
-          cec_msg5.Erre = state_6D2.uiAccErre;
-          cec_msg5.Errf = state_6D2.uiAccErrf;
-          cec_msg5.Errg = state_6D2.uiAccErrg;
-          cec_msg5.Errh = state_6D2.uiAccErrh;
-          cec_msg5.Erri = state_6D2.uiAccErri;
-          cec_msg5.Errj = state_6D2.uiAccErrj;
-          cec_msg5.Errk = state_6D2.uiAccErrk;
-          cec_msg5.Errl = state_6D2.uiAccErrl;
-          cec_msg5.Errm = state_6D2.uiAccErrm;
-          cec_msg5.Errn = state_6D2.uiAccErrn;
-          cec_msg5.Erro = state_6D2.uiAccErro;
-          cec_msg5.Errp = state_6D2.uiAccErrp;
-          cec_msg5.Errq = state_6D2.uiAccErrq;
-          cec_msg5.Errr = state_6D2.uiAccErrr;
-          cec_msg5.Errs = state_6D2.uiAccErrs;
-          cec_msg5.Errt = state_6D2.uiAccErrt;
-          cec_msg5.Erru = state_6D2.uiAccErru;
-          cec_msg5.Errv = state_6D2.uiAccErrv;
-          cec_msg5.Errw = state_6D2.uiAccErrw;
-          cec_msg5.Errx = state_6D2.uiAccErrx;
-          cec_msg5.Erry = state_6D2.uiAccErry;
-          cec_msg5.Errz = state_6D2.uiAccErrz;
+          cec_msg5.Erra = state_6D4.uiAccErra;
+          cec_msg5.Erraa = state_6D4.uiAccErraa;
+          cec_msg5.Errab = state_6D4.uiAccErrab;
+          cec_msg5.Errac = state_6D4.uiAccErrac;
+          cec_msg5.Errad = state_6D4.uiAccErrad;
+          cec_msg5.Errae = state_6D4.uiAccErrae;
+          cec_msg5.Erraf = state_6D4.uiAccErraf;
+          cec_msg5.Errag = state_6D4.uiAccErrag;
+          cec_msg5.Errah = state_6D4.uiAccErrah;
+          cec_msg5.Errai = state_6D4.uiAccErrai;
+          cec_msg5.Errb = state_6D4.uiAccErrb;
+          cec_msg5.Errc = state_6D4.uiAccErrc;
+          cec_msg5.Errd = state_6D4.uiAccErrd;
+          cec_msg5.Erre = state_6D4.uiAccErre;
+          cec_msg5.Errf = state_6D4.uiAccErrf;
+          cec_msg5.Errg = state_6D4.uiAccErrg;
+          cec_msg5.Errh = state_6D4.uiAccErrh;
+          cec_msg5.Erri = state_6D4.uiAccErri;
+          cec_msg5.Errj = state_6D4.uiAccErrj;
+          cec_msg5.Errk = state_6D4.uiAccErrk;
+          cec_msg5.Errl = state_6D4.uiAccErrl;
+          cec_msg5.Errm = state_6D4.uiAccErrm;
+          cec_msg5.Errn = state_6D4.uiAccErrn;
+          cec_msg5.Erro = state_6D4.uiAccErro;
+          cec_msg5.Errp = state_6D4.uiAccErrp;
+          cec_msg5.Errq = state_6D4.uiAccErrq;
+          cec_msg5.Errr = state_6D4.uiAccErrr;
+          cec_msg5.Errs = state_6D4.uiAccErrs;
+          cec_msg5.Errt = state_6D4.uiAccErrt;
+          cec_msg5.Erru = state_6D4.uiAccErru;
+          cec_msg5.Errv = state_6D4.uiAccErrv;
+          cec_msg5.Errw = state_6D4.uiAccErrw;
+          cec_msg5.Errx = state_6D4.uiAccErrx;
+          cec_msg5.Erry = state_6D4.uiAccErry;
+          cec_msg5.Errz = state_6D4.uiAccErrz;
         }
         else if(type == 6)
         {
-          cec_msg6.Erra = state_6D2.uiAccErra;
-          cec_msg6.Erraa = state_6D2.uiAccErraa;
-          cec_msg6.Errab = state_6D2.uiAccErrab;
-          cec_msg6.Errac = state_6D2.uiAccErrac;
-          cec_msg6.Errad = state_6D2.uiAccErrad;
-          cec_msg6.Errae = state_6D2.uiAccErrae;
-          cec_msg6.Erraf = state_6D2.uiAccErraf;
-          cec_msg6.Errag = state_6D2.uiAccErrag;
-          cec_msg6.Errah = state_6D2.uiAccErrah;
-          cec_msg6.Errai = state_6D2.uiAccErrai;
-          cec_msg6.Errb = state_6D2.uiAccErrb;
-          cec_msg6.Errc = state_6D2.uiAccErrc;
-          cec_msg6.Errd = state_6D2.uiAccErrd;
-          cec_msg6.Erre = state_6D2.uiAccErre;
-          cec_msg6.Errf = state_6D2.uiAccErrf;
-          cec_msg6.Errg = state_6D2.uiAccErrg;
-          cec_msg6.Errh = state_6D2.uiAccErrh;
-          cec_msg6.Erri = state_6D2.uiAccErri;
-          cec_msg6.Errj = state_6D2.uiAccErrj;
-          cec_msg6.Errk = state_6D2.uiAccErrk;
-          cec_msg6.Errl = state_6D2.uiAccErrl;
-          cec_msg6.Errm = state_6D2.uiAccErrm;
-          cec_msg6.Errn = state_6D2.uiAccErrn;
-          cec_msg6.Erro = state_6D2.uiAccErro;
-          cec_msg6.Errp = state_6D2.uiAccErrp;
-          cec_msg6.Errq = state_6D2.uiAccErrq;
-          cec_msg6.Errr = state_6D2.uiAccErrr;
-          cec_msg6.Errs = state_6D2.uiAccErrs;
-          cec_msg6.Errt = state_6D2.uiAccErrt;
-          cec_msg6.Erru = state_6D2.uiAccErru;
-          cec_msg6.Errv = state_6D2.uiAccErrv;
-          cec_msg6.Errw = state_6D2.uiAccErrw;
-          cec_msg6.Errx = state_6D2.uiAccErrx;
-          cec_msg6.Erry = state_6D2.uiAccErry;
-          cec_msg6.Errz = state_6D2.uiAccErrz;
+          cec_msg6.Erra = state_6D4.uiAccErra;
+          cec_msg6.Erraa = state_6D4.uiAccErraa;
+          cec_msg6.Errab = state_6D4.uiAccErrab;
+          cec_msg6.Errac = state_6D4.uiAccErrac;
+          cec_msg6.Errad = state_6D4.uiAccErrad;
+          cec_msg6.Errae = state_6D4.uiAccErrae;
+          cec_msg6.Erraf = state_6D4.uiAccErraf;
+          cec_msg6.Errag = state_6D4.uiAccErrag;
+          cec_msg6.Errah = state_6D4.uiAccErrah;
+          cec_msg6.Errai = state_6D4.uiAccErrai;
+          cec_msg6.Errb = state_6D4.uiAccErrb;
+          cec_msg6.Errc = state_6D4.uiAccErrc;
+          cec_msg6.Errd = state_6D4.uiAccErrd;
+          cec_msg6.Erre = state_6D4.uiAccErre;
+          cec_msg6.Errf = state_6D4.uiAccErrf;
+          cec_msg6.Errg = state_6D4.uiAccErrg;
+          cec_msg6.Errh = state_6D4.uiAccErrh;
+          cec_msg6.Erri = state_6D4.uiAccErri;
+          cec_msg6.Errj = state_6D4.uiAccErrj;
+          cec_msg6.Errk = state_6D4.uiAccErrk;
+          cec_msg6.Errl = state_6D4.uiAccErrl;
+          cec_msg6.Errm = state_6D4.uiAccErrm;
+          cec_msg6.Errn = state_6D4.uiAccErrn;
+          cec_msg6.Erro = state_6D4.uiAccErro;
+          cec_msg6.Errp = state_6D4.uiAccErrp;
+          cec_msg6.Errq = state_6D4.uiAccErrq;
+          cec_msg6.Errr = state_6D4.uiAccErrr;
+          cec_msg6.Errs = state_6D4.uiAccErrs;
+          cec_msg6.Errt = state_6D4.uiAccErrt;
+          cec_msg6.Erru = state_6D4.uiAccErru;
+          cec_msg6.Errv = state_6D4.uiAccErrv;
+          cec_msg6.Errw = state_6D4.uiAccErrw;
+          cec_msg6.Errx = state_6D4.uiAccErrx;
+          cec_msg6.Erry = state_6D4.uiAccErry;
+          cec_msg6.Errz = state_6D4.uiAccErrz;
         }
 
         return true;
     }
     else
     {
-    //    LOG(INFO) << Name() << " PublishState_6D2_6 " << type << " data is repeat";
+    //    LOG(INFO) << Name() << " PublishState_6D4_6 " << type << " data is repeat";
         return false;
     }
 }
@@ -1075,7 +1192,7 @@ void Perception::OnTimer(const ros::TimerEvent &)
 
     if(bus_pub_6D2)
     {
-       bus_pub_6D2.publish(cec_msg2);
+       bus_pub_6D2.publish(cec_msg22);
     }
     else
     {
@@ -1084,7 +1201,7 @@ void Perception::OnTimer(const ros::TimerEvent &)
 
     if(bus_pub_6D3)
     {
-       bus_pub_6D3.publish(cec_msg3);
+       bus_pub_6D3.publish(cec_msg23);
     }
     else
     {

src/perception/perception.h

@@ -16,6 +16,7 @@
 #include "perception/seMsg.h"
 #include "perception/si2Msg.h"
 #include "perception/cecMsg.h"
+#include "perception/cecMsg2.h"
 #include "perception/eiMsg.h"
 #include "perception/eeMsg.h"
 #include "perception/powerMsg.h"
@@ -104,10 +105,16 @@ private:
     bool PublishState_6D1(unsigned char* msg);
 
     /**
-    * @brief Publish State_6D2~State_6D6 topic
+    * @brief Publish State_6D2~State_6D3 topic
     */
 
-    bool PublishState_6D2_6(unsigned char* msg, int type, const ros::Publisher &pub);
+    bool PublishState_6D2_3(unsigned char* msg, int type, const ros::Publisher &pub);
+
+    /**
+    * @brief Publish State_6D4~State_6D6 topic
+    */
+
+    bool PublishState_6D4_6(unsigned char* msg, int type, const ros::Publisher &pub);
 
     /**
     * @brief Publish State_2E0 topic
@@ -214,11 +221,11 @@ ros::Subscriber sub;
      robot::canbusSpace::T_STATE_OUT_6D1 s_state_6D1;
     ::perception::seMsg se_msg;
 
-     robot::canbusSpace::T_ACC_ERR s_state_6D2;
-     ::perception::cecMsg cec_msg2;
+     robot::canbusSpace::T_MOT_CTR_ERR_1 s_state_6D2;
+     ::perception::cecMsg2 cec_msg22;
 
-     robot::canbusSpace::T_BRK_ERR s_state_6D3;
-     ::perception::cecMsg cec_msg3;
+     robot::canbusSpace::T_MOT_CTR_ERR_1 s_state_6D3;
+     ::perception::cecMsg2 cec_msg23;
 
      robot::canbusSpace::T_XSFT_ERR s_state_6D4;
      ::perception::cecMsg cec_msg4;