initial commit
This commit is contained in:
huangweifeng
@@ -0,0 +1,98 @@
|
||||
/**
|
||||
* @file basic_function.hpp
|
||||
* @brief basic function
|
||||
* @author DeepRobotics
|
||||
* @version 1.0
|
||||
* @date 2025-11-07
|
||||
*
|
||||
* @copyright Copyright (c) 2025 DeepRobotics
|
||||
*
|
||||
*/
|
||||
#pragma once
|
||||
#include "common_types.h"
|
||||
#include "custom_types.h"
|
||||
|
||||
using namespace types;
|
||||
|
||||
inline float Deg2Rad(float deg){
|
||||
return deg /180.f * M_PI;
|
||||
}
|
||||
|
||||
inline float Rad2Deg(float rad){
|
||||
return rad / M_PI * 180.;
|
||||
}
|
||||
|
||||
inline float LimitNumber(float data, float limit){
|
||||
limit = fabs(limit);
|
||||
if(data > limit) data = limit;
|
||||
else if(data < -limit) data = -limit;
|
||||
return data;
|
||||
}
|
||||
|
||||
inline float LimitNumber(float data, float low, float high){
|
||||
if(low > high){
|
||||
std::cerr << "error limit range" << std::endl;
|
||||
return data;
|
||||
}
|
||||
if(data > high) return high;
|
||||
if(data < low) return low;
|
||||
return data;
|
||||
}
|
||||
|
||||
inline Mat3f RpyToRm(const Vec3f &rpy){
|
||||
Eigen::AngleAxisf yawAngle(rpy(2), Vec3f::UnitZ());
|
||||
Eigen::AngleAxisf pitchAngle(rpy(1), Vec3f::UnitY());
|
||||
Eigen::AngleAxisf rollAngle(rpy(0), Vec3f::UnitX());
|
||||
Eigen::Quaternion<float> q = yawAngle*pitchAngle*rollAngle;
|
||||
return q.matrix();
|
||||
}
|
||||
|
||||
inline float NormalizeAngle(float angle) {
|
||||
float result = std::fmod(angle, 2 * M_PI);
|
||||
if (result < 0) {
|
||||
result += 2 * M_PI;
|
||||
}
|
||||
if (result > M_PI) {
|
||||
result -= 2 * M_PI;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
inline float Sign(const float &i){
|
||||
if (i > 0) {
|
||||
return 1.;
|
||||
} else if (i == 0) {
|
||||
return 0.;
|
||||
} else {
|
||||
return -1.;
|
||||
}
|
||||
}
|
||||
|
||||
inline double GetTimestampMs(){
|
||||
static timespec startup_timestamp;
|
||||
timespec now_timestamp;
|
||||
if (startup_timestamp.tv_sec + startup_timestamp.tv_nsec == 0) {
|
||||
clock_gettime(CLOCK_MONOTONIC,&startup_timestamp);
|
||||
}
|
||||
clock_gettime(CLOCK_MONOTONIC,&now_timestamp);
|
||||
return (now_timestamp.tv_sec-startup_timestamp.tv_sec)*1e3 + (now_timestamp.tv_nsec-startup_timestamp.tv_nsec)/1e6;
|
||||
}
|
||||
|
||||
inline float GetCubicSplinePos(float x0, float v0, float xf, float vf, float t, float T){
|
||||
if(t >= T) return xf;
|
||||
float a, b, c, d;
|
||||
d = x0;
|
||||
c = v0;
|
||||
a = (vf*T - 2*xf + v0*T + 2*x0) / pow(T, 3);
|
||||
b = (3*xf - vf*T - 2*v0*T - 3*x0) / pow(T, 2);
|
||||
return a*pow(t, 3)+b*pow(t, 2)+c*t+d;
|
||||
}
|
||||
|
||||
inline float GetCubicSplineVel(float x0, float v0, float xf, float vf, float t, float T){
|
||||
if(t >= T) return 0;
|
||||
float a, b, c;
|
||||
c = v0;
|
||||
a = (vf*T - 2*xf + v0*T + 2*x0) / pow(T, 3);
|
||||
b = (3*xf - vf*T - 2*v0*T - 3*x0) / pow(T, 2);
|
||||
return 3.*a*pow(t, 2) + 2.*b*t + c;
|
||||
}
|
||||
+24766
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,69 @@
|
||||
/**
|
||||
* @file motion_state_feedback.hpp
|
||||
* @brief motion state feedback
|
||||
* @author DeepRobotics
|
||||
* @version 1.0
|
||||
* @date 2025-11-07
|
||||
*
|
||||
* @copyright Copyright (c) 2025 DeepRobotics
|
||||
*
|
||||
*/
|
||||
#pragma once
|
||||
#include "common_types.h"
|
||||
|
||||
class MotionStateFeedback
|
||||
{
|
||||
private:
|
||||
uint8_t current_state_;
|
||||
uint8_t current_gait_;
|
||||
uint8_t last_state_;
|
||||
uint8_t last_gait_;
|
||||
|
||||
Vec3f current_vel_;
|
||||
Vec3f cmd_vel_;
|
||||
Vec3f max_vel_;
|
||||
|
||||
union{
|
||||
struct{
|
||||
uint32_t joint_pos_limit : 1;
|
||||
uint32_t posture_limit : 1;
|
||||
uint32_t reserved : 30;
|
||||
}error_code_bit_;
|
||||
uint32_t error_code_;
|
||||
};
|
||||
public:
|
||||
MotionStateFeedback(/* args */){
|
||||
memset(this, 0, sizeof(MotionStateFeedback));
|
||||
std::cout << "Motion State Feedback" << std::endl;
|
||||
}
|
||||
~MotionStateFeedback(){
|
||||
|
||||
}
|
||||
|
||||
void UpdateCurrentState(int state){
|
||||
if(uint8_t(state) != current_state_){
|
||||
last_state_ = current_state_;
|
||||
current_state_ = uint8_t(state);
|
||||
}
|
||||
}
|
||||
|
||||
void UpdateCurrentGait(uint8_t gait){
|
||||
if(gait != current_gait_){
|
||||
last_gait_ = current_gait_;
|
||||
current_gait_ = gait;
|
||||
}
|
||||
}
|
||||
|
||||
void ClearMotionError(){error_code_ = 0;}
|
||||
void SetJointPosLimitError(){
|
||||
error_code_bit_.joint_pos_limit = 1;
|
||||
}
|
||||
void SetPostureLimitError(){
|
||||
error_code_bit_.posture_limit = 1;
|
||||
}
|
||||
|
||||
inline uint8_t GetCurrentState(){return current_state_;}
|
||||
inline uint8_t GetCurrentGait(){return current_gait_;}
|
||||
inline uint8_t GetLastState(){return last_state_;}
|
||||
inline uint8_t GetLastGait(){return last_gait_;}
|
||||
};
|
||||
@@ -0,0 +1,404 @@
|
||||
/**
|
||||
* @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();
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
@@ -0,0 +1,64 @@
|
||||
/**
|
||||
* @file thread_pool.hpp
|
||||
* @brief thread pool
|
||||
* @author DeepRobotics
|
||||
* @version 1.0
|
||||
* @date 2026-01-07
|
||||
*
|
||||
* @copyright Copyright (c) 2025 DeepRobotics
|
||||
*
|
||||
*/
|
||||
#pragma once
|
||||
#include <atomic>
|
||||
#include <condition_variable>
|
||||
#include <functional>
|
||||
#include <mutex>
|
||||
#include <queue>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
class ThreadPool {
|
||||
public:
|
||||
ThreadPool(size_t numThreads) : stop(false) {
|
||||
for (size_t i = 0; i < numThreads; ++i) {
|
||||
workers.emplace_back([this]() {
|
||||
while (true) {
|
||||
std::function<void()> task;
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(this->queueMutex);
|
||||
this->condition.wait(lock, [this] { return this->stop || !this->tasks.empty(); });
|
||||
if (this->stop && this->tasks.empty()) return;
|
||||
task = std::move(this->tasks.front());
|
||||
this->tasks.pop();
|
||||
}
|
||||
task();
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
~ThreadPool() {
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(queueMutex);
|
||||
stop = true;
|
||||
}
|
||||
condition.notify_all();
|
||||
for (std::thread& worker : workers) worker.join();
|
||||
}
|
||||
|
||||
void enqueue(std::function<void()> task) {
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(queueMutex);
|
||||
tasks.emplace(task);
|
||||
}
|
||||
condition.notify_one();
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<std::thread> workers;
|
||||
std::queue<std::function<void()>> tasks;
|
||||
|
||||
std::mutex queueMutex;
|
||||
std::condition_variable condition;
|
||||
std::atomic<bool> stop;
|
||||
};
|
||||
Reference in New Issue
Block a user