台架机器人上位机标准版删除usbcan相关代码和decision模块

src/canbus/CMakeLists.txt

@@ -261,7 +261,7 @@ target_link_libraries(canbus
     ${catkin_LIBRARIES}
     glog
     gflags
-    pcan
+#    pcan
     dl
     protobuf
     mysqlpp)

src/canbus/canbus.cpp

@@ -21,6 +21,12 @@
 #include <arpa/inet.h>
 #include <sys/socket.h>
 
+#include <sys/ioctl.h>
+#include <linux/can.h>
+#include <linux/if.h>
+#include <linux/can/raw.h>
+
+
 namespace  robot{
 namespace canbusSpace {
 
@@ -32,7 +38,7 @@ int Canbus::Init() {
   node_handle_.reset(new ros::NodeHandle());
   //20240111 by zhaowei: add tcptocan mode
   if (!ros::param::get("rcu_canbus_mode", canbus_mode))
-    canbus_mode = "usb";
+    canbus_mode = "pcitcp";
   LOG(INFO)<< Name()<<": read param rcu_canbus_mode = " << canbus_mode;
   if (canbus_mode == "tcp") {
     for(int i=0; i < sizeof(s_g_pBufSet); ++i ) {
@@ -40,7 +46,7 @@ int Canbus::Init() {
       s_g_pBufGet[i] = 0x00;
     }
   }
-  //----------
+  /*
   else if (canbus_mode == "usb") {
     //load libpcan.so using dlopen function,return handle for further use
     libm_handle = dlopen("libpcan.so", RTLD_LAZY );
@@ -64,7 +70,8 @@ int Canbus::Init() {
         LOG(ERROR)<<Name()<<" Init Dlsym Error:"<<errorInfo;
         return ROBOT_FAILTURE;
     }
-  }
+  }*/
+
   //initialize the VehHBrkStat as Hold
   status1.uiVehHBrkStat = 1;
 
@@ -103,7 +110,7 @@ int Canbus::Start() {
       LOG(ERROR) << Name() << ": connecting " << canbus_ip << ":" << canbus_port << " failed";
     }
   }
-  else if (canbus_mode == "usb") {
+/*  else if (canbus_mode == "usb") {
     char chVersion[VERSIONSTRING_LEN];            //store information of can version
     const char  *szDevNode = DEFAULT_NODE;  //define const pointer point to device name
     pcan_handle = funLINUX_CAN_Open(szDevNode, O_RDWR );//use mapping function
@@ -130,6 +137,15 @@ int Canbus::Start() {
     {
         LOG(INFO)<<Name()<<": can't open ."<<szDevNode;
     }
+  }*/
+  else if(canbus_mode == "pcitcp"){
+    int ret = 0;
+    ret = can_init_for_linux(&can_fd);
+    if(ret == -1)
+    {
+      LOG(INFO)<<Name()<<"socket can init failed!";
+    }
+
   }
   ros::Rate loop_rate(1/rate);
   // 5. set timer to triger publish info periodly
@@ -298,10 +314,14 @@ void Canbus::Stop() {
   if (canbus_mode == "tcp") {
      close(canbus_socket);
   }
-  else if (canbus_mode == "usb") {
+ /* else if (canbus_mode == "usb") {
     if (pcan_handle) {
         funCAN_Close(pcan_handle);
     }
+  }*/
+  else if (canbus_mode == "pcitcp")
+  {
+    can_close(can_fd);
   }
   LOG(INFO)<<Name()<<": closed. ";
   ros::shutdown();
@@ -363,7 +383,7 @@ int Canbus::read_loop()
         read_data(&(msg.Msg));
       }
     }
-    else if (canbus_mode == "usb") {
+/*    else if (canbus_mode == "usb") {
       if (funLINUX_CAN_Read(pcan_handle, &msg)) {
           LOG(ERROR)<<Name()<<" receive: LINUX_CAN_Read() error.";
           return errno;
@@ -380,6 +400,26 @@ int Canbus::read_loop()
           }
       }
 
+    }*/
+    else if(canbus_mode == "pcitcp"){
+      int can_id = 0x00;
+      char data[8] = {0};
+      int ret = -1;
+      ret = can_read_data(can_fd, &can_id, data);
+      if(ret == -1){
+         return errno;
+      }
+      msg.Msg.ID = can_id;
+      memcpy((unsigned char *)msg.Msg.DATA, (unsigned char *)data, 8);
+      msg.Msg.LEN = 8;
+      msg.Msg.MSGTYPE = MSGTYPE_STANDARD;
+//        for (int i = 0; i < sizeof(msg.Msg.DATA); ++i) {
+//               cout << hex << setw(2) << setfill('0') << static_cast<int>(msg.Msg.DATA[i]) << " ";
+//        }
+//        cout << endl;
+//        LOG(INFO) << Name() << " receive can msg id = " << hex << msg.Msg.ID;
+      read_data(&(msg.Msg));
+
     }
 //TODO
     return ROBOT_SUCCESS;
@@ -533,11 +573,21 @@ int Canbus::write_data(TPCANMsg *pMsgBuff, bool print)
        return errno;
     }
   }
-  else if (canbus_mode == "usb") {
+ /* else if (canbus_mode == "usb") {
     if (funCAN_Write(pcan_handle, pMsgBuff)){
        LOG(ERROR)<<Name()<< " write: LINUX_CAN_write_timeout error. ";
        return errno;
     }
+  }*/
+  else if(canbus_mode == "pcitcp"){
+    char data[8] = {0};
+    memcpy(data, pMsgBuff->DATA, 8);
+    int ret = -1;
+    ret = can_write_data(can_fd,pMsgBuff->ID,data,8);
+    if(ret == -1)
+    {
+      return errno;
+    }
   }
    return ROBOT_SUCCESS;
 }
@@ -3103,6 +3153,159 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
 }
 
 
+/**
+  * @brief  Linux socket can通信初始化
+  * @param  fd：CAN通信文件描述符
+  * @retval 成功返回0，失败返回-1
+  */
+int Canbus::can_init_for_linux(int *fd)
+{
+    //0、设置can通信波特率
+    system("ip link set can0 down");
+    //波特率设置为500Kbps
+    system("ip link set can0 type can bitrate 500000");
+    system("ip link set can0 up");
+
+    int ret = 0;
+    //1、创建socket can套接字
+    *fd = socket(AF_CAN, SOCK_RAW, CAN_RAW);
+    if(*fd == -1)
+    {
+        LOG(ERROR) << Name() <<"create socket can failed";
+        return -1;
+    }
+
+    //2、套接子绑定到can0端口
+    struct sockaddr_can addr;
+    struct ifreq ifr;
+    strcpy(ifr.ifr_name, "can0");
+    ioctl(*fd, SIOCGIFINDEX, &ifr);
+    memset(&addr, 0, sizeof(addr));
+    addr.can_family = AF_CAN;
+    addr.can_ifindex = ifr.ifr_ifindex;
+    ret = bind(*fd, (struct sockaddr *)&addr, sizeof(addr));
+    if(ret < 0)
+    {
+        LOG(ERROR) << Name() <<"socket can bind can0 error";
+        return -1;
+    }
+
+    //4、设置CAN过滤规则
+    #if 0
+    struct can_filter recv_filter;
+    recv_filter.can_id = 0x201;
+    recv_filter.can_mask = CAN_SFF_MASK;
+    setsockopt(*can_fd, SOL_CAN_RAW, CAN_RAW_FILTER, &recv_filter, sizeof(recv_filter));
+    #endif
+
+    //5、设置read、write为非堵塞方式
+    int flags;
+    flags = fcntl(*fd, F_GETFL);
+    flags |= O_NONBLOCK;
+    fcntl(*fd, F_SETFL, flags);
+
+    return 0;
+}
+
+/**
+  * @brief  关闭CAN通信文件描述符
+  * @param  fd：CAN通信文件描述符
+  * @retval void
+  */
+void Canbus::can_close(int fd)
+{
+    close(fd);
+}
+
+/**
+  * @brief  CAN发送数据
+  * @param  fd：CAN通信文件描述符
+  * @param  can_id：can设备id
+  * @param  data：需要发送的数据（一次最大8字节）
+  * @param  data_len：需要发送数据长度(Byte)
+  * @retval 成功返回0，失败返回-1
+  */
+int Canbus::can_write_data(int fd, int can_id,char *data,int data_len)
+{
+    int ret = 0;
+    CanRxMsg msg;
+    struct can_frame send_data;
+    if(fd == -1)
+    {
+        LOG(ERROR) << Name() <<"socket can write data failed!";
+        return -1;
+    }
+    send_data.can_dlc = data_len;
+    send_data.can_id = can_id;
+    memcpy(&send_data.data[0], data, data_len);
+    ret = write(fd, &send_data, sizeof(struct can_frame));
+    if(ret == -1)
+    {
+        LOG(ERROR) << Name() <<"socket can write data failed!";
+        return -1;
+    }else{
+        printf("ID=%03X, DLC=%d, data=%02X %02X %02X %02X %02X %02X %02X %02X \n",  \
+        send_data.can_id, send_data.can_dlc,  \
+        send_data.data[0],\
+        send_data.data[1],\
+        send_data.data[2],\
+        send_data.data[3],\
+        send_data.data[4],\
+        send_data.data[5],\
+        send_data.data[6],\
+        send_data.data[7] );
+    }
+
+    return 0;
+}
+
+/**
+  * @brief  CAN发送数据
+  * @param  fd：CAN通信文件描述符
+  * @param  can_id：can设备id
+  * @param  data：存储CAN接收的8字节数据
+  * @retval 成功返回0，失败返回-1
+  */
+int Canbus::can_read_data(int fd, int *can_id, char *data)
+{
+    if(fd == -1)
+    {
+        LOG(ERROR) << Name() <<"socket can read data failed!";
+        return -1;
+    }
+    int ret = 0;
+    struct can_frame recv_data;
+    recv_data.can_id = *can_id;
+    recv_data.can_dlc = 8;
+
+    ret = read(fd, &recv_data,sizeof(struct can_frame));
+    if(ret == -1)
+    {
+        LOG(ERROR) << Name() <<"socket can read data failed!";
+        return -1;
+    }else{
+        memcpy(data, &recv_data.data[0], recv_data.can_dlc);
+        if(recv_data.can_id != *can_id)
+        {
+            LOG(ERROR) << Name() <<"socket can read recv other can id data";
+
+            return -1;
+        }
+    printf("ID=%03X, DLC=%d, data=%02X %02X %02X %02X %02X %02X %02X %02X \n",  \
+      recv_data.can_id, recv_data.can_dlc,  \
+      recv_data.data[0],\
+      recv_data.data[1],\
+      recv_data.data[2],\
+      recv_data.data[3],\
+      recv_data.data[4],\
+      recv_data.data[5],\
+      recv_data.data[6],\
+      recv_data.data[7] );
+    }
+
+    return 0;
+}
+
 
 }  // namespace canbus
 }  // namespace robot

