添加对毫末底盘的ROS

src/ros/catkin/src/controllertocan_haomo/CMakeLists.txt

@@ -0,0 +1,115 @@
+cmake_minimum_required(VERSION 2.8.11)
+project(controllertocan_haomo)
+
+
+message(STATUS  "Enter controllertocan_haomo")
+
+
+SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
+
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
+
+if(QT_EXIST)
+include_directories(
+  ${QT_PATH}/include
+  ${QT_PATH}/include/QtCore
+  ${AGX_QT_PATH}
+  ${AGX_QT_PATH}/QtCore
+)
+link_directories(
+  ${QT_PATH}/lib
+)
+else()
+  message(FATAL_ERROR "Please set QT_PATH")
+endif()
+
+
+
+
+## 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
+  pluginlib
+  sensor_msgs
+  autoware_msgs
+  adc_system_msg
+)
+find_package(Boost REQUIRED)
+find_package(Protobuf REQUIRED)
+
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTOUIC ON)
+set(CMAKE_AUTORCC ON)
+
+
+if(CMAKE_COMPILER_IS_GNUCXX)
+    set(CMAKE_CXX_FLAGS "-fPIC ${CMAKE_CXX_FLAGS}")
+    message(STATUS "optional:-fPIC")   
+endif(CMAKE_COMPILER_IS_GNUCXX)
+
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+#find_package(Qt5Core  REQUIRED)
+#qt5_wrap_cpp(MOC src/qt_ros_test.h)
+#qt5_wrap_ui(UIC src/qt_ros_test.ui)
+
+## 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()
+
+
+###################################
+## 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 you 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 client_plugin
+   CATKIN_DEPENDS 
+    roscpp std_msgs 
+    sensor_msgs pluginlib
+  DEPENDS
+    Boost
+    Protobuf
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  ${catkin_INCLUDE_DIRS} include
+  ${Boost_INCLUDE_DIR}
+  ${autoware_msgs_INCLUDE_DIRS}
+)
+
+## Declare a C++ executable
+add_executable(controllertocan_haomo src/main.cpp src/controller.cpp src/control_status.cpp src/haomo_code.cpp) 
+target_link_libraries(controllertocan_haomo ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES}   Qt5Core  boost_system  boost_thread  boost_serialization)
+
+install(
+  TARGETS controllertocan_haomo
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+

src/ros/catkin/src/controllertocan_haomo/package.xml

@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<package>
+  <name>controllertocan_haomo</name>
+  <version>0.0.0</version>
+  <description>The controllertocan_haomo package for convert controller message to can message</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="yuchuli@catarc.ac.cn">yuchuli</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>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/client_plugin</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+  <build_depend>pluginlib</build_depend>
+  <run_depend>pluginlib</run_depend>
+
+  <build_depend>sensor_msgs</build_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+  <build_depend>autoware_msgs</build_depend>
+  <run_depend>autoware_msgs</run_depend>
+
+  <build_depend>adc_system_msg</build_depend>
+  <run_depend>adc_system_msg</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/ros/catkin/src/controllertocan_haomo/src/boost.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#ifndef _IV_BOOST_H_
+#define _IV_BOOST_H_
+
+#if defined __GNUC__
+#  pragma GCC system_header 
+#endif
+#ifndef __CUDACC__
+//https://bugreports.qt-project.org/browse/QTBUG-22829
+#ifndef Q_MOC_RUN
+#include <boost/version.hpp>
+#include <boost/numeric/conversion/cast.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition.hpp>
+#include <boost/thread.hpp>
+#include <boost/thread/thread.hpp>
+#include <boost/filesystem.hpp>
+#include <boost/bind.hpp>
+#include <boost/cstdint.hpp>
+#include <boost/function.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+#include <boost/mpl/fold.hpp>
+#include <boost/mpl/inherit.hpp>
+#include <boost/mpl/inherit_linearly.hpp>
+#include <boost/mpl/joint_view.hpp>
+#include <boost/mpl/transform.hpp>
+#include <boost/mpl/vector.hpp>
+#include <boost/algorithm/string.hpp>
+#ifndef Q_MOC_RUN
+#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
+#if BOOST_VERSION >= 104700
+#include <boost/chrono.hpp>
+#endif
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#include <boost/shared_array.hpp>
+#include <boost/interprocess/sync/file_lock.hpp>
+#if BOOST_VERSION >= 104900
+#include <boost/interprocess/permissions.hpp>
+#endif
+#include <boost/iostreams/device/mapped_file.hpp>
+#define BOOST_PARAMETER_MAX_ARITY 7
+#include <boost/signals2.hpp>
+#include <boost/signals2/slot.hpp>
+#endif
+#endif
+#endif    // _IV_BOOST_H_

src/ros/catkin/src/controllertocan_haomo/src/control_status.cpp

@@ -0,0 +1,126 @@
+#include "control_status.h"
+#include <QDebug>
+
+
+void iv::control::ControlStatus::set_brake(float brake)
+{
+    command11.bit.brake = (unsigned)brake;
+}
+
+void iv::control::ControlStatus::set_drivespeed(float speed)
+{
+    command11.bit.drive_speed = (unsigned)speed;
+}
+
+void iv::control::ControlStatus::set_gear(int gear)
+{
+    command11.bit.gear = (unsigned)gear;
+}
+
+void iv::control::ControlStatus::set_driveMode(char driveMode)
+{
+    command11.bit.driveMode = (unsigned)driveMode;
+}
+
+void iv::control::ControlStatus::set_gear_enable(bool enable)
+{
+    if (enable == true)
+        command11.bit.gear_enable = 1;
+    else
+        command11.bit.gear_enable = 0;
+}
+
+void iv::control::ControlStatus::set_aeb_enable(bool enable)
+{
+    if (enable == true)
+        command11.bit.aeb_enable = 1;
+    else
+        command11.bit.aeb_enable = 0;
+}
+
+void iv::control::ControlStatus::set_acc_enable(bool enable)
+{
+    if (enable == true)
+        command11.bit.acc_enable = 1;
+    else
+        command11.bit.acc_enable = 0;
+}
+
+void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right)
+{
+    if ((left == true) && (right == false))
+    {
+        command10.bit.turn_light = 0x01;
+    }
+    else if ((left == false) && (right == true))
+    {
+        command10.bit.turn_light = 0x02;
+    }
+    else if ((left == false) && (right == false))
+    {
+        command10.bit.turn_light = 0x00;
+    }
+    else
+    {
+        command10.bit.turn_light = 0x03;
+    }
+}
+
+
+void iv::control::ControlStatus::set_wheel_angle(float angle)
+{
+
+    int ang = (angle + 870) * 10;
+    ServiceControlStatus.command12.bit.ang_H = (ang) / 256;
+    ServiceControlStatus.command12.bit.ang_L = (ang) % 256;
+}
+
+void iv::control::ControlStatus::set_wheel_speed(float speed)
+{
+
+    int angSpeed = speed;
+    ServiceControlStatus.command12.bit.ang_speed = angSpeed;
+}
+
+void iv::control::ControlStatus::set_wheel_enable(bool enable)
+{
+
+    if (enable)
+    {
+        ServiceControlStatus.command12.bit.ang_enable = 1;
+    }
+    else
+    {
+        ServiceControlStatus.command12.bit.ang_enable = 0;
+    }
+}
+
+
+void iv::control::ControlStatus::set_cmd_checksum(unsigned char cmd_id)
+{
+    unsigned int check_sum = 0;
+    if (cmd_id == 0x10)
+    {
+        for (int i = 0; i < 7; i++)
+        {
+            check_sum ^= (ServiceControlStatus.command10.byte[i]);
+        }
+        ServiceControlStatus.command10.byte[7] = check_sum;
+    }
+    else if (cmd_id == 0x11)
+    {
+        for (int i = 0; i < 7; i++)
+        {
+            check_sum ^= (ServiceControlStatus.command11.byte[i]);
+        }
+        ServiceControlStatus.command11.byte[7] = check_sum;
+    }
+    else if (cmd_id == 0x12)
+    {
+        for (int i = 0; i < 7; i++)
+        {
+            check_sum ^= (ServiceControlStatus.command12.byte[i]);
+        }
+        ServiceControlStatus.command12.byte[7] = check_sum;
+    }
+}

