add adc_systme_msg adn controllertocan.

src/ros/catkin/src/adc_system_msg/CHANGELOG.rst

@@ -0,0 +1,9 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package autoware_system_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.0.0 (2022-1-18)
+-------------------
+* Created
+
+

src/ros/catkin/src/adc_system_msg/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(adc_system_msg)
+
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  std_msgs
+)
+
+add_message_files(
+  FILES
+    canraw.msg
+    canmsg.msg
+)
+
+generate_messages(DEPENDENCIES
+  std_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+    message_runtime
+    std_msgs
+)

src/ros/catkin/src/adc_system_msg/msg/canmsg.msg

@@ -0,0 +1,2 @@
+Header header
+adc_system_msg/canraw[]  rawmsg

src/ros/catkin/src/adc_system_msg/msg/canraw.msg

@@ -0,0 +1,8 @@
+Header header
+int32 id
+int8[]  data
+bool bext
+bool bremote
+int32 len
+int32 channel
+int32 index

src/ros/catkin/src/adc_system_msg/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>adc_system_msg</name>
+  <version>0.0.0</version>
+  <description>The adc_system_msg package</description>
+  <maintainer email="yuchuli@catarc.ac.cn">Yu Chuli</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+
+  <depend>std_msgs</depend>
+
+  <exec_depend>message_runtime</exec_depend>
+</package>

src/ros/catkin/src/controllertocan/CMakeLists.txt

@@ -0,0 +1,106 @@
+cmake_minimum_required(VERSION 2.8.11)
+project(controllertocan)
+
+
+message(STATUS  "Enter controllertocan")
+
+
+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
+  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(${PROJECT_NAME}_node src/main.cpp src/controller.cpp src/control_status.cpp src/ge3code.cpp src/interpolation_2d.cc src/lookuptable.cpp) 
+target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES}   Qt5Core  boost_system  boost_thread  boost_serialization)
+

src/ros/catkin/src/controllertocan/package.xml

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<package>
+  <name>controllertocan</name>
+  <version>0.0.0</version>
+  <description>The controllertocan 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>
+
+
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/ros/catkin/src/controllertocan/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/src/control_status.cpp

