RMP220 1.01.0VER, release for first

src/segway_msgs/CMakeLists.txt

@@ -0,0 +1,76 @@
+cmake_minimum_required(VERSION 3.5)
+project(segway_msgs)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fpic")
+set(CMAKE_C_FLAGS   "${CMAKE_C_FLAGS}   -fpic")
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+####find_package(builtin_interface REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(action_msgs REQUIRED)
+
+set(msg_files
+  "msg/BmsFb.msg"
+  "msg/ChassisCtrlSrcFb.msg"
+  "msg/ChassisMileageMeterFb.msg"
+  "msg/ChassisModeFb.msg"
+  "msg/ErrorCodeFb.msg"
+  "msg/MotorWorkModeFb.msg"
+  "msg/SpeedFb.msg"
+  "msg/TicksFb.msg"
+)
+
+set(srv_files
+  "srv/ChassisSendEvent.srv"
+  "srv/RosGetChargeMosCtrlStatusCmd.srv"
+  "srv/RosGetLoadParamCmd.srv"
+  "srv/RosGetSwVersionCmd.srv"
+  "srv/RosGetVelMaxFeedbackCmd.srv"
+  "srv/RosSetChargeMosCtrlCmd.srv"
+  "srv/RosSetChassisEnableCmd.srv"
+  "srv/RosSetChassisPoweroffCmd.srv"
+  "srv/RosSetLoadParamCmd.srv"
+  "srv/RosSetRemovePushCmd.srv"
+  "srv/RosSetVelMaxCmd.srv")
+
+set(action_files
+  "action/RosSetIapCmd.action"
+)
+rosidl_generate_interfaces(${PROJECT_NAME} 
+  ${msg_files} 
+  ${srv_files}
+  ${action_files}
+  DEPENDENCIES  geometry_msgs std_msgs ADD_LINTER_TESTS)
+
+# ament_export_dependencies(rosidl_default_generators)
+# ament_export_dependencies(rosidl_default_runtime)
+# ament_export_include_directories(include)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/segway_msgs/action/RosSetIapCmd.action

@@ -0,0 +1,6 @@
+int16  iap_board    #1:iap central board;  2:iap motor board
+---
+int16  iap_result  #3: iap_state_complete; 4: iap_state_fail; 5: iap_state_abort
+int16  error_code  #When iap_result value is 4, this value represents the error code
+---
+int16  iap_percent

src/segway_msgs/msg/BmsFb.msg

@@ -0,0 +1,5 @@
+int16  bat_soc    
+int16  bat_charging 
+int32  bat_vol 
+int32  bat_current 
+int16  bat_temp

src/segway_msgs/msg/ChassisCtrlSrcFb.msg

@@ -0,0 +1 @@
+uint16 chassis_ctrl_cmd_src

src/segway_msgs/msg/ChassisMileageMeterFb.msg

@@ -0,0 +1 @@
+uint32 vehicle_meters

src/segway_msgs/msg/ChassisModeFb.msg

@@ -0,0 +1 @@
+uint16 chassis_mode

src/segway_msgs/msg/ErrorCodeFb.msg

@@ -0,0 +1,5 @@
+uint32 host_error
+uint32 central_error
+uint16 left_motor_error
+uint16 right_motor_error
+uint32 bms_error

src/segway_msgs/msg/MotorWorkModeFb.msg

@@ -0,0 +1 @@
+uint16 motor_work_mode		#0: no output torque 1: output torque

src/segway_msgs/msg/SpeedFb.msg

@@ -0,0 +1,5 @@
+float32 car_speed
+float32 turn_speed
+float32 l_speed
+float32 r_speed
+uint64 speed_timestamp

src/segway_msgs/msg/TicksFb.msg

@@ -0,0 +1,3 @@
+int32 l_ticks
+int32 r_ticks
+uint64 ticks_timestamp

src/segway_msgs/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>segway_msgs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="chunxin.chu@ninebot.com">ninebot</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>action_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/segway_msgs/srv/ChassisSendEvent.srv

@@ -0,0 +1,3 @@
+int16  chassis_send_event_id
+---
+bool   ros_is_received

src/segway_msgs/srv/RosGetChargeMosCtrlStatusCmd.srv

@@ -0,0 +1,3 @@
+bool   ros_get_chassis_charge_ctrl_status
+---
+int16   chassis_charge_ctrl_status

src/segway_msgs/srv/RosGetLoadParamCmd.srv

@@ -0,0 +1,3 @@
+bool   ros_get_load_param 
+---
+int16  get_load_param  #0:no_load, 1: full_load

src/segway_msgs/srv/RosGetSwVersionCmd.srv

@@ -0,0 +1,5 @@
+bool   ros_get_sw_version_cmd
+---
+uint16 host_version    
+uint16 central_version   
+uint16 motor_version

src/segway_msgs/srv/RosGetVelMaxFeedbackCmd.srv

@@ -0,0 +1,5 @@
+bool   ros_get_vel_max_fb_cmd
+---
+uint16  forward_max_vel_fb   
+uint16  backward_max_vel_fb    
+uint16  angular_max_vel_fb

src/segway_msgs/srv/RosSetChargeMosCtrlCmd.srv

@@ -0,0 +1,3 @@
+bool   ros_set_chassis_charge_ctrl
+---
+uint8   chassis_set_charge_ctrl_result

src/segway_msgs/srv/RosSetChassisEnableCmd.srv

@@ -0,0 +1,3 @@
+bool   ros_set_chassis_enable_cmd
+---
+uint8   chassis_set_chassis_enable_result

src/segway_msgs/srv/RosSetChassisPoweroffCmd.srv

@@ -0,0 +1,3 @@
+bool   ros_set_chassis_poweroff_cmd
+---
+uint8   chassis_set_poweroff_result

src/segway_msgs/srv/RosSetLoadParamCmd.srv

@@ -0,0 +1,3 @@
+int16  ros_set_load_param #0:no_load, 1: full_load
+---
+uint8   chassis_set_load_param_result

src/segway_msgs/srv/RosSetRemovePushCmd.srv

@@ -0,0 +1,3 @@
+bool  ros_set_remove_push_cmd
+---
+uint8  chassis_set_revove_push_result

src/segway_msgs/srv/RosSetVelMaxCmd.srv

@@ -0,0 +1,5 @@
+float64  ros_set_forward_max_vel  
+float64  ros_set_backward_max_vel  
+float64  ros_set_angular_max_vel
+---
+uint8    chassis_set_max_vel_result

src/segwayrmp/CMakeLists.txt