src/ros/catkin/src/controllertocan_haomo/src/control_status.h

@@ -0,0 +1,63 @@
+#pragma once
+//由于控制器指令共享同一个ID 0x20，建立此类维护控制指令的最新状态
+
+#include "boost.h"
+#include <cstdint>
+#include <boost/serialization/singleton.hpp>
+#include "vv7.h"
+
+namespace iv
+{
+    namespace control
+    {
+        class ControlStatus : public boost::noncopyable
+        {
+        public:
+            /*****************
+             * ****测试标志位*****
+             * ***************/
+            int normal_speed = 0;    //常规速度
+            int swerve_speed = 0;    //转弯速度
+            int high_speed = 0;      //快速
+            int mid_speed = 0;       //中速
+            int low_speed = 0;       //慢速
+            int change_line = -1;    //换道标志
+            int stop_obstacle = -1;  //停障标志
+            int elude_obstacle = -1; //避障标志
+            int special_signle = -1; //特殊信号标志
+            int car_pullover = -1;   //靠边停车标志位
+            float torque = 0.0;
+            float brake = 0.0;
+            float acc = 0.0;
+
+            Command10 command10;
+            Command11 command11;
+            Command12 command12;
+            Command_Response command_reponse;
+
+            int command_ID3E = 0x3E;
+            int command_ID3C = 0x3C;
+            int command_ID10 = 0x10;
+            int command_ID11 = 0x11;
+            int command_ID12 = 0x12;
+
+            void set_brake(float brake);
+            void set_drivespeed(float speed);
+            void set_gear(int gear);
+            void set_driveMode(char mode);
+            void set_gear_enable(bool enable);
+            void set_aeb_enable(bool enable);
+            void set_acc_enable(bool enable);
+
+            void set_turnsignals_control(bool left, bool right);
+
+            void set_wheel_angle(float angle);
+            void set_wheel_speed(float speed);
+            void set_wheel_enable(bool enable);
+
+            void set_cmd_checksum(unsigned char cmd_id);
+        };
+        typedef boost::serialization::singleton<iv::control::ControlStatus> ControlStatusSingleton;
+    }
+}
+#define ServiceControlStatus iv::control::ControlStatusSingleton::get_mutable_instance()