@@ -0,0 +1,199 @@
+#include "control_status.h"
+#include <QDebug>
+
+#if 1
+void iv::control::ControlStatus::set_torque(float percent)
+{
+ //   int torque=(percent+3000)/1.5;
+ //   qDebug("torqure is %f",percent);
+    ServiceControlStatus.command.bit.torque_H = (int)percent/256;
+    ServiceControlStatus.command.bit.torque_L = (int)percent%256;
+
+//    qDebug("torqure L is %d",ServiceControlStatus.command.bit.torque_L );
+}
+void iv::control::ControlStatus::set_wheel_angle(float angle)
+{
+
+    int ang =(angle+780)*10;
+    ServiceControlStatus.command.bit.ang_H = (ang)  /256;
+    ServiceControlStatus.command.bit.ang_L = (ang)  % 256;
+}
+
+  void iv::control::ControlStatus::set_brake(float brake)
+  {
+      command.bit.brake=(unsigned)brake;
+  }
+
+
+
+void iv::control::ControlStatus::set_engine(char enable)
+{
+    command.bit.engine = enable;
+}
+
+
+void iv::control::ControlStatus::set_grade(char grade)
+{
+    command.bit.grade = (unsigned)grade;
+}
+
+
+void iv::control::ControlStatus::set_driveMode(char driveMode)
+{
+    command.bit.driveMode = (unsigned)driveMode;
+}
+
+
+void iv::control::ControlStatus::set_handBrake(bool enable){
+    if (enable == true)
+        command.bit.handBrake = 1;
+    else
+        command.bit.handBrake = 0;
+}
+
+void iv::control::ControlStatus::set_speaker(bool enable){
+    if (enable == true)
+        command.bit.speaker = 1;
+    else
+        command.bit.speaker = 0;
+}
+
+void iv::control::ControlStatus::set_door(char enable){
+    if (enable == true)
+        command.bit.door = 1;
+    else
+        command.bit.door = 0;
+}
+
+void iv::control::ControlStatus::set_doorEnable(bool enable){
+    if (enable == true){
+        command.bit.doorEnable = 1;
+        command.bit.door=1;
+    }
+    else{
+        command.bit.doorEnable = 0;
+        command.bit.door=0;
+    }
+}
+
+//void iv::control::ControlStatus::set_flicker(bool enable){
+//    if (enable == true)
+//        command.bit.flicker = 1;
+//    else
+//        command.bit.flicker = 0;
+//}
+void iv::control::ControlStatus::set_light(bool enable){
+    if (enable == true)
+       command.bit.bright = 1;
+    else
+       command.bit.bright = 0;
+}
+
+void iv::control::ControlStatus::set_leftlight(bool enable){
+    if (enable == true)
+       command.bit.left_turn = 1;
+    else
+       command.bit.left_turn = 0;
+}
+
+
+void iv::control::ControlStatus::set_rightlight(bool enable){
+    if (enable == true)
+       command.bit.right_turn = 1;
+    else
+       command.bit.right_turn = 0;
+}
+
+
+
+
+
+
+void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right){
+    if (left == true)
+    {
+        command.bit.left_turn = 1;
+        command.bit.right_turn = 0;
+    }
+    else if (right == true)
+    {
+        command.bit.left_turn = 0;
+        command.bit.right_turn = 1;
+    }
+    else
+    {
+        command.bit.left_turn = 0;
+        command.bit.right_turn = 0;
+    }
+}
+
+
+
+
+
+void iv::control::ControlStatus::set_dangwei(int dangwei){
+
+        command.bit.dangwei = dangwei;
+
+//        unsigned char bytevalue = command.byte[7];
+
+//        qDebug("bytevalue = %02x",bytevalue);
+
+}
+
+
+#else
+
+void iv::control::ControlStatus::set_accelerate(float percent)
+{
+    command.bit.acce = (unsigned)((percent + 7) * 20);
+}
+void iv::control::ControlStatus::set_wheel_angle(float angle)
+{
+    command.bit.ang_H = angle * 10 >> 8;
+
+    command.bit.ang_L = angle * 10 % 256;
+}
+void iv::control::ControlStatus::set_engine(char enable)
+{
+    command.bit.engine = enable;
+}
+void iv::control::ControlStatus::set_door(char enable){
+    command.bit.door = enable;
+}
+void iv::control::ControlStatus::set_speaker(bool enable){
+    if (enable == true)
+        command.bit.speaker = 1;
+    else
+        command.bit.speaker = 0;
+}
+void iv::control::ControlStatus::set_flicker(bool enable){
+    if (enable == true)
+        command.bit.flicker = 1;
+    else
+        command.bit.flicker = 0;
+}
+void iv::control::ControlStatus::set_light(bool enable){
+    if (enable == true)
+       command.bit.bright = 1;
+    else
+       command.bit.bright = 0;
+}
+void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right){
+    if (left == true)
+    {
+        command.bit.left_turn = 1;
+        command.bit.right_turn = 0;
+    }
+    else if (right == true)
+    {
+        command.bit.left_turn = 0;
+        command.bit.right_turn = 1;
+    }
+    else
+    {
+        command.bit.left_turn = 0;
+        command.bit.right_turn = 0;
+    }
+}
+#endif

src/ros/catkin/src/controllertocan/src/control_status.h

@@ -0,0 +1,65 @@
+#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;
+
+            Command command;
+            Command_Response command_reponse;
+
+            int command_ID = 0x12;
+
+            void set_torque(float percent);
+
+            void set_wheel_angle(float angle);
+
+            void set_brake(float brake);
+
+            void set_engine(char enable);
+            void set_grade(char grade);
+             void set_driveMode(char mode);
+             void set_handBrake(bool handBrake);
+
+            void set_speaker(bool enable);
+             void set_door(char enable);
+            void set_doorEnable(bool enable);
+            void set_light(bool enable);
+            void set_leftlight(bool enable);
+            void set_rightlight(bool enable);
+
+            void set_turnsignals_control(bool left, bool right);
+
+              void set_flicker(bool enable);
+
+              void set_dangwei(int dangwei);
+
+
+        };
+        typedef boost::serialization::singleton<iv::control::ControlStatus> ControlStatusSingleton;
+    }
+}
+#define ServiceControlStatus iv::control::ControlStatusSingleton::get_mutable_instance()