@@ -0,0 +1,81 @@
+cmake_minimum_required(VERSION 3.5)
+project(segwayrmp)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fpic  -Wl,-R${PROJECT_SOURCE_DIR}/lib")
+set(CMAKE_C_FLAGS   "${CMAKE_C_FLAGS}   -fpic")
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(segway_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_components REQUIRED)
+
+add_executable(SmartCar
+  src/SmartCar.cpp 
+  src/robot.cpp 
+)
+target_include_directories(SmartCar PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+target_link_directories(SmartCar PUBLIC 
+  ${CMAKE_CURRENT_SOURCE_DIR}/lib
+)
+# message(WARNING ${PROJECT_SOURCE_DIR} + "!!" + ${CMAKE_CURRENT_SOURCE_DIR} + "!!" + ${PROJECT_NAME})
+ament_target_dependencies(SmartCar
+  "rclcpp" "std_msgs" "nav_msgs" "sensor_msgs" "geometry_msgs" "segway_msgs" "tf2" "tf2_ros"
+) 
+target_link_libraries(SmartCar
+  ${PROJECT_SOURCE_DIR}/lib/libctrl_x86_64.so
+)
+
+add_executable(drive_segway_sample
+  tools/drive_segway_sample.cpp
+)
+target_include_directories(drive_segway_sample PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies(drive_segway_sample
+  "rclcpp" "std_msgs" "nav_msgs" "sensor_msgs" "geometry_msgs" "segway_msgs" "tf2" "tf2_ros"
+) 
+
+install(TARGETS
+  SmartCar
+  drive_segway_sample
+  DESTINATION
+  lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/segwayrmp/include/comm_ctrl.h

@@ -0,0 +1,109 @@
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef COMM_CTRL_H
+#define COMM_CTRL_H
+
+#include <stdbool.h>
+#include <stdint.h>
+#include "comm_ctrl_navigation.h"
+
+
+// Map table transmission value gain coefficient; 
+#define ERROR_LINE_LIMIT_VEL_VALUE		(5 * LINE_SPEED_TRANS_GAIN_MPS)
+#define ERROR_ANGULAR_LIMIT_VEL_VALUE	(5 * ANGULAR_SPEED_TRANS_GAIN_RADPS)
+#define ACC_VALUE_GAIN	100
+
+
+#define		RMP_HOST_ERR_BIT_MCU_HEART_BEAT		0 //The heartbeat with the chassis was interrupted
+#define		RMP_HOST_ERR_BIT_SERIAL_PORT_LOST	1 //Serial port module unplugged
+
+void AprGxJniEventRegister(int32_t (*callback)(int32_t));
+
+//-------------------Timestamp------------------------
+
+//--------------------GX--API------------------
+int init_control(void);
+void exit_control(void);
+
+int16_t get_forward_speed_limit(void);
+int16_t get_backward_speed_limit(void);
+int16_t get_angular_speed_limit(void);
+uint16_t get_Stop_Bottom_Status(void);
+uint8_t set_calib_chassis_imu_gyro(void);//Reserve for later versions
+uint16_t get_Chassis_Imu_Calib_Result(void);   //Reserve for later versions
+void clear_Chassis_Imu_Calib_Result(void);//Reserve for later versions
+uint8_t  set_calib_chassis_current(void);
+uint16_t get_chassis_current_calib_result(void);
+void     clear_chassis_current_calib_result(void);
+
+
+//-----------------GX  IAP--------------------
+void IapAllBoard(const char **file_paths, char **versions);
+int32_t IapSingerBoard(char * path,char * boradname,char* version);
+//int32_t getIapTotalProgress(void);
+//------------------GX Host-------------------
+
+char* GetGxHardVersion(void);
+void TracePrint_Switch(bool status);  //true  or false
+void SetNavigationStatus(uint8_t status);
+void setSaveRecordCmd(void);    //开始保存log接口　　　　　ps:上层调用
+void clearRecordCmd(void);      //清除保存log命令　　　　　ps:上层不需要调<E8A681>?so每次保存完log后会自动调用
+uint8_t getSaveRecordCmd(void); //获取是否开始保存log　　　ps:so自动调用,上层不需要调<E8A681>?
+uint8_t GetNavigationStatus(void);
+
+
+void set_gx_route_bat_voltage(uint16_t voltage);
+
+
+int16_t get_cmd_speed_forward(void);
+int16_t get_fbk_speed_forward(void);
+int16_t get_encode_fbk_speed_forward(void);
+int16_t get_cmd_w_forward(void);
+int16_t get_fbk_w_forward(void);
+int16_t get_rec_cmd_vx(void);
+int16_t get_rec_cmd_w(void);
+int16_t get_pid_target_vx(void);
+int16_t get_pid_target_w(void);
+int16_t get_encode_speed_L(void);
+int16_t get_encode_speed_R(void);
+
+int16_t get_heart_beat_data(board_name_e board_type);
+
+void set_host_error(uint8_t offset, uint8_t err_bool_value);
+uint32_t get_host_error(void);
+uint32_t get_host_speed_cmd_cnt(void);
+uint32_t get_chassis_reply_speed_cmd_cnt(void);
+int32_t get_encoder_l_ticks(void);
+int32_t get_encoder_r_ticks(void);
+char * get_smart_car_serial(void);
+uint8_t get_comu_interface_serial0_can1();
+void showHostBuildTime(void);
+void showChassisCentralBuildTime(void);
+
+/* 暂时不开放此接口，由于map表只能发送整数，故此接口待修改：乘以10或<30>?00下发，电机板收到命令数据后再恢复数据 */
+uint8_t  set_cmd_acc(double linear_x_acc, double linear_x_brake_acc, double angular_z_acc);//Set the linear and angular accelerated speed: m/s2 ; rad/s2.
+
+uint8_t  set_chassis_no_load(void);
+uint8_t  set_chassis_full_load(void);
+
+void    ackWriteCentralMapInit();
+uint8_t ctrlMapAckWrite(    uint8_t dest_id,  uint8_t map_start_index, uint32_t mem_len);
+
+void set_chassis_charge_status(int16_t chargestatus);
+uint32_t get_chassis_central_Err_Status(void);
+uint16_t get_chassis_l_motor_Err_Status(void);
+uint16_t get_chassis_r_motor_Err_Status(void);
+uint32_t get_chassis_bms_Err_Status(void);
+char * show_host_version(void);
+
+	
+int16_t get_charge_mos_ctrl_cmd(void);// Get chassis charge_ctrl mos cmd; 1:turn on; 0: turn off
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+

src/segwayrmp/include/comm_ctrl_navigation.h

@@ -0,0 +1,180 @@
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef COMM_CTRL_NAVIGATION_H
+#define COMM_CTRL_NAVIGATION_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define TYPE_ID_NUM                     10   //Number of callback functions
+//--------------------------DATA CALL BACK INDEX--------------------------
+#define Chassis_Data_Speed       		1
+#define Chassis_Data_Ticks              2
+#define Chassis_Data_Odom_Pose_xy		3
+#define Chassis_Data_Odom_Euler_xy		4
+#define Chassis_Data_Odom_Euler_z		5
+#define Chassis_Data_Odom_Linevel_xy	6
+#define Chassis_Data_Imu_Gyr            7	//IMU Gyroscope data 陀螺仪数据上报
+#define Chassis_Data_Imu_Acc            8	//IMU Accelerometer data 加速度计数据上<E68DAE>?
+
+//-----------------------Event---------------------------------------------
+#define ChassisBootReadyEvent  		1	// Chassis central control boot OK
+#define PadPowerOffEvent      		2	// The chassis will power off
+#define OnEmergeStopEvent     		3	// The chassis emergency stop button is triggered
+#define OutEmergeStopEvent    		4	// The chassis emergency stop button recover
+#define OnLockedRotorProtectEvent	5	// the chassis motor locked-rotor
+#define OutLockedRotorProtectEvent	6   // the chassis motor no longer locked-rotor
+#define OnLostCtrlProtectEvent		7	// the chassis motor lost control
+#define OutLostCtrlProtectEvent		8	// the chassis motor no longer lost control
+#define CalibrateGyroSuccess		9	// Chassis gyroscope calibration successful
+#define CalibrateGyroFail			10  // Chassis gyroscope calibration failed
+#define CalibratePasheCurrentSuccess	11	// Chassis gyroscope calibration successful
+#define CalibratePasheCurrentFail		12  // Chassis gyroscope calibration failed
+
+
+//---------The proportional coefficient of the callback gyro data------------------
+#define CHASSIS_IMU_GYR_VALUE_TRANS_SCALE		900			//900~~32768(MAX value for int16: map table ) * 57.3(degrees for 1 rad) /2000(Gyroscope range: 2000dps)
+#define CHASSIS_IMU_ACC_VALUE_TRANS_SCALE		4000		//4000~~32768(MAX value for int16: map table )/8(Accelerometer range: 8g)
+
+#define LINE_SPEED_TRANS_GAIN_MPS		3600	//3600-->1m/s
+#define ANGULAR_SPEED_TRANS_GAIN_RADPS		1000	//1000-->1rad/s
+
+#define NO_LOAD  	0		// Set the chassis parameters as no-load parameters, The chassis defaults to no load
+#define	FULL_LOAD 	1		// Set the chassis parameters as full load parameters
+
+//-----------------------Version V0.6 and above---------------------------
+#define LOCK_MODE	0		// Lock the car
+#define CTRL_MODE	1		// Control the car
+#define PUSH_MODE	2		// push the car
+#define EMERG_MODE	3		// emergency
+#define ERROR_MODE	4		// Internal error
+
+
+//-------------------Timestamp------------------------
+#define MAX_BASIC_FRM_SZ 0x1F
+
+#pragma pack(1)
+typedef struct StampedBasicFrame_{
+    uint32_t type_id;                 //Data type number
+    uint64_t timestamp;               //Linux timestamp
+    char  data[MAX_BASIC_FRM_SZ];     //Chassis specific data
+} StampedBasicFrame;
+#pragma pack()
+typedef void (*h_aprctrl_datastamped_t)(StampedBasicFrame* frm);
+typedef void (*h_aprctrl_event_t)(int32_t event_num);
+
+typedef struct {
+    h_aprctrl_datastamped_t on_new_data;
+}s_aprctrl_datastamped_t;
+
+typedef struct {
+    h_aprctrl_event_t event_callback;
+}s_aprctrl_event_t;
+
+typedef enum {
+    Host = 1,
+    Central = 2, //底盘只连中控<E4B8AD>?
+    Motor	= 3,
+    BMS		= 4
+}board_name_e;
+
+typedef enum {
+   comu_serial = 0,
+   comu_can = 1
+}comu_choice_e;
+
+typedef struct{
+	int16_t l_speed;//Left wheel speed
+	int16_t r_speed;//Right wheel speed
+	int16_t car_speed;//Vehicle linear speed
+	int16_t turn_speed;//Turning speed
+}chassis_speed_data_t;
+
+typedef struct{
+	int32_t l_ticks;//Left wheel ticks
+	int32_t r_ticks;//Right wheel ticks
+}motor_ticks_t;
+
+typedef struct{
+	float pos_x;
+	float pos_y;
+}odom_pos_xy_t;
+
+typedef struct{
+	float euler_x;
+	float euler_y;
+}odom_euler_xy_t;
+
+typedef struct{
+	float euler_z;
+}odom_euler_z_t;
+
+typedef struct{
+	float vel_line_x;
+	float vel_line_y;
+}odom_vel_line_xy_t;
+
+typedef struct{
+	int16_t gyr[3];
+}imu_gyr_original_data_t;
+
+typedef struct{
+	int16_t acc[3];
+}imu_acc_original_data_t;
+
+
+void aprctrl_datastamped_jni_register(s_aprctrl_datastamped_t* f);  //The callback registration function
+void aprctrl_eventcallback_jni_register(s_aprctrl_event_t* f);     //Event callback registration function
+
+uint32_t get_err_state(board_name_e board_name);//Gets the software error status
+int16_t  get_bat_soc(void);//Gets the percentage of battery left
+int16_t  get_bat_charging(void);//Gets the charging status of the battery
+int32_t  get_bat_mvol(void);//Gets battery voltage, mV
+int32_t  get_bat_mcurrent(void);//Gets battery current, mA
+int16_t  get_bat_temp(void);// Gets battery temperature
+int16_t  get_chassis_work_model(void);//Get the chassis working mode
+uint8_t  get_chassis_load_state(void);//Gets whether the chassis parameters are empty or full load, 0: no_load, 1: full_load
+uint16_t get_chassis_mode(void);//Gets chassis mode
+int16_t  get_ctrl_cmd_src(void); // Get the source of the control command 
+int32_t  get_vehicle_meter(void); //Get the total mileage meters of the chssis
+uint16_t get_host_version(void);
+uint16_t get_chassis_central_version(void);
+uint16_t get_chassis_motor_version(void);
+int16_t  get_line_forward_max_vel_fb(void);
+int16_t  get_line_backward_max_vel_fb(void);
+int16_t  get_angular_max_vel_fb(void);
+int32_t  getIapTotalProgress(void);
+void 	iapCentralBoard(void);//Operate on the Central board: IAP
+void 	iapMotorBoard(void);//Operate on the Motor board: IAP
+bool    isHostIapOver(void);//Query whether a single IAP ends
+int16_t getHostIapResult(void);
+int16_t getHostIapErrorCode(void);
+int16_t get_chassis_hang_mode(void);//Get chassis hang_mode; 1: in Hang_mode; 0: not in Hand_mode.
+int16_t get_charge_mos_ctrl_status(void);//Get the status of switch for charging MOS on the central board, charging:1; no charge:0
+
+
+
+void 	set_cmd_vel(double linear_x,double angular_z);//Set the linear and angular speeds of the vehicle: m/s ; rad/s.
+uint8_t set_line_forward_max_vel(double linear_forward_max_x);//Set the maximum linear velocity in the direction of advance
+uint8_t set_line_backward_max_vel(double linear_backward_max_x);//Set the maximum linear velocity in the backward direction
+uint8_t set_angular_max_vel(double angular_max_z);//Set the maximum angular velocity
+uint8_t set_enable_ctrl(uint16_t enable_flag);//Set chassis movement enable
+int  	init_control_ctrl(void);//Chassis initialization
+void 	exit_control_ctrl(void);//Chassis software finished running
+void 	set_smart_car_serial(const char * serial_no);//Set the serial port name
+void 	set_comu_interface(comu_choice_e comu_choice); //Set communication interface: 'comu_serial': serial port; 'comu_can': CAN port
+uint8_t set_chassis_load_state(int16_t newLoadSet);//Sets whether the chassis parameters are empty or full load, 0: no_load, 1: full_load
+uint8_t set_chassis_poweroff(void);//Set two - wheel differential chassis shutdown
+uint8_t set_remove_push_cmd(void);//An order to remove the push status
+void    setHostIapCanceled(void);
+void    set_chassis_hang_mode(int16_t enterHand);//Set chassis hang_mode; 1: enter the Hang_mode; 0: exit the Hand_mode.
+uint8_t	set_charge_mos_ctrl(bool on);//Set the switch for charging MOS on the central board; charging:1, stop charging:0.
+
+extern int32_t IapSingerBoard(char * path,char * boradname,char* version);
+#endif
+
+#ifdef __cplusplus
+}
+#endif

src/segwayrmp/include/segwayrmp/robot.h

@@ -0,0 +1,169 @@
+#ifndef _ROBOT_H
+#define _ROBOT_H
+
+#include <functional>
+#include <chrono>
+#include <memory>
+#include <string>
+#include "stdint.h"
+#include <time.h>
+#include <sys/time.h>
+
+#include <rosidl_typesupport_cpp/message_type_support.hpp>
+#include "rclcpp/rclcpp.hpp"
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include "tf2_ros/transform_broadcaster.h"
+#include <tf2_ros/static_transform_broadcaster.h>
+#include "tf2/LinearMath/Quaternion.h"
+#include "comm_ctrl_navigation.h"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "segway_msgs/msg/bms_fb.hpp"
+#include "segway_msgs/msg/chassis_ctrl_src_fb.hpp"
+#include "segway_msgs/msg/chassis_mileage_meter_fb.hpp"
+#include "segway_msgs/msg/chassis_mode_fb.hpp"
+#include "segway_msgs/msg/error_code_fb.hpp"
+#include "segway_msgs/msg/motor_work_mode_fb.hpp"
+#include "segway_msgs/msg/speed_fb.hpp"
+#include "segway_msgs/msg/ticks_fb.hpp"
+#include "segway_msgs/srv/chassis_send_event.hpp"
+#include "segway_msgs/srv/ros_get_charge_mos_ctrl_status_cmd.hpp"
+#include "segway_msgs/srv/ros_get_load_param_cmd.hpp"
+#include "segway_msgs/srv/ros_get_sw_version_cmd.hpp"
+#include "segway_msgs/srv/ros_get_vel_max_feedback_cmd.hpp"
+#include "segway_msgs/srv/ros_set_charge_mos_ctrl_cmd.hpp"
+#include "segway_msgs/srv/ros_set_chassis_enable_cmd.hpp"
+#include "segway_msgs/srv/ros_set_chassis_poweroff_cmd.hpp"
+#include "segway_msgs/srv/ros_set_load_param_cmd.hpp"
+#include "segway_msgs/srv/ros_set_remove_push_cmd.hpp"
+#include "segway_msgs/srv/ros_set_vel_max_cmd.hpp"
+#include "segway_msgs/action/ros_set_iap_cmd.hpp"
+
+#define pi   3.141592654
+#define pi_2 6.283185308
+
+namespace robot
+{
+    class Chassis
+    {
+        public:
+            using iapCmd = segway_msgs::action::RosSetIapCmd;
+            using goalHandaleIapCmd = rclcpp_action::ServerGoalHandle<iapCmd>;
+            Chassis(rclcpp::Node::SharedPtr nh);
+            static void pub_event_callback(int event_no);
+        private:
+            std::shared_ptr<rclcpp::Node> node;
+
+            std::string bins_directory;
+            std::string chassis_version;
+            std::string route_version;
+            std::string connect_version;
+
+            tf2::Quaternion odom_quat;
+            std::shared_ptr<tf2_ros::StaticTransformBroadcaster> odom_broadcaster;
+
+            segway_msgs::msg::BmsFb bms_fb;
+            segway_msgs::msg::ChassisCtrlSrcFb chassis_ctrl_src_fb;
+            segway_msgs::msg::ChassisMileageMeterFb chassis_mileage_meter_fb;
+            segway_msgs::msg::ChassisModeFb chassis_mode_fb;
+            segway_msgs::msg::ErrorCodeFb error_code_fb;
+            segway_msgs::msg::MotorWorkModeFb motor_work_mode_fb;
+            segway_msgs::msg::SpeedFb speed_fb;
+            segway_msgs::msg::TicksFb ticks_fb;
+            geometry_msgs::msg::TransformStamped odom_trans;
+            nav_msgs::msg::Odometry odom_fb;
+            sensor_msgs::msg::Imu imu_fb;     
+
+            rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr velocity_sub;
+
+            rclcpp::Publisher<segway_msgs::msg::BmsFb>::SharedPtr bms_fb_pub;
+            rclcpp::Publisher<segway_msgs::msg::ChassisCtrlSrcFb>::SharedPtr chassis_ctrl_src_fb_pub;
+            rclcpp::Publisher<segway_msgs::msg::ChassisMileageMeterFb>::SharedPtr chassis_mileage_meter_fb_pub;
+            rclcpp::Publisher<segway_msgs::msg::ChassisModeFb>::SharedPtr chassis_mode_fb_pub;
+            rclcpp::Publisher<segway_msgs::msg::ErrorCodeFb>::SharedPtr error_code_fb_pub;
+            rclcpp::Publisher<segway_msgs::msg::MotorWorkModeFb>::SharedPtr motor_work_mode_fb_pub;
+            rclcpp::Publisher<segway_msgs::msg::SpeedFb>::SharedPtr speed_fb_pub;
+            rclcpp::Publisher<segway_msgs::msg::TicksFb>::SharedPtr ticks_fb_pub;
+            rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub;
+            rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub;
+
+
+            rclcpp::Service<segway_msgs::srv::RosGetChargeMosCtrlStatusCmd>::SharedPtr ros_get_charge_mos_ctrl_status_cmd_server;
+            rclcpp::Service<segway_msgs::srv::RosGetLoadParamCmd>::SharedPtr ros_get_load_param_cmd_server;
+            rclcpp::Service<segway_msgs::srv::RosGetSwVersionCmd>::SharedPtr ros_get_sw_version_cmd_server;
+            rclcpp::Service<segway_msgs::srv::RosGetVelMaxFeedbackCmd>::SharedPtr ros_get_vel_max_feedback_cmd_server;
+            rclcpp::Service<segway_msgs::srv::RosSetChargeMosCtrlCmd>::SharedPtr ros_set_charge_mos_ctrl_cmd_server;
+            rclcpp::Service<segway_msgs::srv::RosSetChassisEnableCmd>::SharedPtr ros_set_chassis_enable_cmd_server;
+            rclcpp::Service<segway_msgs::srv::RosSetChassisPoweroffCmd>::SharedPtr ros_set_chassis_poweroff_cmd_server;
+            rclcpp::Service<segway_msgs::srv::RosSetLoadParamCmd>::SharedPtr ros_set_load_param_cmd_server;
+            rclcpp::Service<segway_msgs::srv::RosSetRemovePushCmd>::SharedPtr ros_set_remove_push_cmd_server;
+            rclcpp::Service<segway_msgs::srv::RosSetVelMaxCmd>::SharedPtr ros_set_vel_max_cmd_server;
+
+            void ros_get_charge_mos_ctrl_status_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetChargeMosCtrlStatusCmd::Request> request,
+                std::shared_ptr<segway_msgs::srv::RosGetChargeMosCtrlStatusCmd::Response> response);
+            void ros_get_load_param_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetLoadParamCmd::Request> request,
+                std::shared_ptr<segway_msgs::srv::RosGetLoadParamCmd::Response> response);
+            void ros_get_sw_version_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetSwVersionCmd::Request> request,
+                std::shared_ptr<segway_msgs::srv::RosGetSwVersionCmd::Response> response);
+            void ros_get_vel_max_feedback_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetVelMaxFeedbackCmd::Request> request,
+                std::shared_ptr<segway_msgs::srv::RosGetVelMaxFeedbackCmd::Response> response);
+            void ros_set_charge_mos_ctrl_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetChargeMosCtrlCmd::Request> request,
+                std::shared_ptr<segway_msgs::srv::RosSetChargeMosCtrlCmd::Response> response);
+            void ros_set_chassis_enable_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetChassisEnableCmd::Request> request,
+                std::shared_ptr<segway_msgs::srv::RosSetChassisEnableCmd::Response> response);
+            void ros_set_chassis_poweroff_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetChassisPoweroffCmd::Request> request,
+                std::shared_ptr<segway_msgs::srv::RosSetChassisPoweroffCmd::Response> response);
+            void ros_set_load_param_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetLoadParamCmd::Request> request,
+                std::shared_ptr<segway_msgs::srv::RosSetLoadParamCmd::Response> response);
+            void ros_set_remove_push_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetRemovePushCmd::Request> request,
+                std::shared_ptr<segway_msgs::srv::RosSetRemovePushCmd::Response> response);
+            void ros_set_vel_max_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetVelMaxCmd::Request> request,
+                std::shared_ptr<segway_msgs::srv::RosSetVelMaxCmd::Response> response);
+
+            rclcpp_action::Server<iapCmd>::SharedPtr iap_action_server;
+
+            rclcpp::TimerBase::SharedPtr timer_1hz;
+            rclcpp::TimerBase::SharedPtr timer_1000hz;
+
+            s_aprctrl_datastamped_t timestamp_data;
+            s_aprctrl_event_t       event_data;
+
+            void timer_1000hz_callback(void);
+            void timer_1hz_callback(void);
+            void speed_pub_callback(void);
+            void ticks_pub_callback(void);
+            void imu_pub_callback(void);
+            void pub_odom_callback(void);
+            void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) const;
+
+            void iapCmdExecute(const std::shared_ptr<goalHandaleIapCmd> goal_handle);
+            rclcpp_action::GoalResponse handle_iapCmdGoal(
+                const rclcpp_action::GoalUUID & uuid,
+                std::shared_ptr<const iapCmd::Goal> goal)
+            {
+                RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "Receive goal request with iap_board %d", goal->iap_board);
+                (void)uuid;
+                return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+            }
+
+            rclcpp_action::CancelResponse handle_iapCmdCancel(
+                const std::shared_ptr<goalHandaleIapCmd> goal_handle)
+            {
+                RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "Received request to cancel goal");
+                (void)goal_handle;
+                return rclcpp_action::CancelResponse::ACCEPT;
+            }
+
+            void handle_iapCmdAccepted(const std::shared_ptr<goalHandaleIapCmd> goal_handle)
+            {
+                using namespace std::placeholders;
+                (void)goal_handle;
+                std::thread{std::bind(&Chassis::iapCmdExecute, this, _1), goal_handle}.detach();
+            }
+    };
+
+}
+
+#endif