src/ros/catkin/src/controllertocan_haomo/src/controlcan.h

@@ -0,0 +1,145 @@
+#ifndef CONTROLCAN1_H
+#define CONTROLCAN1_H
+
+
+#ifdef linux
+//接口卡类型定义
+#define VCI_PCI5121		1
+#define VCI_PCI9810		2
+#define VCI_USBCAN1		3
+#define VCI_USBCAN2		4
+#define VCI_PCI9820		5
+#define VCI_CAN232		6
+#define VCI_PCI5110		7
+#define VCI_CANLite		8
+#define VCI_ISA9620		9
+#define VCI_ISA5420		10
+
+//CAN错误码
+#define	ERR_CAN_OVERFLOW			0x0001	//CAN控制器内部FIFO溢出
+#define	ERR_CAN_ERRALARM			0x0002	//CAN控制器错误报警
+#define	ERR_CAN_PASSIVE				0x0004	//CAN控制器消极错误
+#define	ERR_CAN_LOSE				0x0008	//CAN控制器仲裁丢失
+#define	ERR_CAN_BUSERR				0x0010	//CAN控制器总线错误
+
+//通用错误码
+#define	ERR_DEVICEOPENED			0x0100	//设备已经打开
+#define	ERR_DEVICEOPEN				0x0200	//打开设备错误
+#define	ERR_DEVICENOTOPEN			0x0400	//设备没有打开
+#define	ERR_BUFFEROVERFLOW			0x0800	//缓冲区溢出
+#define	ERR_DEVICENOTEXIST			0x1000	//此设备不存在
+#define	ERR_LOADKERNELDLL			0x2000	//装载动态库失败
+#define ERR_CMDFAILED				0x4000	//执行命令失败错误码
+#define	ERR_BUFFERCREATE			0x8000	//内存不足
+
+
+//函数调用返回状态值
+#define	STATUS_OK					1
+#define STATUS_ERR					0
+	
+#define USHORT unsigned short int
+#define BYTE unsigned char
+#define CHAR char
+#define UCHAR unsigned char
+#define UINT unsigned int
+#define DWORD unsigned int
+#define PVOID void*
+#define ULONG unsigned int
+#define INT int
+#define UINT32 UINT
+#define LPVOID void*
+#define BOOL BYTE
+#define TRUE 1
+#define FALSE 0
+
+
+#if 1
+
+typedef  struct  _VCI_BOARD_INFO{//1.ZLGCAN系列接口卡信息的数据类型。
+		USHORT	hw_Version;
+		USHORT	fw_Version;
+		USHORT	dr_Version;
+		USHORT	in_Version;
+		USHORT	irq_Num;
+		BYTE	can_Num;
+		CHAR	str_Serial_Num[20];
+		CHAR	str_hw_Type[40];
+		USHORT	Reserved[4];
+} VCI_BOARD_INFO,*PVCI_BOARD_INFO; 
+
+
+typedef  struct  _VCI_CAN_OBJ{//2.定义CAN信息帧的数据类型。
+	UINT	ID;
+	UINT	TimeStamp;
+	BYTE	TimeFlag;
+	BYTE	SendType;
+	BYTE	RemoteFlag;//是否是远程帧
+	BYTE	ExternFlag;//是否是扩展帧
+	BYTE	DataLen;
+	BYTE	Data[8];
+	BYTE	Reserved[3];
+}VCI_CAN_OBJ,*PVCI_CAN_OBJ;
+
+
+typedef struct _VCI_CAN_STATUS{//3.定义CAN控制器状态的数据类型。
+	UCHAR	ErrInterrupt;
+	UCHAR	regMode;
+	UCHAR	regStatus;
+	UCHAR	regALCapture;
+	UCHAR	regECCapture; 
+	UCHAR	regEWLimit;
+	UCHAR	regRECounter; 
+	UCHAR	regTECounter;
+	DWORD	Reserved;
+}VCI_CAN_STATUS,*PVCI_CAN_STATUS;
+
+
+typedef struct _ERR_INFO{//4.定义错误信息的数据类型。
+		UINT	ErrCode;
+		BYTE	Passive_ErrData[3];
+		BYTE	ArLost_ErrData;
+} VCI_ERR_INFO,*PVCI_ERR_INFO;
+
+
+typedef struct _INIT_CONFIG{//5.定义初始化CAN的数据类型
+	DWORD	AccCode;
+	DWORD	AccMask;
+	DWORD	Reserved;
+	UCHAR	Filter;
+	UCHAR	Timing0;	
+	UCHAR	Timing1;	
+	UCHAR	Mode;
+}VCI_INIT_CONFIG,*PVCI_INIT_CONFIG;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+DWORD VCI_OpenDevice(DWORD DeviceType,DWORD DeviceInd,DWORD Reserved);
+DWORD VCI_CloseDevice(DWORD DeviceType,DWORD DeviceInd);
+DWORD VCI_InitCAN(DWORD DeviceType, DWORD DeviceInd, DWORD CANInd, PVCI_INIT_CONFIG pInitConfig);
+
+DWORD VCI_ReadBoardInfo(DWORD DeviceType,DWORD DeviceInd,PVCI_BOARD_INFO pInfo);
+DWORD VCI_ReadErrInfo(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_ERR_INFO pErrInfo);
+DWORD VCI_ReadCANStatus(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_STATUS pCANStatus);
+
+DWORD VCI_GetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
+DWORD VCI_SetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
+
+ULONG VCI_GetReceiveNum(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+DWORD VCI_ClearBuffer(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+
+DWORD VCI_StartCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+DWORD VCI_ResetCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+
+ULONG VCI_Transmit(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pSend,UINT Len);
+ULONG VCI_Receive(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pReceive,UINT Len,INT WaitTime);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
+#endif
+
+#endif

src/ros/catkin/src/controllertocan_haomo/src/controller.cpp

@@ -0,0 +1,66 @@
+#include  "controller.h"
+
+iv::control::Controller::Controller() {
+
+}
+
+iv::control::Controller::~Controller() {
+
+}
+
+void iv::control::Controller::inialize() {
+
+}
+
+//校验和
+void iv::control::Controller::cmd_checksum(unsigned char cmd_id) {
+    ServiceControlStatus.set_cmd_checksum(cmd_id);
+}
+//方向盘控制
+void iv::control::Controller::control_wheel(float angle) {
+    ServiceControlStatus.set_wheel_angle(angle);
+}
+
+void iv::control::Controller:: control_angle_speed(float angSpeed){
+    ServiceControlStatus.set_wheel_speed(angSpeed);
+}
+
+void iv::control::Controller:: control_angle_enable(bool enable){
+    ServiceControlStatus.set_wheel_enable(enable);
+}
+//速度控制
+void iv::control::Controller:: control_drivespeed(float speed){
+    ServiceControlStatus.set_drivespeed(speed);
+}
+
+void iv::control::Controller::control_acc_en(bool enanble){
+    ServiceControlStatus.set_acc_enable(enanble);
+}
+
+
+void iv::control::Controller::control_aeb_en(bool enable){
+    ServiceControlStatus.set_aeb_enable(enable);
+}
+//刹车控制
+void iv::control::Controller::control_brake(float brake){
+    ServiceControlStatus.set_brake(brake);
+}
+//档位控制
+void iv::control::Controller::control_gear(float gear){
+    ServiceControlStatus.set_gear(gear);
+}
+void iv::control::Controller::control_gear_en(bool enable){
+    ServiceControlStatus.set_gear_enable(enable);
+}
+
+//驾驶模式
+void iv::control::Controller::control_mode(char mode){
+    ServiceControlStatus.set_driveMode(mode);
+}
+
+
+//车灯控制
+void iv::control::Controller::control_turnsignals(bool left, bool right){
+    ServiceControlStatus.set_turnsignals_control(left,right);
+}
+

src/ros/catkin/src/controllertocan_haomo/src/controller.h

@@ -0,0 +1,44 @@
+#pragma once
+/*
+*控制器
+*/
+#ifndef _IV_CONTROL_CONTROLLER_
+#define _IV_CONTROL_CONTROLLER_
+
+#include "boost.h"
+//#include <common/car_status.h>
+#include "control_status.h"
+
+namespace iv {
+    namespace control {
+        class Controller
+        {
+        public:
+            Controller();
+            ~Controller();
+            void inialize();// 初始化
+
+            void control_wheel(float angle);		//方向盘控制
+            void control_angle_speed(float angSpeed);
+            void control_angle_enable(bool enable);
+
+            void control_drivespeed(float speed);	//油门开度控制
+             void control_brake(float brake);
+            void control_gear(float gear);
+            void control_mode(char mode);
+            void control_gear_en(bool enable);
+            void control_aeb_en(bool enable);
+            void control_acc_en(bool enanble);
+
+ 
+            void control_turnsignals(bool left, bool right);
+            void cmd_checksum(unsigned char cmd_id);
+            ///* 获取当前车辆状态*/
+            //void getCurrentCarStatus(iv::CarStatus & car_status);
+
+        private:
+        };
+    }
+}
+
+#endif // !_IV_CONTROL_CONTROLLER_

src/ros/catkin/src/controllertocan_haomo/src/haomo_code.cpp

@@ -0,0 +1,70 @@
+#include "haomo_code.h"
+
+HaoMoCode::HaoMoCode()
+{
+    for (int i = 0; i < 7; i++)
+    {
+        ServiceControlStatus.command10.byte[0] = 0;
+        ServiceControlStatus.command11.byte[0] = 0;
+        ServiceControlStatus.command12.byte[0] = 0;
+    }
+}
+
+HaoMoCode::~HaoMoCode()
+{
+}
+
+void HaoMoCode::Code(const autoware_msgs::ControlCommandStampedConstPtr &cmdmsg, adc_system_msg::canmsg &xmsg, boost::shared_ptr<iv::control::Controller> xcontroller)
+{
+    static int nindex = 0;
+
+    xcontroller->control_mode(1);
+    xcontroller->control_gear_en(true);
+    xcontroller->control_acc_en(true);
+    xcontroller->control_aeb_en(true);
+    xcontroller->control_angle_enable(true);
+
+    xcontroller->control_gear(4);
+
+    double fVel = cmdmsg->cmd.linear_velocity;
+
+    xcontroller->control_brake(0);
+    xcontroller->control_wheel(cmdmsg->cmd.steering_angle * (180.0 / M_PI) * 15.0);
+    xcontroller->control_drivespeed(fVel);
+
+    adc_system_msg::canraw xraw10;
+    xraw10.data.resize(8);
+    xraw10.bext = false;
+    xraw10.bremote = false;
+    xraw10.id = ServiceControlStatus.command_ID10;
+    xraw10.len = 8;
+    xraw10.index = nindex;
+    nindex++;
+    xraw10.channel = 0;
+    memcpy(xraw10.data.data(), (char *)ServiceControlStatus.command10.byte, 8);
+    xmsg.rawmsg.push_back(xraw10);
+
+    adc_system_msg::canraw xraw11;
+    xraw11.data.resize(8);
+    xraw11.bext = false;
+    xraw11.bremote = false;
+    xraw11.id = ServiceControlStatus.command_ID11;
+    xraw11.len = 8;
+    xraw11.index = nindex;
+    nindex++;
+    xraw11.channel = 0;
+    memcpy(xraw11.data.data(), (char *)ServiceControlStatus.command10.byte, 8);
+    xmsg.rawmsg.push_back(xraw11);
+
+    adc_system_msg::canraw xraw12;
+    xraw12.data.resize(8);
+    xraw12.bext = false;
+    xraw12.bremote = false;
+    xraw12.id = ServiceControlStatus.command_ID12;
+    xraw12.len = 8;
+    xraw12.index = nindex;
+    nindex++;
+    xraw12.channel = 0;
+    memcpy(xraw12.data.data(), (char *)ServiceControlStatus.command12.byte, 8);
+    xmsg.rawmsg.push_back(xraw12);
+}

src/ros/catkin/src/controllertocan_haomo/src/haomo_code.h

@@ -0,0 +1,18 @@
+#ifndef  HAOMO_CODE_H
+#define  HAOMO_CODE_H
+
+#include "protocolcode.h"
+
+
+class HaoMoCode: public ProtocolCode
+{
+    public:
+        HaoMoCode();
+        ~HaoMoCode();
+
+    public:
+        virtual void Code(const autoware_msgs::ControlCommandStampedConstPtr & cmdmsg, adc_system_msg::canmsg & xmsg,boost::shared_ptr<iv::control::Controller>  xcontroller);
+		
+};
+
+#endif

src/ros/catkin/src/controllertocan_haomo/src/main.cpp

@@ -0,0 +1,88 @@
+#include "ros/ros.h"
+
+#include <QMutex>
+
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+
+#include "adc_system_msg/canmsg.h"
+
+#include "autoware_msgs/VehicleCmd.h"
+#include "autoware_msgs/ControlCommandStamped.h"
+
+#include <thread>
+
+#include "control_status.h"
+#include "controller.h"
+
+#include "haomo_code.h"
+
+static std::string _pose_topic = "/current_pose";
+static std::string _twist_topic = "/current_velocity";
+
+static std::string _twistraw_topic = "/twist_raw";
+static std::string _ctrlcmd_topic = "/ctrl_raw";
+static std::string _canmsg_topic = "/can_msg";
+
+static std::string gstrvehtype = "HAOMO"; 
+static std::string gstrtablepath = "/home/nvidia/velacctable.txt";
+
+ros::Subscriber ctrlcmd_sub;
+ros::Subscriber twist_sub;
+
+ros::Publisher canmsg_pub;
+
+boost::shared_ptr<iv::control::Controller> gcontroller; //实际车辆控制器
+
+ProtocolCode *gPC = NULL;
+
+void twistrawCalllback(const geometry_msgs::TwistStampedConstPtr &msg)
+{
+}
+
+void ctrlrawCallback(const autoware_msgs::ControlCommandStampedConstPtr &msg)
+{
+    if (gPC == NULL)
+        return;
+    adc_system_msg::canmsg xmsg;
+    gPC->Code(msg, xmsg, gcontroller);
+    canmsg_pub.publish(xmsg);
+}
+
+int main(int argc, char *argv[])
+{
+    ros::init(argc, argv, "controllertocan_haomo");
+    //	ros::NodeHandle n;
+    ros::NodeHandle private_nh("~");
+    ros::Rate loop_rate(100);
+
+    private_nh.getParam("pose_topic", _pose_topic);
+    private_nh.getParam("twist_topic", _twist_topic);
+    std::cout << "pose_topic  is " << _pose_topic << std::endl;
+    std::cout << "twist_topic : " << _twist_topic << std::endl;
+
+    private_nh.getParam("canmsg_topic", _canmsg_topic);
+    private_nh.getParam("vehicletype", gstrvehtype);
+    private_nh.getParam("tablepath", gstrtablepath);
+    std::cout << "canmst_topic:" << _canmsg_topic << std::endl;
+    std::cout << "vehicletype:" << gstrvehtype << std::endl;
+    std::cout << "tablepath:" << gstrtablepath << std::endl;
+
+    gcontroller = boost::shared_ptr<iv::control::Controller>(new iv::control::Controller());
+
+    if (gstrvehtype == "HAOMO")
+    {
+        std::cout << " HAOMO " << std::endl;
+        gPC = new HaoMoCode();
+    }
+
+    canmsg_pub = private_nh.advertise<adc_system_msg::canmsg>(_canmsg_topic, 10);
+
+    ctrlcmd_sub = private_nh.subscribe(_ctrlcmd_topic, 1, ctrlrawCallback);
+    twist_sub = private_nh.subscribe(_twistraw_topic, 1, twistrawCalllback);
+
+    // std::cout<<" go hear. "<<std::endl;
+    ros::spin();
+}

src/ros/catkin/src/controllertocan_haomo/src/protocolcode.h

@@ -0,0 +1,22 @@
+#ifndef PROTOCOLCODE_H
+#define PROTOCOLCODE_H
+
+#include "adc_system_msg/canmsg.h"
+
+#include "autoware_msgs/VehicleCmd.h"
+#include "autoware_msgs/ControlCommandStamped.h"
+
+#include "controller.h"
+
+class ProtocolCode
+{
+    public:
+        ProtocolCode(){}
+    
+    public:
+        virtual void Code(const autoware_msgs::ControlCommandStampedConstPtr & cmdmsg, adc_system_msg::canmsg & xmsg, boost::shared_ptr<iv::control::Controller>  xcontroller){
+
+        }
+};
+
+#endif

src/ros/catkin/src/controllertocan_haomo/src/vv7.h

@@ -0,0 +1,107 @@
+#pragma once
+
+//实际物理值=总线值*比例因子+偏移量
+
+struct Command_Bit10
+{
+
+    unsigned char Reserved0;
+    unsigned char Reserved1;
+    unsigned char Reserved2;
+
+    unsigned char turn_light : 2;
+    unsigned char Reserved3 : 1;
+    unsigned char near_light : 1;
+    unsigned char horn : 1;
+    unsigned char Reserved4 : 3;
+
+    unsigned char Reserved5;
+    unsigned char Reserved6;
+    unsigned char Reserved7;
+
+    unsigned char checkSum;
+};
+
+union Command10
+{
+    Command_Bit10 bit;
+    unsigned char byte[8];
+};
+
+struct Command_Bit11
+{
+    unsigned char Reserved0;
+    unsigned char Reserved1;
+    unsigned char Reserved2;
+
+    unsigned char drive_speed;  
+    //速度总线值=实际值*10，
+    //如果是踏板开度控制总线上的0~100代表实际踏板开度，
+    //对于长城底盘，0~100代表目标速度0~100km/h，前进的时候最大10*3.6km/h，倒车的时候最大速度是5*3.6km/h。
+    //驱动控制和制动控制不能同时给，如果同时给优先响应制动。
+
+    unsigned char brake;
+    //如果是踏板开度控制总线上的0~100代表实际制动踏板开度，长城底盘车辆是踏板开度控制。
+
+    unsigned char gear : 3;
+    unsigned char Reserved3 : 1;
+    unsigned char Reserved4 : 2;
+    unsigned char Reserved5 : 2;
+
+    unsigned char Reserved6 : 2;
+    unsigned char Reserved7 : 2;
+    unsigned char driveMode : 1;
+    unsigned char gear_enable : 1;
+    unsigned char aeb_enable : 1;
+    unsigned char acc_enable : 1;
+
+    unsigned char checkSum;
+};
+
+union Command11
+{
+    Command_Bit11 bit;
+    unsigned char byte[8];
+};
+
+struct Command_Bit12
+{
+    unsigned char ang_speed;
+
+    unsigned char ang_H;
+    unsigned char ang_L;
+
+    unsigned char reserved0;
+    unsigned char reserved1;
+    unsigned char reserved2;
+
+    unsigned char Reserved : 6;
+    unsigned char ang_enable : 1;
+    unsigned char Reserved0 : 1;
+
+    unsigned char checkSum;
+};
+
+union Command12
+{
+    Command_Bit12 bit;
+    unsigned char byte[8];
+};
+
+struct Command_Response_Bit
+{
+    unsigned char head;
+
+    unsigned char grade : 2;
+    unsigned char driveMode : 2;
+    unsigned char epb : 1;
+    unsigned char epsMode : 2;
+    unsigned char obligate : 1;
+};
+
+union Command_Response
+{
+    Command_Response_Bit bit;
+
+    unsigned char byte[2];
+};