405 lines
14 KiB
C++
405 lines
14 KiB
C++
/**
|
|
* @file safe_controller.hpp
|
|
* @brief safe controller to make robot safe
|
|
* @author DeepRobotics
|
|
* @version 1.0
|
|
* @date 2025-11-07
|
|
*
|
|
* @copyright Copyright (c) 2025 DeepRobotics
|
|
*
|
|
*/
|
|
|
|
/**
|
|
* 一、关节端
|
|
* 1.关节数据异常（nan值）
|
|
* 2.关节数据不更新
|
|
* 3.关节温度过温
|
|
* 4.驱动器报错
|
|
* 二、imu
|
|
* 1.imu数据异常
|
|
* 2.imu数据不更新
|
|
* 三、电池
|
|
* 1.电池电量过低
|
|
* 2.电池数据不上报？
|
|
* 3.电池温度？
|
|
* 四、手柄
|
|
* 1.手柄断连
|
|
*
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "robot_interface.h"
|
|
#include "user_command_interface.h"
|
|
|
|
|
|
union RobotErrorState {
|
|
struct {
|
|
uint32_t imu_num_error: 1;
|
|
uint32_t imu_update_overtime: 1;
|
|
uint32_t command_heartbeat_overtime: 1;
|
|
uint32_t driver_error: 1;
|
|
uint32_t motor_heat_warn: 1;
|
|
uint32_t joint_num_error: 1;
|
|
uint32_t joint_update_error: 1;
|
|
uint32_t battery_low_warn: 1;
|
|
uint32_t battery_heat_warn: 1;
|
|
uint32_t cpu_heat_warn: 1;
|
|
uint32_t cpu_freq_warn: 1;
|
|
uint32_t reserved: 21;
|
|
};
|
|
uint32_t error_code;
|
|
};
|
|
|
|
class SafeController {
|
|
private:
|
|
RobotType robot_type_;
|
|
std::shared_ptr<interface::RobotInterface> ri_ptr_;
|
|
std::shared_ptr<interface::UserCommandInterface> uc_ptr_;
|
|
std::thread judge_thread_;
|
|
int run_cnt_ = 0;
|
|
|
|
double last_imu_ts_, imu_check_time_;
|
|
bool imu_check_flag_ = true;
|
|
|
|
double ri_check_time_, last_ri_ts_;
|
|
VecXf last_joint_pos_, last_joint_vel_, last_joint_tau_;
|
|
std::vector<int> joint_data_same_cnt_;
|
|
bool joint_check_flag_ = true;
|
|
|
|
float max_temp_;
|
|
int max_temp_idx_;
|
|
|
|
double last_cmd_ts_, cmd_check_time_;
|
|
bool cmd_check_flag_ = true;
|
|
|
|
types::UserCommand *usr_cmd_;
|
|
bool start_thread_flag_ = false;
|
|
|
|
double driver_error_ts_;
|
|
double joint_data_error_ts_;
|
|
double current_time_;
|
|
|
|
RobotErrorState robot_error_state_;
|
|
uint32_t last_error_code_;
|
|
std::map<uint8_t, uint16_t> error_driver_map_;
|
|
uint32_t joint_update_state_;
|
|
|
|
|
|
bool IsImuDataNormal() {
|
|
double current_imu_ts_ = ri_ptr_->GetImuTimestamp();
|
|
Vec3f rpy = ri_ptr_->GetImuRpy();
|
|
Vec3f acc = ri_ptr_->GetImuAcc();
|
|
Vec3f omg = ri_ptr_->GetImuOmega();
|
|
if (imu_check_flag_) {
|
|
last_imu_ts_ = current_imu_ts_;
|
|
imu_check_time_ = current_time_;
|
|
imu_check_flag_ = false;
|
|
}
|
|
|
|
bool res = true;
|
|
robot_error_state_.imu_update_overtime = 0;
|
|
if (current_imu_ts_ != last_imu_ts_) {
|
|
last_imu_ts_ = current_imu_ts_;
|
|
imu_check_time_ = current_time_;
|
|
}
|
|
if (current_time_ - imu_check_time_ == 0.2) {
|
|
std::cout << "========== imu lag warning ========>> "
|
|
<< current_time_ - imu_check_time_ << std::endl;
|
|
}
|
|
|
|
for (int i = 0; i < 3; ++i) {
|
|
if (std::isnan(rpy(i)) || std::isnan(acc(i)) || std::isnan(omg(i))) {
|
|
res = false;
|
|
robot_error_state_.imu_num_error = 1;
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
|
|
bool IsJointDataNormal() {
|
|
double ri_ts_ = ri_ptr_->GetInterfaceTimeStamp();
|
|
VecXf joint_pos = ri_ptr_->GetJointPosition();
|
|
VecXf joint_vel = ri_ptr_->GetJointVelocity();
|
|
VecXf joint_tau = ri_ptr_->GetJointTorque();
|
|
std::vector<uint16_t> joint_id = ri_ptr_->GetJointDataID();
|
|
if (joint_check_flag_) {
|
|
joint_check_flag_ = false;
|
|
last_ri_ts_ = ri_ts_;
|
|
last_joint_pos_ = joint_pos;
|
|
last_joint_vel_ = joint_vel;
|
|
last_joint_tau_ = joint_tau;
|
|
}
|
|
bool res = true;
|
|
robot_error_state_.joint_update_error = 0;
|
|
static bool first_flag = false;
|
|
if (last_ri_ts_ != ri_ts_) {
|
|
last_ri_ts_ = ri_ts_;
|
|
ri_check_time_ = current_time_;
|
|
first_flag = true;
|
|
}
|
|
|
|
|
|
robot_error_state_.joint_num_error = 0;
|
|
joint_update_state_ = 0;
|
|
for (int i = 0; i < joint_pos.size(); ++i) {
|
|
if (std::isnan(joint_pos(i)) || std::isnan(joint_vel(i)) || std::isnan(joint_tau(i))) {
|
|
robot_error_state_.joint_num_error = 1;
|
|
res = false;
|
|
}
|
|
if (joint_pos(i) == last_joint_pos_(i)
|
|
&& joint_vel(i) == last_joint_vel_(i)
|
|
&& joint_tau(i) == last_joint_tau_(i)) {
|
|
joint_data_same_cnt_[i]++;
|
|
} else {
|
|
joint_data_same_cnt_[i] = 0;
|
|
}
|
|
// if (joint_data_same_cnt_[i] > 200) {
|
|
// robot_error_state_.joint_num_error = 1;
|
|
// joint_update_state_ |= (1 << i);
|
|
// res = false;
|
|
// }
|
|
}
|
|
last_joint_pos_ = joint_pos;
|
|
last_joint_vel_ = joint_vel;
|
|
last_joint_tau_ = joint_tau;
|
|
if (run_cnt_ % 5000 == 0) {
|
|
std::cout << "joint_pos: " << joint_pos.transpose() << std::endl;
|
|
std::cout << "joint_vel: " << joint_vel.transpose() << std::endl;
|
|
}
|
|
return res;
|
|
}
|
|
|
|
bool IsDriverStatusNormal() {
|
|
std::vector<uint16_t> status_word = ri_ptr_->GetDriverStatusWord();
|
|
bool res = true;
|
|
robot_error_state_.driver_error = 0;
|
|
for (int i = 0; i < status_word.size(); ++i) {
|
|
if (status_word[i] != 1) {
|
|
error_driver_map_[uint8_t(i)] = status_word[i];
|
|
robot_error_state_.driver_error = 1;
|
|
res = false;
|
|
}
|
|
}
|
|
if (robot_error_state_.driver_error == 0) {
|
|
error_driver_map_.clear();
|
|
}
|
|
return res;
|
|
}
|
|
|
|
bool IsMotorTempertureNormal() {
|
|
VecXf m_t = ri_ptr_->GetMotorTemperture();
|
|
VecXf::Index max_idx, min_idx;
|
|
float max_temp = m_t.maxCoeff(&max_idx);
|
|
float min_temp = m_t.minCoeff(&min_idx);
|
|
max_temp_ = max_temp;
|
|
max_temp_idx_ = max_idx;
|
|
if (run_cnt_ % 5000 == 0) {
|
|
std::cout << "Motor Temperture: " << m_t.transpose() << std::endl;
|
|
std::cout << "Max&Min: " << max_idx << " : " << max_temp << " | " << min_idx << " : " << min_temp
|
|
<< std::endl;
|
|
}
|
|
if (max_temp > 100) {
|
|
robot_error_state_.motor_heat_warn = 1;
|
|
return false;
|
|
} else {
|
|
robot_error_state_.motor_heat_warn = 0;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool IsBatteryNormal() {
|
|
std::vector<uint16_t> battery_data = ri_ptr_->GetBatteryData();
|
|
if (battery_data[0] < 10) {
|
|
robot_error_state_.battery_low_warn = 1;
|
|
return false;
|
|
} else {
|
|
robot_error_state_.battery_low_warn = 0;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool IsUserCommandNormal() {
|
|
double cmd_ts_ = uc_ptr_->GetUserCommand()->time_stamp;
|
|
if (cmd_check_flag_) {
|
|
cmd_check_flag_ = false;
|
|
last_cmd_ts_ = cmd_ts_;
|
|
cmd_check_time_ = current_time_;
|
|
}
|
|
robot_error_state_.command_heartbeat_overtime = 0;
|
|
if (last_cmd_ts_ != cmd_ts_) {
|
|
last_cmd_ts_ = cmd_ts_;
|
|
cmd_check_time_ = current_time_;
|
|
}
|
|
if (current_time_ - cmd_check_time_ > 1000.0) {
|
|
robot_error_state_.command_heartbeat_overtime = 1;
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
void PrintRobotErrorState() {
|
|
std::cout << "Error occurred!" << std::endl;
|
|
|
|
if (robot_error_state_.imu_num_error) {
|
|
std::cout << "IMU number error: invalid IMU data count | "
|
|
<< ri_ptr_->GetImuRpy().transpose() << " | "
|
|
<< ri_ptr_->GetImuAcc().transpose() << " | "
|
|
<< ri_ptr_->GetImuOmega().transpose() << std::endl;
|
|
}
|
|
|
|
if (robot_error_state_.imu_update_overtime) {
|
|
std::cout << "IMU update overtime: no IMU data received since timestamp " << last_imu_ts_ << std::endl;
|
|
}
|
|
|
|
if (robot_error_state_.command_heartbeat_overtime) {
|
|
std::cout << "Heartbeat overtime: lost communication with controller (no heartbeat signal received)" << std::endl;
|
|
}
|
|
|
|
if (robot_error_state_.driver_error) {
|
|
std::cout << "Driver error: motor driver reported fault(s):" << std::endl;
|
|
for (const auto &pair : error_driver_map_) {
|
|
std::cout << " Driver " << int(pair.first) << ": error code 0x" << std::hex << std::uppercase << int(pair.second) << std::dec << std::endl;
|
|
}
|
|
}
|
|
|
|
if (robot_error_state_.motor_heat_warn) {
|
|
std::cout << "Motor overheat warning: motor " << max_temp_idx_ << " reached " << max_temp_ << "°C" << std::endl;
|
|
}
|
|
|
|
if (robot_error_state_.joint_num_error) {
|
|
std::cout << "Joint number error: mismatched joint count | pos: "
|
|
<< ri_ptr_->GetJointPosition().transpose() << " | vel: "
|
|
<< ri_ptr_->GetJointVelocity().transpose() << " | tau: "
|
|
<< ri_ptr_->GetJointTorque().transpose() << std::endl;
|
|
}
|
|
|
|
if (robot_error_state_.joint_update_error) {
|
|
std::cout << "Joint update error: no joint data update (current_time 0x" << std::hex << current_time_
|
|
<< " vs last_check 0x" << ri_check_time_ << ")" << std::dec << std::endl;
|
|
}
|
|
|
|
if (robot_error_state_.battery_low_warn) {
|
|
std::cout << "Battery low warning: battery voltage/level critically low" << std::endl;
|
|
}
|
|
|
|
if (robot_error_state_.battery_heat_warn) {
|
|
std::cout << "Battery overheat warning: battery temperature too high" << std::endl;
|
|
}
|
|
|
|
if (robot_error_state_.cpu_heat_warn) {
|
|
std::cout << "CPU overheat warning: CPU temperature exceeds safe limit" << std::endl;
|
|
}
|
|
|
|
if (robot_error_state_.cpu_freq_warn) {
|
|
std::cout << "CPU frequency warning: CPU throttling due to thermal or power limits" << std::endl;
|
|
}
|
|
}
|
|
|
|
public:
|
|
SafeController(RobotType robot_type, const std::string &path) : robot_type_(robot_type) {
|
|
robot_error_state_.error_code = 0;
|
|
last_error_code_ = 0;
|
|
error_driver_map_.clear();
|
|
}
|
|
|
|
~SafeController() {
|
|
}
|
|
|
|
void SetRobotDataSource(std::shared_ptr<interface::RobotInterface> ri) {
|
|
ri_ptr_ = ri;
|
|
joint_data_same_cnt_.resize(ri_ptr_->dof_num_, 0);
|
|
}
|
|
|
|
void SetUserCommandDataSource(std::shared_ptr<interface::UserCommandInterface> uc) {
|
|
uc_ptr_ = uc;
|
|
usr_cmd_ = uc_ptr_->GetUserCommand();
|
|
}
|
|
|
|
void Start() {
|
|
start_thread_flag_ = true;
|
|
judge_thread_ = std::thread(std::bind(&SafeController::Run, this));
|
|
}
|
|
|
|
void Run() {
|
|
int tfd; //定时器描述符
|
|
int efd; //epoll描述符
|
|
int fds;
|
|
uint64_t value;
|
|
struct epoll_event ev, *evptr;
|
|
struct itimerspec time_intv; //用来存储时间
|
|
|
|
tfd = timerfd_create(CLOCK_MONOTONIC, 0); //创建定时器
|
|
if (tfd == -1) return;
|
|
|
|
time_intv.it_value.tv_sec = 1.;
|
|
time_intv.it_value.tv_nsec = 1000 * 1000;
|
|
time_intv.it_interval.tv_sec = 0;
|
|
time_intv.it_interval.tv_nsec = time_intv.it_value.tv_nsec;
|
|
|
|
timerfd_settime(tfd, 0, &time_intv, NULL); //启动定时器
|
|
|
|
efd = epoll_create1(0); //创建epoll实例
|
|
if (efd == -1) close(tfd);
|
|
|
|
evptr = (struct epoll_event *) calloc(1, sizeof(struct epoll_event));
|
|
if (evptr == NULL) {
|
|
close(tfd);
|
|
close(efd);
|
|
}
|
|
|
|
ev.data.fd = tfd;
|
|
ev.events = EPOLLIN; //监听定时器读事件，当定时器超时时，定时器描述符可读。
|
|
epoll_ctl(efd, EPOLL_CTL_ADD, tfd, &ev); //添加到epoll监听队列中
|
|
while (start_thread_flag_) {
|
|
fds = epoll_wait(efd, evptr, 1, -1); //阻塞监听，直到有事件发生
|
|
if (evptr[0].events & EPOLLIN) {
|
|
int ret = read(evptr->data.fd, &value, sizeof(uint64_t));
|
|
if (ret == -1) {
|
|
continue;
|
|
} else {
|
|
}
|
|
}
|
|
run_cnt_++;
|
|
current_time_ = GetTimestampMs() / 1000;
|
|
if (!IsDriverStatusNormal()) {
|
|
usr_cmd_->safe_control_mode = 2;
|
|
driver_error_ts_ = current_time_;
|
|
std::cout << "Driver status error!" << std::endl;
|
|
}
|
|
if (!IsJointDataNormal()) {
|
|
usr_cmd_->safe_control_mode = 3;
|
|
joint_data_error_ts_ = current_time_;
|
|
std::cout << "Joint data error!" << std::endl;
|
|
}
|
|
if (run_cnt_ % 1000 == 0 && !IsMotorTempertureNormal()) {
|
|
usr_cmd_->safe_control_mode = 2;
|
|
std::cout << "Motor temperture error!" << std::endl;
|
|
}
|
|
if (!IsImuDataNormal()) {
|
|
std::cout << "IMU error!" << std::endl;
|
|
usr_cmd_->safe_control_mode = 2;
|
|
}
|
|
if (last_error_code_ != robot_error_state_.error_code) {
|
|
if (robot_error_state_.error_code != 0) {
|
|
std::cout << "========== error_code ========>> " <<
|
|
std::hex << robot_error_state_.error_code << std::endl;
|
|
PrintRobotErrorState();
|
|
}
|
|
last_error_code_ = robot_error_state_.error_code;
|
|
}
|
|
}
|
|
}
|
|
|
|
void Stop() {
|
|
start_thread_flag_ = false;
|
|
if (judge_thread_.joinable()) {
|
|
judge_thread_.join();
|
|
}
|
|
}
|
|
|
|
};
|
|
|