src/ros/catkin/src/controllertocan/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/src/controller.cpp

@@ -0,0 +1,92 @@
+#include  "controller.h"
+
+iv::control::Controller::Controller() {
+
+}
+
+iv::control::Controller::~Controller() {
+
+}
+
+void iv::control::Controller::inialize() {
+
+}
+
+void iv::control::Controller::control_accelerate(float percent) {
+    ServiceControlStatus.set_torque(percent);
+}
+
+void iv::control::Controller::control_torque(float torque) {
+    ServiceControlStatus.set_torque(torque);
+}
+
+void iv::control::Controller::control_wheel(float angle) {
+    ServiceControlStatus.set_wheel_angle(angle);
+}
+void iv::control::Controller::control_brake(float brake) {
+    ServiceControlStatus.set_brake( brake);
+}
+
+void iv::control::Controller::control_engine(char para){
+    ServiceControlStatus.set_engine(para);
+}
+
+void iv::control::Controller::control_grade(char grade){
+    ServiceControlStatus.set_grade( grade);
+}
+
+void iv::control::Controller::control_mode(char mode){
+     ServiceControlStatus.set_driveMode(mode);
+ }
+
+void iv::control::Controller::control_handBrake(bool  enable){
+     ServiceControlStatus.set_handBrake(enable);
+ }
+
+void iv::control::Controller::control_speaker(bool enable){
+    ServiceControlStatus.set_speaker(enable);
+}
+
+void iv::control::Controller::control_door(char enable){
+    ServiceControlStatus.set_door(enable);
+}
+
+void iv::control::Controller::control_doorEnable(bool enable){
+    ServiceControlStatus.set_doorEnable(enable);
+}
+
+void iv::control::Controller::control_light(bool enable){
+    ServiceControlStatus.set_light(enable);
+}
+
+
+void iv::control::Controller::control_leftlight(bool enable){
+    ServiceControlStatus.set_leftlight(enable);
+}
+
+void iv::control::Controller::control_rightlight(bool enable){
+    ServiceControlStatus.set_rightlight(enable);
+}
+
+
+
+
+void iv::control::Controller::control_turnsignals(bool left, bool right){
+    ServiceControlStatus.set_turnsignals_control(left,right);
+}
+
+void iv::control::Controller::control_dangWei(int dangWei){
+    ServiceControlStatus.set_dangwei(dangWei);
+}
+
+
+
+
+
+
+//void iv::control::Controller::control_flicker(bool enable){
+//    ServiceControlStatus.set_flicker(enable);
+//}
+void iv::control::Controller::control_braking(float percent){
+
+}

src/ros/catkin/src/controllertocan/src/controller.h

@@ -0,0 +1,49 @@
+#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_accelerate(float percet);	//油门开度控制
+            void control_wheel(float angle);		//方向盘控制
+            void control_turnsignals(bool left, bool right);
+            void control_engine(char para);
+            void control_door(char enable);
+            void control_doorEnable(bool enable);
+            void control_speaker(bool enable);
+            void control_light(bool enable);
+  //          void control_flicker(bool enable);
+             void control_torque(float torque);	//油门开度控制
+            void control_braking(float percent);
+            void control_brake(float brake);
+            void control_grade(char grade);
+            void control_mode(char mode);
+            void control_handBrake(bool  enable);
+            void control_leftlight(bool enable);
+            void control_rightlight(bool enable);
+
+            void set_handBrake(bool enable);
+            void control_dangWei(int dangWei);
+            ///* 获取当前车辆状态*/
+            //void getCurrentCarStatus(iv::CarStatus & car_status);
+
+        private:
+        };
+    }
+}
+
+#endif // !_IV_CONTROL_CONTROLLER_

src/ros/catkin/src/controllertocan/src/ge3code.cpp

