EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add state switch

This commit is contained in:
Huang Zhenbiao 2024-09-11 14:01:07 +08:00
parent a494a4e854
commit 26ff7f4974
10 changed files with 91 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
commands/keyboard_input/readme.md
View File

@ -2,6 +2,16 @@
This node will read the keyboard input and publish a control_input_msgs/Input message.
```bash
cd ~/ros2_ws
colcon build --packages-up-to keyboard_input
```
```bash
source ~/ros2_ws/install/setup.bash
ros2 run keyboard_input keyboard_input
```
## 1. Running Instructions
### 1.1 Control Mode
* Passive Mode: Keyboard 1

8
commands/keyboard_input/src/KeyboardInput.cpp
View File

@ -20,7 +20,7 @@ void Keyboardinput::timer_callback() {
if (kbhit()) {
char key = getchar();
check_command(key);
if (inputs_.command == -1) check_value(key);
if (inputs_.command == 0) check_value(key);
publisher_->publish(inputs_);
}
}
@ -40,7 +40,7 @@ void Keyboardinput::check_command(const char key) {
inputs_.command = 4; // START
break;
case '0':
inputs_.command = 0; // L1_X
inputs_.command = 10; // L1_X
break;
case '9':
inputs_.command = 9; // L1_A
@ -53,10 +53,10 @@ void Keyboardinput::check_command(const char key) {
inputs_.ly = 0;
inputs_.rx = 0;
inputs_.ry = 0;
inputs_.command = -1;
inputs_.command = 0;
break;
default:
inputs_.command = -1;
inputs_.command = 0;
break;
}
}

2
controllers/unitree_guide_controller/CMakeLists.txt
View File

@ -13,11 +13,11 @@ set(CONTROLLER_INCLUDE_DEPENDS
controller_interface
realtime_tools
std_msgs
control_input_msgs
)
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(controller_manager 4.0.0)
foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach ()

16
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h
View File

@ -18,6 +18,22 @@ public:
void exit() override;
FSMStateName checkChange() override;
private:
double target_pos_[12] = {
0.00571868, 0.608813, -1.21763, -0.00571868,
0.608813, -1.21763, 0.00571868, 0.608813,
-1.21763, -0.00571868, 0.608813, -1.21763
};
double start_pos_[12] = {
0.0473455, 1.22187, -2.44375, -0.0473455,
1.22187, -2.44375, 0.0473455, 1.22187,
-2.44375, -0.0473455, 1.22187, -2.44375
};
float duration_ = 600; // steps
double percent_ = 0; //%
double phase = 0.0;
};

3
controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h
View File

