84 lines
2.1 KiB
C++
84 lines
2.1 KiB
C++
#ifndef ROBOT_CONTROL_SYSTEM_H
|
|
#define ROBOT_CONTROL_SYSTEM_H
|
|
|
|
#include "robot_control/ros/RobotMotionAction.h"
|
|
#include "robot_control/planning/RealtimeInterpolator.h"
|
|
#include "robot_control/state_machine/RobotStateMachine.h"
|
|
#include "robot_control/model/RobotModel.h"
|
|
#include <rclcpp/rclcpp.hpp>
|
|
|
|
namespace robot_control {
|
|
|
|
class RobotControlManager {
|
|
public:
|
|
using Ptr = std::shared_ptr<RobotControlManager>;
|
|
using ConstPtr = std::shared_ptr<const RobotControlManager>;
|
|
|
|
RobotControlManager(rclcpp::Node::SharedPtr node);
|
|
~RobotControlManager() = default;
|
|
|
|
// 初始化系统
|
|
bool initialize(const std::string& upperBodyUrdf, const std::string& lowerBodyUrdf);
|
|
|
|
// 启动系统
|
|
void start();
|
|
|
|
// 停止系统
|
|
void stop();
|
|
|
|
// 更新系统状态
|
|
void update();
|
|
|
|
// 获取当前状态
|
|
state_machine::RobotState getCurrentState() const;
|
|
|
|
// 处理运动目标
|
|
void handleMotionGoal(const ros::MotionGoal& goal);
|
|
|
|
// 处理运动取消
|
|
void handleMotionCancel();
|
|
|
|
private:
|
|
rclcpp::Node::SharedPtr node_;
|
|
|
|
// 模型
|
|
model::UpperBodyModel::Ptr upperBodyModel_;
|
|
model::LowerBodyModel::Ptr lowerBodyModel_;
|
|
|
|
// 控制器
|
|
control::UpperBodyController::Ptr upperBodyController_;
|
|
control::LowerBodyController::Ptr lowerBodyController_;
|
|
|
|
// ROS通信
|
|
ros::RobotMotionAction::Ptr motionAction_;
|
|
|
|
// 实时插补器
|
|
planning::RealtimeInterpolator::Ptr upperBodyInterpolator_;
|
|
planning::RealtimeInterpolator::Ptr lowerBodyInterpolator_;
|
|
|
|
// 状态机
|
|
state_machine::RobotStateMachine::Ptr stateMachine_;
|
|
|
|
// 控制频率
|
|
double controlFrequency_;
|
|
|
|
// 定时器
|
|
rclcpp::TimerBase::SharedPtr controlTimer_;
|
|
|
|
// 状态机回调函数
|
|
void registerStateCallbacks();
|
|
|
|
// 控制回调
|
|
void controlCallback();
|
|
|
|
// 发布反馈
|
|
void publishFeedback();
|
|
|
|
// 处理状态转换
|
|
void onStateTransition(state_machine::RobotState newState);
|
|
};
|
|
|
|
} // namespace robot_control
|
|
|
|
#endif // ROBOT_CONTROL_SYSTEM_H
|