src/canbus/canbus.h

@@ -17,7 +17,7 @@
 #include <vector>
 #include <stdio.h>
 #include <dlfcn.h>
-#include <libpcan.h>
+//#include <libpcan.h>
 #include <fcntl.h>
 #include <errno.h>
 #include <signal.h>
@@ -76,17 +76,17 @@ namespace robot {
 namespace canbusSpace {
 
 //define mapping function according to target function in libpcan.h
-typedef DWORD   (*funCAN_Init_TYPE)(HANDLE hHandle, WORD wBTR0BTR1, int nCANMsgType);
-typedef HANDLE  (*funLINUX_CAN_Open_TYPE)(const char *szDeviceName, int nFlag);
-typedef DWORD   (*funCAN_VersionInfo_TYPE)(HANDLE hHandle, LPSTR lpszTextBuff);
-typedef DWORD   (*funCAN_Close_TYPE)(HANDLE hHandle);
-typedef DWORD   (*funLINUX_CAN_Read_TYPE)(HANDLE hHandle, TPCANRdMsg* pMsgBuff);
-typedef DWORD   (*funCAN_Status_TYPE)(HANDLE hHandle);
-typedef DWORD   (*funCAN_Write_TYPE)(HANDLE hHandle, TPCANMsg* pMsgBuff);
-typedef int     (*funnGetLastError_TYPE)(void);
-
-//the target device name
-#define DEFAULT_NODE "/dev/pcanusb32"
+//typedef DWORD   (*funCAN_Init_TYPE)(HANDLE hHandle, WORD wBTR0BTR1, int nCANMsgType);
+//typedef HANDLE  (*funLINUX_CAN_Open_TYPE)(const char *szDeviceName, int nFlag);
+//typedef DWORD   (*funCAN_VersionInfo_TYPE)(HANDLE hHandle, LPSTR lpszTextBuff);
+//typedef DWORD   (*funCAN_Close_TYPE)(HANDLE hHandle);
+//typedef DWORD   (*funLINUX_CAN_Read_TYPE)(HANDLE hHandle, TPCANRdMsg* pMsgBuff);
+//typedef DWORD   (*funCAN_Status_TYPE)(HANDLE hHandle);
+//typedef DWORD   (*funCAN_Write_TYPE)(HANDLE hHandle, TPCANMsg* pMsgBuff);
+//typedef int     (*funnGetLastError_TYPE)(void);
+
+////the target device name
+//#define DEFAULT_NODE "/dev/pcanusb32"
 #define ROBOT_SUCCESS     0
 #define ROBOT_FAILTURE   -1
 
@@ -154,14 +154,14 @@ class Canbus : public robot::common::RobotApp{
   void RackTestReplyDetailOnTimer(const ros::TimerEvent &event); //台架测试任务细节图所用数据回复消息定时器回调函数
 
   //define function pointer,there is a one-to-one mapping between target function and your defined function
-  funCAN_Init_TYPE        funCAN_Init;
-  funLINUX_CAN_Open_TYPE  funLINUX_CAN_Open;
-  funCAN_VersionInfo_TYPE funCAN_VersionInfo;
-  funCAN_Close_TYPE       funCAN_Close;
-  funLINUX_CAN_Read_TYPE  funLINUX_CAN_Read;
-  funCAN_Status_TYPE      funCAN_Status;
-  funnGetLastError_TYPE   funnGetLastError;
-  funCAN_Write_TYPE       funCAN_Write;
+//  funCAN_Init_TYPE        funCAN_Init;
+//  funLINUX_CAN_Open_TYPE  funLINUX_CAN_Open;
+//  funCAN_VersionInfo_TYPE funCAN_VersionInfo;
+//  funCAN_Close_TYPE       funCAN_Close;
+//  funLINUX_CAN_Read_TYPE  funLINUX_CAN_Read;
+//  funCAN_Status_TYPE      funCAN_Status;
+//  funnGetLastError_TYPE   funnGetLastError;
+//  funCAN_Write_TYPE       funCAN_Write;
 
   /**
   * @brief can bus data read loop
@@ -291,6 +291,39 @@ class Canbus : public robot::common::RobotApp{
   */
   void RTKLocationCallback(const localization::localMsg::ConstPtr& msg);
 
+  /**
+    * @brief  Linux socket can通信初始化
+    * @param  fd：CAN通信文件描述符
+    * @retval 成功返回0，失败返回-1
+    */
+  int can_init_for_linux(int *fd);
+
+  /**
+    * @brief  关闭CAN通信文件描述符
+    * @param  fd：CAN通信文件描述符
+    * @retval void
+    */
+  void can_close(int fd);
+
+  /**
+    * @brief  CAN发送数据
+    * @param  fd：CAN通信文件描述符
+    * @param  can_id：can设备id
+    * @param  data：需要发送的数据（一次最大8字节）
+    * @param  data_len：需要发送数据长度(Byte)
+    * @retval 成功返回0，失败返回-1
+    */
+  int can_write_data(int fd, int can_id,char *data,int data_len);
+
+  /**
+    * @brief  CAN发送数据
+    * @param  fd：CAN通信文件描述符
+    * @param  can_id：can设备id
+    * @param  data：存储CAN接收的8字节数据
+    * @retval 成功返回0，失败返回-1
+    */
+  int can_read_data(int fd, int *can_id, char *data);
+
 private:
 
   int64_t last_timestamp_ = 0;
@@ -418,10 +451,10 @@ private:
 
   const double rate = 0.0001;  //0.0001s
   //GLOBALS
-  void *libm_handle = NULL;//define pointer used for file acess of libpcan.so
-  HANDLE pcan_handle =NULL;//void *pcan_handle
-  unsigned short unBaudrate = CAN_BAUD_500K; //set the communicate baud rate of can bus
-  int nExtended = CAN_INIT_TYPE_ST;       //set can message int standard model   CAN_INIT_TYPE_EX/CAN_INIT_TYPE_ST
+//  void *libm_handle = NULL;//define pointer used for file acess of libpcan.so
+//  HANDLE pcan_handle =NULL;//void *pcan_handle
+//  unsigned short unBaudrate = CAN_BAUD_500K; //set the communicate baud rate of can bus
+//  int nExtended = CAN_INIT_TYPE_ST;       //set can message int standard model   CAN_INIT_TYPE_EX/CAN_INIT_TYPE_ST
 
   T_VEHICLE_STATUS_1 status1 = {0};    //VehicleStaus1
 
@@ -460,6 +493,8 @@ private:
    std::mutex mtx_taskdata;
    std::mutex mtx_taskdataui;
 
+   int can_fd;
+
 };
 
 }  // namespace canbus

src/canbus/candatatype.h

@@ -2362,7 +2362,56 @@ typedef struct
   int BrkMax;
 } RACK_SYS_PARA;
 
-
+/************SocketCan通信 *******/
+typedef struct
+{
+  unsigned int StdId;    /* CAN标准帧ID，占11bit，范围：0~0x7FF */
+  unsigned int ExtId;    /* CAN扩展帧ID，占29bit，范围：0~0x1FFFFFFF */
+  unsigned char IDE;     /* CAN报文ID类型，CAN_ID_STD 或 CAN_ID_EXT */
+  unsigned char RTR;     /* CAN报文类型，CAN_RTR_DATA 或 CAN_RTR_REMOTE */
+  unsigned char DLC;     /* CAN报文数据长度， 范围：0~8  */
+  unsigned char Data[8]; /* CAN报文数据内容，每个字节范围：0~0xFF*/
+} CanTxMsg;
+
+typedef struct
+{
+  unsigned int StdId;    /* CAN标准帧ID，范围：0~0x7FF */
+  unsigned int ExtId;    /* CAN扩展帧ID，范围：0~0x1FFFFFFF */
+  unsigned char IDE;     /* CAN报文ID类型，CAN_ID_STD 或 CAN_ID_EXT */
+  unsigned char RTR;     /* CAN报文类型，CAN_RTR_DATA 或 CAN_RTR_REMOTE */
+  unsigned char DLC;     /* CAN报文数据长度， 范围：0~8  */
+  unsigned char Data[8]; /* CAN报文数据内容，每个字节范围：0~0xFF*/
+  unsigned char FMI;     /* 过滤模式，总共有14中，定义有宏，其值依次为0x1,0x2,0x4,0x8,0x10,0x20,0x40……,此处可以不考虑，忽略*/
+} CanRxMsg;
+
+
+
+
+typedef unsigned int __u32;
+typedef unsigned char __u8;
+typedef unsigned short __u16;
+#define DWORD  __u32
+#define WORD   __u16
+#define BYTE   __u8
+/*
+ * MSGTYPE bits of element MSGTYPE in structure TPCANMsg
+ */
+#define MSGTYPE_STANDARD      0x00     // marks a standard frame
+#define MSGTYPE_RTR           0x01     // marks a remote frame
+#define MSGTYPE_EXTENDED      0x02     // declares a extended frame
+#define MSGTYPE_SELFRECEIVE   0x04     // self-received message
+#define MSGTYPE_STATUS        0x80     // used to mark a status TPCANMsg
+typedef struct pcan_msg {
+  DWORD ID;              // 11/29 bit code
+  BYTE  MSGTYPE;         // bits of MSGTYPE_*
+  BYTE  LEN;             // count of data bytes (0..8)
+  BYTE  DATA[8];         // data bytes, up to 8
+} TPCANMsg;
+typedef struct pcan_rd_msg {
+  TPCANMsg Msg;          // the above message
+  DWORD    dwTime;       // a timestamp in msec, read only
+  WORD     wUsec;        // remainder in micro-seconds
+} TPCANRdMsg; // for PCAN_WRITE_MSG
 
 } // namespace canbus
 