@@ -0,0 +1,53 @@
+#include "ge3code.h"
+
+
+GE3Code::GE3Code(std::string strtablepath)
+{
+    mpLT = new LookupTable(strtablepath);
+}
+
+GE3Code::~GE3Code()
+{
+    delete mpLT;
+}
+
+void GE3Code::Code(const autoware_msgs::ControlCommandStampedConstPtr & cmdmsg, adc_system_msg::canmsg & xmsg,boost::shared_ptr<iv::control::Controller>  xcontroller)
+{
+    static int nindex = 0;
+    xcontroller->control_engine(0);
+    xcontroller->control_grade(1);
+    xcontroller->control_mode(1);
+    xcontroller->control_handBrake(false);
+    xcontroller->control_speaker(false);
+    xcontroller->control_door(0);
+ //   gcontroller->control_light(decition.bri);
+    xcontroller->control_leftlight(false);
+    xcontroller->control_rightlight(false);
+    xcontroller->control_dangWei(1);
+    xcontroller->control_accelerate(cmdmsg->cmd.linear_acceleration);
+
+    double facc = cmdmsg->cmd.linear_acceleration;
+    double ftorque = 0;
+    double fbrake = 0;
+    double fVel = cmdmsg->cmd.linear_velocity;
+
+    if(mpLT->IsINterpolationOK())
+    {
+        mpLT->GetTorqueBrake(fVel*3.6,facc,ftorque,fbrake);
+    }
+
+    xcontroller->control_brake(fbrake);
+    xcontroller->control_wheel(cmdmsg->cmd.steering_angle * (180.0/M_PI) * 15.0);
+    xcontroller->control_torque(ftorque);
+
+    adc_system_msg::canraw xraw;
+    xraw.data.resize(8);
+    xraw.bext = false;
+    xraw.bremote = false;
+    xraw.id = ServiceControlStatus.command_ID;
+    xraw.len = 8;
+    xraw.index = nindex;nindex++;
+    xraw.channel = 0;
+    memcpy(xraw.data.data(),(char * )ServiceControlStatus.command.byte,8);
+    xmsg.rawmsg.push_back(xraw);
+}

src/ros/catkin/src/controllertocan/src/ge3code.h

@@ -0,0 +1,21 @@
+#ifndef  GE3CODE_H
+#define  GE3CODE_H
+
+#include "protocolcode.h"
+#include "lookuptable.h"
+
+
+class GE3Code: public ProtocolCode
+{
+    public:
+        GE3Code(std::string strtablepath);
+        ~GE3Code();
+
+    public:
+        virtual void Code(const autoware_msgs::ControlCommandStampedConstPtr & cmdmsg, adc_system_msg::canmsg & xmsg,boost::shared_ptr<iv::control::Controller>  xcontroller);
+
+    private:
+        LookupTable * mpLT;
+};
+
+#endif

src/ros/catkin/src/controllertocan/src/interpolation_2d.cc

@@ -0,0 +1,120 @@
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "interpolation_2d.h"
+
+#include <cmath>
+#include <iostream>
+
+//#include "cyber/common/log.h"
+
+namespace {
+
+const double kDoubleEpsilon = 1.0e-6;
+
+}  // namespace
+
+namespace apollo {
+namespace control {
+
+bool Interpolation2D::Init(const DataType &xyz) {
+  if (xyz.empty()) {
+    std::cout << "empty input."<<std::endl;
+    return false;
+  }
+  for (const auto &t : xyz) {
+    xyz_[std::get<0>(t)][std::get<1>(t)] = std::get<2>(t);
+  }
+  return true;
+}
+
+double Interpolation2D::Interpolate(const KeyType &xy) const {
+  double max_x = xyz_.rbegin()->first;
+  double min_x = xyz_.begin()->first;
+  if (xy.first >= max_x - kDoubleEpsilon) {
+    return InterpolateYz(xyz_.rbegin()->second, xy.second);
+  }
+  if (xy.first <= min_x + kDoubleEpsilon) {
+    return InterpolateYz(xyz_.begin()->second, xy.second);
+  }
+
+  auto itr_after = xyz_.lower_bound(xy.first);
+  auto itr_before = itr_after;
+  if (itr_before != xyz_.begin()) {
+    --itr_before;
+  }
+
+  double x_before = itr_before->first;
+  double z_before = InterpolateYz(itr_before->second, xy.second);
+  double x_after = itr_after->first;
+  double z_after = InterpolateYz(itr_after->second, xy.second);
+
+  double x_diff_before = std::fabs(xy.first - x_before);
+  double x_diff_after = std::fabs(xy.first - x_after);
+
+  return InterpolateValue(z_before, x_diff_before, z_after, x_diff_after);
+}
+
+double Interpolation2D::InterpolateYz(const std::map<double, double> &yz_table,
+                                      double y) const {
+  if (yz_table.empty()) {
+    std::cout << "Unable to interpolateYz because yz_table is empty."<<std::endl;
+    return y;
+  }
+  double max_y = yz_table.rbegin()->first;
+  double min_y = yz_table.begin()->first;
+  if (y >= max_y - kDoubleEpsilon) {
+    return yz_table.rbegin()->second;
+  }
+  if (y <= min_y + kDoubleEpsilon) {
+    return yz_table.begin()->second;
+  }
+
+  auto itr_after = yz_table.lower_bound(y);
+  auto itr_before = itr_after;
+
+  if (itr_before != yz_table.begin()) {
+    --itr_before;
+  }
+
+  double y_before = itr_before->first;
+  double z_before = itr_before->second;
+  double y_after = itr_after->first;
+  double z_after = itr_after->second;
+
+  double y_diff_before = std::fabs(y - y_before);
+  double y_diff_after = std::fabs(y - y_after);
+
+  return InterpolateValue(z_before, y_diff_before, z_after, y_diff_after);
+}
+
+double Interpolation2D::InterpolateValue(const double value_before,
+                                         const double dist_before,
+                                         const double value_after,
+                                         const double dist_after) const {
+  if (dist_before < kDoubleEpsilon) {
+    return value_before;
+  }
+  if (dist_after < kDoubleEpsilon) {
+    return value_after;
+  }
+  double value_gap = value_after - value_before;
+  double value_buff = value_gap * dist_before / (dist_before + dist_after);
+  return value_before + value_buff;
+}
+
+}  // namespace control
+}  // namespace apollo

