diff --git a/src/segway_msgs/CMakeLists.txt b/src/segway_msgs/CMakeLists.txt new file mode 100755 index 0000000..7dc8e36 --- /dev/null +++ b/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() diff --git a/src/segway_msgs/action/RosSetIapCmd.action b/src/segway_msgs/action/RosSetIapCmd.action new file mode 100644 index 0000000..5e7da2b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/segway_msgs/msg/BmsFb.msg b/src/segway_msgs/msg/BmsFb.msg new file mode 100644 index 0000000..aa841bf --- /dev/null +++ b/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 diff --git a/src/segway_msgs/msg/ChassisCtrlSrcFb.msg b/src/segway_msgs/msg/ChassisCtrlSrcFb.msg new file mode 100644 index 0000000..d159dd7 --- /dev/null +++ b/src/segway_msgs/msg/ChassisCtrlSrcFb.msg @@ -0,0 +1 @@ +uint16 chassis_ctrl_cmd_src \ No newline at end of file diff --git a/src/segway_msgs/msg/ChassisMileageMeterFb.msg b/src/segway_msgs/msg/ChassisMileageMeterFb.msg new file mode 100644 index 0000000..71d1bec --- /dev/null +++ b/src/segway_msgs/msg/ChassisMileageMeterFb.msg @@ -0,0 +1 @@ +uint32 vehicle_meters \ No newline at end of file diff --git a/src/segway_msgs/msg/ChassisModeFb.msg b/src/segway_msgs/msg/ChassisModeFb.msg new file mode 100644 index 0000000..3745d2a --- /dev/null +++ b/src/segway_msgs/msg/ChassisModeFb.msg @@ -0,0 +1 @@ +uint16 chassis_mode \ No newline at end of file diff --git a/src/segway_msgs/msg/ErrorCodeFb.msg b/src/segway_msgs/msg/ErrorCodeFb.msg new file mode 100644 index 0000000..e2fa46b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/segway_msgs/msg/MotorWorkModeFb.msg b/src/segway_msgs/msg/MotorWorkModeFb.msg new file mode 100644 index 0000000..1ee2876 --- /dev/null +++ b/src/segway_msgs/msg/MotorWorkModeFb.msg @@ -0,0 +1 @@ +uint16 motor_work_mode #0: no output torque 1: output torque \ No newline at end of file diff --git a/src/segway_msgs/msg/SpeedFb.msg b/src/segway_msgs/msg/SpeedFb.msg new file mode 100644 index 0000000..e2c22b4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/segway_msgs/msg/TicksFb.msg b/src/segway_msgs/msg/TicksFb.msg new file mode 100644 index 0000000..49553f4 --- /dev/null +++ b/src/segway_msgs/msg/TicksFb.msg @@ -0,0 +1,3 @@ +int32 l_ticks +int32 r_ticks +uint64 ticks_timestamp \ No newline at end of file diff --git a/src/segway_msgs/package.xml b/src/segway_msgs/package.xml new file mode 100755 index 0000000..0de6505 --- /dev/null +++ b/src/segway_msgs/package.xml @@ -0,0 +1,26 @@ + + + + segway_msgs + 0.0.0 + TODO: Package description + ninebot + TODO: License declaration + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + std_msgs + geometry_msgs + action_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/segway_msgs/srv/ChassisSendEvent.srv b/src/segway_msgs/srv/ChassisSendEvent.srv new file mode 100644 index 0000000..1dd5276 --- /dev/null +++ b/src/segway_msgs/srv/ChassisSendEvent.srv @@ -0,0 +1,3 @@ +int16 chassis_send_event_id +--- +bool ros_is_received \ No newline at end of file diff --git a/src/segway_msgs/srv/RosGetChargeMosCtrlStatusCmd.srv b/src/segway_msgs/srv/RosGetChargeMosCtrlStatusCmd.srv new file mode 100644 index 0000000..a9eac33 --- /dev/null +++ b/src/segway_msgs/srv/RosGetChargeMosCtrlStatusCmd.srv @@ -0,0 +1,3 @@ +bool ros_get_chassis_charge_ctrl_status +--- +int16 chassis_charge_ctrl_status \ No newline at end of file diff --git a/src/segway_msgs/srv/RosGetLoadParamCmd.srv b/src/segway_msgs/srv/RosGetLoadParamCmd.srv new file mode 100644 index 0000000..5c5f212 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/segway_msgs/srv/RosGetSwVersionCmd.srv b/src/segway_msgs/srv/RosGetSwVersionCmd.srv new file mode 100644 index 0000000..8ea2df5 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/segway_msgs/srv/RosGetVelMaxFeedbackCmd.srv b/src/segway_msgs/srv/RosGetVelMaxFeedbackCmd.srv new file mode 100644 index 0000000..f1bf0d2 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/segway_msgs/srv/RosSetChargeMosCtrlCmd.srv b/src/segway_msgs/srv/RosSetChargeMosCtrlCmd.srv new file mode 100644 index 0000000..9c69946 --- /dev/null +++ b/src/segway_msgs/srv/RosSetChargeMosCtrlCmd.srv @@ -0,0 +1,3 @@ +bool ros_set_chassis_charge_ctrl +--- +uint8 chassis_set_charge_ctrl_result \ No newline at end of file diff --git a/src/segway_msgs/srv/RosSetChassisEnableCmd.srv b/src/segway_msgs/srv/RosSetChassisEnableCmd.srv new file mode 100644 index 0000000..b7a82c5 --- /dev/null +++ b/src/segway_msgs/srv/RosSetChassisEnableCmd.srv @@ -0,0 +1,3 @@ +bool ros_set_chassis_enable_cmd +--- +uint8 chassis_set_chassis_enable_result \ No newline at end of file diff --git a/src/segway_msgs/srv/RosSetChassisPoweroffCmd.srv b/src/segway_msgs/srv/RosSetChassisPoweroffCmd.srv new file mode 100644 index 0000000..df2e8dd --- /dev/null +++ b/src/segway_msgs/srv/RosSetChassisPoweroffCmd.srv @@ -0,0 +1,3 @@ +bool ros_set_chassis_poweroff_cmd +--- +uint8 chassis_set_poweroff_result \ No newline at end of file diff --git a/src/segway_msgs/srv/RosSetLoadParamCmd.srv b/src/segway_msgs/srv/RosSetLoadParamCmd.srv new file mode 100644 index 0000000..5d61f2e --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/segway_msgs/srv/RosSetRemovePushCmd.srv b/src/segway_msgs/srv/RosSetRemovePushCmd.srv new file mode 100644 index 0000000..a13175f --- /dev/null +++ b/src/segway_msgs/srv/RosSetRemovePushCmd.srv @@ -0,0 +1,3 @@ +bool ros_set_remove_push_cmd +--- +uint8 chassis_set_revove_push_result \ No newline at end of file diff --git a/src/segway_msgs/srv/RosSetVelMaxCmd.srv b/src/segway_msgs/srv/RosSetVelMaxCmd.srv new file mode 100644 index 0000000..8a26de4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/segwayrmp/CMakeLists.txt b/src/segwayrmp/CMakeLists.txt new file mode 100755 index 0000000..92797ab --- /dev/null +++ b/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 + $ + $ +) +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 + $ + $ +) +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() diff --git a/src/segwayrmp/include/comm_ctrl.h b/src/segwayrmp/include/comm_ctrl.h new file mode 100644 index 0000000..71abc33 --- /dev/null +++ b/src/segwayrmp/include/comm_ctrl.h @@ -0,0 +1,109 @@ +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef COMM_CTRL_H +#define COMM_CTRL_H + +#include +#include +#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:上层不需要调�?so每次保存完log后会自动调用 +uint8_t getSaveRecordCmd(void); //获取是否开始保存log　　　ps:so自动调用,上层不需要调�? +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或�?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 + diff --git a/src/segwayrmp/include/comm_ctrl_navigation.h b/src/segwayrmp/include/comm_ctrl_navigation.h new file mode 100644 index 0000000..9a3d365 --- /dev/null +++ b/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 +#include + +#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 加速度计数据上�? + +//-----------------------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, //底盘只连中控�? + 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 diff --git a/src/segwayrmp/include/segwayrmp/robot.h b/src/segwayrmp/include/segwayrmp/robot.h new file mode 100755 index 0000000..e7fef60 --- /dev/null +++ b/src/segwayrmp/include/segwayrmp/robot.h @@ -0,0 +1,169 @@ +#ifndef _ROBOT_H +#define _ROBOT_H + +#include +#include +#include +#include +#include "stdint.h" +#include +#include + +#include +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include "tf2_ros/transform_broadcaster.h" +#include +#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; + Chassis(rclcpp::Node::SharedPtr nh); + static void pub_event_callback(int event_no); + private: + std::shared_ptr node; + + std::string bins_directory; + std::string chassis_version; + std::string route_version; + std::string connect_version; + + tf2::Quaternion odom_quat; + std::shared_ptr 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::SharedPtr velocity_sub; + + rclcpp::Publisher::SharedPtr bms_fb_pub; + rclcpp::Publisher::SharedPtr chassis_ctrl_src_fb_pub; + rclcpp::Publisher::SharedPtr chassis_mileage_meter_fb_pub; + rclcpp::Publisher::SharedPtr chassis_mode_fb_pub; + rclcpp::Publisher::SharedPtr error_code_fb_pub; + rclcpp::Publisher::SharedPtr motor_work_mode_fb_pub; + rclcpp::Publisher::SharedPtr speed_fb_pub; + rclcpp::Publisher::SharedPtr ticks_fb_pub; + rclcpp::Publisher::SharedPtr odom_pub; + rclcpp::Publisher::SharedPtr imu_pub; + + + rclcpp::Service::SharedPtr ros_get_charge_mos_ctrl_status_cmd_server; + rclcpp::Service::SharedPtr ros_get_load_param_cmd_server; + rclcpp::Service::SharedPtr ros_get_sw_version_cmd_server; + rclcpp::Service::SharedPtr ros_get_vel_max_feedback_cmd_server; + rclcpp::Service::SharedPtr ros_set_charge_mos_ctrl_cmd_server; + rclcpp::Service::SharedPtr ros_set_chassis_enable_cmd_server; + rclcpp::Service::SharedPtr ros_set_chassis_poweroff_cmd_server; + rclcpp::Service::SharedPtr ros_set_load_param_cmd_server; + rclcpp::Service::SharedPtr ros_set_remove_push_cmd_server; + rclcpp::Service::SharedPtr ros_set_vel_max_cmd_server; + + void ros_get_charge_mos_ctrl_status_cmd_callback(const std::shared_ptr request, + std::shared_ptr response); + void ros_get_load_param_cmd_callback(const std::shared_ptr request, + std::shared_ptr response); + void ros_get_sw_version_cmd_callback(const std::shared_ptr request, + std::shared_ptr response); + void ros_get_vel_max_feedback_cmd_callback(const std::shared_ptr request, + std::shared_ptr response); + void ros_set_charge_mos_ctrl_cmd_callback(const std::shared_ptr request, + std::shared_ptr response); + void ros_set_chassis_enable_cmd_callback(const std::shared_ptr request, + std::shared_ptr response); + void ros_set_chassis_poweroff_cmd_callback(const std::shared_ptr request, + std::shared_ptr response); + void ros_set_load_param_cmd_callback(const std::shared_ptr request, + std::shared_ptr response); + void ros_set_remove_push_cmd_callback(const std::shared_ptr request, + std::shared_ptr response); + void ros_set_vel_max_cmd_callback(const std::shared_ptr request, + std::shared_ptr response); + + rclcpp_action::Server::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 goal_handle); + rclcpp_action::GoalResponse handle_iapCmdGoal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr 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 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 goal_handle) + { + using namespace std::placeholders; + (void)goal_handle; + std::thread{std::bind(&Chassis::iapCmdExecute, this, _1), goal_handle}.detach(); + } + }; + +} + +#endif diff --git a/src/segwayrmp/lib/ctrl_arm64-v8a b/src/segwayrmp/lib/ctrl_arm64-v8a new file mode 100755 index 0000000..34f71bc Binary files /dev/null and b/src/segwayrmp/lib/ctrl_arm64-v8a differ diff --git a/src/segwayrmp/lib/ctrl_x86_64 b/src/segwayrmp/lib/ctrl_x86_64 new file mode 100755 index 0000000..bcc4f16 Binary files /dev/null and b/src/segwayrmp/lib/ctrl_x86_64 differ diff --git a/src/segwayrmp/lib/libctrl_arm64-v8a.so b/src/segwayrmp/lib/libctrl_arm64-v8a.so new file mode 100755 index 0000000..dd6df59 Binary files /dev/null and b/src/segwayrmp/lib/libctrl_arm64-v8a.so differ diff --git a/src/segwayrmp/lib/libctrl_x86_64.so b/src/segwayrmp/lib/libctrl_x86_64.so new file mode 100755 index 0000000..5d6c3a3 Binary files /dev/null and b/src/segwayrmp/lib/libctrl_x86_64.so differ diff --git a/src/segwayrmp/package.xml b/src/segwayrmp/package.xml new file mode 100755 index 0000000..b4c9143 --- /dev/null +++ b/src/segwayrmp/package.xml @@ -0,0 +1,31 @@ + + + + segwayrmp + 0.0.0 + TODO: Package description + ninebot + TODO: License declaration + + ament_cmake + + rclcpp + rclpy + std_msgs + nav_msgs + geometry_msgs + + segway_msgs + tf2 + tf2_ros + rclcpp_action + rclcpp_components + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/segwayrmp/src/SmartCar.cpp b/src/segwayrmp/src/SmartCar.cpp new file mode 100755 index 0000000..f54da0f --- /dev/null +++ b/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("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; +} \ No newline at end of file diff --git a/src/segwayrmp/src/robot.cpp b/src/segwayrmp/src/robot.cpp new file mode 100755 index 0000000..8122a96 --- /dev/null +++ b/src/segwayrmp/src/robot.cpp @@ -0,0 +1,489 @@ +#include "segwayrmp/robot.h" +#include +#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 car_node; +rclcpp::Client::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::SharedFuture; + auto event_request = std::make_shared(); + 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(×tamp_data); + event_data.event_callback = EventPubData; + aprctrl_eventcallback_jni_register(&event_data); + + node->declare_parameter("bins_directory", "/sdcard/firmware/"); + node->declare_parameter("chassis_version", "1.01"); + node->declare_parameter("route_version", "1.01"); + node->declare_parameter("connect_version", "1.01"); + + bms_fb_pub = node->create_publisher("/bms_fb", 1); + chassis_ctrl_src_fb_pub = node->create_publisher("/chassis_ctrl_src_fb", 1); + chassis_mileage_meter_fb_pub = node->create_publisher("/chassis_mileage_meter_fb", 1); + chassis_mode_fb_pub = node->create_publisher("/chassis_mode_fb", 1); + error_code_fb_pub = node->create_publisher("/error_code_fb", 1); + motor_work_mode_fb_pub = node->create_publisher("/motor_work_mode_fb", 1); + speed_fb_pub = node->create_publisher("/speed_fb", 1); + ticks_fb_pub = node->create_publisher("/ticks_fb", 1); + odom_pub = node->create_publisher("/odom", 1); + imu_pub = node->create_publisher("/imu", 1); + + event_client = node->create_client("event_srv"); + + ros_get_charge_mos_ctrl_status_cmd_server = node->create_service( + "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( + "get_load_param", std::bind(&Chassis::ros_get_load_param_cmd_callback, this, _1, _2)); + ros_get_sw_version_cmd_server = node->create_service( + "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( + "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( + "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( + "set_chassis_enable", std::bind(&Chassis::ros_set_chassis_enable_cmd_callback, this, _1, _2)); + ros_set_chassis_poweroff_cmd_server = node->create_service( + "set_chassis_poweroff", std::bind(&Chassis::ros_set_chassis_poweroff_cmd_callback, this, _1, _2)); + ros_set_load_param_cmd_server = node->create_service( + "set_load_param", std::bind(&Chassis::ros_set_load_param_cmd_callback, this, _1, _2)); + ros_set_remove_push_cmd_server = node->create_service( + "set_remove_push", std::bind(&Chassis::ros_set_remove_push_cmd_callback, this, _1, _2)); + ros_set_vel_max_cmd_server = node->create_service( + "set_vel_max", std::bind(&Chassis::ros_set_vel_max_cmd_callback, this, _1, _2)); + + velocity_sub = node->create_subscription( + "cmd_vel", 1, std::bind(&Chassis::cmd_vel_callback, this, std::placeholders::_1)); + + + + iap_action_server = rclcpp_action::create_server( + 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(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 request, + std::shared_ptr 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 request, + std::shared_ptr 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 request, + std::shared_ptr 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 request, + std::shared_ptr 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 request, + std::shared_ptr 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 request, + std::shared_ptr 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 request, + std::shared_ptr 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 request, + std::shared_ptr 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 request, + std::shared_ptr 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 request, + std::shared_ptr 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 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(); + auto result = std::make_shared(); + 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); + } +} + +} diff --git a/src/segwayrmp/tools/drive_segway_sample.cpp b/src/segwayrmp/tools/drive_segway_sample.cpp new file mode 100755 index 0000000..a9568e0 --- /dev/null +++ b/src/segwayrmp/tools/drive_segway_sample.cpp @@ -0,0 +1,364 @@ +#include "segwayrmp/robot.h" +#include +#include +#include +#include +#include +#include +#include + +#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; + +std::shared_ptr drive_node; +rclcpp::Publisher::SharedPtr velocity_pub; +rclcpp::Subscription::SharedPtr error_sub; + +rclcpp::Client::SharedPtr enable_client; +// rclcpp::Client::SharedPtr clear_scram_client; +// rclcpp::Client::SharedPtr get_err_client; +rclcpp::Service::SharedPtr event_server; + +rclcpp_action::Client::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 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 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(); + // auto clear_scram_requst = std::make_shared(); + // auto get_err_request = std::make_shared(); + + 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::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::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::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::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 request, + std::shared_ptr 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("cmd_vel", 1); + error_sub = drive_node->create_subscription( + "error_code_fb", 1, std::bind(&get_error_code_callback, std::placeholders::_1)); + + enable_client = drive_node->create_client("set_chassis_enable"); + // clear_scram_client = drive_node->create_client("clear_scram_status_srv"); + // get_err_client = drive_node->create_client("get_error_srv"); + event_server = drive_node->create_service( + "event_srv", std::bind(&get_chassis_event_callback, std::placeholders::_1, std::placeholders::_2)); + + iap_client = rclcpp_action::create_client(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; +} \ No newline at end of file