src/decision/CMakeLists.txt

@@ -1,237 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(decision)
-
-## Compile as C++11, supported in ROS Kinetic and newer
-add_compile_options(-std=c++11)
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-    roscpp message_generation std_msgs rospy canbus
-)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
- add_message_files(
-   FILES
-   rulesMsg.msg
-   controlMsg.msg
-   localizationMsg.msg
- )
-
-## Generate services in the 'srv' folder
- add_service_files(
-   FILES
-   CommandModeCmd.srv
- )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
- generate_messages(
-   DEPENDENCIES
-   std_msgs  # Or other packages containing msgs
- )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES desision
-  CATKIN_DEPENDS #other_catkin_pkg
-  message_runtime
-#  DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
- ${catkin_INCLUDE_DIRS}
-    /usr/local/include
-
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/desision.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/desision_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_desision.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
-
-add_executable(decision
-    ../common/time.h
-    ../common/message.h
-    ../common/gloghelper.h
-    ../common/gloghelper.cpp
-    ../common/robotapp.h
-    ../common/robotapp.cpp
-    ../common/sqlite3helper.h
-    ../common/sqlite3helper.cpp
-    ../common/mpc.h
-    ../common/mpc.cpp
-    decision_app.cpp
-    decision.h
-    decision.cpp
-    decisiondatabase.h
-    decisiondatabase.cpp
-    decisiondatatype.h
- )
-add_dependencies(decision ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-target_link_libraries(decision
-    ${catkin_LIBRARIES}
-    glog
-    gflags
-    ipopt
-    dl
-    protobuf
-    sqlite3
-    )

src/decision/decision.cpp

@@ -1,584 +0,0 @@
-/******************************************************************************
- * 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"
-#include <strstream>
-#include <string.h>
-#include <canbus/AccPedalActCtrReq.h>
-#include <canbus/BrkPedalActCtrReq.h>
-#include <canbus/SftActCtrReq.h>
-#include <canbus/StrActCtrReq.h>
-#include <decision/localizationMsg.h>
-#include <decision/rulesMsg.h>
-#include <decision/controlMsg.h>
-#include <stdlib.h>
-#include <math.h>
-
-
-namespace robot {
-namespace decisionSpace {
-
-
-
-std::string Decision::Name() const { return "decision"; }
-
-int Decision::Init(){
-
-    LOG(INFO)<<Name()<<": init() start...";
-    node_handle_.reset(new ros::NodeHandle());
-    DecisionDatabase::InitDB();
-    iCommandCmd = CMD_UNDEFIED;// set undefied mode
-    iAbandon = iRulesTotalNumbers = ilocalTotalNumbers =fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
-    iPredictionPointsPerNumber = 5;
-    uiCarMode = CTR_MD_REQ_ROBOT_UNDIFED; // undefied
-    fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
-    PlanRouteName = "siasun";
-//    for (int i =0; i< 10; i++)
-//        TestMPC();
-    LOG(INFO)<<Name()<<" init() end.";
-    return ROBOT_SUCCESS;
-
-}
-
-int Decision::Start(){
-    LOG(INFO)<<Name()<<" start() start...";
-    // under ros service server
-    ros::ServiceServer CommandService = node_handle_->advertiseService(robot::common::CommandModeCmdService,
-                                                                &Decision::CommandRequestService, this);
-    // under ros subscriber
-    ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 1,
-                                                          &Decision::RTKLocationCallback, this);
-
-    ros::Subscriber OBDSub = node_handle_->subscribe(robot::common::ObdData, 1, &Decision::OBDCallback, 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, 100);
-
-    Geometry_pub = node_handle_->advertise<decision::localizationMsg>(robot::common::GeometryPlan, 100);
-
-    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);
-
-    clientAccPedalActCtrReq = node_handle_->serviceClient<canbus::AccPedalActCtrReq>(robot::common::AccPedalActCtrService);
-    clientBrkPedalActCtrReq = node_handle_->serviceClient<canbus::BrkPedalActCtrReq>(robot::common::BrkPedalActCtrService);
-
-    clientStrActCtrReq = node_handle_->serviceClient<canbus::StrActCtrReq>(robot::common::SteerActCtrService);
-
-    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.";
-    return ROBOT_SUCCESS;
-}
-
-void Decision::Stop(){
-    LOG(INFO)<<Name()<<": stop start...";
-    LOG(INFO)<<Name()<<": stop end.";
-    return ;
-}
-
-//  执行自动驾驶模式和路径规划模式，自动驾驶模式踩刹车然后挂前进当；路径规划模式删除数据库记录
-bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
-                      decision::CommandModeCmd::Response &res)
-{
-    LOG(INFO)<<Name()<<"CommandRequestService get command ID:."<< req.Cmd;
-    if (req.Cmd == CMD_AUTODRIE_START)
-    {
-        LoadPlanRoute();
-        throttle_Last = -70; // 踩刹车到70%
-        ros::Rate loop_rate(1);
-        while(ros::ok())
-        {
-            if (SpeedControl(throttle_Last) == 1)
-            {
-                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 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);
-        ptsy.push_back(0);
-        fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
-    }
-    else if (req.Cmd == CMD_PLAN_START)
-    {
-        DecisionDatabase::DeleteActionDirective(PlanRouteName.c_str());
-        DecisionDatabase::DeleteLocationRuleInfo(PlanRouteName.c_str());
-        iAbandon = fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
-    }
-
-    res.CmdResult = iCommandCmd = req.Cmd;
-    LOG(INFO)<<Name()<<"CommandRequestService end."<< res.CmdResult;
-    return true;
-}
-
-
-// 加载规划路径，同时把规划的路径轨迹和测试的用例发送给显示界面
-void Decision::LoadPlanRoute()
-{
-    ilocalTotalNumbers = DecisionDatabase::countOfLocationRuleInfo();
-    iRulesTotalNumbers = DecisionDatabase::countOfActionDirective();
-    LOG(INFO)<<Name()<<" LoadPlanRoute select ilocalTotalNumbers: "<<ilocalTotalNumbers<<"  iRulesTotalNumbers: "<<iRulesTotalNumbers;
-    std::vector<LOCATION_RULE_INFO> tempInfo = DecisionDatabase::QueryLocationRuleInfo(0, ilocalTotalNumbers);
-    for (int i = 0; i< ilocalTotalNumbers; i++)
-    {
-        decision::localizationMsg localMsg;
-        localMsg.Locationid = tempInfo[i].LocationID;
-        localMsg.Longitude = tempInfo[i].LocationLon;
-        localMsg.Latitude = tempInfo[i].LocationLat;
-        localMsg.Height = tempInfo[i].LocationHeight;
-        localMsg.LocationSpeed = tempInfo[i].LocationSpeed;
-        Geometry_pub.publish(localMsg);
-    }
-    std::vector<ACTION_DIRECTIVE> tempAction = DecisionDatabase::QueryActionDirective(0, iRulesTotalNumbers);
-    for (int i = 0; i< iRulesTotalNumbers; i++)
-    {
-        decision::rulesMsg ruleMsg;
-        ruleMsg.DirectiveID = tempAction[i].DirectiveID;
-        ruleMsg.DirectiveInfo = tempAction[i].DirectiveInfo;
-        ruleMsg.DirectiveResult = tempAction[i].DirectiveResult;
-        Rules_pub.publish(ruleMsg);
-    }
-
-}
-
-// 接收ＲＴＫ数据分为自动驾驶模式开始和结束和路径规划模式开始和结束
-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;
-    }
-}
-
-
-void Decision::OBDCallback(const canbus::obdMsg::ConstPtr& msg)
-{
-    fCarNowSpeed = msg->VehSpd;
-    LOG(INFO)<<Name()<< " car real speed:"<<fCarNowSpeed;
-}
-
-void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
-{
-   uiCarMode = msg->CtrMdStat;
-   //LOG(INFO)<<Name()<< " car excute mode :"<<uiCarMode;
-}
-
-//自动生成测试用例
-void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
-{
-    if (iAbandon< 40){ // 2 second excute
-        iAbandon ++;
-        return;
-    }
-    else{
-        iAbandon = 0;
-    }
-
-    try {
-        iLocalNowNumber ++;
-        LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;
-        iRulesNowNumber++;
-        std::stringstream strRuleContent;
-        if (iRulesNowNumber == 1)
-            strRuleContent<<"动作"<<iRulesNowNumber<<":起步请加速到"<< fCarNowSpeed<<"km/h";
-        else{
-            if (fCarNowSpeed > fCarLastSpeed){
-                strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
-            }
-            else if (fCarNowSpeed < fCarLastSpeed){
-                strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
-            }
-            else if (fCarNowSpeed == fCarLastSpeed){
-                strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
-            }
-       }
-       int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
-                        strRuleContent.str().c_str(), RULE_EXCUTE_RESULT_INIT, iLocalNowNumber, msg->Longitude,
-                        msg->Latitude, msg->Height, fCarNowSpeed);
-       if (iRet < 0) {
-            iLocalNowNumber--;
-            iRulesNowNumber--;
-       }
-       else{
-           decision::localizationMsg localMsg;
-           localMsg.Locationid = iLocalNowNumber;
-           localMsg.Longitude = msg->Longitude;
-           localMsg.Latitude = msg->Latitude;
-           localMsg.Height = msg->Height;
-           localMsg.LocationSpeed = fCarNowSpeed;
-           localMsg.AngleDeviated = msg->AngleDeviated;
-           Geometry_pub.publish(localMsg);
-       }
-       fCarLastSpeed = fCarNowSpeed;
-
-    }
-    catch (ros::Exception ex)
-    {
-        LOG(ERROR)<<Name()<<":ExcutePlanRoute error."<< ex.what();
-    }
-}
-
-// 自动生成测试用例停止
-void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
-{
-     LOG(INFO)<<Name()<<"StopPlanRoute last record iLocalNowNumber = "<< iLocalNowNumber;
-     iCommandCmd = CMD_UNDEFIED;// set undefied mode
-     iLocalNowNumber++;
-     iRulesNowNumber++;
-     std::stringstream strRuleContent;
-     if (fCarNowSpeed > fCarLastSpeed){
-         strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
-     }
-     else if (fCarNowSpeed < fCarLastSpeed){
-         strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
-     }
-     else if (fCarNowSpeed == fCarLastSpeed){
-         strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
-     }
-    int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
-                     strRuleContent.str().c_str(), 0, iLocalNowNumber, msg->Longitude, msg->Latitude,
-                     msg->Height, fCarNowSpeed);
-    if (iRet < 0) {
-         LOG(ERROR)<<Name()<<"StopPlanRoute insert iLocalNowNumber = "<< iLocalNowNumber<< "  last record error";
-         iLocalNowNumber--;
-         iRulesNowNumber--;
-    }
-}
-
-// 执行自动驾驶ＭＰＣ算法
-void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
-{
-   // 判断当前RCU是否为自动驾驶模式
-    if (uiCarMode != CTR_MD_REQ_AUTO_DRIVE){
-        LOG(INFO)<<Name()<<" ExcuteSelfDriving uiCarMode is not auto drive.";
-        return;
-    }
-    LOG(INFO)<<Name()<<" ilocalTotalNumbers:"<<ilocalTotalNumbers << "iLocalNowNumber:"<<iLocalNowNumber;
-    if (CarAutoDriveStop())
-        return;
-    int iPredictPoints = iPredictionPointsPerNumber;
-    double px = msg->Longitude;
-    double py = msg->Latitude;
-    bool bRetLon = (((ptsx[1] - ptsx[0])>= 0)&&((px - ptsx[1])> 0))||
-            (((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){
-        std::vector<LOCATION_RULE_INFO> tempInfo;
-        tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
-        ptsx.clear();
-        ptsy.clear();
-        for (int i= 0; i< iPredictPoints; i++)
-        {
-           ptsx.push_back( tempInfo[i].LocationLon);
-           ptsy.push_back( tempInfo[i].LocationLat);
-        }
-        mpc.ref_speed = tempInfo[1].LocationSpeed;
-        iLocalNowNumber ++;
-        UpdataCarRuleResults();
-        LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route  altered.";
-    }
-    else{
-        LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route does not alter.";
-    }
-    double psi = msg->AngleDeviated + 90;
-    double v =  (double)fCarNowSpeed;
-    MPC_Controller(ptsx, ptsy, px, py, psi, v);
-
-}
-
-void Decision::TestMPC()
-{
-    std::vector<LOCATION_RULE_INFO> tempInfo;
-    tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, 5);
-    ptsx.clear();
-    ptsy.clear();
-    for (int i= 0; i< 5; i++)
-    {
-       ptsx.push_back( tempInfo[i].LocationLon);
-       ptsy.push_back( tempInfo[i].LocationLat);
-    }
-    mpc.ref_speed = tempInfo[1].LocationSpeed;
-    double psi = deg2rad(230 + 90);
-    double v =  (double)fCarNowSpeed;
-    MPC_Controller(ptsx, ptsy, ptsx[1], ptsy[1], psi, v);
-}
-
-// 自动驾驶停止
-bool  Decision::CarAutoDriveStop()
-{
-    try{
-
-        if (ilocalTotalNumbers == iLocalNowNumber + iPredictionPointsPerNumber){
-            iCommandCmd = CMD_AUTODRIE_END;
-            int breakCtr = -70; // 踩刹车到70%
-            //int breakCtr = (iRulesTotalNumbers -iRulesNowNumber) *10 -70; // 踩刹车到70%
-
-            //iLocalNowNumber ++;
-            while(true){
-                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)
-                return true;
-            //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
-            {
-                 ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_SUCCESS;
-            }
-            else
-            {
-                ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_FIAL;
-            }
-            int iResult = ruleMsg.DirectiveResult;
-            DecisionDatabase::UpdateActionDirective(PlanRouteName.c_str(), iRulesNowNumber, iResult);
-            ruleMsg.DirectiveID = iRulesNowNumber;
-            Rules_pub.publish(ruleMsg);
-            return true;
-        }
-        else
-            return false;
-    }
-    catch (ros::Exception ex)
-    {
-        LOG(ERROR)<<Name()<<":CarAutoDriveStop error."<< ex.what();
-        return false;
-    }
-}
-
-
-bool Decision::UpdataCarRuleResults()
-{
-    try{
-        iRulesNowNumber ++;
-        decision::rulesMsg ruleMsg;
-        if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速（ <10km/h）：± 0.25km/h  高速（ >10km/h）：± 1km/h
-        {
-             ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_SUCCESS;
-        }
-        else
-        {
-            ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_FIAL;
-        }
-        int iResult = ruleMsg.DirectiveResult;
-        DecisionDatabase::UpdateActionDirective(PlanRouteName.c_str(), iRulesNowNumber, iResult);
-        ruleMsg.DirectiveID = iRulesNowNumber;
-        Rules_pub.publish(ruleMsg);
-        return true;
-    }
-    catch (ros::Exception ex)
-    {
-        LOG(ERROR)<<Name()<<"  UpdataCarRuleResults error."<< ex.what();
-        return false;
-    }
-}
-
-
-void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &ptsy,
-                              double &px, double &py,  double &psi, double &v)
-{
-     // 转换坐标到当前车辆坐标
-     Eigen::MatrixXd waypoints = transformGlobal2Vehicle(px, py, psi, ptsx, ptsy);
-     Eigen::VectorXd Ptsx = waypoints.row(0);
-     Eigen::VectorXd Ptsy = waypoints.row(1);
-//     for (unsigned i=0; i < ptsx.size(); ++i) {
-//         LOG(INFO)<<Name()<< " lon: "<< Ptsx(i)<< " lat: "<<Ptsy(i);
-//     }
-     // 获取状态值
-     auto coeffs = polyfit(Ptsx, Ptsy, 3);
-     double cte = evaluateCte(coeffs);
-     double epsi = evaluateEpsi(coeffs);
-     LOG(INFO)<<Name()<< " cte: "<< cte<< " epsi: "<<epsi;
-     Eigen::VectorXd state(6);
-     state << 0, 0, 0, v, cte, epsi;
-     // 计算方向和油门，范围都在[-1, 1]之间
-     // 这个地方有延迟，取第三个
-     auto vars = mpc.Solve(state, coeffs);
-     // 展示MPC预测的路径
-     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);
-     decision::controlMsg control;
-     control.nowSpeed = fCarNowSpeed;
-     control.refSpeed = mpc.ref_speed;
-     control.steer =  steer_value;
-     control.throttle = throttle_value;
-     control.cte = cte;
-     Control_pub.publish(control);
-}
-
-unsigned int Decision::SpeedControl(int &speed)
-{
-     canbus::AccPedalActCtrReq accsrv;
-     canbus::BrkPedalActCtrReq brksrv;
-     if (speed >0){
-         accsrv.request.uiAccPsngCtrReq = speed *10;
-         brksrv.request.uiBrkPsngCtrReq =0;
-     }
-     else{
-         accsrv.request.uiAccPsngCtrReq = 0;
-         brksrv.request.uiBrkPsngCtrReq = -speed *10;
-     }
-     accsrv.request.uiAccSpdCtrReq = 20;
-     brksrv.request.uiBrkCtrMdReq = 1;
-     brksrv.request.uiBrkSpdCtrReq = 90;
-     brksrv.request.uiBrkFocCtrReq = 350;
-     if (clientAccPedalActCtrReq.call(accsrv))
-     {
-         LOG(INFO)<<Name()<<" Speed Control service return success, value = "<<accsrv.response.uiReturn;
-     }
-     else
-     {
-         LOG(ERROR)<<Name()<<" Speed Control service return falil";
-     }
-
-     if (clientBrkPedalActCtrReq.call(brksrv))
-     {
-         LOG(INFO)<<Name()<<" Speed Control service return success, value = "<<brksrv.response.uiReturn;
-     }
-     else
-     {
-         LOG(ERROR)<<Name()<<" Speed Control service return falil";
-     }
-     int result = 0;
-     if(brksrv.response.uiReturn == 1 && accsrv.response.uiReturn == 1)
-     {
-       result =1;
-     }
-    return result;
-}
-
-
-unsigned int Decision::StreeControl(int &streer)
-{
-
-     canbus::StrActCtrReq srv;
-     srv.request.iStrAngCtrReq = streer *100;
-     srv.request.uiStrCtrMdReq =1;
-     srv.request.iStrSpdCtrReq = 290;
-     srv.request.iStrTrqCtrReq = 50;
-     if (clientStrActCtrReq.call(srv))
-     {
-         LOG(INFO)<<Name()<<" stree Control service return success, value = "<<srv.response.uiReturn;
-     }
-     else
-     {
-         LOG(ERROR)<<Name()<<" stree Control service return falil";
-     }
-     return srv.response.uiReturn;
-}
-
-
-unsigned int Decision::BreakCtrControl(unsigned int breakmode)
-{
-     canbus::SftActCtrReq srv;
-     srv.request.uiSftPsngCtrReq = breakmode;
-     srv.request.uiSftMdReq = SFT_MD_REQ_2_SECOND;
-     srv.request.uiClchPsngCtrReq = 1;
-     srv.request.iClchSpdCtrReq = 0;
-     if (clientSftActCtrReq.call(srv))
-     {
-         LOG(INFO)<<Name()<<" break Control service return success, value = "<<srv.response.uiReturn;
-     }
-     else
-     {
-         LOG(ERROR)<<Name()<<" break Control service return falil";
-     }
-     return srv.response.uiReturn;
-}
-
- // 根据输入值x,y解方程
- Eigen::VectorXd Decision::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) {
-     assert(xvals.size() == yvals.size());
-     assert(order >= 1 && order <= xvals.size() - 1);
-     Eigen::MatrixXd A(xvals.size(), order + 1);
-     for (int i = 0; i < xvals.size(); i++) {
-         A(i, 0) = 1.0;
-     }
-     for (int j = 0; j < xvals.size(); j++) {
-         for (int i = 0; i < order; i++) {
-             A(j, i + 1) = A(j, i) * xvals(j);
-         }
-     }
-     auto Q = A.householderQr();
-     auto result = Q.solve(yvals);
-     return result;
- }
-
-// 计算CTE误差
- double Decision::evaluateCte(Eigen::VectorXd coeffs) {
-   return polyeval(coeffs, 0);
- }
-
- // 计算Epsi误差
- double Decision::evaluateEpsi(Eigen::VectorXd coeffs) {
-   return -atan(coeffs[1]);
- }
-
- // 把全局坐标转换为汽车坐标，以当前汽车的位置作为原点
- Eigen::MatrixXd Decision::transformGlobal2Vehicle(double x, double y, double psi,
-        const vector<double> & ptsx, const vector<double> & ptsy) {
-    assert(ptsx.size() == ptsy.size());
-    unsigned len = ptsx.size();
-    auto waypoints = Eigen::MatrixXd(2, len);
-    for (auto i = 0; i < len; ++i) {
-        waypoints(0, i) = cos(psi) * (ptsx[i] - x) + sin(psi) * (ptsy[i] - y);
-        waypoints(1, i) = -sin(psi) * (ptsx[i] - x) + cos(psi) * (ptsy[i] - y);
-    }
-    return waypoints;
-}
-
-} // decisionspace
-} // robot
-

src/decision/decision.h

@@ -1,222 +0,0 @@
-/******************************************************************************
- * 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
-#define ROBOT_ROBOT_DECISION_H
-
-
-
-#include "../common/robotapp.h"
-#include "ros/ros.h"
-#include "../common/sqlite3helper.h"
-#include "../common/mpc.h"
-#include "decisiondatabase.h"
-#include <decision/CommandModeCmd.h>
-#include <localization/localMsg.h>
-#include <canbus/obdMsg.h>
-#include <perception/si2Msg.h>
-#include <vector>
-#include <math.h>
-//#include <eigen3/Eigen/Core>
-//#include <eigen3/Eigen/QR>
-#include "../common/Eigen-3.3/Eigen/Core"
-#include "../common/Eigen-3.3/Eigen/QR"
-
-namespace robot {
-namespace decisionSpace {
-
-#define ROBOT_SUCCESS     0
-#define ROBOT_FAILTURE   -1
-
-
-
-// 角度和弧度之间的转换
-constexpr double pi() {
-    return M_PI;
-}
-
-/**
-* @class Decision
-*
-* @brief decision module main class.
-* 本类是决策类
-* It collect the control data from other model and send decison message to other model.
-*/
-
-class Decision: public robot::common::RobotApp{
-
-public:
-    Decision(){
-
-    }
-    /**
-    * decision节点名称
-    */
-    std::string Name() const override;
-
-    /**
-    * decision模块初始化
-    * @brief module initialization function
-    * @return initialization status
-    */
-    int Init() override;
-
-    /**
-    * decision模块启动
-    * @brief module start function
-    * @return start status
-    */
-    int Start() override;
-
-    /**
-    * decision模块的停止
-    * @brief module stop function.
-    * @return start status
-    */
-    void Stop() override;
-
-private:
-    /**
-    * RTK定位回调函数
-    */
-    void RTKLocationCallback(const localization::localMsg::ConstPtr& msg);
-
-    /**
-    * OBD车辆信息回调函数
-    */
-    void OBDCallback(const canbus::obdMsg::ConstPtr& msg);
-
-    /**
-    * RCU当前模式回调函数
-    */
-    void RCUModeCallback(const perception::si2Msg::ConstPtr& msg);
-
-    /**
-    * ICU当前执行命令服务
-    */
-    bool CommandRequestService(decision::CommandModeCmd::Request &req,
-                        decision::CommandModeCmd::Response &res);
-
-    /**
-    * 执行保存规划路径
-    */
-    void ExcutePlanRoute(const localization::localMsg::ConstPtr& msg);
-
-    /**
-    * 停止规划路径
-    */
-    void StopPlanRoute(const localization::localMsg::ConstPtr& msg);
-
-    /**
-    * 执行自动驾驶任务
-    */
-    void ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg);
-
-
-    void TestMPC();
-    //速度控制
-    unsigned int SpeedControl(int &speed);
-    //方向控制
-    unsigned int StreeControl(int &streer);
-    //档位控制
-    unsigned int BreakCtrControl(unsigned int breakmode);
-    //加载规划路径
-    void LoadPlanRoute();
-    //车辆自动驾驶停止
-    bool CarAutoDriveStop();
-    //更新测试用例结果
-    bool UpdataCarRuleResults();
-
-    void MPC_Controller(std::vector<double> &ptsx, std::vector<double> &ptsy,
-                        double &px, double &py,  double &psi, double &v);
-
-
-    inline double deg2rad(double x) {
-        return x * pi() / 180;
-    }
-    inline double rad2deg(double x) {
-        return x * 180 / pi();
-    }
-
-    // 根据系数和x值获取y值
-    inline double polyeval(Eigen::VectorXd coeffs, double x) {
-        double result = 0.0;
-        for (int i = 0; i < coeffs.size(); i++) {
-            result += coeffs[i] * pow(x, i);
-        }
-        return result;
-    }
-
-    // 根据输入值x,y解方程
-    Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
-
-    // 计算CTE误差
-    double evaluateCte(Eigen::VectorXd coeffs);
-    // 计算Epsi误差
-    double evaluateEpsi(Eigen::VectorXd coeffs);
-
-    // 把全局坐标转换为汽车坐标，以当前汽车的位置作为远点
-    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:
-
-    int iAbandon;
-
-    std::string PlanRouteName;
-
-    int32_t iCommandCmd; // excute mode 0: undefined 1: plan start 2: auto-drive start  3: plan stop 4: auto-drive stop
-
-    float_t fCarNowSpeed;
-
-    float_t fCarLastSpeed; //plan
-
-    int64_t iLocalNowNumber; // plan or auto drive localization numbers
-
-    int64_t ilocalTotalNumbers;
-
-    int64_t iRulesNowNumber; // plan or auto drive rule numbers
-
-    int64_t iRulesTotalNumbers;
-
-    int32_t iPredictionPointsPerNumber;
-
-    unsigned int uiCarMode;
-
-    robot::common::MPC mpc;
-
-    ros::Publisher Rules_pub;
-
-    ros::Publisher Geometry_pub;
-
-    ros::Publisher Control_pub;
-
-    ros::ServiceClient clientAccPedalActCtrReq;
-    ros::ServiceClient clientBrkPedalActCtrReq;
-
-    ros::ServiceClient clientStrActCtrReq;
-
-    ros::ServiceClient clientSftActCtrReq;
-
-    std::vector<double> ptsy;
-    std::vector<double> ptsx;
-
-    std::vector<double> vPx;
-    std::vector<double> vPy;
-
-    int throttle_Last = 0;
-    int steer_Last = 0;
-
-};
-
-
-}
-}
-#endif // ROBOT_ROBOT_DECISION_H