src/ros/catkin/src/controllertocan/src/interpolation_2d.h

@@ -0,0 +1,73 @@
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ */
+
+#pragma once
+
+#include <map>
+#include <memory>
+#include <tuple>
+#include <utility>
+#include <vector>
+
+/**
+ * @namespace apollo::control
+ * @brief apollo::control
+ */
+namespace apollo {
+namespace control {
+/**
+ * @class Interpolation2D
+ *
+ * @brief linear interpolation from key (double, double) to one double value.
+ */
+class Interpolation2D {
+ public:
+  typedef std::vector<std::tuple<double, double, double>> DataType;
+  typedef std::pair<double, double> KeyType;
+
+  Interpolation2D() = default;
+
+  /**
+   * @brief initialize Interpolation2D internal table
+   * @param xyz passing interpolation initialization table data
+   * @return true if init is ok.
+   */
+  bool Init(const DataType &xyz);
+
+  /**
+   * @brief linear interpolate from 2D key (double, double) to one double value.
+   * @param xyz passing interpolation initialization table data
+   * @return true if init is ok.
+   */
+  double Interpolate(const KeyType &xy) const;
+
+ private:
+  double InterpolateYz(const std::map<double, double> &yz_table,
+                       double y) const;
+
+  double InterpolateValue(const double value_before, const double dist_before,
+                          const double value_after,
+                          const double dist_after) const;
+
+  std::map<double, std::map<double, double>> xyz_;
+};
+
+}  // namespace control
+}  // namespace apollo

src/ros/catkin/src/controllertocan/src/lookuptable.cpp

