Broke out segwayrmp and segway_msgs
This commit is contained in:
Ryan Lewis
parent
37aac69eee
commit
c26af222a5
@ -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.
|
||||
|
||||
@ -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
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
target_link_directories(SmartCar PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/lib
|
||||
)
|
||||
# message(WARNING ${PROJECT_SOURCE_DIR} + "!!" + ${CMAKE_CURRENT_SOURCE_DIR} + "!!" + ${PROJECT_NAME})
|
||||
ament_target_dependencies(SmartCar
|
||||
"rclcpp" "std_msgs" "nav_msgs" "sensor_msgs" "geometry_msgs" "segway_msgs" "tf2" "tf2_ros"
|
||||
)
|
||||
target_link_libraries(SmartCar
|
||||
${PROJECT_SOURCE_DIR}/lib/libctrl_arm64-v8a.so
|
||||
)
|
||||
|
||||
add_executable(drive_segway_sample
|
||||
tools/drive_segway_sample.cpp
|
||||
)
|
||||
target_include_directories(drive_segway_sample PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
ament_target_dependencies(drive_segway_sample
|
||||
"rclcpp" "std_msgs" "nav_msgs" "sensor_msgs" "geometry_msgs" "segway_msgs" "tf2" "tf2_ros"
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
SmartCar
|
||||
drive_segway_sample
|
||||
DESTINATION
|
||||
lib/${PROJECT_NAME})
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# uncomment the line when a copyright and license is not present in all source files
|
||||
#set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# uncomment the line when this package is not in a git repo
|
||||
#set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@ -1,109 +0,0 @@
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#ifndef COMM_CTRL_H
|
||||
#define COMM_CTRL_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include "comm_ctrl_navigation.h"
|
||||
|
||||
|
||||
// Map table transmission value gain coefficient;
|
||||
#define ERROR_LINE_LIMIT_VEL_VALUE (5 * LINE_SPEED_TRANS_GAIN_MPS)
|
||||
#define ERROR_ANGULAR_LIMIT_VEL_VALUE (5 * ANGULAR_SPEED_TRANS_GAIN_RADPS)
|
||||
#define ACC_VALUE_GAIN 100
|
||||
|
||||
|
||||
#define RMP_HOST_ERR_BIT_MCU_HEART_BEAT 0 //The heartbeat with the chassis was interrupted
|
||||
#define RMP_HOST_ERR_BIT_SERIAL_PORT_LOST 1 //Serial port module unplugged
|
||||
|
||||
void AprGxJniEventRegister(int32_t (*callback)(int32_t));
|
||||
|
||||
//-------------------Timestamp------------------------
|
||||
|
||||
//--------------------GX--API------------------
|
||||
int init_control(void);
|
||||
void exit_control(void);
|
||||
|
||||
int16_t get_forward_speed_limit(void);
|
||||
int16_t get_backward_speed_limit(void);
|
||||
int16_t get_angular_speed_limit(void);
|
||||
uint16_t get_Stop_Bottom_Status(void);
|
||||
uint8_t set_calib_chassis_imu_gyro(void);//Reserve for later versions
|
||||
uint16_t get_Chassis_Imu_Calib_Result(void); //Reserve for later versions
|
||||
void clear_Chassis_Imu_Calib_Result(void);//Reserve for later versions
|
||||
uint8_t set_calib_chassis_current(void);
|
||||
uint16_t get_chassis_current_calib_result(void);
|
||||
void clear_chassis_current_calib_result(void);
|
||||
|
||||
|
||||
//-----------------GX IAP--------------------
|
||||
void IapAllBoard(const char **file_paths, char **versions);
|
||||
int32_t IapSingerBoard(char * path,char * boradname,char* version);
|
||||
//int32_t getIapTotalProgress(void);
|
||||
//------------------GX Host-------------------
|
||||
|
||||
char* GetGxHardVersion(void);
|
||||
void TracePrint_Switch(bool status); //true or false
|
||||
void SetNavigationStatus(uint8_t status);
|
||||
void setSaveRecordCmd(void); //开始保存log接口 ps:上层调用
|
||||
void clearRecordCmd(void); //清除保存log命令 ps:上层不需要调<E8A681>?so每次保存完log后会自动调用
|
||||
uint8_t getSaveRecordCmd(void); //获取是否开始保存log ps:so自动调用,上层不需要调<E8A681>?
|
||||
uint8_t GetNavigationStatus(void);
|
||||
|
||||
|
||||
void set_gx_route_bat_voltage(uint16_t voltage);
|
||||
|
||||
|
||||
int16_t get_cmd_speed_forward(void);
|
||||
int16_t get_fbk_speed_forward(void);
|
||||
int16_t get_encode_fbk_speed_forward(void);
|
||||
int16_t get_cmd_w_forward(void);
|
||||
int16_t get_fbk_w_forward(void);
|
||||
int16_t get_rec_cmd_vx(void);
|
||||
int16_t get_rec_cmd_w(void);
|
||||
int16_t get_pid_target_vx(void);
|
||||
int16_t get_pid_target_w(void);
|
||||
int16_t get_encode_speed_L(void);
|
||||
int16_t get_encode_speed_R(void);
|
||||
|
||||
int16_t get_heart_beat_data(board_name_e board_type);
|
||||
|
||||
void set_host_error(uint8_t offset, uint8_t err_bool_value);
|
||||
uint32_t get_host_error(void);
|
||||
uint32_t get_host_speed_cmd_cnt(void);
|
||||
uint32_t get_chassis_reply_speed_cmd_cnt(void);
|
||||
int32_t get_encoder_l_ticks(void);
|
||||
int32_t get_encoder_r_ticks(void);
|
||||
char * get_smart_car_serial(void);
|
||||
uint8_t get_comu_interface_serial0_can1();
|
||||
void showHostBuildTime(void);
|
||||
void showChassisCentralBuildTime(void);
|
||||
|
||||
/* 暂时不开放此接口，由于map表只能发送整数，故此接口待修改：乘以10或<30>?00下发，电机板收到命令数据后再恢复数据 */
|
||||
uint8_t set_cmd_acc(double linear_x_acc, double linear_x_brake_acc, double angular_z_acc);//Set the linear and angular accelerated speed: m/s2 ; rad/s2.
|
||||
|
||||
uint8_t set_chassis_no_load(void);
|
||||
uint8_t set_chassis_full_load(void);
|
||||
|
||||
void ackWriteCentralMapInit();
|
||||
uint8_t ctrlMapAckWrite( uint8_t dest_id, uint8_t map_start_index, uint32_t mem_len);
|
||||
|
||||
void set_chassis_charge_status(int16_t chargestatus);
|
||||
uint32_t get_chassis_central_Err_Status(void);
|
||||
uint16_t get_chassis_l_motor_Err_Status(void);
|
||||
uint16_t get_chassis_r_motor_Err_Status(void);
|
||||
uint32_t get_chassis_bms_Err_Status(void);
|
||||
char * show_host_version(void);
|
||||
|
||||
|
||||
int16_t get_charge_mos_ctrl_cmd(void);// Get chassis charge_ctrl mos cmd; 1:turn on; 0: turn off
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -1,180 +0,0 @@
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#ifndef COMM_CTRL_NAVIGATION_H
|
||||
#define COMM_CTRL_NAVIGATION_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#define TYPE_ID_NUM 10 //Number of callback functions
|
||||
//--------------------------DATA CALL BACK INDEX--------------------------
|
||||
#define Chassis_Data_Speed 1
|
||||
#define Chassis_Data_Ticks 2
|
||||
#define Chassis_Data_Odom_Pose_xy 3
|
||||
#define Chassis_Data_Odom_Euler_xy 4
|
||||
#define Chassis_Data_Odom_Euler_z 5
|
||||
#define Chassis_Data_Odom_Linevel_xy 6
|
||||
#define Chassis_Data_Imu_Gyr 7 //IMU Gyroscope data 陀螺仪数据上报
|
||||
#define Chassis_Data_Imu_Acc 8 //IMU Accelerometer data 加速度计数据上<E68DAE>?
|
||||
|
||||
//-----------------------Event---------------------------------------------
|
||||
#define ChassisBootReadyEvent 1 // Chassis central control boot OK
|
||||
#define PadPowerOffEvent 2 // The chassis will power off
|
||||
#define OnEmergeStopEvent 3 // The chassis emergency stop button is triggered
|
||||
#define OutEmergeStopEvent 4 // The chassis emergency stop button recover
|
||||
#define OnLockedRotorProtectEvent 5 // the chassis motor locked-rotor
|
||||
#define OutLockedRotorProtectEvent 6 // the chassis motor no longer locked-rotor
|
||||
#define OnLostCtrlProtectEvent 7 // the chassis motor lost control
|
||||
#define OutLostCtrlProtectEvent 8 // the chassis motor no longer lost control
|
||||
#define CalibrateGyroSuccess 9 // Chassis gyroscope calibration successful
|
||||
#define CalibrateGyroFail 10 // Chassis gyroscope calibration failed
|
||||
#define CalibratePasheCurrentSuccess 11 // Chassis gyroscope calibration successful
|
||||
#define CalibratePasheCurrentFail 12 // Chassis gyroscope calibration failed
|
||||
|
||||
|
||||
//---------The proportional coefficient of the callback gyro data------------------
|
||||
#define CHASSIS_IMU_GYR_VALUE_TRANS_SCALE 900 //900~~32768(MAX value for int16: map table ) * 57.3(degrees for 1 rad) /2000(Gyroscope range: 2000dps)
|
||||
#define CHASSIS_IMU_ACC_VALUE_TRANS_SCALE 4000 //4000~~32768(MAX value for int16: map table )/8(Accelerometer range: 8g)
|
||||
|
||||
#define LINE_SPEED_TRANS_GAIN_MPS 3600 //3600-->1m/s
|
||||
#define ANGULAR_SPEED_TRANS_GAIN_RADPS 1000 //1000-->1rad/s
|
||||
|
||||
#define NO_LOAD 0 // Set the chassis parameters as no-load parameters, The chassis defaults to no load
|
||||
#define FULL_LOAD 1 // Set the chassis parameters as full load parameters
|
||||
|
||||
//-----------------------Version V0.6 and above---------------------------
|
||||
#define LOCK_MODE 0 // Lock the car
|
||||
#define CTRL_MODE 1 // Control the car
|
||||
#define PUSH_MODE 2 // push the car
|
||||
#define EMERG_MODE 3 // emergency
|
||||
#define ERROR_MODE 4 // Internal error
|
||||
|
||||
|
||||
//-------------------Timestamp------------------------
|
||||
#define MAX_BASIC_FRM_SZ 0x1F
|
||||
|
||||
#pragma pack(1)
|
||||
typedef struct StampedBasicFrame_{
|
||||
uint32_t type_id; //Data type number
|
||||
uint64_t timestamp; //Linux timestamp
|
||||
char data[MAX_BASIC_FRM_SZ]; //Chassis specific data
|
||||
} StampedBasicFrame;
|
||||
#pragma pack()
|
||||
typedef void (*h_aprctrl_datastamped_t)(StampedBasicFrame* frm);
|
||||
typedef void (*h_aprctrl_event_t)(int32_t event_num);
|
||||
|
||||
typedef struct {
|
||||
h_aprctrl_datastamped_t on_new_data;
|
||||
}s_aprctrl_datastamped_t;
|
||||
|
||||
typedef struct {
|
||||
h_aprctrl_event_t event_callback;
|
||||
}s_aprctrl_event_t;
|
||||
|
||||
typedef enum {
|
||||
Host = 1,
|
||||
Central = 2, //底盘只连中控<E4B8AD>?
|
||||
Motor = 3,
|
||||
BMS = 4
|
||||
}board_name_e;
|
||||
|
||||
typedef enum {
|
||||
comu_serial = 0,
|
||||
comu_can = 1
|
||||
}comu_choice_e;
|
||||
|
||||
typedef struct{
|
||||
int16_t l_speed;//Left wheel speed
|
||||
int16_t r_speed;//Right wheel speed
|
||||
int16_t car_speed;//Vehicle linear speed
|
||||
int16_t turn_speed;//Turning speed
|
||||
}chassis_speed_data_t;
|
||||
|
||||
typedef struct{
|
||||
int32_t l_ticks;//Left wheel ticks
|
||||
int32_t r_ticks;//Right wheel ticks
|
||||
}motor_ticks_t;
|
||||
|
||||
typedef struct{
|
||||
float pos_x;
|
||||
float pos_y;
|
||||
}odom_pos_xy_t;
|
||||
|
||||
typedef struct{
|
||||
float euler_x;
|
||||
float euler_y;
|
||||
}odom_euler_xy_t;
|
||||
|
||||
typedef struct{
|
||||
float euler_z;
|
||||
}odom_euler_z_t;
|
||||
|
||||
typedef struct{
|
||||
float vel_line_x;
|
||||
float vel_line_y;
|
||||
}odom_vel_line_xy_t;
|
||||
|
||||
typedef struct{
|
||||
int16_t gyr[3];
|
||||
}imu_gyr_original_data_t;
|
||||
|
||||
typedef struct{
|
||||
int16_t acc[3];
|
||||
}imu_acc_original_data_t;
|
||||
|
||||
|
||||
void aprctrl_datastamped_jni_register(s_aprctrl_datastamped_t* f); //The callback registration function
|
||||
void aprctrl_eventcallback_jni_register(s_aprctrl_event_t* f); //Event callback registration function
|
||||
|
||||
uint32_t get_err_state(board_name_e board_name);//Gets the software error status
|
||||
int16_t get_bat_soc(void);//Gets the percentage of battery left
|
||||
int16_t get_bat_charging(void);//Gets the charging status of the battery
|
||||
int32_t get_bat_mvol(void);//Gets battery voltage, mV
|
||||
int32_t get_bat_mcurrent(void);//Gets battery current, mA
|
||||
int16_t get_bat_temp(void);// Gets battery temperature
|
||||
int16_t get_chassis_work_model(void);//Get the chassis working mode
|
||||
uint8_t get_chassis_load_state(void);//Gets whether the chassis parameters are empty or full load, 0: no_load, 1: full_load
|
||||
uint16_t get_chassis_mode(void);//Gets chassis mode
|
||||
int16_t get_ctrl_cmd_src(void); // Get the source of the control command
|
||||
int32_t get_vehicle_meter(void); //Get the total mileage meters of the chssis
|
||||
uint16_t get_host_version(void);
|
||||
uint16_t get_chassis_central_version(void);
|
||||
uint16_t get_chassis_motor_version(void);
|
||||
int16_t get_line_forward_max_vel_fb(void);
|
||||
int16_t get_line_backward_max_vel_fb(void);
|
||||
int16_t get_angular_max_vel_fb(void);
|
||||
int32_t getIapTotalProgress(void);
|
||||
void iapCentralBoard(void);//Operate on the Central board: IAP
|
||||
void iapMotorBoard(void);//Operate on the Motor board: IAP
|
||||
bool isHostIapOver(void);//Query whether a single IAP ends
|
||||
int16_t getHostIapResult(void);
|
||||
int16_t getHostIapErrorCode(void);
|
||||
int16_t get_chassis_hang_mode(void);//Get chassis hang_mode; 1: in Hang_mode; 0: not in Hand_mode.
|
||||
int16_t get_charge_mos_ctrl_status(void);//Get the status of switch for charging MOS on the central board, charging:1; no charge:0
|
||||
|
||||
|
||||
|
||||
void set_cmd_vel(double linear_x,double angular_z);//Set the linear and angular speeds of the vehicle: m/s ; rad/s.
|
||||
uint8_t set_line_forward_max_vel(double linear_forward_max_x);//Set the maximum linear velocity in the direction of advance
|
||||
uint8_t set_line_backward_max_vel(double linear_backward_max_x);//Set the maximum linear velocity in the backward direction
|
||||
uint8_t set_angular_max_vel(double angular_max_z);//Set the maximum angular velocity
|
||||
uint8_t set_enable_ctrl(uint16_t enable_flag);//Set chassis movement enable
|
||||
int init_control_ctrl(void);//Chassis initialization
|
||||
void exit_control_ctrl(void);//Chassis software finished running
|
||||
void set_smart_car_serial(const char * serial_no);//Set the serial port name
|
||||
void set_comu_interface(comu_choice_e comu_choice); //Set communication interface: 'comu_serial': serial port; 'comu_can': CAN port
|
||||
uint8_t set_chassis_load_state(int16_t newLoadSet);//Sets whether the chassis parameters are empty or full load, 0: no_load, 1: full_load
|
||||
uint8_t set_chassis_poweroff(void);//Set two - wheel differential chassis shutdown
|
||||
uint8_t set_remove_push_cmd(void);//An order to remove the push status
|
||||
void setHostIapCanceled(void);
|
||||
void set_chassis_hang_mode(int16_t enterHand);//Set chassis hang_mode; 1: enter the Hang_mode; 0: exit the Hand_mode.
|
||||
uint8_t set_charge_mos_ctrl(bool on);//Set the switch for charging MOS on the central board; charging:1, stop charging:0.
|
||||
|
||||
extern int32_t IapSingerBoard(char * path,char * boradname,char* version);
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
@ -1,169 +0,0 @@
|
||||
#ifndef _ROBOT_H
|
||||
#define _ROBOT_H
|
||||
|
||||
#include <functional>
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include "stdint.h"
|
||||
#include <time.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
#include <rosidl_typesupport_cpp/message_type_support.hpp>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
#include <tf2_ros/static_transform_broadcaster.h>
|
||||
#include "tf2/LinearMath/Quaternion.h"
|
||||
#include "comm_ctrl_navigation.h"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "segway_msgs/msg/bms_fb.hpp"
|
||||
#include "segway_msgs/msg/chassis_ctrl_src_fb.hpp"
|
||||
#include "segway_msgs/msg/chassis_mileage_meter_fb.hpp"
|
||||
#include "segway_msgs/msg/chassis_mode_fb.hpp"
|
||||
#include "segway_msgs/msg/error_code_fb.hpp"
|
||||
#include "segway_msgs/msg/motor_work_mode_fb.hpp"
|
||||
#include "segway_msgs/msg/speed_fb.hpp"
|
||||
#include "segway_msgs/msg/ticks_fb.hpp"
|
||||
#include "segway_msgs/srv/chassis_send_event.hpp"
|
||||
#include "segway_msgs/srv/ros_get_charge_mos_ctrl_status_cmd.hpp"
|
||||
#include "segway_msgs/srv/ros_get_load_param_cmd.hpp"
|
||||
#include "segway_msgs/srv/ros_get_sw_version_cmd.hpp"
|
||||
#include "segway_msgs/srv/ros_get_vel_max_feedback_cmd.hpp"
|
||||
#include "segway_msgs/srv/ros_set_charge_mos_ctrl_cmd.hpp"
|
||||
#include "segway_msgs/srv/ros_set_chassis_enable_cmd.hpp"
|
||||
#include "segway_msgs/srv/ros_set_chassis_poweroff_cmd.hpp"
|
||||
#include "segway_msgs/srv/ros_set_load_param_cmd.hpp"
|
||||
#include "segway_msgs/srv/ros_set_remove_push_cmd.hpp"
|
||||
#include "segway_msgs/srv/ros_set_vel_max_cmd.hpp"
|
||||
#include "segway_msgs/action/ros_set_iap_cmd.hpp"
|
||||
|
||||
#define pi 3.141592654
|
||||
#define pi_2 6.283185308
|
||||
|
||||
namespace robot
|
||||
{
|
||||
class Chassis
|
||||
{
|
||||
public:
|
||||
using iapCmd = segway_msgs::action::RosSetIapCmd;
|
||||
using goalHandaleIapCmd = rclcpp_action::ServerGoalHandle<iapCmd>;
|
||||
Chassis(rclcpp::Node::SharedPtr nh);
|
||||
static void pub_event_callback(int event_no);
|
||||
private:
|
||||
std::shared_ptr<rclcpp::Node> node;
|
||||
|
||||
std::string bins_directory;
|
||||
std::string chassis_version;
|
||||
std::string route_version;
|
||||
std::string connect_version;
|
||||
|
||||
tf2::Quaternion odom_quat;
|
||||
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> odom_broadcaster;
|
||||
|
||||
segway_msgs::msg::BmsFb bms_fb;
|
||||
segway_msgs::msg::ChassisCtrlSrcFb chassis_ctrl_src_fb;
|
||||
segway_msgs::msg::ChassisMileageMeterFb chassis_mileage_meter_fb;
|
||||
segway_msgs::msg::ChassisModeFb chassis_mode_fb;
|
||||
segway_msgs::msg::ErrorCodeFb error_code_fb;
|
||||
segway_msgs::msg::MotorWorkModeFb motor_work_mode_fb;
|
||||
segway_msgs::msg::SpeedFb speed_fb;
|
||||
segway_msgs::msg::TicksFb ticks_fb;
|
||||
geometry_msgs::msg::TransformStamped odom_trans;
|
||||
nav_msgs::msg::Odometry odom_fb;
|
||||
sensor_msgs::msg::Imu imu_fb;
|
||||
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr velocity_sub;
|
||||
|
||||
rclcpp::Publisher<segway_msgs::msg::BmsFb>::SharedPtr bms_fb_pub;
|
||||
rclcpp::Publisher<segway_msgs::msg::ChassisCtrlSrcFb>::SharedPtr chassis_ctrl_src_fb_pub;
|
||||
rclcpp::Publisher<segway_msgs::msg::ChassisMileageMeterFb>::SharedPtr chassis_mileage_meter_fb_pub;
|
||||
rclcpp::Publisher<segway_msgs::msg::ChassisModeFb>::SharedPtr chassis_mode_fb_pub;
|
||||
rclcpp::Publisher<segway_msgs::msg::ErrorCodeFb>::SharedPtr error_code_fb_pub;
|
||||
rclcpp::Publisher<segway_msgs::msg::MotorWorkModeFb>::SharedPtr motor_work_mode_fb_pub;
|
||||
rclcpp::Publisher<segway_msgs::msg::SpeedFb>::SharedPtr speed_fb_pub;
|
||||
rclcpp::Publisher<segway_msgs::msg::TicksFb>::SharedPtr ticks_fb_pub;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub;
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub;
|
||||
|
||||
|
||||
rclcpp::Service<segway_msgs::srv::RosGetChargeMosCtrlStatusCmd>::SharedPtr ros_get_charge_mos_ctrl_status_cmd_server;
|
||||
rclcpp::Service<segway_msgs::srv::RosGetLoadParamCmd>::SharedPtr ros_get_load_param_cmd_server;
|
||||
rclcpp::Service<segway_msgs::srv::RosGetSwVersionCmd>::SharedPtr ros_get_sw_version_cmd_server;
|
||||
rclcpp::Service<segway_msgs::srv::RosGetVelMaxFeedbackCmd>::SharedPtr ros_get_vel_max_feedback_cmd_server;
|
||||
rclcpp::Service<segway_msgs::srv::RosSetChargeMosCtrlCmd>::SharedPtr ros_set_charge_mos_ctrl_cmd_server;
|
||||
rclcpp::Service<segway_msgs::srv::RosSetChassisEnableCmd>::SharedPtr ros_set_chassis_enable_cmd_server;
|
||||
rclcpp::Service<segway_msgs::srv::RosSetChassisPoweroffCmd>::SharedPtr ros_set_chassis_poweroff_cmd_server;
|
||||
rclcpp::Service<segway_msgs::srv::RosSetLoadParamCmd>::SharedPtr ros_set_load_param_cmd_server;
|
||||
rclcpp::Service<segway_msgs::srv::RosSetRemovePushCmd>::SharedPtr ros_set_remove_push_cmd_server;
|
||||
rclcpp::Service<segway_msgs::srv::RosSetVelMaxCmd>::SharedPtr ros_set_vel_max_cmd_server;
|
||||
|
||||
void ros_get_charge_mos_ctrl_status_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetChargeMosCtrlStatusCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosGetChargeMosCtrlStatusCmd::Response> response);
|
||||
void ros_get_load_param_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetLoadParamCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosGetLoadParamCmd::Response> response);
|
||||
void ros_get_sw_version_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetSwVersionCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosGetSwVersionCmd::Response> response);
|
||||
void ros_get_vel_max_feedback_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetVelMaxFeedbackCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosGetVelMaxFeedbackCmd::Response> response);
|
||||
void ros_set_charge_mos_ctrl_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetChargeMosCtrlCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosSetChargeMosCtrlCmd::Response> response);
|
||||
void ros_set_chassis_enable_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetChassisEnableCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosSetChassisEnableCmd::Response> response);
|
||||
void ros_set_chassis_poweroff_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetChassisPoweroffCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosSetChassisPoweroffCmd::Response> response);
|
||||
void ros_set_load_param_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetLoadParamCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosSetLoadParamCmd::Response> response);
|
||||
void ros_set_remove_push_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetRemovePushCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosSetRemovePushCmd::Response> response);
|
||||
void ros_set_vel_max_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetVelMaxCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosSetVelMaxCmd::Response> response);
|
||||
|
||||
rclcpp_action::Server<iapCmd>::SharedPtr iap_action_server;
|
||||
|
||||
rclcpp::TimerBase::SharedPtr timer_1hz;
|
||||
rclcpp::TimerBase::SharedPtr timer_1000hz;
|
||||
|
||||
s_aprctrl_datastamped_t timestamp_data;
|
||||
s_aprctrl_event_t event_data;
|
||||
|
||||
void timer_1000hz_callback(void);
|
||||
void timer_1hz_callback(void);
|
||||
void speed_pub_callback(void);
|
||||
void ticks_pub_callback(void);
|
||||
void imu_pub_callback(void);
|
||||
void pub_odom_callback(void);
|
||||
void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) const;
|
||||
|
||||
void iapCmdExecute(const std::shared_ptr<goalHandaleIapCmd> goal_handle);
|
||||
rclcpp_action::GoalResponse handle_iapCmdGoal(
|
||||
const rclcpp_action::GoalUUID & uuid,
|
||||
std::shared_ptr<const iapCmd::Goal> goal)
|
||||
{
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "Receive goal request with iap_board %d", goal->iap_board);
|
||||
(void)uuid;
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
|
||||
rclcpp_action::CancelResponse handle_iapCmdCancel(
|
||||
const std::shared_ptr<goalHandaleIapCmd> goal_handle)
|
||||
{
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "Received request to cancel goal");
|
||||
(void)goal_handle;
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void handle_iapCmdAccepted(const std::shared_ptr<goalHandaleIapCmd> goal_handle)
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
(void)goal_handle;
|
||||
std::thread{std::bind(&Chassis::iapCmdExecute, this, _1), goal_handle}.detach();
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1,31 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>segwayrmp</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="chunxin.chu@ninebot.com">ninebot</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<!-- <depend>sensor_msgs</depned> -->
|
||||
<depend>segway_msgs</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -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<std::string>("serial_full_name", "/dev/ttyUSB0");
|
||||
std::string serial_full_name;
|
||||
node->get_parameter("serial_full_name", serial_full_name);
|
||||
// init_control()parameter: Fill in the full path name of the actual serial port being used
|
||||
|
||||
set_smart_car_serial((char*)serial_full_name.c_str());//If a serial port is used, set the serial port name.
|
||||
set_comu_interface(comu_serial);//Before calling init_control_ctrl, need to call this function set whether the communication port is serial or CAN.
|
||||
|
||||
if (init_control_ctrl() == -1) {
|
||||
printf("init_control failed!\n");
|
||||
exit_control_ctrl();
|
||||
} else {
|
||||
printf("init_control success!\n");
|
||||
}
|
||||
robot::Chassis sbv(node);
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@ -1,489 +0,0 @@
|
||||
#include "segwayrmp/robot.h"
|
||||
#include <cwchar>
|
||||
#define VEL_DT 0.05 //50ms
|
||||
#define RAD_DEGREE_CONVER 57.2958
|
||||
#define HOST_SPEED_UINIT 3600 //
|
||||
#define HOST_ANGULAR_SPEED_UINIT 2271
|
||||
|
||||
typedef struct{
|
||||
uint32_t typeId;
|
||||
uint8_t dataSize;
|
||||
uint64_t *segwayTimeStamp;
|
||||
uint8_t * updateFlag;
|
||||
void * dataPtr;
|
||||
}SegwayData;
|
||||
|
||||
chassis_speed_data_t SpeedData;
|
||||
uint64_t Speed_TimeStamp;
|
||||
uint8_t Speed_update;
|
||||
|
||||
motor_ticks_t TickData;
|
||||
uint64_t Tick_TimeStamp;
|
||||
uint8_t Tick_update;
|
||||
|
||||
imu_gyr_original_data_t ImuGyroData;
|
||||
uint64_t ImuGyro_TimeStamp;
|
||||
uint8_t ImuGyro_update;
|
||||
|
||||
imu_acc_original_data_t ImuAccData;
|
||||
uint64_t ImuAcc_TimeStamp;
|
||||
uint8_t ImuAcc_update;
|
||||
|
||||
odom_pos_xy_t OdomPoseXy;
|
||||
odom_euler_xy_t OdomEulerXy;
|
||||
odom_euler_z_t OdomEulerZ;
|
||||
odom_vel_line_xy_t OdomVelLineXy;
|
||||
uint64_t Odom_TimeStamp;
|
||||
uint8_t OdomPoseXy_update;
|
||||
uint8_t OdomEulerXy_update;
|
||||
uint8_t OdomEulerZ_update;
|
||||
uint8_t OdomVelLineXy_update;
|
||||
|
||||
static SegwayData segway_data_tbl[] = {
|
||||
{Chassis_Data_Speed, sizeof(SpeedData), &Speed_TimeStamp, &Speed_update, &SpeedData},
|
||||
{Chassis_Data_Ticks, sizeof(TickData), &Tick_TimeStamp, &Tick_update, &TickData},
|
||||
{Chassis_Data_Imu_Gyr, sizeof(ImuGyroData), &ImuGyro_TimeStamp, &ImuGyro_update, &ImuGyroData},
|
||||
{Chassis_Data_Imu_Acc, sizeof(ImuAccData), &ImuAcc_TimeStamp, &ImuAcc_update, &ImuAccData},
|
||||
{Chassis_Data_Odom_Pose_xy, sizeof(OdomPoseXy), &Odom_TimeStamp, &OdomPoseXy_update, &OdomPoseXy},
|
||||
{Chassis_Data_Odom_Euler_xy, sizeof(OdomEulerXy), &Odom_TimeStamp, &OdomEulerXy_update, &OdomEulerXy},
|
||||
{Chassis_Data_Odom_Euler_z, sizeof(OdomEulerZ), &Odom_TimeStamp, &OdomEulerZ_update, &OdomEulerZ},
|
||||
{Chassis_Data_Odom_Linevel_xy, sizeof(OdomVelLineXy), &Odom_TimeStamp, &OdomVelLineXy_update, &OdomVelLineXy}
|
||||
};
|
||||
|
||||
std::shared_ptr<rclcpp::Node> car_node;
|
||||
rclcpp::Client<segway_msgs::srv::ChassisSendEvent>::SharedPtr event_client;
|
||||
|
||||
void PubData(StampedBasicFrame_ *frame)
|
||||
{
|
||||
uint8_t i = 0;
|
||||
for (; i < sizeof(segway_data_tbl)/sizeof(segway_data_tbl[0]); i++)
|
||||
{
|
||||
if (frame->type_id == segway_data_tbl[i].typeId) break;
|
||||
}
|
||||
if (i < sizeof(segway_data_tbl)/sizeof(segway_data_tbl[0])){
|
||||
*(segway_data_tbl[i].segwayTimeStamp) = frame->timestamp;
|
||||
*(segway_data_tbl[i].updateFlag) =1;
|
||||
memcpy( segway_data_tbl[i].dataPtr, frame->data, segway_data_tbl[i].dataSize);
|
||||
}
|
||||
}
|
||||
|
||||
void EventPubData(int event_no)
|
||||
{
|
||||
robot::Chassis::pub_event_callback(event_no);
|
||||
}
|
||||
|
||||
namespace robot
|
||||
{
|
||||
void Chassis::pub_event_callback(int event_no)
|
||||
{
|
||||
using eventServiceResponseFutrue = rclcpp::Client<segway_msgs::srv::ChassisSendEvent>::SharedFuture;
|
||||
auto event_request = std::make_shared<segway_msgs::srv::ChassisSendEvent::Request>();
|
||||
event_request->chassis_send_event_id = event_no;
|
||||
auto event_response_receive_callback = [](eventServiceResponseFutrue futrue) {
|
||||
(void)futrue;
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "event sended successfully");
|
||||
};
|
||||
auto event_future_result = event_client->async_send_request(event_request, event_response_receive_callback);
|
||||
}
|
||||
|
||||
Chassis::Chassis(rclcpp::Node::SharedPtr nh) : node(nh)
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
car_node = nh;
|
||||
|
||||
timestamp_data.on_new_data = PubData;
|
||||
aprctrl_datastamped_jni_register(×tamp_data);
|
||||
event_data.event_callback = EventPubData;
|
||||
aprctrl_eventcallback_jni_register(&event_data);
|
||||
|
||||
node->declare_parameter<std::string>("bins_directory", "/sdcard/firmware/");
|
||||
node->declare_parameter<std::string>("chassis_version", "1.01");
|
||||
node->declare_parameter<std::string>("route_version", "1.01");
|
||||
node->declare_parameter<std::string>("connect_version", "1.01");
|
||||
|
||||
bms_fb_pub = node->create_publisher<segway_msgs::msg::BmsFb>("/bms_fb", 1);
|
||||
chassis_ctrl_src_fb_pub = node->create_publisher<segway_msgs::msg::ChassisCtrlSrcFb>("/chassis_ctrl_src_fb", 1);
|
||||
chassis_mileage_meter_fb_pub = node->create_publisher<segway_msgs::msg::ChassisMileageMeterFb>("/chassis_mileage_meter_fb", 1);
|
||||
chassis_mode_fb_pub = node->create_publisher<segway_msgs::msg::ChassisModeFb>("/chassis_mode_fb", 1);
|
||||
error_code_fb_pub = node->create_publisher<segway_msgs::msg::ErrorCodeFb>("/error_code_fb", 1);
|
||||
motor_work_mode_fb_pub = node->create_publisher<segway_msgs::msg::MotorWorkModeFb>("/motor_work_mode_fb", 1);
|
||||
speed_fb_pub = node->create_publisher<segway_msgs::msg::SpeedFb>("/speed_fb", 1);
|
||||
ticks_fb_pub = node->create_publisher<segway_msgs::msg::TicksFb>("/ticks_fb", 1);
|
||||
odom_pub = node->create_publisher<nav_msgs::msg::Odometry>("/odom", 1);
|
||||
imu_pub = node->create_publisher<sensor_msgs::msg::Imu>("/imu", 1);
|
||||
|
||||
event_client = node->create_client<segway_msgs::srv::ChassisSendEvent>("event_srv");
|
||||
|
||||
ros_get_charge_mos_ctrl_status_cmd_server = node->create_service<segway_msgs::srv::RosGetChargeMosCtrlStatusCmd>(
|
||||
"get_charge_mos_ctrl_status", std::bind(&Chassis::ros_get_charge_mos_ctrl_status_cmd_callback, this, _1, _2));
|
||||
ros_get_load_param_cmd_server = node->create_service<segway_msgs::srv::RosGetLoadParamCmd>(
|
||||
"get_load_param", std::bind(&Chassis::ros_get_load_param_cmd_callback, this, _1, _2));
|
||||
ros_get_sw_version_cmd_server = node->create_service<segway_msgs::srv::RosGetSwVersionCmd>(
|
||||
"get_sw_version", std::bind(&Chassis::ros_get_sw_version_cmd_callback, this, _1, _2));
|
||||
ros_get_vel_max_feedback_cmd_server = node->create_service<segway_msgs::srv::RosGetVelMaxFeedbackCmd>(
|
||||
"get_vel_max_feedback", std::bind(&Chassis::ros_get_vel_max_feedback_cmd_callback, this, _1, _2));
|
||||
ros_set_charge_mos_ctrl_cmd_server = node->create_service<segway_msgs::srv::RosSetChargeMosCtrlCmd>(
|
||||
"set_charge_mos_ctrl", std::bind(&Chassis::ros_set_charge_mos_ctrl_cmd_callback, this, _1, _2));
|
||||
ros_set_chassis_enable_cmd_server = node->create_service<segway_msgs::srv::RosSetChassisEnableCmd>(
|
||||
"set_chassis_enable", std::bind(&Chassis::ros_set_chassis_enable_cmd_callback, this, _1, _2));
|
||||
ros_set_chassis_poweroff_cmd_server = node->create_service<segway_msgs::srv::RosSetChassisPoweroffCmd>(
|
||||
"set_chassis_poweroff", std::bind(&Chassis::ros_set_chassis_poweroff_cmd_callback, this, _1, _2));
|
||||
ros_set_load_param_cmd_server = node->create_service<segway_msgs::srv::RosSetLoadParamCmd>(
|
||||
"set_load_param", std::bind(&Chassis::ros_set_load_param_cmd_callback, this, _1, _2));
|
||||
ros_set_remove_push_cmd_server = node->create_service<segway_msgs::srv::RosSetRemovePushCmd>(
|
||||
"set_remove_push", std::bind(&Chassis::ros_set_remove_push_cmd_callback, this, _1, _2));
|
||||
ros_set_vel_max_cmd_server = node->create_service<segway_msgs::srv::RosSetVelMaxCmd>(
|
||||
"set_vel_max", std::bind(&Chassis::ros_set_vel_max_cmd_callback, this, _1, _2));
|
||||
|
||||
velocity_sub = node->create_subscription<geometry_msgs::msg::Twist>(
|
||||
"cmd_vel", 1, std::bind(&Chassis::cmd_vel_callback, this, std::placeholders::_1));
|
||||
|
||||
|
||||
|
||||
iap_action_server = rclcpp_action::create_server<iapCmd>(
|
||||
node,
|
||||
"iapCmd",
|
||||
std::bind(&Chassis::handle_iapCmdGoal, this, _1, _2),
|
||||
std::bind(&Chassis::handle_iapCmdCancel, this, _1),
|
||||
std::bind(&Chassis::handle_iapCmdAccepted, this, _1));
|
||||
|
||||
odom_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node);
|
||||
timer_1000hz = node->create_wall_timer(std::chrono::milliseconds(1), std::bind(&Chassis::timer_1000hz_callback, this));
|
||||
timer_1hz = node->create_wall_timer(std::chrono::milliseconds(1000), std::bind(&Chassis::timer_1hz_callback, this));
|
||||
}
|
||||
|
||||
void Chassis::ros_get_charge_mos_ctrl_status_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetChargeMosCtrlStatusCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosGetChargeMosCtrlStatusCmd::Response> response)
|
||||
{
|
||||
int16_t ret = 0;
|
||||
if (request->ros_get_chassis_charge_ctrl_status == true){
|
||||
ret = get_charge_mos_ctrl_status();
|
||||
response->chassis_charge_ctrl_status = ret;
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "receive RosGetChargeMosCtrlStatusCmd cmd[%d], charge_ctrl_status[%d]",
|
||||
1, ret);
|
||||
}
|
||||
else {
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_get_chassis_charge_ctrl_status false");
|
||||
}
|
||||
}
|
||||
void Chassis::ros_get_load_param_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetLoadParamCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosGetLoadParamCmd::Response> response)
|
||||
{
|
||||
if (request->ros_get_load_param == false) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_get_load_param fasle");
|
||||
return ;
|
||||
}
|
||||
response->get_load_param = get_chassis_load_state();
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "get_load_param[%d]", response->get_load_param );
|
||||
}
|
||||
void Chassis::ros_get_sw_version_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetSwVersionCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosGetSwVersionCmd::Response> response)
|
||||
{
|
||||
if (request->ros_get_sw_version_cmd == false) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_get_sw_version_cmd fasle");
|
||||
return;
|
||||
}
|
||||
response->host_version = get_host_version();
|
||||
response->central_version = get_chassis_central_version();
|
||||
response->motor_version = get_chassis_motor_version();
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_get_sw_version_cmd true");
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "host_version:%d, central_version:%d, motor_version:%d",
|
||||
response->host_version, response->central_version, response->motor_version);
|
||||
}
|
||||
void Chassis::ros_get_vel_max_feedback_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosGetVelMaxFeedbackCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosGetVelMaxFeedbackCmd::Response> response)
|
||||
{
|
||||
if (request->ros_get_vel_max_fb_cmd == false) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_get_vel_max_fb_cmd fasle");
|
||||
return;
|
||||
}
|
||||
response->forward_max_vel_fb = get_line_forward_max_vel_fb();
|
||||
response->backward_max_vel_fb = get_line_backward_max_vel_fb();
|
||||
response->angular_max_vel_fb = get_angular_max_vel_fb();
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_get_vel_max_fb_cmd true");
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "forward_max_vel_fb:%d, backward_max_vel_fb:%d, angular_max_vel_fb:%d",
|
||||
response->forward_max_vel_fb, response->backward_max_vel_fb, response->angular_max_vel_fb);
|
||||
}
|
||||
void Chassis::ros_set_charge_mos_ctrl_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetChargeMosCtrlCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosSetChargeMosCtrlCmd::Response> response)
|
||||
{
|
||||
uint8_t ret;
|
||||
if (request->ros_set_chassis_charge_ctrl == false) {
|
||||
ret = set_charge_mos_ctrl(false);
|
||||
}
|
||||
else {
|
||||
ret = set_charge_mos_ctrl(true);
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_set_chassis_charge_ctrl cmd[%d]", request->ros_set_chassis_charge_ctrl);
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "chassis_set_charge_ctrl_result[%d]", ret);
|
||||
response->chassis_set_charge_ctrl_result = ret;
|
||||
}
|
||||
void Chassis::ros_set_chassis_enable_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetChassisEnableCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosSetChassisEnableCmd::Response> response)
|
||||
{
|
||||
uint8_t ret;
|
||||
if (request->ros_set_chassis_enable_cmd == false) {
|
||||
ret = set_enable_ctrl(0);
|
||||
}
|
||||
else {
|
||||
ret = set_enable_ctrl(1);
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_set_chassis_enable_cmd cmd[%d]", request->ros_set_chassis_enable_cmd);
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "chassis_set_chassis_enable_result[%d]", ret);
|
||||
response->chassis_set_chassis_enable_result = ret;
|
||||
}
|
||||
void Chassis::ros_set_chassis_poweroff_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetChassisPoweroffCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosSetChassisPoweroffCmd::Response> response)
|
||||
{
|
||||
if (request->ros_set_chassis_poweroff_cmd == false) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_set_chassis_poweroff_cmd fasle");
|
||||
return;
|
||||
}
|
||||
uint8_t ret = set_chassis_poweroff();
|
||||
response->chassis_set_poweroff_result = ret;
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "chassis_set_poweroff_result[%d]", ret);
|
||||
}
|
||||
void Chassis::ros_set_load_param_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetLoadParamCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosSetLoadParamCmd::Response> response)
|
||||
{
|
||||
int16_t value = request->ros_set_load_param;
|
||||
if (value != 0 && value != 1){
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_set_load_param[%d] out of normal range ", value);
|
||||
return;
|
||||
}
|
||||
uint8_t ret = set_chassis_load_state(value);
|
||||
response->chassis_set_load_param_result = ret;
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "chassis_set_load_param_result[%d]", ret);
|
||||
}
|
||||
void Chassis::ros_set_remove_push_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetRemovePushCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosSetRemovePushCmd::Response> response)
|
||||
{
|
||||
if (request->ros_set_remove_push_cmd == false) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_set_remove_push_cmd fasle");
|
||||
return;
|
||||
}
|
||||
uint8_t ret = set_remove_push_cmd();
|
||||
response->chassis_set_revove_push_result = ret;
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "chassis_set_revove_push_result[%d]", ret);
|
||||
}
|
||||
void Chassis::ros_set_vel_max_cmd_callback(const std::shared_ptr<segway_msgs::srv::RosSetVelMaxCmd::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::RosSetVelMaxCmd::Response> response)
|
||||
{
|
||||
double forward = request->ros_set_forward_max_vel;
|
||||
double backward = request->ros_set_backward_max_vel;
|
||||
double angular = request->ros_set_angular_max_vel;
|
||||
uint8_t ret_forw = set_line_forward_max_vel(forward);
|
||||
uint8_t ret_back = set_line_backward_max_vel(backward);
|
||||
uint8_t ret_angl = set_angular_max_vel(angular);
|
||||
response->chassis_set_max_vel_result = ret_forw | ret_back | ret_angl;
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "ros_set_forward_max_vel[%lf], ros_set_backward_max_vel[%lf], ros_set_angular_max_vel[%lf]",
|
||||
forward, backward, angular);
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "set_line_forward_max_vel result[%d], set_line_backward_max_vel result[%d], set_angular_max_vel result[%d]",
|
||||
ret_forw, ret_back, ret_angl);
|
||||
}
|
||||
|
||||
void Chassis::iapCmdExecute(const std::shared_ptr<goalHandaleIapCmd> goal_handle)
|
||||
{
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "Executing goal");
|
||||
rclcpp::Rate loop_rate(1);
|
||||
const auto goal = goal_handle->get_goal();
|
||||
auto feedback = std::make_shared<iapCmd::Feedback>();
|
||||
auto result = std::make_shared<iapCmd::Result>();
|
||||
int32_t iap_percent_fb;
|
||||
|
||||
node->get_parameter("bins_directory", bins_directory);
|
||||
node->get_parameter("chassis_version", chassis_version);
|
||||
node->get_parameter("route_version", route_version);
|
||||
node->get_parameter("connect_version", connect_version);
|
||||
char bin_dir[100] = {0};
|
||||
char ver[100] = {0};
|
||||
switch (goal->iap_board)
|
||||
{
|
||||
case 1:
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "iap central board");
|
||||
bins_directory = bins_directory + "/central.bin";
|
||||
bins_directory.copy(bin_dir, bins_directory.length(), 0);
|
||||
chassis_version.copy(ver, chassis_version.length(), 0);
|
||||
IapSingerBoard(bin_dir, (char*)"central", ver);
|
||||
break;
|
||||
case 2:
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "iap motor board");
|
||||
bins_directory = bins_directory + "/motor.bin";
|
||||
bins_directory.copy(bin_dir, bins_directory.length(), 0);
|
||||
route_version.copy(ver, route_version.length(), 0);
|
||||
IapSingerBoard(bin_dir, (char*)"motor", ver);
|
||||
break;
|
||||
default:
|
||||
RCLCPP_ERROR(rclcpp::get_logger("SmartCar"),
|
||||
"iap_board value error, out of [1 2]", goal->iap_board);
|
||||
result->set__iap_result(5);
|
||||
goal_handle->canceled(result);
|
||||
return;
|
||||
}
|
||||
|
||||
while (rclcpp::ok())
|
||||
{
|
||||
if (goal_handle->is_canceling()) {
|
||||
result->set__iap_result(5);
|
||||
goal_handle->canceled(result);
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "Goal canceled");
|
||||
return;
|
||||
}
|
||||
iap_percent_fb = getIapTotalProgress();
|
||||
feedback->set__iap_percent(iap_percent_fb);
|
||||
goal_handle->publish_feedback(feedback);
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "iap percent: %d", iap_percent_fb);
|
||||
loop_rate.sleep();
|
||||
if (iap_percent_fb == 100) {
|
||||
RCLCPP_INFO_ONCE(rclcpp::get_logger("SmartCar"), "iap successful");
|
||||
break;
|
||||
} else if (iap_percent_fb < 0) {
|
||||
RCLCPP_INFO_ONCE(rclcpp::get_logger("SmartCar"), "iap failed");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (rclcpp::ok()) {
|
||||
if (iap_percent_fb == 100) {
|
||||
result->set__iap_result(3);
|
||||
goal_handle->succeed(result);
|
||||
RCLCPP_INFO(rclcpp::get_logger("SmartCar"), "goal succeeded, iap succeeded");
|
||||
} else {
|
||||
result->set__iap_result(getHostIapResult());
|
||||
result->set__error_code(getHostIapErrorCode());
|
||||
goal_handle->succeed(result);
|
||||
RCLCPP_ERROR(rclcpp::get_logger("SmartCar"), "goal failed, iap failed");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Chassis::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) const
|
||||
{
|
||||
double angular_vel = msg->angular.z;
|
||||
double linear_vel = msg->linear.x;
|
||||
angular_vel = (angular_vel > 3.0 ? 3.0 : (angular_vel < -3.0 ? -3.0 : angular_vel));
|
||||
linear_vel = (linear_vel > 3.0 ? 3.0 : (linear_vel < -1.0 ? -1.0 : linear_vel));
|
||||
|
||||
set_cmd_vel(linear_vel, angular_vel);
|
||||
// printf("vl:%lf, va:%lf\n", linear_vel, angular_vel);
|
||||
}
|
||||
|
||||
void Chassis::timer_1hz_callback(void)
|
||||
{
|
||||
bms_fb.bat_soc = get_bat_soc();
|
||||
bms_fb.bat_charging = get_bat_charging();
|
||||
bms_fb.bat_vol = get_bat_mvol();
|
||||
bms_fb.bat_current = get_bat_mcurrent();
|
||||
bms_fb.bat_temp = get_bat_temp();
|
||||
bms_fb_pub->publish(bms_fb);
|
||||
|
||||
chassis_ctrl_src_fb.chassis_ctrl_cmd_src = get_ctrl_cmd_src();
|
||||
chassis_ctrl_src_fb_pub->publish(chassis_ctrl_src_fb);
|
||||
|
||||
chassis_mileage_meter_fb.vehicle_meters = get_vehicle_meter();
|
||||
chassis_mileage_meter_fb_pub->publish(chassis_mileage_meter_fb);
|
||||
|
||||
chassis_mode_fb.chassis_mode = get_chassis_mode();//0-lock_mode; 1-ctrl_mode; 2-push_mode; 3-emergency_mode; 4-error_mode
|
||||
chassis_mode_fb_pub->publish(chassis_mode_fb);
|
||||
|
||||
error_code_fb.host_error = get_err_state(Host);
|
||||
error_code_fb.central_error = get_err_state(Central);
|
||||
error_code_fb.left_motor_error = get_err_state(Motor) & 0xffff;
|
||||
error_code_fb.right_motor_error = (get_err_state(Motor) >> 16) & 0xffff;
|
||||
error_code_fb.bms_error = get_err_state(BMS);
|
||||
error_code_fb_pub->publish(error_code_fb);
|
||||
|
||||
motor_work_mode_fb.motor_work_mode = get_chassis_work_model();
|
||||
motor_work_mode_fb_pub->publish(motor_work_mode_fb);
|
||||
}
|
||||
|
||||
void Chassis::timer_1000hz_callback(void)
|
||||
{
|
||||
speed_pub_callback();
|
||||
ticks_pub_callback();
|
||||
imu_pub_callback();
|
||||
pub_odom_callback();
|
||||
}
|
||||
|
||||
void Chassis::speed_pub_callback(void)
|
||||
{
|
||||
if (Speed_update == 1) {
|
||||
Speed_update = 0;
|
||||
speed_fb.car_speed = SpeedData.car_speed;
|
||||
speed_fb.car_speed /= LINE_SPEED_TRANS_GAIN_MPS;
|
||||
speed_fb.turn_speed = SpeedData.turn_speed;
|
||||
speed_fb.turn_speed /= ANGULAR_SPEED_TRANS_GAIN_RADPS;
|
||||
speed_fb.l_speed = SpeedData.l_speed;
|
||||
speed_fb.l_speed /= LINE_SPEED_TRANS_GAIN_MPS;
|
||||
speed_fb.r_speed = SpeedData.r_speed;
|
||||
speed_fb.r_speed /= LINE_SPEED_TRANS_GAIN_MPS;
|
||||
speed_fb.speed_timestamp = Speed_TimeStamp;
|
||||
speed_fb_pub->publish(speed_fb);
|
||||
}
|
||||
}
|
||||
|
||||
void Chassis::ticks_pub_callback(void)
|
||||
{
|
||||
if (Tick_update == 1) {
|
||||
Tick_update = 0;
|
||||
ticks_fb.l_ticks = TickData.l_ticks;
|
||||
ticks_fb.r_ticks = TickData.r_ticks;
|
||||
ticks_fb.ticks_timestamp = Tick_TimeStamp;
|
||||
ticks_fb_pub->publish(ticks_fb);
|
||||
}
|
||||
}
|
||||
|
||||
void Chassis::imu_pub_callback(void)
|
||||
{
|
||||
if (ImuGyro_update == 1 && ImuAcc_update == 1){
|
||||
ImuGyro_update = 0;
|
||||
ImuAcc_update = 0;
|
||||
imu_fb.header.stamp = node->now();
|
||||
imu_fb.header.frame_id = "base_link";
|
||||
imu_fb.angular_velocity.x = (double)ImuGyroData.gyr[0] / 900.0;
|
||||
imu_fb.angular_velocity.y = (double)ImuGyroData.gyr[1] / 900.0;
|
||||
imu_fb.angular_velocity.z = (double)ImuGyroData.gyr[2] / 900.0;
|
||||
imu_fb.linear_acceleration.x = (double)ImuAccData.acc[0] / 4000.0 * 9.8;
|
||||
imu_fb.linear_acceleration.y = (double)ImuAccData.acc[1] / 4000.0 * 9.8;
|
||||
imu_fb.linear_acceleration.z = (double)ImuAccData.acc[2] / 4000.0 * 9.8;
|
||||
imu_pub->publish(imu_fb);
|
||||
}
|
||||
}
|
||||
|
||||
void Chassis::pub_odom_callback(void)
|
||||
{
|
||||
if ((OdomPoseXy_update & OdomEulerXy_update & OdomEulerZ_update & OdomVelLineXy_update) == 1) {
|
||||
OdomPoseXy_update = 0, OdomEulerXy_update = 0, OdomEulerZ_update = 0, OdomVelLineXy_update = 0;
|
||||
|
||||
odom_quat.setRPY(0, 0, OdomEulerZ.euler_z / RAD_DEGREE_CONVER);
|
||||
odom_trans.header.stamp = node->now();
|
||||
odom_trans.header.frame_id = "odom";
|
||||
odom_trans.child_frame_id = "base_link";
|
||||
odom_trans.transform.translation.x = OdomPoseXy.pos_x;
|
||||
odom_trans.transform.translation.y = OdomPoseXy.pos_y;
|
||||
odom_trans.transform.translation.z = 0.0;
|
||||
odom_trans.transform.rotation.x = odom_quat.x();
|
||||
odom_trans.transform.rotation.y = odom_quat.y();
|
||||
odom_trans.transform.rotation.z = odom_quat.z();
|
||||
odom_trans.transform.rotation.w = odom_quat.w();
|
||||
odom_broadcaster->sendTransform(odom_trans);
|
||||
|
||||
odom_fb.header.stamp = node->now();
|
||||
odom_fb.header.frame_id = "odom";
|
||||
odom_fb.pose.pose.position.x = OdomPoseXy.pos_x;
|
||||
odom_fb.pose.pose.position.y = OdomPoseXy.pos_y;
|
||||
odom_fb.pose.pose.position.z = 0.0;
|
||||
odom_fb.pose.pose.orientation.x = odom_quat.x();
|
||||
odom_fb.pose.pose.orientation.y = odom_quat.y();
|
||||
odom_fb.pose.pose.orientation.z = odom_quat.z();
|
||||
odom_fb.pose.pose.orientation.w = odom_quat.w();
|
||||
|
||||
odom_fb.child_frame_id = "base_link";
|
||||
odom_fb.twist.twist.linear.x = (double)SpeedData.car_speed / LINE_SPEED_TRANS_GAIN_MPS;
|
||||
odom_fb.twist.twist.linear.y = 0;
|
||||
odom_fb.twist.twist.angular.z = (double)SpeedData.turn_speed / ANGULAR_SPEED_TRANS_GAIN_RADPS;;
|
||||
odom_pub->publish(odom_fb);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -1,364 +0,0 @@
|
||||
#include "segwayrmp/robot.h"
|
||||
#include <termio.h>
|
||||
#include <stdio.h>
|
||||
#include <iostream>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
#define PRINTHELP 'h'
|
||||
#define ADDLINEVEL 'w'
|
||||
#define DECLINEVEL 's'
|
||||
#define ADDANGULARVEL 'a'
|
||||
#define DECANGULARVEL 'd'
|
||||
#define PRINTCURVEL 'f'
|
||||
#define VELRESETZERO 'g'
|
||||
#define ENABLECMD 'e'
|
||||
#define CHASSISPAUSE 'q'
|
||||
// #define CLEARSCRAM 'c'
|
||||
// #define GETERROR 'x'
|
||||
#define IAPCENTRAL 'v'
|
||||
#define IAPMOTOR 'b'
|
||||
#define PRINTERRSW 'r'
|
||||
|
||||
using iapCmd = segway_msgs::action::RosSetIapCmd;
|
||||
using goalHandleIapCmd = rclcpp_action::ClientGoalHandle<iapCmd>;
|
||||
|
||||
std::shared_ptr<rclcpp::Node> drive_node;
|
||||
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_pub;
|
||||
rclcpp::Subscription<segway_msgs::msg::ErrorCodeFb>::SharedPtr error_sub;
|
||||
|
||||
rclcpp::Client<segway_msgs::srv::RosSetChassisEnableCmd>::SharedPtr enable_client;
|
||||
// rclcpp::Client<segway_msgs::srv::ClearChassisScramStatusCmd>::SharedPtr clear_scram_client;
|
||||
// rclcpp::Client<segway_msgs::srv::GetGxErrorCmd>::SharedPtr get_err_client;
|
||||
rclcpp::Service<segway_msgs::srv::ChassisSendEvent>::SharedPtr event_server;
|
||||
|
||||
rclcpp_action::Client<iapCmd>::SharedPtr iap_client;
|
||||
uint8_t print_error = 0;
|
||||
|
||||
char const* print_help() {
|
||||
char const* printHelp =
|
||||
"\t h : Displays the required keys and their meaning\n"
|
||||
"\t w : Increase forward speed by 0.1m/s\n"
|
||||
"\t s : Decrease forward speed by 0.1m/s\n"
|
||||
"\t a : Increase the angular velocity by 0.1rad/s\n"
|
||||
"\t d : Decrease the angular velocity by 0.1rad/s\n"
|
||||
"\t f : Displays current speed Settings\n"
|
||||
"\t g : Speed reset to zero\n"
|
||||
"\t e : Chassis enable switch\n"
|
||||
"\t q : Running pause. Click 'q'key again to resume running by the previous speed. W/S/A/D keys can also restore chassis running\n"
|
||||
"\t v : Iap the central board, please put the bin file in /sdcard/firmware/\n"
|
||||
"\t b : Iap the motor board, please put the bin file in /sdcard/firmware/\n"
|
||||
"\t r : Open or close the switch: printing error code\n"
|
||||
"\t others : Do nothing\n";
|
||||
return printHelp;
|
||||
}
|
||||
|
||||
void changemode(int dir)
|
||||
{
|
||||
static struct termios oldt, newt;
|
||||
|
||||
if ( dir == 1 )
|
||||
{
|
||||
tcgetattr( STDIN_FILENO, &oldt);
|
||||
newt = oldt;
|
||||
newt.c_lflag &= ~( ICANON | ECHO );
|
||||
tcsetattr( STDIN_FILENO, TCSANOW, &newt);
|
||||
}
|
||||
else
|
||||
tcsetattr( STDIN_FILENO, TCSANOW, &oldt);
|
||||
}
|
||||
|
||||
int kbhit (void)
|
||||
{
|
||||
struct timeval tv;
|
||||
fd_set rdfs; //一组fd集合
|
||||
|
||||
tv.tv_sec = 0;
|
||||
tv.tv_usec = 0; //时间为0，非阻塞
|
||||
|
||||
FD_ZERO(&rdfs); //fd集合清空
|
||||
FD_SET (STDIN_FILENO, &rdfs); //增加新的fd
|
||||
|
||||
select(STDIN_FILENO+1, &rdfs, NULL, NULL, &tv); //非阻塞检测fd为0的tty读取状态，rdfs特定位置1
|
||||
return FD_ISSET(STDIN_FILENO, &rdfs); //判断指定的标准输入fd是否在rdfs集合中
|
||||
}
|
||||
|
||||
static int scanKeyboard()
|
||||
{
|
||||
int input_char = 0;
|
||||
struct termios new_settings;
|
||||
struct termios stored_settings;
|
||||
tcgetattr(0, &stored_settings);
|
||||
new_settings = stored_settings;
|
||||
new_settings.c_lflag &= (~ICANON); //非规范模式：每次返回单个字符
|
||||
new_settings.c_cc[VTIME] = 0;
|
||||
tcgetattr(0,&stored_settings);
|
||||
new_settings.c_cc[VMIN] = 1; //读取的最小字节数
|
||||
tcsetattr(0, TCSANOW, &new_settings);
|
||||
|
||||
changemode(1);
|
||||
if (kbhit()) {
|
||||
input_char = getchar();
|
||||
printf("\n");
|
||||
}
|
||||
changemode(0);
|
||||
|
||||
tcsetattr(0, TCSANOW, &stored_settings);
|
||||
return input_char;
|
||||
}
|
||||
|
||||
char get_keyboard()
|
||||
{
|
||||
char keyvalue = scanKeyboard();
|
||||
return keyvalue;
|
||||
}
|
||||
|
||||
void goal_response_callback(std::shared_future<goalHandleIapCmd::SharedPtr> future)
|
||||
{
|
||||
auto goal_handle = future.get();
|
||||
if (!goal_handle) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger("drive_segway_sample"), "Goal was rejected by server");
|
||||
} else {
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "Goal accepted by server, waiting for result");
|
||||
}
|
||||
}
|
||||
|
||||
void feedback_callback(
|
||||
goalHandleIapCmd::SharedPtr,
|
||||
const std::shared_ptr<const iapCmd::Feedback> feedback)
|
||||
{
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "IAP process percentage：[%d]", feedback->iap_percent);
|
||||
}
|
||||
|
||||
void result_callback(const goalHandleIapCmd::WrappedResult & result)
|
||||
{
|
||||
switch (result.code) {
|
||||
case rclcpp_action::ResultCode::SUCCEEDED:
|
||||
break;
|
||||
case rclcpp_action::ResultCode::ABORTED:
|
||||
RCLCPP_ERROR(rclcpp::get_logger("drive_segway_sample"), "Goal was aborted");
|
||||
return;
|
||||
case rclcpp_action::ResultCode::CANCELED:
|
||||
RCLCPP_ERROR(rclcpp::get_logger("drive_segway_sample"), "Goal was canceled");
|
||||
return;
|
||||
default:
|
||||
RCLCPP_ERROR(rclcpp::get_logger("drive_segway_sample"), "Unknown result code");
|
||||
return;
|
||||
}
|
||||
if (result.result->iap_result == 3) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "iap success!");
|
||||
} else {
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "iap fail!, error_code[%#x]", result.result->error_code);
|
||||
}
|
||||
}
|
||||
|
||||
void drive_chassis_test()
|
||||
{
|
||||
static uint16_t set_enable_cmd = 1;
|
||||
uint8_t enable_switch = 0;
|
||||
static double set_line_speed;
|
||||
static double set_angular_speed;
|
||||
static double send_line_speed;
|
||||
static double send_angular_speed;
|
||||
static uint8_t chassis_pause = 0;
|
||||
// uint8_t clear_scram_flag = 0;
|
||||
// uint8_t get_err_flag = 0;
|
||||
uint8_t iap_flag = 0;
|
||||
|
||||
auto enable_request = std::make_shared<segway_msgs::srv::RosSetChassisEnableCmd::Request>();
|
||||
// auto clear_scram_requst = std::make_shared<segway_msgs::srv::ClearChassisScramStatusCmd::Request>();
|
||||
// auto get_err_request = std::make_shared<segway_msgs::srv::GetGxErrorCmd::Request>();
|
||||
|
||||
char keyvalue = get_keyboard();
|
||||
switch (keyvalue)
|
||||
{
|
||||
case PRINTHELP:
|
||||
printf("%s\n", print_help());
|
||||
break;
|
||||
case ADDLINEVEL :
|
||||
set_line_speed += 0.1;
|
||||
send_line_speed = set_line_speed;
|
||||
send_angular_speed = set_angular_speed;
|
||||
printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
|
||||
break;
|
||||
case DECLINEVEL :
|
||||
set_line_speed -= 0.1;
|
||||
send_line_speed = set_line_speed;
|
||||
send_angular_speed = set_angular_speed;
|
||||
printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
|
||||
break;
|
||||
case ADDANGULARVEL :
|
||||
set_angular_speed += 0.1;
|
||||
send_line_speed = set_line_speed;
|
||||
send_angular_speed = set_angular_speed;
|
||||
printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
|
||||
break;
|
||||
case DECANGULARVEL :
|
||||
set_angular_speed -= 0.1;
|
||||
send_line_speed = set_line_speed;
|
||||
send_angular_speed = set_angular_speed;
|
||||
printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
|
||||
break;
|
||||
case PRINTCURVEL :
|
||||
printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
|
||||
break;
|
||||
case VELRESETZERO :
|
||||
set_line_speed = 0;
|
||||
set_angular_speed = 0;
|
||||
send_line_speed = 0;
|
||||
send_angular_speed = 0;
|
||||
printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
|
||||
break;
|
||||
case ENABLECMD :
|
||||
enable_switch = 1;
|
||||
enable_request->ros_set_chassis_enable_cmd = set_enable_cmd;
|
||||
++set_enable_cmd;
|
||||
set_enable_cmd %= 2;
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"),
|
||||
"enable chassis switch[%d]", enable_request->ros_set_chassis_enable_cmd);
|
||||
printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
|
||||
break;
|
||||
case CHASSISPAUSE :
|
||||
++chassis_pause;
|
||||
chassis_pause %= 2;
|
||||
if (chassis_pause) {
|
||||
send_line_speed = 0;
|
||||
send_angular_speed = 0;
|
||||
printf("Stop the chassis temporarily\n");
|
||||
} else {
|
||||
send_line_speed = set_line_speed;
|
||||
send_angular_speed = set_angular_speed;
|
||||
printf("current set_line_speed[%lf], set_angular_speed[%lf]\n", set_line_speed, set_angular_speed);
|
||||
}
|
||||
break;
|
||||
// case CLEARSCRAM :
|
||||
// // clear_scram_flag = 1;
|
||||
// break;
|
||||
// case GETERROR:
|
||||
// // get_err_flag = 1;
|
||||
// break;
|
||||
case IAPCENTRAL:
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "iap chassis");
|
||||
iap_flag = 1;
|
||||
break;
|
||||
case IAPMOTOR:
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "iap route");
|
||||
iap_flag = 2;
|
||||
break;
|
||||
case PRINTERRSW:
|
||||
print_error = ~print_error;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Twist cmd_vel;
|
||||
cmd_vel.linear.x = send_line_speed;
|
||||
cmd_vel.angular.z = send_angular_speed;
|
||||
velocity_pub->publish(cmd_vel);
|
||||
|
||||
if (enable_switch){
|
||||
using enableServiceResponseFutrue = rclcpp::Client<segway_msgs::srv::RosSetChassisEnableCmd>::SharedFuture;
|
||||
auto enable_response_receive_callback = [](enableServiceResponseFutrue futrue) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"),
|
||||
"event sended successfully, result:%d", futrue.get()->chassis_set_chassis_enable_result);
|
||||
};
|
||||
auto enable_future_result = enable_client->async_send_request(enable_request, enable_response_receive_callback);
|
||||
}
|
||||
|
||||
// if (clear_scram_flag) {
|
||||
// using clearServiceResponseFuture = rclcpp::Client<segway_msgs::srv::ClearChassisScramStatusCmd>::SharedFuture;
|
||||
// auto clear_response_receive_callback = [](clearServiceResponseFuture future) {
|
||||
// RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"),
|
||||
// "clear scram status successfully, result:%d", future.get()->clear_scram_result);
|
||||
// };
|
||||
// auto clear_future_result = clear_scram_client->async_send_request(clear_scram_requst, clear_response_receive_callback);
|
||||
// }
|
||||
|
||||
// if (get_err_flag) {
|
||||
// using errorServiceResponseFuture = rclcpp::Client<segway_msgs::srv::GetGxErrorCmd>::SharedFuture;
|
||||
// auto error_response_receive_callback = [](errorServiceResponseFuture future) {
|
||||
// RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"),
|
||||
// "chassis_error_code[%d], route_error_code[%d], connect_error_code[%d]",
|
||||
// future.get()->chassis_error_code,
|
||||
// future.get()->route_error_code,
|
||||
// future.get()->connect_error_code);
|
||||
// };
|
||||
// auto error_future_result = get_err_client->async_send_request(get_err_request, error_response_receive_callback);
|
||||
// }
|
||||
|
||||
if (iap_flag & 3) {
|
||||
auto goal_msg = iapCmd::Goal();
|
||||
goal_msg.iap_board = iap_flag;
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "sending goal, iap cmd goal");
|
||||
auto send_goal_options = rclcpp_action::Client<iapCmd>::SendGoalOptions();
|
||||
send_goal_options.goal_response_callback = std::bind(&goal_response_callback, std::placeholders::_1);
|
||||
send_goal_options.feedback_callback = std::bind(&feedback_callback, std::placeholders::_1, std::placeholders::_2);
|
||||
send_goal_options.result_callback = std::bind(&result_callback, std::placeholders::_1);
|
||||
iap_client->async_send_goal(goal_msg, send_goal_options);
|
||||
}
|
||||
}
|
||||
|
||||
void get_error_code_callback(const segway_msgs::msg::ErrorCodeFb::SharedPtr msg)
|
||||
{
|
||||
if (print_error != 0) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "host_error[%#x], central_error[%#x], \
|
||||
left_motor_error[%#x], right_motor_error[%#x], bms_err[%#x]",
|
||||
msg->host_error, msg->central_error, msg->left_motor_error, msg->right_motor_error, msg->bms_error);
|
||||
}
|
||||
}
|
||||
|
||||
void get_chassis_event_callback(const std::shared_ptr<segway_msgs::srv::ChassisSendEvent::Request> request,
|
||||
std::shared_ptr<segway_msgs::srv::ChassisSendEvent::Response> response)
|
||||
{
|
||||
(void)response;
|
||||
switch (request->chassis_send_event_id)
|
||||
{
|
||||
case OnEmergeStopEvent:
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "CHASSIS EVENT: The chassis emergency stop button is triggered");
|
||||
break;
|
||||
case OutEmergeStopEvent:
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "CHASSIS EVENT: OThe chassis emergency stop button recover");
|
||||
break;
|
||||
case PadPowerOffEvent:
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "CHASSIS EVENT: The chassis will power off");
|
||||
break;
|
||||
case OnLockedRotorProtectEvent:
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "CHASSIS EVENT: the chassis motor locked-rotor");
|
||||
break;
|
||||
case OutLockedRotorProtectEvent:
|
||||
RCLCPP_INFO(rclcpp::get_logger("drive_segway_sample"), "CHASSIS EVENT: the chassis motor no longer locked-rotor");
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
drive_node = rclcpp::Node::make_shared("drive_segway_sample");
|
||||
|
||||
velocity_pub = drive_node->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
|
||||
error_sub = drive_node->create_subscription<segway_msgs::msg::ErrorCodeFb>(
|
||||
"error_code_fb", 1, std::bind(&get_error_code_callback, std::placeholders::_1));
|
||||
|
||||
enable_client = drive_node->create_client<segway_msgs::srv::RosSetChassisEnableCmd>("set_chassis_enable");
|
||||
// clear_scram_client = drive_node->create_client<segway_msgs::srv::ClearChassisScramStatusCmd>("clear_scram_status_srv");
|
||||
// get_err_client = drive_node->create_client<segway_msgs::srv::GetGxErrorCmd>("get_error_srv");
|
||||
event_server = drive_node->create_service<segway_msgs::srv::ChassisSendEvent>(
|
||||
"event_srv", std::bind(&get_chassis_event_callback, std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
iap_client = rclcpp_action::create_client<iapCmd>(drive_node, "iapCmd");
|
||||
|
||||
rclcpp::TimerBase::SharedPtr timer_100hz =
|
||||
drive_node->create_wall_timer(std::chrono::milliseconds(10), &drive_chassis_test);
|
||||
|
||||
printf("%s\n", print_help());
|
||||
|
||||
rclcpp::spin(drive_node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user