diff --git a/src/segway_msgs/CMakeLists.txt b/CMakeLists.txt similarity index 100% rename from src/segway_msgs/CMakeLists.txt rename to CMakeLists.txt diff --git a/README.md b/README.md index ec83088..4ed50b0 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,5 @@ -# ROS2_ws_for_RMP220 -ROS2.0 support package for Segway RMP chassis, through which you can drive the RMP220 chassis and get the chassis status information. +# segway_msgs + +ROS2 messages for Segway RMP chassis, through which you can drive the RMP220 chassis and get the chassis status information. + +Derived from [this fork](https://github.com/LuckierDodge/ROS2_ws_for_RMP220) of the original segway package. diff --git a/src/segway_msgs/action/RosSetIapCmd.action b/action/RosSetIapCmd.action similarity index 100% rename from src/segway_msgs/action/RosSetIapCmd.action rename to action/RosSetIapCmd.action diff --git a/src/segway_msgs/msg/BmsFb.msg b/msg/BmsFb.msg similarity index 100% rename from src/segway_msgs/msg/BmsFb.msg rename to msg/BmsFb.msg diff --git a/src/segway_msgs/msg/ChassisCtrlSrcFb.msg b/msg/ChassisCtrlSrcFb.msg similarity index 100% rename from src/segway_msgs/msg/ChassisCtrlSrcFb.msg rename to msg/ChassisCtrlSrcFb.msg diff --git a/src/segway_msgs/msg/ChassisMileageMeterFb.msg b/msg/ChassisMileageMeterFb.msg similarity index 100% rename from src/segway_msgs/msg/ChassisMileageMeterFb.msg rename to msg/ChassisMileageMeterFb.msg diff --git a/src/segway_msgs/msg/ChassisModeFb.msg b/msg/ChassisModeFb.msg similarity index 100% rename from src/segway_msgs/msg/ChassisModeFb.msg rename to msg/ChassisModeFb.msg diff --git a/src/segway_msgs/msg/ErrorCodeFb.msg b/msg/ErrorCodeFb.msg similarity index 100% rename from src/segway_msgs/msg/ErrorCodeFb.msg rename to msg/ErrorCodeFb.msg diff --git a/src/segway_msgs/msg/MotorWorkModeFb.msg b/msg/MotorWorkModeFb.msg similarity index 100% rename from src/segway_msgs/msg/MotorWorkModeFb.msg rename to msg/MotorWorkModeFb.msg diff --git a/src/segway_msgs/msg/SpeedFb.msg b/msg/SpeedFb.msg similarity index 100% rename from src/segway_msgs/msg/SpeedFb.msg rename to msg/SpeedFb.msg diff --git a/src/segway_msgs/msg/TicksFb.msg b/msg/TicksFb.msg similarity index 100% rename from src/segway_msgs/msg/TicksFb.msg rename to msg/TicksFb.msg diff --git a/src/segway_msgs/package.xml b/package.xml similarity index 100% rename from src/segway_msgs/package.xml rename to package.xml diff --git a/src/segwayrmp/CMakeLists.txt b/src/segwayrmp/CMakeLists.txt deleted file mode 100755 index 2922090..0000000 --- a/src/segwayrmp/CMakeLists.txt +++ /dev/null @@ -1,81 +0,0 @@ -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_arm64-v8a.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 deleted file mode 100644 index 71abc33..0000000 --- a/src/segwayrmp/include/comm_ctrl.h +++ /dev/null @@ -1,109 +0,0 @@ -#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 deleted file mode 100644 index 9a3d365..0000000 --- a/src/segwayrmp/include/comm_ctrl_navigation.h +++ /dev/null @@ -1,180 +0,0 @@ -#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 deleted file mode 100755 index e7fef60..0000000 --- a/src/segwayrmp/include/segwayrmp/robot.h +++ /dev/null @@ -1,169 +0,0 @@ -#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 deleted file mode 100755 index 34f71bc..0000000 Binary files a/src/segwayrmp/lib/ctrl_arm64-v8a and /dev/null differ diff --git a/src/segwayrmp/lib/ctrl_x86_64 b/src/segwayrmp/lib/ctrl_x86_64 deleted file mode 100755 index bcc4f16..0000000 Binary files a/src/segwayrmp/lib/ctrl_x86_64 and /dev/null differ diff --git a/src/segwayrmp/lib/libctrl_arm64-v8a.so b/src/segwayrmp/lib/libctrl_arm64-v8a.so deleted file mode 100755 index dd6df59..0000000 Binary files a/src/segwayrmp/lib/libctrl_arm64-v8a.so and /dev/null differ diff --git a/src/segwayrmp/lib/libctrl_x86_64.so b/src/segwayrmp/lib/libctrl_x86_64.so deleted file mode 100755 index 5d6c3a3..0000000 Binary files a/src/segwayrmp/lib/libctrl_x86_64.so and /dev/null differ diff --git a/src/segwayrmp/package.xml b/src/segwayrmp/package.xml deleted file mode 100755 index b4c9143..0000000 --- a/src/segwayrmp/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - 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 deleted file mode 100755 index f54da0f..0000000 --- a/src/segwayrmp/src/SmartCar.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#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 deleted file mode 100755 index 8122a96..0000000 --- a/src/segwayrmp/src/robot.cpp +++ /dev/null @@ -1,489 +0,0 @@ -#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 deleted file mode 100755 index a9568e0..0000000 --- a/src/segwayrmp/tools/drive_segway_sample.cpp +++ /dev/null @@ -1,364 +0,0 @@ -#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 diff --git a/src/segway_msgs/srv/ChassisSendEvent.srv b/srv/ChassisSendEvent.srv similarity index 100% rename from src/segway_msgs/srv/ChassisSendEvent.srv rename to srv/ChassisSendEvent.srv diff --git a/src/segway_msgs/srv/RosGetChargeMosCtrlStatusCmd.srv b/srv/RosGetChargeMosCtrlStatusCmd.srv similarity index 100% rename from src/segway_msgs/srv/RosGetChargeMosCtrlStatusCmd.srv rename to srv/RosGetChargeMosCtrlStatusCmd.srv diff --git a/src/segway_msgs/srv/RosGetLoadParamCmd.srv b/srv/RosGetLoadParamCmd.srv similarity index 100% rename from src/segway_msgs/srv/RosGetLoadParamCmd.srv rename to srv/RosGetLoadParamCmd.srv diff --git a/src/segway_msgs/srv/RosGetSwVersionCmd.srv b/srv/RosGetSwVersionCmd.srv similarity index 100% rename from src/segway_msgs/srv/RosGetSwVersionCmd.srv rename to srv/RosGetSwVersionCmd.srv diff --git a/src/segway_msgs/srv/RosGetVelMaxFeedbackCmd.srv b/srv/RosGetVelMaxFeedbackCmd.srv similarity index 100% rename from src/segway_msgs/srv/RosGetVelMaxFeedbackCmd.srv rename to srv/RosGetVelMaxFeedbackCmd.srv diff --git a/src/segway_msgs/srv/RosSetChargeMosCtrlCmd.srv b/srv/RosSetChargeMosCtrlCmd.srv similarity index 100% rename from src/segway_msgs/srv/RosSetChargeMosCtrlCmd.srv rename to srv/RosSetChargeMosCtrlCmd.srv diff --git a/src/segway_msgs/srv/RosSetChassisEnableCmd.srv b/srv/RosSetChassisEnableCmd.srv similarity index 100% rename from src/segway_msgs/srv/RosSetChassisEnableCmd.srv rename to srv/RosSetChassisEnableCmd.srv diff --git a/src/segway_msgs/srv/RosSetChassisPoweroffCmd.srv b/srv/RosSetChassisPoweroffCmd.srv similarity index 100% rename from src/segway_msgs/srv/RosSetChassisPoweroffCmd.srv rename to srv/RosSetChassisPoweroffCmd.srv diff --git a/src/segway_msgs/srv/RosSetLoadParamCmd.srv b/srv/RosSetLoadParamCmd.srv similarity index 100% rename from src/segway_msgs/srv/RosSetLoadParamCmd.srv rename to srv/RosSetLoadParamCmd.srv diff --git a/src/segway_msgs/srv/RosSetRemovePushCmd.srv b/srv/RosSetRemovePushCmd.srv similarity index 100% rename from src/segway_msgs/srv/RosSetRemovePushCmd.srv rename to srv/RosSetRemovePushCmd.srv diff --git a/src/segway_msgs/srv/RosSetVelMaxCmd.srv b/srv/RosSetVelMaxCmd.srv similarity index 100% rename from src/segway_msgs/srv/RosSetVelMaxCmd.srv rename to srv/RosSetVelMaxCmd.srv