@ -69,7 +69,7 @@ namespace unitree_guide_controller {
std::vector<std::string> command_interface_types_;
std::vector<std::string> state_interface_types_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr state_command_subscriber_;
rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
std::unordered_map<
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
@ -87,6 +87,7 @@ namespace unitree_guide_controller {
FSMStateList state_list_;
std::shared_ptr<FSMState> current_state_;
std::shared_ptr<FSMState> next_state_;
std::shared_ptr<FSMState> getNextState(FSMStateName stateName) const;
std::unordered_map<
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *>

7
controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h
View File

@ -8,6 +8,7 @@
#include <vector>
#include <hardware_interface/loaned_command_interface.hpp>
#include <hardware_interface/loaned_state_interface.hpp>
#include <control_input_msgs/msg/inputs.hpp>
struct CtrlComponent {
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
@ -29,7 +30,11 @@ struct CtrlComponent {
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_velocity_state_interface_;
public:
control_input_msgs::msg::Inputs default_inputs_;
std::reference_wrapper<control_input_msgs::msg::Inputs> control_inputs_;
CtrlComponent() : control_inputs_(default_inputs_) {}
void clear() {
joint_effort_command_interface_.clear();
joint_position_command_interface_.clear();

1
controllers/unitree_guide_controller/package.xml
View File

@ -12,6 +12,7 @@
<depend>backward_ros</depend>
<depend>controller_interface</depend>
<depend>pluginlib</depend>
<depend>control_input_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

6
controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
View File

@ -9,6 +9,9 @@ StateFixedStand::StateFixedStand(CtrlComponent ctrlComp): FSMState(
}
void StateFixedStand::enter() {
for (int i = 0; i < 12; i++) {
ctrlComp_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
}
}
void StateFixedStand::run() {
@ -18,5 +21,8 @@ void StateFixedStand::exit() {
}
FSMStateName StateFixedStand::checkChange() {
if (ctrlComp_.control_inputs_.get().command == 1) {
return FSMStateName::PASSIVE;
}
return FSMStateName::FIXEDSTAND;
}

7
controllers/unitree_guide_controller/src/FSM/StatePassive.cpp
View File

@ -28,7 +28,6 @@ void StatePassive::enter() {
for (auto i: ctrlComp_.joint_kd_command_interface_) {
i.get().set_value(3.5);
}
std::cout<<"passive"<<std::endl;
}
void StatePassive::run() {
@ -38,8 +37,8 @@ void StatePassive::exit() {
}
FSMStateName StatePassive::checkChange() {
// if (controller_->state_name_ == "fixed_stand") {
// return FSMStateName::FIXEDSTAND;
// }
if (ctrlComp_.control_inputs_.get().command == 2) {
return FSMStateName::FIXEDSTAND;
}
return FSMStateName::PASSIVE;
}

46
controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
View File

@ -38,6 +38,20 @@ namespace unitree_guide_controller {
update(const rclcpp::Time &time, const rclcpp::Duration &period) {
if (mode_ == FSMMode::NORMAL) {
current_state_->run();
next_state_name_ = current_state_->checkChange();
if (next_state_name_ != current_state_->state_name) {
mode_ = FSMMode::CHANGE;
next_state_ = getNextState(next_state_name_);
RCLCPP_INFO(get_node()->get_logger(), "Switched from %s to %s",
current_state_->state_name_string.c_str(), next_state_->state_name_string.c_str());
}
} else if (mode_ == FSMMode::CHANGE) {
current_state_->exit();
current_state_ = next_state_;
current_state_->enter();
mode_ = FSMMode::NORMAL;
current_state_->run();
}
return controller_interface::return_type::OK;
@ -62,11 +76,14 @@ namespace unitree_guide_controller {
controller_interface::CallbackReturn UnitreeGuideController::on_configure(
const rclcpp_lifecycle::State &previous_state) {
state_command_subscriber_ = get_node()->create_subscription<std_msgs::msg::String>(
"state_command", 10, [this](const std_msgs::msg::String::SharedPtr msg) {
control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
"/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
// Handle message
RCLCPP_INFO(get_node()->get_logger(), "Switch State: %s", msg->data.c_str());
state_name_ = msg->data;
ctrl_comp_.control_inputs_.get().command = msg->command;
ctrl_comp_.control_inputs_.get().lx = msg->lx;
ctrl_comp_.control_inputs_.get().ly = msg->ly;
ctrl_comp_.control_inputs_.get().rx = msg->rx;
ctrl_comp_.control_inputs_.get().ry = msg->ry;
});
return CallbackReturn::SUCCESS;
}
@ -119,6 +136,27 @@ namespace unitree_guide_controller {
UnitreeGuideController::on_shutdown(const rclcpp_lifecycle::State &previous_state) {
return CallbackReturn::SUCCESS;
}
std::shared_ptr<FSMState> UnitreeGuideController::getNextState(FSMStateName stateName) const {
switch (stateName) {
case FSMStateName::INVALID:
return state_list_.invalid;
case FSMStateName::PASSIVE:
return state_list_.passive;
case FSMStateName::FIXEDSTAND:
return state_list_.fixedStand;
// case FSMStateName::FREESTAND:
// return state_list_.freeStand;
// case FSMStateName::TROTTING:
// return state_list_.trotting;
// case FSMStateName::SWINGTEST:
// return state_list_.swingTest;
// case FSMStateName::BALANCETEST:
// return state_list_.balanceTest;
default:
return state_list_.invalid;
}
}
}
#include "pluginlib/class_list_macros.hpp"