src/decision/decision_app.cpp

@@ -1,10 +0,0 @@
-/******************************************************************************
- * 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"
-
-ROBOT_MAIN(robot::decisionSpace::Decision);
-

src/decision/decisiondatabase.cpp

@@ -1,646 +0,0 @@
-/******************************************************************************
- * 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 {
-namespace decisionSpace {
-
-
-std::string DecisionDatabase::Name() { return "Database"; }
-
-std::string to_string_with_precision(double a_value, int n){
-    std::ostringstream out;
-    out<<std::setprecision(n)<<a_value;
-    return out.str();
-}
-
-int DecisionDatabase::InitDB(){
-    LOG(INFO)<<Name()<<":Decision database init...";
-
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<":Decision opendb fail";
-        return 0;
-    }
-    /***************************打开数据库 end ******************************/
-
-    /****************检查,初始化表 action_directive start *******************/
-    int haveTable = robot::common::Sqlite3helper::Instance().isTableExits(sqlite,"action_directive");
-    if(haveTable ==-1){
-        LOG(INFO)<<Name()<<":Decision exits table ->action_directive fail";
-        return 0;
-    }else if(haveTable == 0){
-        int createResult = robot::common::Sqlite3helper::Instance().createTable(sqlite,
-                                       "CREATE TABLE action_directive (         \
-                                       rule_name        VARCHAR (50)  NOT NULL, \
-                                       directive_id     BIGINT        NOT NULL, \
-                                       directive_info   VARCHAR (255),          \
-                                       directive_result INT,                    \
-                                       PRIMARY KEY (                            \
-                                           rule_name,                           \
-                                           directive_id                         \
-                                       )                                        \
-                                       );"
-        );
-        if(createResult == 0){
-            LOG(INFO)<<Name()<<":Decision create table ->action_directive fail";
-            return 0;
-        }
-    }
-    /****************检查,初始化表 action_directive end **********************/
-
-    /******************检查,初始化表 location_map start **********************/
-//    haveTable = robot::common::Sqlite3helper::Instance().isTableExits(sqlite,"location_map");
-//    if(haveTable ==-1){
-//        LOG(INFO)<<Name()<<":Decision exits table -> location_map fail";
-//        return 0;
-//    }else if(haveTable == 0){
-//        int createResult = robot::common::Sqlite3helper::Instance().createTable(sqlite,
-//                                       "CREATE TABLE location_map (             \
-//                                       location_lon_1 FLOAT (53)    NOT NULL,   \
-//                                       location_lat_1 FLOAT (53)    NOT NULL,   \
-//                                       location_lon_2 FLOAT (53)    NOT NULL,   \
-//                                       location_lat_2 FLOAT (53)    NOT NULL,   \
-//                                       location_X_1   FLOAT (53)    NOT NULL,   \
-//                                       location_Y_1   FLOAT (53)    NOT NULL,   \
-//                                       map_id         VARCHAR (100) PRIMARY KEY \
-//                                                                    NOT NULL,   \
-//                                       location_X_2   FLOAT (53)    NOT NULL,   \
-//                                       location_Y_2   FLOAT (53)    NOT NULL    \
-//                                       );"
-//        );
-//        if(createResult == 0){
-//            LOG(INFO)<<Name()<<":Decision create table -> location_map fail";
-//            return 0;
-//        }
-//    }
-    /******************检查,初始化表 location_map end ************************/
-
-    /**************检查,初始化表 location_rule_info start ********************/
-    haveTable = robot::common::Sqlite3helper::Instance().isTableExits(sqlite,"location_rule_info");
-    if(haveTable ==-1){
-        LOG(INFO)<<Name()<<":Decision exits table -> location_rule_info fail";
-        return 0;
-    }else if(haveTable == 0){
-        int createResult = robot::common::Sqlite3helper::Instance().createTable(sqlite,
-                                       "CREATE TABLE location_rule_info (       \
-                                       rule_name       VARCHAR (50) NOT NULL,   \
-                                       location_id     BIGINT       NOT NULL,   \
-                                       location_lon    DECIMAL (38) NOT NULL,   \
-                                       location_lat    DECIMAL (38) NOT NULL,   \
-                                       location_height DECIMAL (38) NOT NULL,   \
-                                       location_speed  FLOAT (53)   NOT NULL,   \
-                                       PRIMARY KEY (                            \
-                                           rule_name,                           \
-                                           location_id                          \
-                                       )                                        \
-                                       );"
-        );
-        if(createResult == 0){
-            LOG(INFO)<<Name()<<":Decision create table -> location_map fail";
-            return 0;
-        }
-    }
-    /**************检查,初始化表 location_rule_info end **********************/
-    robot::common::Sqlite3helper::Instance().close(sqlite);
-    return 1;
-}
-
-int DecisionDatabase::InsertActionDirective(const char *ruleName, int iDirectiveID, const char *directiveInfo, int iDirectiveResult){
-
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<":Decision opendb fail";
-        return 0;
-    }
-    /***************************打开数据库 end ******************************/
-
-    /********************向表中插入一条记录 start *****************************/
-    std::string sRuleName = ruleName;
-    std::string sDirectiveID = std::to_string(iDirectiveID);
-    std::string sDirectiveInfo = directiveInfo;
-    std::string sDirectiveResult = std::to_string(iDirectiveResult);
-    std::string s_sql="INSERT INTO action_directive (   \
-            rule_name,                                  \
-            directive_id,                               \
-            directive_info,                             \
-            directive_result                            \
-        )                                               \
-        VALUES (\'"+sRuleName+"\',                      \
-                  "+sDirectiveID+",                     \
-                \'"+sDirectiveInfo+"\',                 \
-                  "+sDirectiveResult+");";
-    LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    robot::common::Sqlite3helper::Instance().update(sqlite,sql);
-    /********************向表中插入一条记录 end *******************************/
-
-    robot::common::Sqlite3helper::Instance().close(sqlite);
-    return 1;
-}
-
-int DecisionDatabase::InsertLocationMap(float fLocationLon1,
-                                float fLocationLat1,
-                                float fLocationLon2,
-                                float fLocationLat2,
-                                float fLocationX1,
-                                float fLocationY1,
-                                const char *mapId,
-                                float fLocationX2,
-                                float fLocationY2){
-
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return 0;
-    }
-    /***************************打开数据库 end ******************************/
-
-    /********************向表中插入一条记录 start *****************************/
-    std::string sLocationLon1=std::to_string(fLocationLon1);
-    std::string sLocationLat1=std::to_string(fLocationLat1);
-    std::string sLocationLon2=std::to_string(fLocationLon2);
-    std::string sLocationLat2=std::to_string(fLocationLat2);
-    std::string sLocationX1=std::to_string(fLocationX1);
-    std::string sLocationY1=std::to_string(fLocationY1);
-    std::string sMapId=mapId;
-    std::string sLocationX2=std::to_string(fLocationX2);
-    std::string sLocationY2=std::to_string(fLocationY2);
-
-    std::string s_sql="INSERT INTO location_map (   \
-            location_lon_1,                         \
-            location_lat_1,                         \
-            location_lon_2,                         \
-            location_lat_2,                         \
-            location_X_1,                           \
-            location_Y_1,                           \
-            map_id,                                 \
-            location_X_2,                           \
-            location_Y_2                            \
-        )                                           \
-        VALUES (                                    \
-            "+sLocationLon1+",                      \
-            "+sLocationLat1+",                      \
-            "+sLocationLon2+",                      \
-            "+sLocationLat2+",                      \
-            "+sLocationX1+",                        \
-            "+sLocationY1+",                        \
-            \'"+sMapId+"\',                         \
-            "+sLocationX2+",                        \
-            "+sLocationY2+"                         \
-        );";
-    LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    robot::common::Sqlite3helper::Instance().update(sqlite,sql);
-    /********************向表中插入一条记录 end *******************************/
-
-    robot::common::Sqlite3helper::Instance().close(sqlite);
-    return 1;
-
-}
-
-int DecisionDatabase::InsertLocationRuleInfo(const char *ruleName,
-                                     int iLocationID,
-                                     double dLocationLon,
-                                     double dLocationLat,
-                                     float dLocationHeight,
-                                     float fLocationSpeed){
-
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return 0;
-    }
-    /***************************打开数据库 end ******************************/
-
-    /********************向表中插入一条记录 start *****************************/
-    std::string sRuleName=ruleName;
-    std::string sLocationID=std::to_string(iLocationID);
-    std::string sLocationLon=to_string_with_precision(dLocationLon,12);
-    std::string sLocationLat=to_string_with_precision(dLocationLat,11);
-    std::string sLocationHeight=to_string_with_precision(dLocationHeight,4);
-    std::string sLocationSpeed=to_string_with_precision(fLocationSpeed, 4);
-    std::string s_sql="INSERT INTO location_rule_info ( \
-            rule_name,                                  \
-            location_id,                                \
-            location_lon,                               \
-            location_lat,                               \
-            location_height,                            \
-            location_speed                              \
-        )                                               \
-        VALUES (                                        \
-            \'"+sRuleName+"\',                          \
-            "+sLocationID+",                            \
-            "+sLocationLon+",                           \
-            "+sLocationLat+",                           \
-            "+sLocationHeight+",                        \
-            "+sLocationSpeed+"                          \
-        );";
-    //LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    robot::common::Sqlite3helper::Instance().update(sqlite,sql);
-    /********************向表中插入一条记录 end *******************************/
-
-    robot::common::Sqlite3helper::Instance().close(sqlite);
-    return 1;
-}
-
-int DecisionDatabase::InsertLocationRuleInfoAndActionDirective(const char* ruleName,
-                                                    int iDirectiveID,
-                                                    const char* directiveInfo,
-                                                    int iDirectiveResult,
-                                                    int iLocationID,
-                                                    double dLocationLon,
-                                                    double dLocationLat,
-                                                    float dLocationHeight,
-                                                    float fLocationSpeed)
-{
-    int iRet = 0;
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return 0;
-    }
-    /***************************打开数据库 end ******************************/
-    robot::common::Sqlite3helper::Instance().transaction(sqlite);
-    try
-    {
-
-
-        /********************向表中插入一条记录 start *****************************/
-        std::string sRuleName=ruleName;
-        std::string sLocationID=std::to_string(iLocationID);
-        std::string sLocationLon=to_string_with_precision(dLocationLon,12);
-        std::string sLocationLat=to_string_with_precision(dLocationLat,11);
-        std::string sLocationHeight=to_string_with_precision(dLocationHeight,4);
-        std::string sLocationSpeed=to_string_with_precision(fLocationSpeed, 4);
-        std::string s_sqlsqlLocation="INSERT INTO location_rule_info ( \
-                rule_name,                                  \
-                location_id,                                \
-                location_lon,                               \
-                location_lat,                               \
-                location_height,                            \
-                location_speed                              \
-            )                                               \
-            VALUES (                                        \
-                \'"+sRuleName+"\',                          \
-                "+sLocationID+",                            \
-                "+sLocationLon+",                           \
-                "+sLocationLat+",                           \
-                "+sLocationHeight+",                        \
-                "+sLocationSpeed+"                          \
-            );";
-        //LOG(INFO)<<s_sqlsqlLocation;
-        const char* sqlLocation=s_sqlsqlLocation.c_str();
-        robot::common::Sqlite3helper::Instance().update(sqlite, sqlLocation);
-
-        //std::string sRuleName = ruleName;
-        std::string sDirectiveID = std::to_string(iDirectiveID);
-        std::string sDirectiveInfo = directiveInfo;
-        std::string sDirectiveResult = std::to_string(iDirectiveResult);
-        std::string s_sql="INSERT INTO action_directive (   \
-                rule_name,                                  \
-                directive_id,                               \
-                directive_info,                             \
-                directive_result                            \
-            )                                               \
-            VALUES (\'"+sRuleName+"\',                      \
-                      "+sDirectiveID+",                     \
-                    \'"+sDirectiveInfo+"\',                 \
-                      "+sDirectiveResult+");";
-        //LOG(INFO)<<s_sql;
-        const char* sql=s_sql.c_str();
-        robot::common::Sqlite3helper::Instance().update(sqlite,sql);
-        /********************向表中插入一条记录 end *******************************/
-         robot::common::Sqlite3helper::Instance().commitTransaction(sqlite);
-         iRet =1;
-    }
-    catch (ros::Exception ex)
-    {
-        robot::common::Sqlite3helper::Instance().rollbackTransaction(sqlite);
-        iRet = -1;
-    }
-    robot::common::Sqlite3helper::Instance().close(sqlite);
-    return iRet;
-}
-
-int DecisionDatabase::DeleteActionDirective(const char *ruleName){
-
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return 0;
-    }
-    /***************************打开数据库 end ******************************/
-
-    std::string sRuleName= ruleName;
-    std::string s_sql="DELETE FROM action_directive WHERE rule_name=\'" + sRuleName+"\'";
-    //LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
-    robot::common::Sqlite3helper::Instance().close(sqlite);
-    if(deleteResult == 0){
-        return 0;
-    }else{
-        return 1;
-    }
-
-}
-
-int DecisionDatabase::DeleteActionDirective(int iDirectiveID){
-
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return 0;
-    }
-    /***************************打开数据库 end ******************************/
-
-    std::string sDirectiveID=std::to_string(iDirectiveID);
-    std::string s_sql="DELETE FROM action_directive WHERE directive_id=" + sDirectiveID+";";
-    //LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
-    robot::common::Sqlite3helper::Instance().close(sqlite);
-    if(deleteResult == 0){
-        return 0;
-    }else{
-        return 1;
-    }
-
-}
-
-int DecisionDatabase::DeleteLocationMap(const char *mapID){
-
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return 0;
-    }
-    /***************************打开数据库 end ******************************/
-
-    std::string sMapID=mapID;
-    std::string s_sql="DELETE FROM location_map WHERE map_id=" + sMapID;
-    //LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
-    robot::common::Sqlite3helper::Instance().close(sqlite);
-    if(deleteResult == 0){
-        return 0;
-    }else{
-        return 1;
-    }
-
-}
-
-int DecisionDatabase::DeleteLocationRuleInfo(const char *ruleName){
-
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return 0;
-    }
-    /***************************打开数据库 end ******************************/
-
-    std::string sRuleName=ruleName;
-    std::string s_sql="DELETE FROM location_rule_info WHERE rule_name=\'" + sRuleName+"\'";
-    //LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
-    robot::common::Sqlite3helper::Instance().close(sqlite);
-    if(deleteResult == 0){
-        return 0;
-    }else{
-        return 1;
-    }
-
-}
-
-int DecisionDatabase::DeleteLocationRuleInfo(int iLocationID){
-
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return 0;
-    }
-    /***************************打开数据库 end ******************************/
-
-    std::string sLocationID=std::to_string(iLocationID);
-    std::string s_sql="DELETE FROM location_rule_info WHERE location_id=" + sLocationID;
-    //LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
-    robot::common::Sqlite3helper::Instance().close(sqlite);
-    if(deleteResult == 0){
-        return 0;
-    }else{
-        return 1;
-    }
-
-}
-
-int DecisionDatabase::UpdateActionDirective(const char* ruleName,int iDirectiveID,int iNewDirectiveResult){
-
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return 0;
-    }
-    /***************************打开数据库 end ******************************/
-
-    std::string sRuleName=ruleName;
-    std::string sDirectiveID=std::to_string(iDirectiveID);
-    std::string sNewDirectiveResult=std::to_string(iNewDirectiveResult);
-    std::string s_sql="UPDATE action_directive SET                      \
-                        directive_result=\'"+sNewDirectiveResult+"\'    \
-                        WHERE rule_name=\'"+sRuleName+"\' AND           \
-                        directive_id="+sDirectiveID+";";
-    //LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    int updateResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
-    if(updateResult == 0){
-        return 0;
-    }else{
-        return 1;
-    }
-}
-
-int DecisionDatabase::UpdateLocationMap(float fNewLocationLon1,
-                      float fNewLocationLat1,
-                      float fNewLocationLon2,
-                      float fNewLocationLat2,
-                      float fNewLocationX1,
-                      float fNewLocationY1,
-                      float fNewLocationX2,
-                      float fNewLocationY2,
-                      const char * mapId){
-
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return 0;
-    }
-    /***************************打开数据库 end ******************************/
-
-    std::string sLocationLon1=std::to_string(fNewLocationLon1);
-    std::string sLocationLat1=std::to_string(fNewLocationLat1);
-    std::string sLocationLon2=std::to_string(fNewLocationLon2);
-    std::string sLocationLat2=std::to_string(fNewLocationLat2);
-    std::string sLocationX1=std::to_string(fNewLocationX1);
-    std::string sLocationY1=std::to_string(fNewLocationY1);
-    std::string sMapId=mapId;
-    std::string sLocationX2=std::to_string(fNewLocationX2);
-    std::string sLocationY2=std::to_string(fNewLocationY2);
-    std::string s_sql="UPDATE location_map SET  \
-            location_lon_1="+sLocationLon1+",   \
-            location_lat_1="+sLocationLat1+",   \
-            location_lon_2="+sLocationLon2+",   \
-            location_lat_2="+sLocationLat2+",   \
-            location_X_1="+sLocationX1+",       \
-            location_Y_1="+sLocationY1+",       \
-            location_X_2="+sLocationX2+",       \
-            location_Y_2="+sLocationY2+"        \
-            WHERE map_id=\'"+sMapId+"\';";
-    //LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    int updateResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
-    robot::common::Sqlite3helper::Instance().close(sqlite);
-    if(updateResult == 0){
-        return 0;
-    }else{
-        return 1;
-    }
-
-}
-
-std::vector<ACTION_DIRECTIVE> DecisionDatabase::QueryActionDirective(int iStart, int iLength){
-    std::vector<ACTION_DIRECTIVE> actionDirectives;
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return std::vector<ACTION_DIRECTIVE>(0);
-    }
-    /***************************打开数据库 end ******************************/
-    std::string sStart=std::to_string(iStart);
-    std::string sLength=std::to_string(iLength);
-    std::string s_sql="SELECT * FROM action_directive limit "+sStart+","+sLength;
-    //LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    robot::common::query_result_t* result=robot::common::Sqlite3helper::Instance().query(sqlite,sql);
-    while(result->next()){
-        ACTION_DIRECTIVE actionDirective;
-        actionDirective.RuleName=result->value((char*)"rule_name");
-        actionDirective.DirectiveID=std::atoi(result->value((char*)"directive_id"));
-        actionDirective.DirectiveResult=std::atoi(result->value((char*)"directive_result"));
-        actionDirective.DirectiveInfo=result->value((char*)"directive_info");
-        actionDirectives.push_back(actionDirective);
-    }
-    return actionDirectives;
-}
-
-
-
-
-std::vector<LOCATION_RULE_INFO> DecisionDatabase::QueryLocationRuleInfo(int iStart, int iLength){
-    std::vector<LOCATION_RULE_INFO> locationRuleInfos;
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return std::vector<LOCATION_RULE_INFO>(0);
-    }
-    /***************************打开数据库 end ******************************/
-    std::string sStart=std::to_string(iStart);
-    std::string sLength=std::to_string(iLength);
-    std::string s_sql="SELECT * FROM location_rule_info limit "+sStart+","+sLength;
-    LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    robot::common::query_result_t* result=robot::common::Sqlite3helper::Instance().query(sqlite,sql);
-    while(result->next()){
-        LOCATION_RULE_INFO locationRuleInfo;
-        locationRuleInfo.RuleName=result->value((char*)"rule_name");
-        locationRuleInfo.LocationLon=std::atof(result->value((char*)"location_lon"));
-        locationRuleInfo.LocationLat=std::atof(result->value((char*)"location_lat"));
-        locationRuleInfo.LocationHeight=std::atof(result->value((char*)"location_height"));
-        locationRuleInfo.LocationID=std::atoi(result->value((char*)"location_id"));
-        locationRuleInfo.LocationSpeed=std::atof(result->value((char*)"location_speed"));
-        locationRuleInfos.push_back(locationRuleInfo);
-    }
-    return locationRuleInfos;
-}
-
-
-
-int DecisionDatabase::countOfActionDirective(){
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return -1;
-    }
-    /***************************打开数据库 end ******************************/
-    std::string s_sql="SELECT count(*) FROM action_directive";
-    //LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    robot::common::query_result_t* result=robot::common::Sqlite3helper::Instance().query(sqlite,sql);
-    while(result->next()){
-        return std::atoi(result->value((char*)"count(*)"));
-    }
-}
-
-int DecisionDatabase::countOfLocationMap(){
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return -1;
-    }
-    /***************************打开数据库 end ******************************/
-    std::string s_sql="SELECT count(*) FROM location_map";
-    //LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    robot::common::query_result_t* result=robot::common::Sqlite3helper::Instance().query(sqlite,sql);
-    while(result->next()){
-        return std::atoi(result->value((char*)"count(*)"));
-    }
-}
-
-int DecisionDatabase::countOfLocationRuleInfo(){
-    /***************************打开数据库 start ****************************/
-    sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
-    if(sqlite==nullptr){
-        LOG(INFO)<<Name()<<"Decision opendb fail";
-        return -1;
-    }
-    /***************************打开数据库 end ******************************/
-    std::string s_sql="SELECT count(*) FROM location_rule_info";
-    //LOG(INFO)<<s_sql;
-    const char* sql=s_sql.c_str();
-    robot::common::query_result_t* result=robot::common::Sqlite3helper::Instance().query(sqlite,sql);
-    while(result->next()){
-        return std::atoi(result->value((char*)"count(*)"));
-    }
-
-}
-
-
-
-}
-
-}

