135 lines
4.4 KiB
C++
135 lines
4.4 KiB
C++
/**
|
|
* @file quadruped_state_machine.hpp
|
|
* @brief for robot to switch control state by user command input
|
|
* @author DeepRobotics
|
|
* @version 1.0
|
|
* @date 2025-11-07
|
|
*
|
|
* @copyright Copyright (c) 2025 DeepRobotics
|
|
*
|
|
*/
|
|
#pragma once
|
|
|
|
#include "state_base.h"
|
|
#include "state_machine_base.h"
|
|
#include "quadruped_wheel/idle_state.hpp"
|
|
#include "quadruped_wheel/standup_state.hpp"
|
|
#include "quadruped_wheel/joint_damping_state.hpp"
|
|
#include "quadruped_wheel/rl_control_state.hpp"
|
|
#include "quadruped_wheel/liedown_state.hpp"
|
|
#include "keyboard_interface.hpp"
|
|
#include "hardware/m20_interface.hpp"
|
|
#include "hardware/fire_ant_interface.hpp"
|
|
#include "udp_server.hpp"
|
|
|
|
namespace qw{
|
|
class QwStateMachine : public StateMachineBase{
|
|
private:
|
|
|
|
std::shared_ptr<StateBase> idle_controller_;
|
|
std::shared_ptr<StateBase> standup_controller_;
|
|
std::shared_ptr<StateBase> rl_controller_;
|
|
std::shared_ptr<StateBase> joint_damping_controller_;
|
|
std::shared_ptr<StateBase> car_move_controller_;
|
|
std::shared_ptr<StateBase> liedown_controller_;
|
|
|
|
std::shared_ptr<UdpServer> udp_server_;
|
|
|
|
public:
|
|
const RobotName robot_name_;
|
|
const RemoteCommandType remote_cmd_type_;
|
|
|
|
QwStateMachine(RobotName robot_name, RemoteCommandType rct):StateMachineBase(RobotType::QuadrupedWheel),
|
|
robot_name_(robot_name),
|
|
remote_cmd_type_(rct) {}
|
|
~QwStateMachine(){}
|
|
|
|
void Start(){
|
|
if(remote_cmd_type_ == RemoteCommandType::kKeyBoard){
|
|
uc_ptr_ = std::make_shared<KeyboardInterface>(robot_name_);
|
|
}else if(remote_cmd_type_ == RemoteCommandType::kGamepad){
|
|
auto gp_ptr = std::make_shared<GamepadInterface>(robot_name_);
|
|
udp_server_ = std::make_shared<UdpServer>(gp_ptr.get());
|
|
uc_ptr_ = gp_ptr;
|
|
}else{
|
|
std::cerr << "error user command interface! " << std::endl;
|
|
exit(0);
|
|
}
|
|
uc_ptr_->SetMotionStateFeedback(&StateBase::msfb_);
|
|
|
|
if(robot_name_ == RobotName::M20){
|
|
|
|
ri_ptr_ = std::make_shared<M20Interface>("M20");
|
|
|
|
cp_ptr_ = std::make_shared<ControlParameters>(robot_name_);
|
|
} else if (robot_name_ == RobotName::FireAnt) {
|
|
ri_ptr_ = std::make_shared<FireAntInterface>("FireAnt");
|
|
cp_ptr_ = std::make_shared<ControlParameters>(robot_name_);
|
|
} else {
|
|
throw std::runtime_error("Unsupported robot type");
|
|
}
|
|
|
|
std::shared_ptr<ControllerData> data_ptr = std::make_shared<ControllerData>();
|
|
data_ptr->ri_ptr = ri_ptr_;
|
|
data_ptr->uc_ptr = uc_ptr_;
|
|
data_ptr->cp_ptr = cp_ptr_;
|
|
|
|
sc_ptr_ = std::make_shared<SafeController>(QuadrupedWheel, "");
|
|
sc_ptr_->SetRobotDataSource(ri_ptr_);
|
|
sc_ptr_->SetUserCommandDataSource(uc_ptr_);
|
|
|
|
idle_controller_ = std::make_shared<IdleState>(robot_name_, "idle_state", data_ptr);
|
|
standup_controller_ = std::make_shared<StandUpState>(robot_name_, "standup_state", data_ptr);
|
|
rl_controller_ = std::make_shared<RLControlState>(robot_name_, "rl_control", data_ptr);
|
|
joint_damping_controller_ = std::make_shared<JointDampingState>(robot_name_, "joint_damping", data_ptr);
|
|
liedown_controller_ = std::make_shared<LieDownState>(robot_name_, "liedown_state", data_ptr);
|
|
|
|
current_controller_ = idle_controller_;
|
|
current_state_name_ = kIdle;
|
|
next_state_name_ = kIdle;
|
|
|
|
ri_ptr_->Start();
|
|
uc_ptr_->Start();
|
|
sc_ptr_->Start();
|
|
current_controller_->OnEnter();
|
|
}
|
|
|
|
|
|
std::shared_ptr<StateBase> GetStateControllerPtr(StateName state_name){
|
|
switch(state_name){
|
|
case StateName::kInvalid:{
|
|
return nullptr;
|
|
}
|
|
case StateName::kIdle:{
|
|
return idle_controller_;
|
|
}
|
|
case StateName::kStandUp:{
|
|
return standup_controller_;
|
|
}
|
|
case StateName::kRLControl:{
|
|
return rl_controller_;
|
|
}
|
|
case StateName::kJointDamping:{
|
|
return joint_damping_controller_;
|
|
}
|
|
case StateName::kLieDown:
|
|
{
|
|
return liedown_controller_;
|
|
}
|
|
default:{
|
|
std::cerr << "error state name" << std::endl;
|
|
return joint_damping_controller_;
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
void Stop(){
|
|
sc_ptr_->Stop();
|
|
uc_ptr_->Stop();
|
|
ri_ptr_->Stop();
|
|
}
|
|
|
|
};
|
|
};
|