src/segwayrmp/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>segwayrmp</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="chunxin.chu@ninebot.com">ninebot</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <!-- <depend>sensor_msgs</depned> -->
+  <depend>segway_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_components</depend>
+  
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/segwayrmp/src/SmartCar.cpp

@@ -0,0 +1,27 @@
+#include "rclcpp/rclcpp.hpp"
+#include "segwayrmp/robot.h"
+
+int main(int argc, char * argv[])
+{
+    rclcpp::init(argc, argv);
+    auto node = rclcpp::Node::make_shared("SmartCar");
+
+    node->declare_parameter<std::string>("serial_full_name", "/dev/ttyUSB0");
+    std::string serial_full_name;
+    node->get_parameter("serial_full_name", serial_full_name);
+    // init_control()parameter: Fill in the full path name of the actual serial port being used
+
+    set_smart_car_serial((char*)serial_full_name.c_str());//If a serial port is used, set the serial port name.
+    set_comu_interface(comu_serial);//Before calling init_control_ctrl, need to call this function set whether the communication port is serial or CAN.
+
+    if (init_control_ctrl() == -1) { 
+        printf("init_control failed!\n");
+        exit_control_ctrl();
+    } else {
+        printf("init_control success!\n");
+    }
+    robot::Chassis sbv(node);
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

src/segwayrmp/src/robot.cpp

@@ -0,0 +1,489 @@
+#include "segwayrmp/robot.h"
+#include <cwchar>
+#define VEL_DT              0.05  //50ms
+#define RAD_DEGREE_CONVER   57.2958
+#define HOST_SPEED_UINIT            3600 //
+#define HOST_ANGULAR_SPEED_UINIT    2271
+
+typedef struct{
+    uint32_t typeId;
+    uint8_t   dataSize;
+    uint64_t *segwayTimeStamp;
+    uint8_t   * updateFlag;
+    void * dataPtr;
+}SegwayData;
+
+chassis_speed_data_t SpeedData;
+uint64_t    Speed_TimeStamp;
+uint8_t      Speed_update;
+
+motor_ticks_t TickData;
+uint64_t    Tick_TimeStamp;
+uint8_t     Tick_update;
+
+imu_gyr_original_data_t  ImuGyroData;
+uint64_t    ImuGyro_TimeStamp;
+uint8_t     ImuGyro_update;
+
+imu_acc_original_data_t ImuAccData;
+uint64_t    ImuAcc_TimeStamp;
+uint8_t     ImuAcc_update;
+
+odom_pos_xy_t OdomPoseXy;
+odom_euler_xy_t OdomEulerXy;
+odom_euler_z_t OdomEulerZ;
+odom_vel_line_xy_t OdomVelLineXy;
+uint64_t Odom_TimeStamp;
+uint8_t OdomPoseXy_update;
+uint8_t OdomEulerXy_update;
+uint8_t OdomEulerZ_update;
+uint8_t OdomVelLineXy_update;
+
+static SegwayData segway_data_tbl[] = {
+    {Chassis_Data_Speed,  sizeof(SpeedData), &Speed_TimeStamp, &Speed_update, &SpeedData},
+    {Chassis_Data_Ticks,  sizeof(TickData), &Tick_TimeStamp, &Tick_update, &TickData},
+    {Chassis_Data_Imu_Gyr,  sizeof(ImuGyroData), &ImuGyro_TimeStamp, &ImuGyro_update, &ImuGyroData},
+    {Chassis_Data_Imu_Acc,  sizeof(ImuAccData), &ImuAcc_TimeStamp, &ImuAcc_update, &ImuAccData},
+    {Chassis_Data_Odom_Pose_xy,  sizeof(OdomPoseXy), &Odom_TimeStamp, &OdomPoseXy_update, &OdomPoseXy},
+    {Chassis_Data_Odom_Euler_xy,  sizeof(OdomEulerXy), &Odom_TimeStamp, &OdomEulerXy_update, &OdomEulerXy},
+    {Chassis_Data_Odom_Euler_z,  sizeof(OdomEulerZ), &Odom_TimeStamp, &OdomEulerZ_update, &OdomEulerZ},
+    {Chassis_Data_Odom_Linevel_xy,  sizeof(OdomVelLineXy), &Odom_TimeStamp, &OdomVelLineXy_update, &OdomVelLineXy}  
+};
+
+std::shared_ptr<rclcpp::Node> car_node;
+rclcpp::Client<segway_msgs::srv::ChassisSendEvent>::SharedPtr event_client;
+
+void PubData(StampedBasicFrame_ *frame)
+{
+    uint8_t i = 0;
+    for (; i < sizeof(segway_data_tbl)/sizeof(segway_data_tbl[0]); i++)
+    {
+        if (frame->type_id == segway_data_tbl[i].typeId) break;
+    }
+    if (i < sizeof(segway_data_tbl)/sizeof(segway_data_tbl[0])){
+        *(segway_data_tbl[i].segwayTimeStamp) = frame->timestamp;
+        *(segway_data_tbl[i].updateFlag) =1;
+        memcpy( segway_data_tbl[i].dataPtr, frame->data,  segway_data_tbl[i].dataSize);
+    }
+}
+
+void EventPubData(int event_no)
+{
+    robot::Chassis::pub_event_callback(event_no);
+}
+
+namespace robot
+{
+void Chassis::pub_event_callback(int event_no)
+{
+    using eventServiceResponseFutrue = rclcpp::Client<segway_msgs::srv::ChassisSendEvent>::SharedFuture;
+    auto event_request = std::make_shared<segway_msgs::srv::ChassisSendEvent::Request>();
+    event_request->chassis_send_event_id = event_no;
+    auto event_response_receive_callback = [](eventServiceResponseFutrue futrue) {
+        (void)futrue;
+        RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "event sended successfully");
+    };
+    auto event_future_result = event_client->async_send_request(event_request, event_response_receive_callback);
+}
+
+Chassis::Chassis(rclcpp::Node::SharedPtr nh) : node(nh)
+{
+    using namespace std::placeholders;
+    car_node = nh;
+
+    timestamp_data.on_new_data = PubData;
+    aprctrl_datastamped_jni_register(&timestamp_data);
+    event_data.event_callback = EventPubData;
+    aprctrl_eventcallback_jni_register(&event_data);
+
+    node->declare_parameter<std::string>("bins_directory", "/sdcard/firmware/");
+    node->declare_parameter<std::string>("chassis_version", "1.01");
+    node->declare_parameter<std::string>("route_version", "1.01");
+    node->declare_parameter<std::string>("connect_version", "1.01");
+
+    bms_fb_pub = node->create_publisher<segway_msgs::msg::BmsFb>("/bms_fb", 1);
+    chassis_ctrl_src_fb_pub = node->create_publisher<segway_msgs::msg::ChassisCtrlSrcFb>("/chassis_ctrl_src_fb", 1);
+    chassis_mileage_meter_fb_pub = node->create_publisher<segway_msgs::msg::ChassisMileageMeterFb>("/chassis_mileage_meter_fb", 1);
+    chassis_mode_fb_pub = node->create_publisher<segway_msgs::msg::ChassisModeFb>("/chassis_mode_fb", 1);
+    error_code_fb_pub = node->create_publisher<segway_msgs::msg::ErrorCodeFb>("/error_code_fb", 1);
+    motor_work_mode_fb_pub = node->create_publisher<segway_msgs::msg::MotorWorkModeFb>("/motor_work_mode_fb", 1);
+    speed_fb_pub = node->create_publisher<segway_msgs::msg::SpeedFb>("/speed_fb", 1);
+    ticks_fb_pub = node->create_publisher<segway_msgs::msg::TicksFb>("/ticks_fb", 1);
+    odom_pub = node->create_publisher<nav_msgs::msg::Odometry>("/odom", 1);
+    imu_pub = node->create_publisher<sensor_msgs::msg::Imu>("/imu", 1);
+
+    event_client = node->create_client<segway_msgs::srv::ChassisSendEvent>("event_srv");
+
+    ros_get_charge_mos_ctrl_status_cmd_server = node->create_service<segway_msgs::srv::RosGetChargeMosCtrlStatusCmd>(
+       "get_charge_mos_ctrl_status", std::bind(&Chassis::ros_get_charge_mos_ctrl_status_cmd_callback, this, _1, _2));
+    ros_get_load_param_cmd_server = node->create_service<segway_msgs::srv::RosGetLoadParamCmd>(
+       "get_load_param", std::bind(&Chassis::ros_get_load_param_cmd_callback, this, _1, _2));
+    ros_get_sw_version_cmd_server = node->create_service<segway_msgs::srv::RosGetSwVersionCmd>(
+       "get_sw_version", std::bind(&Chassis::ros_get_sw_version_cmd_callback, this, _1, _2));
+    ros_get_vel_max_feedback_cmd_server = node->create_service<segway_msgs::srv::RosGetVelMaxFeedbackCmd>(
+       "get_vel_max_feedback", std::bind(&Chassis::ros_get_vel_max_feedback_cmd_callback, this, _1, _2));
+    ros_set_charge_mos_ctrl_cmd_server = node->create_service<segway_msgs::srv::RosSetChargeMosCtrlCmd>(
+       "set_charge_mos_ctrl", std::bind(&Chassis::ros_set_charge_mos_ctrl_cmd_callback, this, _1, _2));
+    ros_set_chassis_enable_cmd_server = node->create_service<segway_msgs::srv::RosSetChassisEnableCmd>(
+       "set_chassis_enable", std::bind(&Chassis::ros_set_chassis_enable_cmd_callback, this, _1, _2));
+    ros_set_chassis_poweroff_cmd_server = node->create_service<segway_msgs::srv::RosSetChassisPoweroffCmd>(
+       "set_chassis_poweroff", std::bind(&Chassis::ros_set_chassis_poweroff_cmd_callback, this, _1, _2));
+    ros_set_load_param_cmd_server = node->create_service<segway_msgs::srv::RosSetLoadParamCmd>(
+       "set_load_param", std::bind(&Chassis::ros_set_load_param_cmd_callback, this, _1, _2));
+    ros_set_remove_push_cmd_server = node->create_service<segway_msgs::srv::RosSetRemovePushCmd>(
+       "set_remove_push", std::bind(&Chassis::ros_set_remove_push_cmd_callback, this, _1, _2));
+    ros_set_vel_max_cmd_server = node->create_service<segway_msgs::srv::RosSetVelMaxCmd>(
+       "set_vel_max", std::bind(&Chassis::ros_set_vel_max_cmd_callback, this, _1, _2));
+
+    velocity_sub = node->create_subscription<geometry_msgs::msg::Twist>(
+        "cmd_vel", 1, std::bind(&Chassis::cmd_vel_callback, this, std::placeholders::_1));
+
+
+
+    iap_action_server = rclcpp_action::create_server<iapCmd>(
+        node, 
+        "iapCmd", 
+        std::bind(&Chassis::handle_iapCmdGoal, this, _1, _2),
+        std::bind(&Chassis::handle_iapCmdCancel, this, _1),
+        std::bind(&Chassis::handle_iapCmdAccepted, this, _1));
+ 
+    odom_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node);
+    timer_1000hz = node->create_wall_timer(std::chrono::milliseconds(1), std::bind(&Chassis::timer_1000hz_callback, this)); 
+    timer_1hz = node->create_wall_timer(std::chrono::milliseconds(1000), std::bind(&Chassis::timer_1hz_callback, this));
+}
+
+void Chassis::ros_get_charge_mos_ctrl_status_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetChargeMosCtrlStatusCmd::Request> request,
+    std::shared_ptr<segway_msgs::srv::RosGetChargeMosCtrlStatusCmd::Response> response)
+{
+    int16_t ret = 0;
+    if (request->ros_get_chassis_charge_ctrl_status == true){
+        ret = get_charge_mos_ctrl_status();
+        response->chassis_charge_ctrl_status = ret;
+        RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "receive RosGetChargeMosCtrlStatusCmd cmd[%d], charge_ctrl_status[%d]", 
+                1, ret);
+    }
+    else {
+        RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_get_chassis_charge_ctrl_status false");
+    }
+}
+void Chassis::ros_get_load_param_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetLoadParamCmd::Request> request,
+    std::shared_ptr<segway_msgs::srv::RosGetLoadParamCmd::Response> response)
+{
+    if (request->ros_get_load_param == false) {
+        RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_get_load_param fasle");
+        return ;
+    }
+    response->get_load_param = get_chassis_load_state();
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "get_load_param[%d]", response->get_load_param );
+}
+void Chassis::ros_get_sw_version_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetSwVersionCmd::Request> request,
+    std::shared_ptr<segway_msgs::srv::RosGetSwVersionCmd::Response> response)
+{
+    if (request->ros_get_sw_version_cmd == false) {
+        RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_get_sw_version_cmd fasle");
+        return;
+    }
+    response->host_version = get_host_version();
+    response->central_version = get_chassis_central_version();
+    response->motor_version = get_chassis_motor_version();
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_get_sw_version_cmd true");
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "host_version:%d, central_version:%d, motor_version:%d", 
+        response->host_version, response->central_version, response->motor_version);
+}
+void Chassis::ros_get_vel_max_feedback_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetVelMaxFeedbackCmd::Request> request,
+    std::shared_ptr<segway_msgs::srv::RosGetVelMaxFeedbackCmd::Response> response)
+{
+    if (request->ros_get_vel_max_fb_cmd == false) {
+        RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_get_vel_max_fb_cmd fasle");
+        return;
+    }
+    response->forward_max_vel_fb = get_line_forward_max_vel_fb();
+    response->backward_max_vel_fb = get_line_backward_max_vel_fb();
+    response->angular_max_vel_fb = get_angular_max_vel_fb();
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_get_vel_max_fb_cmd true");
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "forward_max_vel_fb:%d, backward_max_vel_fb:%d, angular_max_vel_fb:%d", 
+        response->forward_max_vel_fb, response->backward_max_vel_fb, response->angular_max_vel_fb);
+}
+void Chassis::ros_set_charge_mos_ctrl_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetChargeMosCtrlCmd::Request> request,
+    std::shared_ptr<segway_msgs::srv::RosSetChargeMosCtrlCmd::Response> response)
+{
+    uint8_t ret;
+    if (request->ros_set_chassis_charge_ctrl == false) {
+        ret = set_charge_mos_ctrl(false);
+    }
+    else {
+        ret = set_charge_mos_ctrl(true);
+    }
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_set_chassis_charge_ctrl cmd[%d]", request->ros_set_chassis_charge_ctrl);
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "chassis_set_charge_ctrl_result[%d]", ret);
+    response->chassis_set_charge_ctrl_result = ret;
+}
+void Chassis::ros_set_chassis_enable_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetChassisEnableCmd::Request> request,
+    std::shared_ptr<segway_msgs::srv::RosSetChassisEnableCmd::Response> response)
+{
+    uint8_t ret;
+    if (request->ros_set_chassis_enable_cmd == false) {
+        ret = set_enable_ctrl(0);
+    }
+    else {
+        ret = set_enable_ctrl(1);
+    }
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_set_chassis_enable_cmd cmd[%d]", request->ros_set_chassis_enable_cmd);
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "chassis_set_chassis_enable_result[%d]", ret);
+    response->chassis_set_chassis_enable_result = ret;
+}
+void Chassis::ros_set_chassis_poweroff_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetChassisPoweroffCmd::Request> request,
+    std::shared_ptr<segway_msgs::srv::RosSetChassisPoweroffCmd::Response> response)
+{
+    if (request->ros_set_chassis_poweroff_cmd == false) {
+        RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_set_chassis_poweroff_cmd fasle");
+        return;
+    }
+    uint8_t ret = set_chassis_poweroff();
+    response->chassis_set_poweroff_result = ret;
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "chassis_set_poweroff_result[%d]", ret);
+}
+void Chassis::ros_set_load_param_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetLoadParamCmd::Request> request,
+    std::shared_ptr<segway_msgs::srv::RosSetLoadParamCmd::Response> response)
+{
+    int16_t value = request->ros_set_load_param;
+    if (value != 0 && value != 1){
+        RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_set_load_param[%d] out of normal range ", value);
+        return;
+    }
+    uint8_t ret = set_chassis_load_state(value);
+    response->chassis_set_load_param_result = ret;
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "chassis_set_load_param_result[%d]", ret);
+}
+void Chassis::ros_set_remove_push_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetRemovePushCmd::Request> request,
+    std::shared_ptr<segway_msgs::srv::RosSetRemovePushCmd::Response> response)
+{
+    if (request->ros_set_remove_push_cmd == false) {
+        RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_set_remove_push_cmd fasle");
+        return;
+    }
+    uint8_t ret = set_remove_push_cmd();
+    response->chassis_set_revove_push_result = ret;
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "chassis_set_revove_push_result[%d]", ret);
+}
+void Chassis::ros_set_vel_max_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetVelMaxCmd::Request> request,
+    std::shared_ptr<segway_msgs::srv::RosSetVelMaxCmd::Response> response)
+{
+    double forward = request->ros_set_forward_max_vel;
+    double backward = request->ros_set_backward_max_vel;
+    double angular = request->ros_set_angular_max_vel;
+    uint8_t ret_forw = set_line_forward_max_vel(forward);
+    uint8_t ret_back = set_line_backward_max_vel(backward);
+    uint8_t ret_angl = set_angular_max_vel(angular);
+    response->chassis_set_max_vel_result = ret_forw | ret_back | ret_angl;
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_set_forward_max_vel[%lf], ros_set_backward_max_vel[%lf], ros_set_angular_max_vel[%lf]", 
+        forward, backward, angular);
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "set_line_forward_max_vel result[%d], set_line_backward_max_vel result[%d], set_angular_max_vel result[%d]", 
+        ret_forw, ret_back, ret_angl);
+}
+
+void Chassis::iapCmdExecute(const std::shared_ptr<goalHandaleIapCmd> goal_handle)
+{
+    RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "Executing goal");
+    rclcpp::Rate loop_rate(1);
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<iapCmd::Feedback>();
+    auto result = std::make_shared<iapCmd::Result>();
+    int32_t iap_percent_fb;
+    
+    node->get_parameter("bins_directory", bins_directory);
+    node->get_parameter("chassis_version", chassis_version);
+    node->get_parameter("route_version", route_version);
+    node->get_parameter("connect_version", connect_version);
+    char bin_dir[100] = {0};
+    char ver[100] = {0};
+    switch (goal->iap_board)
+    {
+    case 1:
+        RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "iap central board");
+        bins_directory = bins_directory + "/central.bin";
+        bins_directory.copy(bin_dir, bins_directory.length(), 0);
+        chassis_version.copy(ver, chassis_version.length(), 0);
+        IapSingerBoard(bin_dir, (char*)"central", ver);
+        break;
+    case 2:
+        RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "iap motor board");
+        bins_directory = bins_directory + "/motor.bin";
+        bins_directory.copy(bin_dir, bins_directory.length(), 0);
+        route_version.copy(ver, route_version.length(), 0);
+        IapSingerBoard(bin_dir, (char*)"motor", ver);
+        break;
+    default:
+        RCLCPP_ERROR(rclcpp::get_logger("SmartCar"), 
+        "iap_board value error, out of [1 2]", goal->iap_board);
+        result->set__iap_result(5);
+        goal_handle->canceled(result);
+        return;
+    }
+
+    while (rclcpp::ok())
+    {
+        if (goal_handle->is_canceling()) {
+            result->set__iap_result(5);
+            goal_handle->canceled(result);
+            RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "Goal canceled");
+            return;
+        }
+        iap_percent_fb = getIapTotalProgress();
+        feedback->set__iap_percent(iap_percent_fb);
+        goal_handle->publish_feedback(feedback);
+        RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "iap percent: %d", iap_percent_fb);
+        loop_rate.sleep();
+        if (iap_percent_fb == 100) {
+            RCLCPP_INFO_ONCE(rclcpp::get_logger("SmartCar"), "iap successful");
+            break;
+        } else if (iap_percent_fb < 0) {
+            RCLCPP_INFO_ONCE(rclcpp::get_logger("SmartCar"), "iap failed");
+            break;
+        }
+    }
+    
+    if (rclcpp::ok()) {
+        if (iap_percent_fb == 100) {
+            result->set__iap_result(3);
+            goal_handle->succeed(result);
+            RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "goal succeeded, iap succeeded");
+        } else {
+            result->set__iap_result(getHostIapResult());
+            result->set__error_code(getHostIapErrorCode());
+            goal_handle->succeed(result);
+            RCLCPP_ERROR(rclcpp::get_logger("SmartCar"), "goal failed, iap failed");
+        }
+    }
+}
+
+void Chassis::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) const
+{
+    double angular_vel = msg->angular.z;
+    double linear_vel  = msg->linear.x;
+    angular_vel = (angular_vel > 3.0 ? 3.0 : (angular_vel < -3.0 ? -3.0 : angular_vel));
+    linear_vel = (linear_vel > 3.0 ? 3.0 : (linear_vel < -1.0 ? -1.0 : linear_vel));
+
+    set_cmd_vel(linear_vel, angular_vel);
+    // printf("vl:%lf, va:%lf\n", linear_vel, angular_vel);
+}
+
+void Chassis::timer_1hz_callback(void)
+{
+    bms_fb.bat_soc = get_bat_soc();
+    bms_fb.bat_charging = get_bat_charging();
+    bms_fb.bat_vol = get_bat_mvol();
+    bms_fb.bat_current = get_bat_mcurrent();
+    bms_fb.bat_temp = get_bat_temp();
+    bms_fb_pub->publish(bms_fb);
+
+    chassis_ctrl_src_fb.chassis_ctrl_cmd_src = get_ctrl_cmd_src();
+    chassis_ctrl_src_fb_pub->publish(chassis_ctrl_src_fb);
+
+    chassis_mileage_meter_fb.vehicle_meters = get_vehicle_meter();
+    chassis_mileage_meter_fb_pub->publish(chassis_mileage_meter_fb);
+
+    chassis_mode_fb.chassis_mode = get_chassis_mode();//0-lock_mode; 1-ctrl_mode; 2-push_mode; 3-emergency_mode; 4-error_mode
+    chassis_mode_fb_pub->publish(chassis_mode_fb);
+
+    error_code_fb.host_error = get_err_state(Host);
+    error_code_fb.central_error = get_err_state(Central);
+    error_code_fb.left_motor_error = get_err_state(Motor) & 0xffff;
+    error_code_fb.right_motor_error = (get_err_state(Motor) >> 16) & 0xffff;
+    error_code_fb.bms_error = get_err_state(BMS);
+    error_code_fb_pub->publish(error_code_fb);
+
+    motor_work_mode_fb.motor_work_mode = get_chassis_work_model();
+    motor_work_mode_fb_pub->publish(motor_work_mode_fb);
+}
+
+void Chassis::timer_1000hz_callback(void)
+{
+    speed_pub_callback();
+    ticks_pub_callback();
+    imu_pub_callback();
+    pub_odom_callback();
+}
+
+void Chassis::speed_pub_callback(void)
+{
+    if (Speed_update == 1) {
+        Speed_update = 0;
+        speed_fb.car_speed = SpeedData.car_speed;
+        speed_fb.car_speed /= LINE_SPEED_TRANS_GAIN_MPS;
+        speed_fb.turn_speed = SpeedData.turn_speed;
+        speed_fb.turn_speed /= ANGULAR_SPEED_TRANS_GAIN_RADPS;
+        speed_fb.l_speed = SpeedData.l_speed;
+        speed_fb.l_speed /= LINE_SPEED_TRANS_GAIN_MPS;
+        speed_fb.r_speed = SpeedData.r_speed;
+        speed_fb.r_speed /= LINE_SPEED_TRANS_GAIN_MPS;
+        speed_fb.speed_timestamp = Speed_TimeStamp;
+        speed_fb_pub->publish(speed_fb);
+    }
+}
+
+void Chassis::ticks_pub_callback(void)
+{
+    if (Tick_update == 1) {
+        Tick_update = 0;
+        ticks_fb.l_ticks = TickData.l_ticks;
+        ticks_fb.r_ticks = TickData.r_ticks;
+        ticks_fb.ticks_timestamp = Tick_TimeStamp;
+        ticks_fb_pub->publish(ticks_fb);
+    }
+}
+
+void Chassis::imu_pub_callback(void)
+{
+    if (ImuGyro_update == 1 && ImuAcc_update == 1){
+        ImuGyro_update = 0;
+        ImuAcc_update = 0;
+        imu_fb.header.stamp = node->now();
+        imu_fb.header.frame_id = "base_link";
+        imu_fb.angular_velocity.x = (double)ImuGyroData.gyr[0] / 900.0;
+        imu_fb.angular_velocity.y = (double)ImuGyroData.gyr[1] / 900.0;
+        imu_fb.angular_velocity.z = (double)ImuGyroData.gyr[2] / 900.0;
+        imu_fb.linear_acceleration.x = (double)ImuAccData.acc[0] / 4000.0 * 9.8;
+        imu_fb.linear_acceleration.y = (double)ImuAccData.acc[1] / 4000.0 * 9.8;
+        imu_fb.linear_acceleration.z = (double)ImuAccData.acc[2] / 4000.0 * 9.8;
+        imu_pub->publish(imu_fb);
+    }
+}
+
+void Chassis::pub_odom_callback(void)
+{    
+    if ((OdomPoseXy_update & OdomEulerXy_update & OdomEulerZ_update & OdomVelLineXy_update) == 1)  {
+        OdomPoseXy_update = 0, OdomEulerXy_update = 0, OdomEulerZ_update = 0, OdomVelLineXy_update = 0;
+
+        odom_quat.setRPY(0, 0, OdomEulerZ.euler_z / RAD_DEGREE_CONVER);
+        odom_trans.header.stamp = node->now();
+        odom_trans.header.frame_id = "odom";
+        odom_trans.child_frame_id = "base_link";
+        odom_trans.transform.translation.x = OdomPoseXy.pos_x;
+        odom_trans.transform.translation.y = OdomPoseXy.pos_y;
+        odom_trans.transform.translation.z = 0.0;
+        odom_trans.transform.rotation.x = odom_quat.x();
+        odom_trans.transform.rotation.y = odom_quat.y();
+        odom_trans.transform.rotation.z = odom_quat.z();
+        odom_trans.transform.rotation.w = odom_quat.w();
+        odom_broadcaster->sendTransform(odom_trans);
+
+        odom_fb.header.stamp = node->now();
+        odom_fb.header.frame_id = "odom";
+        odom_fb.pose.pose.position.x = OdomPoseXy.pos_x;
+        odom_fb.pose.pose.position.y = OdomPoseXy.pos_y;
+        odom_fb.pose.pose.position.z = 0.0;
+        odom_fb.pose.pose.orientation.x = odom_quat.x();
+        odom_fb.pose.pose.orientation.y = odom_quat.y();
+        odom_fb.pose.pose.orientation.z = odom_quat.z();
+        odom_fb.pose.pose.orientation.w = odom_quat.w();
+
+        odom_fb.child_frame_id = "base_link";
+        odom_fb.twist.twist.linear.x = (double)SpeedData.car_speed / LINE_SPEED_TRANS_GAIN_MPS;
+        odom_fb.twist.twist.linear.y = 0;
+        odom_fb.twist.twist.angular.z = (double)SpeedData.turn_speed / ANGULAR_SPEED_TRANS_GAIN_RADPS;;
+        odom_pub->publish(odom_fb);
+    }
+}
+
+}