src/decision/decisiondatabase.h

@@ -1,178 +0,0 @@
-/******************************************************************************
- * 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
-
-
-#include "ros/ros.h"
-#include "../common/sqlite3helper.h"
-#include "decisiondatatype.h"
-
-namespace robot {
-namespace decisionSpace {
-
-#define DATABASE_SUCCESS     0
-#define DATABASE_FAILTURE   -1
-
-class DecisionDatabase{
-
-public :
-    DecisionDatabase(){}
-
-    static std::string Name();
-    /**
-    * @brief decision模块的数据库初始化，如果相关表不存在，则建立相关的表
-    * @brief 相关的表:action_directive,location_map,location_rule_info
-    * @return 0失败
-    */
-    static int InitDB();
-
-    /**
-    * @brief 向action_Directive表中插入一条
-    * @return 0失败
-    */
-    static int InsertActionDirective(const char* ruleName,
-                              int iDirectiveID,
-                              const char* directiveInfo,
-                              int iDirectiveResult);
-
-    /**
-    * @brief 向location_map表中插入一条
-    * @return 0失败
-    */
-    static int InsertLocationMap(float fLocationLon1,
-                          float fLocationLat1,
-                          float fLocationLon2,
-                          float fLocationLat2,
-                          float fLocationX1,
-                          float fLocationY1,
-                          const char * mapId ,
-                          float fLocationX2,
-                          float fLocationY2);
-
-    /**
-    * @brief 向location_rule_info表中插入一条
-    *
-    * @return 0失败
-    */
-    static int InsertLocationRuleInfo(const char * ruleName,
-                               int iLocationID,
-                               double dLocationLon,
-                               double dLocationLat,
-                               float dLocationHeight,
-                               float fLocationSpeed
-                               );
-
-
-    /**
-    * @brief 向location_rule_info表中插入一条 action_Directive表中插入一条
-    * @return 0失败
-    */
-    static int InsertLocationRuleInfoAndActionDirective(const char* ruleName,
-                                                        int iDirectiveID,
-                                                        const char* directiveInfo,
-                                                        int iDirectiveResult,
-                                                        int iLocationID,
-                                                        double dLocationLon,
-                                                        double dLocationLat,
-                                                        float dLocationHeight,
-                                                        float fLocationSpeed);
-
-    /**
-    * @brief 在action_firective表中,
-    * @brief 删除rule_name的记录
-    * @return 0失败
-    */
-    static int DeleteActionDirective(const char* ruleName);
-
-    /**
-    * @brief 在action_firective表中,
-    * @brief 删除directive_id的记录
-    * @return 0失败
-    */
-    static int DeleteActionDirective(int iDirectiveID);
-
-    /**
-    * @brief 在location_map表中,
-    * @brief 删除map_id的记录
-    * @return 0失败
-    */
-    static int DeleteLocationMap(const char* mapID);
-
-    /**
-    * @brief 在location_rule_info表中,
-    * @brief 删除rule_name的记录
-    * @return 0失败
-    */
-    static int DeleteLocationRuleInfo(const char* RuleName);
-
-    /**
-    * @brief 在location_rule_info表中,
-    * @brief 删除rule_name的记录
-    * @return 0失败
-    */
-    static int DeleteLocationRuleInfo(int iLocationID);
-
-    /**
-    * @brief 修改rule_name和directive_id的directive_result的记录
-    * @return 0失败
-    */
-    static int UpdateActionDirective(const char* ruleName,int iDirectiveID,int iNewDirectiveResult);
-
-    static int UpdateLocationMap(float fNewLocationLon1,
-                          float fNewLocationLat1,
-                          float fNewLocationLon2,
-                          float fNewLocationLat2,
-                          float fNewLocationX1,
-                          float fNewLocationY1,
-                          float fNewLocationX2,
-                          float fNewLocationY2,
-                          const char * mapId);
-    /**
-    * @brief 按起始位置和数量查询的action_Directive的记录
-    * @return 0失败
-    */
-    static std::vector<ACTION_DIRECTIVE> QueryActionDirective(int iStart, int iLength);
-
-    /**
-    * @brief 按起始位置和数量查询的location_rule_info的记录
-    * @return 0失败
-    */
-    static std::vector<LOCATION_RULE_INFO> QueryLocationRuleInfo(int iStart, int iLength);
-
-
-    /**
-    * @brief 查询总数的action_Directive的记录
-    * @return 0失败
-    */
-    static int countOfActionDirective();
-
-    /**
-    * @brief 查询总数的location_map的记录
-    * @return 0失败
-    */
-    static int countOfLocationMap();
-
-    /**
-    * @brief 查询总数的location_rule_info的记录
-    * @return 0失败
-    */
-    static int countOfLocationRuleInfo();
-    };
-
-}
-}
-
-
-
-
-
-
-
-
-
-
-#endif // DECISIONDATABASE_H