@@ -0,0 +1,55 @@
+#include "lookuptable.h"
+
+LookupTable::LookupTable(std::string strtablepath)
+{
+    std::vector<std::tuple<double, double, double>> xvectortable_torque,xvectortable_brake;
+    QFile xFile;
+    xFile.setFileName(strtablepath.data());
+    if(xFile.open(QIODevice::ReadOnly))
+    {
+        QByteArray ba = xFile.readAll();
+        QString strba;
+        strba.append(ba);
+        QStringList strlinelist =strba.split("\n");// strba.split(QRegExp("[\t ;]+"));
+        int nline = strlinelist.size();
+        int i;
+        for(i=0;i<nline;i++)
+        {
+            QString str = strlinelist.at(i);
+            str = str.trimmed();
+            QStringList strvaluelist = str.split(QRegExp("[\t ;]+"));
+            if(strvaluelist.size()>=4)
+            {
+                double vel,acc,torque,brake;
+                vel = QString(strvaluelist.at(0)).toDouble();
+                acc = QString(strvaluelist.at(1)).toDouble();
+                torque = QString(strvaluelist.at(2)).toDouble();
+                brake = QString(strvaluelist.at(3)).toDouble();
+                xvectortable_torque.push_back(std::make_tuple(vel,acc,torque));
+                xvectortable_brake.push_back(std::make_tuple(vel,acc,brake));
+            }
+        }
+
+    }
+    xFile.close();
+
+    if(xvectortable_torque.size()>=4)
+    {
+        mInterpolation2D_torque.Init(xvectortable_torque);
+        mInterpolation2D_brake.Init(xvectortable_brake);
+        mbInterpolation2DOK = true;
+    }
+}
+
+int LookupTable::GetTorqueBrake(double fVeh,double fAcc,double & fTorque, double & fBrake)
+{
+    if(mbInterpolation2DOK == false)return -1;
+    fTorque = mInterpolation2D_torque.Interpolate(std::make_pair(fVeh,fAcc));
+    fBrake = mInterpolation2D_brake.Interpolate(std::make_pair(fVeh,fAcc));
+    return 0;
+}
+
+bool LookupTable::IsINterpolationOK()
+{
+    return mbInterpolation2DOK;
+}

src/ros/catkin/src/controllertocan/src/lookuptable.h

@@ -0,0 +1,21 @@
+#ifndef LOOKUPTABLE_H
+#define LOOKUPTABLE_H
+
+#include <QFile>
+
+#include "interpolation_2d.h"
+
+class LookupTable
+{
+public:
+    LookupTable(std::string strtablepath);
+private:
+    apollo::control::Interpolation2D mInterpolation2D_torque,mInterpolation2D_brake;
+    bool mbInterpolation2DOK = false;
+
+public:
+    int GetTorqueBrake(double fVeh,double fAcc,double & fTorque, double & fBrake);
+    bool IsINterpolationOK();
+};
+
+#endif // LOOKUPTABLE_H

src/ros/catkin/src/controllertocan/src/main.cpp

@@ -0,0 +1,100 @@
+#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 "ge3code.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 = "GE3";
+static std::string gstrtablepath = "/home/yuchuli/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, "pilottoros");
+//	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 == "GE3")
+{
+    std::cout<<" GE3 "<<std::endl;
+    gPC = new GE3Code(gstrtablepath);
+}
+
+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);
+
+  //  pose_pub = private_nh.advertise<geometry_msgs::PoseStamped>(_pose_topic,10);
+  //  twist_pub = private_nh.advertise<geometry_msgs::TwistStamped>(_twist_topic,10);
+
+
+//std::thread * pthread = new std::thread(ThreadRecvRTK);
+
+
+ros::spin();
+
+}

src/ros/catkin/src/controllertocan/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/src/vv7.h

@@ -0,0 +1,97 @@
+#pragma once
+
+struct Command_Bit
+{
+    unsigned char head;
+
+    unsigned char  torque_H;
+
+    unsigned char  torque_L;
+
+    unsigned char ang_H;
+
+    unsigned char ang_L;
+
+    unsigned char brake;
+
+
+      unsigned char door : 1;
+
+      unsigned char speaker : 1;
+
+       unsigned char handBrake : 1;
+
+       unsigned char driveMode : 1;
+
+       unsigned char grade : 1;
+
+       unsigned char  engine: 2;
+
+       unsigned char  doorEnable: 1;
+
+
+
+       unsigned char right_turn : 1;
+
+       unsigned char left_turn : 1;
+
+       unsigned char bright : 1;
+
+       unsigned char Reserved :3;
+
+       unsigned char dangwei:2;
+
+
+
+
+//       unsigned char heartbreak;
+
+//       unsigned char checksum;
+
+
+};
+
+union Command
+{
+    Command_Bit 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];
+};
+
+
+
+
+
+
+
+

src/ros/catkin/src/rtk_hcp2/src/main.cpp

@@ -13,32 +13,18 @@
 
 #include "gnss_coordinate_convert.h"
 