src/segwayrmp/tools/drive_segway_sample.cpp

@@ -0,0 +1,364 @@
+#include "segwayrmp/robot.h"
+#include <termio.h>
+#include <stdio.h>
+#include <iostream>
+#include <termios.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/time.h>
+
+#define PRINTHELP		  'h'
+#define ADDLINEVEL		  'w'
+#define DECLINEVEL		  's'
+#define ADDANGULARVEL	  'a'
+#define DECANGULARVEL	  'd'
+#define PRINTCURVEL		  'f'
+#define VELRESETZERO	  'g'
+#define ENABLECMD         'e'
+#define CHASSISPAUSE      'q'
+// #define CLEARSCRAM        'c'
+// #define GETERROR          'x'
+#define IAPCENTRAL        'v'
+#define IAPMOTOR          'b'
+#define PRINTERRSW        'r'
+
+using iapCmd = segway_msgs::action::RosSetIapCmd;
+using goalHandleIapCmd = rclcpp_action::ClientGoalHandle<iapCmd>;
+
+std::shared_ptr<rclcpp::Node> drive_node;
+rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_pub;
+rclcpp::Subscription<segway_msgs::msg::ErrorCodeFb>::SharedPtr error_sub;
+
+rclcpp::Client<segway_msgs::srv::RosSetChassisEnableCmd>::SharedPtr enable_client;
+// rclcpp::Client<segway_msgs::srv::ClearChassisScramStatusCmd>::SharedPtr clear_scram_client;
+// rclcpp::Client<segway_msgs::srv::GetGxErrorCmd>::SharedPtr get_err_client;
+rclcpp::Service<segway_msgs::srv::ChassisSendEvent>::SharedPtr event_server;
+
+rclcpp_action::Client<iapCmd>::SharedPtr iap_client;
+uint8_t print_error = 0;
+
+char const* print_help() {
+    char const* printHelp = 
+        "\t h : Displays the required keys and their meaning\n"
+        "\t w : Increase forward speed by 0.1m/s\n"
+        "\t s : Decrease forward speed by 0.1m/s\n"
+        "\t a : Increase the angular velocity by 0.1rad/s\n"
+        "\t d : Decrease the angular velocity by 0.1rad/s\n"
+        "\t f : Displays current speed Settings\n"
+        "\t g : Speed reset to zero\n"
+        "\t e : Chassis enable switch\n"
+        "\t q : Running pause. Click 'q'key again to resume running by the previous speed. W/S/A/D keys can also restore chassis running\n"
+        "\t v : Iap the central board, please put the bin file in /sdcard/firmware/\n"
+        "\t b : Iap the motor board, please put the bin file in /sdcard/firmware/\n"
+        "\t r : Open or close the switch: printing error code\n"
+        "\t others : Do nothing\n";
+    return printHelp;
+}
+
+void changemode(int dir)
+{
+  static struct termios oldt, newt;
+ 
+  if ( dir == 1 )
+  {
+    tcgetattr( STDIN_FILENO, &oldt);
+    newt = oldt;
+    newt.c_lflag &= ~( ICANON | ECHO );
+    tcsetattr( STDIN_FILENO, TCSANOW, &newt);
+  }
+  else
+    tcsetattr( STDIN_FILENO, TCSANOW, &oldt);
+}
+
+int kbhit (void)
+{
+  struct timeval tv;
+  fd_set rdfs;  //一组fd集合
+ 
+  tv.tv_sec = 0;
+  tv.tv_usec = 0;   //时间为0，非阻塞
+ 
+  FD_ZERO(&rdfs);   //fd集合清空
+  FD_SET (STDIN_FILENO, &rdfs); //增加新的fd
+ 
+  select(STDIN_FILENO+1, &rdfs, NULL, NULL, &tv);   //非阻塞检测fd为0的tty读取状态，rdfs特定位置1
+  return FD_ISSET(STDIN_FILENO, &rdfs);     //判断指定的标准输入fd是否在rdfs集合中
+}
+
+static int scanKeyboard()
+{
+    int input_char = 0;
+    struct termios new_settings;
+    struct termios stored_settings;
+    tcgetattr(0, &stored_settings);
+    new_settings = stored_settings;
+    new_settings.c_lflag &= (~ICANON);  //非规范模式：每次返回单个字符
+    new_settings.c_cc[VTIME] = 0;
+    tcgetattr(0,&stored_settings);
+    new_settings.c_cc[VMIN] = 1;    //读取的最小字节数
+    tcsetattr(0, TCSANOW, &new_settings);
+
+    changemode(1);
+    if (kbhit()) {
+        input_char = getchar();
+        printf("\n");
+    }
+    changemode(0);
+
+    tcsetattr(0, TCSANOW, &stored_settings);
+    return input_char;
+}
+
+char get_keyboard()
+{
+    char keyvalue = scanKeyboard();
+    return keyvalue;
+}
+
+void goal_response_callback(std::shared_future<goalHandleIapCmd::SharedPtr> future)
+{
+  auto goal_handle = future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(rclcpp::get_logger("drive_segway_sample"), "Goal was rejected by server");
+  } else {
+    RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "Goal accepted by server, waiting for result");
+  }
+}
+
+void feedback_callback(
+  goalHandleIapCmd::SharedPtr,
+  const std::shared_ptr<const iapCmd::Feedback> feedback)
+{
+  RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "IAP process percentage：[%d]", feedback->iap_percent);
+}
+
+void result_callback(const goalHandleIapCmd::WrappedResult & result)
+{
+  switch (result.code) {
+    case rclcpp_action::ResultCode::SUCCEEDED:
+      break;
+    case rclcpp_action::ResultCode::ABORTED:
+      RCLCPP_ERROR(rclcpp::get_logger("drive_segway_sample"), "Goal was aborted");
+      return;
+    case rclcpp_action::ResultCode::CANCELED:
+      RCLCPP_ERROR(rclcpp::get_logger("drive_segway_sample"), "Goal was canceled");
+      return;
+    default:
+      RCLCPP_ERROR(rclcpp::get_logger("drive_segway_sample"), "Unknown result code");
+      return;
+  }
+  if (result.result->iap_result == 3) {
+    RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "iap success!");
+  } else {
+    RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "iap fail!, error_code[%#x]", result.result->error_code);
+  }
+}
+
+void drive_chassis_test()
+{
+    static uint16_t set_enable_cmd = 1;
+    uint8_t enable_switch = 0;
+    static double set_line_speed; 
+    static double set_angular_speed;
+    static double send_line_speed;
+    static double send_angular_speed;
+    static uint8_t chassis_pause = 0;
+    // uint8_t clear_scram_flag = 0;
+    // uint8_t get_err_flag = 0;
+    uint8_t iap_flag = 0;
+
+    auto enable_request = std::make_shared<segway_msgs::srv::RosSetChassisEnableCmd::Request>();
+    // auto clear_scram_requst = std::make_shared<segway_msgs::srv::ClearChassisScramStatusCmd::Request>();
+    // auto get_err_request = std::make_shared<segway_msgs::srv::GetGxErrorCmd::Request>();
+
+    char keyvalue = get_keyboard();
+    switch (keyvalue)
+    {
+    case PRINTHELP:
+        printf("%s\n", print_help());
+        break;
+    case ADDLINEVEL		:
+        set_line_speed += 0.1;
+        send_line_speed = set_line_speed;
+        send_angular_speed = set_angular_speed;
+        printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
+        break;
+    case DECLINEVEL		:
+        set_line_speed -= 0.1;
+        send_line_speed = set_line_speed;
+        send_angular_speed = set_angular_speed;
+        printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
+        break;
+    case ADDANGULARVEL	:
+        set_angular_speed += 0.1;
+        send_line_speed = set_line_speed;
+        send_angular_speed = set_angular_speed;
+        printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
+        break;
+    case DECANGULARVEL	:
+        set_angular_speed -= 0.1;
+        send_line_speed = set_line_speed;
+        send_angular_speed = set_angular_speed;
+        printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
+        break;
+    case PRINTCURVEL	:
+        printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
+        break;
+    case VELRESETZERO	:
+        set_line_speed = 0; 
+        set_angular_speed = 0;
+        send_line_speed = 0;
+        send_angular_speed = 0;
+        printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
+        break;
+    case ENABLECMD      : 
+        enable_switch = 1;
+        enable_request->ros_set_chassis_enable_cmd = set_enable_cmd;
+        ++set_enable_cmd;        
+        set_enable_cmd %= 2;
+        RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), 
+                    "enable chassis switch[%d]", enable_request->ros_set_chassis_enable_cmd);
+        printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
+        break;  
+    case CHASSISPAUSE   :
+        ++chassis_pause;
+        chassis_pause %= 2;
+        if (chassis_pause) {
+            send_line_speed = 0;
+            send_angular_speed = 0;
+            printf("Stop the chassis temporarily\n");
+        } else {
+            send_line_speed = set_line_speed;
+            send_angular_speed = set_angular_speed;
+            printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
+        }
+        break;   
+    // case CLEARSCRAM :
+    //     // clear_scram_flag = 1;
+    //     break;
+    // case GETERROR:
+    //     // get_err_flag = 1;
+    //     break;
+    case IAPCENTRAL:
+        RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "iap chassis");
+        iap_flag = 1;
+        break;
+    case IAPMOTOR:
+        RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "iap route");
+        iap_flag = 2;
+        break;
+    case PRINTERRSW:
+        print_error = ~print_error;
+        break;
+    default:
+        break;
+    }
+
+    geometry_msgs::msg::Twist cmd_vel;
+    cmd_vel.linear.x = send_line_speed;
+    cmd_vel.angular.z = send_angular_speed;
+    velocity_pub->publish(cmd_vel);
+
+    if (enable_switch){
+        using enableServiceResponseFutrue = rclcpp::Client<segway_msgs::srv::RosSetChassisEnableCmd>::SharedFuture;
+        auto enable_response_receive_callback = [](enableServiceResponseFutrue futrue) {
+            RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), 
+            "event sended successfully, result:%d", futrue.get()->chassis_set_chassis_enable_result);
+        };
+        auto enable_future_result = enable_client->async_send_request(enable_request, enable_response_receive_callback);
+    }
+
+    // if (clear_scram_flag) {
+    //     using clearServiceResponseFuture = rclcpp::Client<segway_msgs::srv::ClearChassisScramStatusCmd>::SharedFuture;
+    //     auto clear_response_receive_callback = [](clearServiceResponseFuture future) {
+    //         RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), 
+    //         "clear scram status successfully, result:%d", future.get()->clear_scram_result);
+    //     };
+    //     auto clear_future_result = clear_scram_client->async_send_request(clear_scram_requst, clear_response_receive_callback);
+    // }
+
+    // if (get_err_flag) {
+    //     using errorServiceResponseFuture = rclcpp::Client<segway_msgs::srv::GetGxErrorCmd>::SharedFuture;
+    //     auto error_response_receive_callback = [](errorServiceResponseFuture future) {
+    //        RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), 
+    //             "chassis_error_code[%d], route_error_code[%d], connect_error_code[%d]", 
+    //         future.get()->chassis_error_code, 
+    //         future.get()->route_error_code,
+    //         future.get()->connect_error_code);
+    //     };
+    //     auto error_future_result = get_err_client->async_send_request(get_err_request, error_response_receive_callback);
+    // }
+
+    if (iap_flag & 3) {
+        auto goal_msg = iapCmd::Goal();
+        goal_msg.iap_board = iap_flag;
+        RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "sending goal, iap cmd goal");
+        auto send_goal_options = rclcpp_action::Client<iapCmd>::SendGoalOptions();
+        send_goal_options.goal_response_callback = std::bind(&goal_response_callback, std::placeholders::_1);
+        send_goal_options.feedback_callback = std::bind(&feedback_callback, std::placeholders::_1, std::placeholders::_2);
+        send_goal_options.result_callback = std::bind(&result_callback, std::placeholders::_1);
+        iap_client->async_send_goal(goal_msg, send_goal_options);
+    }
+}
+
+void get_error_code_callback(const segway_msgs::msg::ErrorCodeFb::SharedPtr msg)
+{
+    if (print_error != 0) {
+        RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "host_error[%#x], central_error[%#x], \
+        left_motor_error[%#x], right_motor_error[%#x], bms_err[%#x]", 
+        msg->host_error, msg->central_error, msg->left_motor_error, msg->right_motor_error, msg->bms_error);
+    }
+}
+
+void get_chassis_event_callback(const std::shared_ptr<segway_msgs::srv::ChassisSendEvent::Request> request,
+    std::shared_ptr<segway_msgs::srv::ChassisSendEvent::Response> response)
+{
+    (void)response;
+    switch (request->chassis_send_event_id)
+    {
+    case OnEmergeStopEvent:
+        RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "CHASSIS EVENT: The chassis emergency stop button is triggered");
+        break;
+    case OutEmergeStopEvent:
+        RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "CHASSIS EVENT: OThe chassis emergency stop button recover");
+        break;
+    case PadPowerOffEvent:
+        RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "CHASSIS EVENT: The chassis will power off");
+        break;
+    case OnLockedRotorProtectEvent:
+        RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "CHASSIS EVENT: the chassis motor locked-rotor");
+        break;
+    case OutLockedRotorProtectEvent:
+        RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "CHASSIS EVENT: the chassis motor no longer locked-rotor");
+        break;    
+    default:
+        break;
+    }    
+}
+
+int main(int argc, char * argv[])
+{
+    rclcpp::init(argc, argv);
+    drive_node = rclcpp::Node::make_shared("drive_segway_sample");
+
+    velocity_pub = drive_node->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+    error_sub = drive_node->create_subscription<segway_msgs::msg::ErrorCodeFb>(
+        "error_code_fb", 1, std::bind(&get_error_code_callback, std::placeholders::_1));
+
+    enable_client = drive_node->create_client<segway_msgs::srv::RosSetChassisEnableCmd>("set_chassis_enable");
+    // clear_scram_client = drive_node->create_client<segway_msgs::srv::ClearChassisScramStatusCmd>("clear_scram_status_srv");
+    // get_err_client = drive_node->create_client<segway_msgs::srv::GetGxErrorCmd>("get_error_srv");
+    event_server = drive_node->create_service<segway_msgs::srv::ChassisSendEvent>(
+        "event_srv", std::bind(&get_chassis_event_callback, std::placeholders::_1, std::placeholders::_2));
+
+    iap_client = rclcpp_action::create_client<iapCmd>(drive_node, "iapCmd");
+    
+    rclcpp::TimerBase::SharedPtr timer_100hz = 
+        drive_node->create_wall_timer(std::chrono::milliseconds(10), &drive_chassis_test);
+
+    printf("%s\n", print_help());
+    
+    rclcpp::spin(drive_node);
+    rclcpp::shutdown();
+    return 0;
+}