src/decision/decisiondatatype.h

@@ -1,64 +0,0 @@
-/******************************************************************************
- * 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
-#define ROBOT_DECISIONDATATYPE_H
-#include <string>
-
-
-namespace robot {
-namespace decisionSpace {
-
-#define CMD_UNDEFIED         0
-#define CMD_PLAN_START       1
-#define CMD_AUTODRIE_START   2
-#define CMD_PLAN_END         3
-#define CMD_AUTODRIE_END     4
-
-#define	SFT_PSNG_LRN_REQ_PARK			1	//挡位位置学习请求-停车档（自动档）
-#define	SFT_PSNG_LRN_REQ_REVERSE		2	//挡位位置学习请求-倒档
-#define	SFT_PSNG_LRN_REQ_NEUTRAL		3	//挡位位置学习请求-空档
-#define	SFT_PSNG_LRN_REQ_FORWARD_1		4	//挡位位置学习请求-前进档1(手动档)
-#define	SFT_MD_REQ_3_SECOND		0			//换挡模式-平滑型
-#define	SFT_MD_REQ_2_SECOND		1			//换挡模式-快速型
-#define	SFT_MD_REQ_1_SECOND		2			//换挡模式-激进型
-
-#define	CTR_MD_REQ_AUTO_LEARN		0	//控制模式请求-自动学习模式
-#define	CTR_MD_REQ_HAND_LEARN		1	//控制模式请求-手动学习模式
-#define	CTR_MD_REQ_AUTO_DRIVE		2	//控制模式请求-自动驾驶模式
-#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
-#define RULE_EXCUTE_RESULT_SUCCESS        2
-
-typedef struct
-{
-  std::string RuleName;
-  int DirectiveID;
-  std::string DirectiveInfo;
-  int DirectiveResult;
-} ACTION_DIRECTIVE;
-
-
-typedef struct
-{
-    std::string RuleName;
-    int LocationID;
-    double LocationLon;
-    double LocationLat;
-    double LocationHeight;
-    double LocationSpeed;
-
-} LOCATION_RULE_INFO;
-
-
-
-} // namespace decisionspace
-} // robot
-#endif // ROBOT_DECISIONDATATYPE_H

src/decision/msg/controlMsg.msg

@@ -1,5 +0,0 @@
-float64 steer
-float64 throttle
-float64 refSpeed
-float64 nowSpeed
-float64 cte

src/decision/msg/localizationMsg.msg

@@ -1,6 +0,0 @@
-int32 Locationid
-float64 Longitude
-float64 Latitude
-float64 Height
-float64 LocationSpeed
-float64 AngleDeviated

src/decision/msg/rulesMsg.msg

@@ -1,3 +0,0 @@
-int32 DirectiveID
-string DirectiveInfo
-int32 DirectiveResult

src/decision/package.xml

@@ -1,64 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>decision</name>
-  <version>0.1.0</version>
-  <description>The decision package</description>
-
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="madesheng@todo.todo">madesheng</maintainer>
-
-
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>Apache 2.0</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/desision</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-  <author >liujingwei</author>
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>roscpp message_generation std_msgs rospy</build_depend>
-  <build_export_depend>roscpp message_generation std_msgs rospy</build_export_depend>
-  <exec_depend>roscpp message_generation std_msgs rospy</exec_depend>
-  <exec_depend>message_runtime</exec_depend>
-
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
-
-  </export>
-</package>

src/decision/srv/CommandModeCmd.srv

@@ -1,3 +0,0 @@
-int32 Cmd
----
-int32 CmdResult

src/launch/robot.launch

@@ -14,7 +14,7 @@ ns = "NAME_SPACE" 这个属性可以让你在自定义的命名空间里运行
 <node pkg="perception" name="perception" type="perception" output="screen" respawn="true"/>
 <node pkg="localization" name="localization" type="localization" output="screen" respawn="true"/>
 <node pkg="canbus" name="canbus" type="canbus" output="screen" respawn="true"/>
-<param name = "rcu_canbus_mode" value = "tcp" /><!--can总线模式: usb(默认)/tcp-->
+<param name = "rcu_canbus_mode" value = "pcitcp" /><!--can总线模式: usb(默认)/tcp/pcitcp-->
 <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)-->