-static std::string _point_topic = "/points_raw";
-static std::string _point_msgname = "lidar_pc";
-
-static std::string _image_topic = "/image_raw";
-static std::string _image_msgname = "picfront";
-
-static std::string  _odom_topic = "/odom";
-static std::string _odom_msgname = "odom";
-
-static std::string _waypoints_topic = "/final_waypoints";
-static std::string _waypoints_msgname = "waypoints";
-
 static std::string _pose_topic = "/current_pose";
 static std::string _twist_topic =  "/current_velocity";
 
 static std::string gstrserialportname = "/dev/ttyUSB0";
 
-ros::Publisher pose_pub;
-ros::Publisher twist_pub;
-
+static int gnBaudRate = 230400;
 static double glon0 = 117;
 static double glat0 = 39;
 static double ghead0 = 0;
 
-static bool _use_rtk_odom = true; //if use rtk , use_rtk_odom is true;
-static bool  _use_pilot_waypoints = true; //if use pilot to calculate waypoints, this is true
+ros::Publisher pose_pub;
+ros::Publisher twist_pub;
 
 QString mstrHCP2;
 
@@ -305,7 +291,7 @@ void SerialGPSDecodeSen(QString strsen)
 
     msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
 
-        geometry_msgs::PoseStamped  xPose;
+    geometry_msgs::PoseStamped  xPose;
     xPose.pose = msg.pose.pose;
     geometry_msgs::TwistStamped xtwist;
     xtwist.twist = msg.twist.twist;
@@ -313,15 +299,13 @@ void SerialGPSDecodeSen(QString strsen)
     pose_pub.publish(xPose);
     twist_pub.publish(xtwist);
 
-   
-
-
 }
 
 
 void ThreadRecvRTK()
 {
     QSerialPort  *m_serialPort_GPS;
+    m_serialPort_GPS = new QSerialPort();
     m_serialPort_GPS->setPortName(gstrserialportname.data());
     m_serialPort_GPS->setBaudRate(230400);
     m_serialPort_GPS->setParity(QSerialPort::NoParity);
@@ -364,30 +348,28 @@ int main(int argc, char *argv[])
 	ros::init(argc, argv, "pilottoros");
 //	ros::NodeHandle n;
     ros::NodeHandle private_nh("~");
-	ros::Rate loop_rate(1);
+	ros::Rate loop_rate(100);
 
 	    
-    private_nh.getParam("points_node",_point_topic);
-	private_nh.getParam("points_msgname",_point_msgname);
-    std::cout<<"_point_topic is "<<_point_topic<<std::endl;
-	std::cout<<"_point_msgname : "<<_point_msgname<<std::endl;
+    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("image_node",_image_topic);
-	private_nh.getParam("image_msgname",_image_msgname);
-    std::cout<<"_image_topic is "<<_image_topic<<std::endl;
-	std::cout<<"_image_msgname : "<<_image_msgname<<std::endl;
+    private_nh.getParam("PortName",gstrserialportname);
+    std::cout<<"Port Name : "<<gstrserialportname<<std::endl;
 
-    private_nh.getParam("waypoints_node",_waypoints_topic);
-    private_nh.getParam("waypoints_msgname",_waypoints_msgname);
-    std::cout<<"_waypoints_topic is "<<_waypoints_topic<<std::endl;
-	std::cout<<"_waypoints_msgname : "<<_waypoints_msgname<<std::endl;
+    private_nh.getParam("BaudRate",gnBaudRate);
+    std::cout<<"BaudRate : "<<gnBaudRate<<std::endl;
 
-    private_nh.getParam("use_rtk_odom",_use_rtk_odom);
-    private_nh.getParam("use_pilot_waypoints",_use_pilot_waypoints);
+    private_nh.getParam("Lon0",glon0);
+    std::cout<<"Lon0 ： "<<glon0<<std::endl;
 
+    private_nh.getParam("Lat0",glat0);
+    std::cout<<"Lat0 : "<<glat0<<std::endl;
 
-            pose_pub = private_nh.advertise<geometry_msgs::PoseStamped>(_pose_topic,10);
-        twist_pub = private_nh.advertise<geometry_msgs::TwistStamped>(_twist_topic,10);
+    pose_pub = private_nh.advertise<geometry_msgs::PoseStamped>(_pose_topic,10);
+    twist_pub = private_nh.advertise<geometry_msgs::TwistStamped>(_twist_topic,10);
 
 
 std::thread * pthread = new std::thread(ThreadRecvRTK);