From 26ff7f4974c515f11a98b0be749bfcab04218ad3 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Wed, 11 Sep 2024 14:01:07 +0800 Subject: [PATCH] add state switch --- commands/keyboard_input/readme.md | 10 ++++ commands/keyboard_input/src/KeyboardInput.cpp | 8 ++-- .../unitree_guide_controller/CMakeLists.txt | 2 +- .../FSM/StateFixedStand.h | 16 +++++++ .../UnitreeGuideController.h | 3 +- .../common/interface.h | 7 ++- .../unitree_guide_controller/package.xml | 1 + .../src/FSM/StateFixedStand.cpp | 6 +++ .../src/FSM/StatePassive.cpp | 7 ++- .../src/UnitreeGuideController.cpp | 46 +++++++++++++++++-- 10 files changed, 91 insertions(+), 15 deletions(-) diff --git a/commands/keyboard_input/readme.md b/commands/keyboard_input/readme.md index f46cb95..adaaf03 100644 --- a/commands/keyboard_input/readme.md +++ b/commands/keyboard_input/readme.md @@ -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 diff --git a/commands/keyboard_input/src/KeyboardInput.cpp b/commands/keyboard_input/src/KeyboardInput.cpp index e24eb40..25d02c8 100644 --- a/commands/keyboard_input/src/KeyboardInput.cpp +++ b/commands/keyboard_input/src/KeyboardInput.cpp @@ -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; } } diff --git a/controllers/unitree_guide_controller/CMakeLists.txt b/controllers/unitree_guide_controller/CMakeLists.txt index b58e3ca..0587f8a 100644 --- a/controllers/unitree_guide_controller/CMakeLists.txt +++ b/controllers/unitree_guide_controller/CMakeLists.txt @@ -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 () diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h index d3e0a01..86e550e 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h @@ -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; }; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h index 9b0b9ff..9516d4c 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h @@ -69,7 +69,7 @@ namespace unitree_guide_controller { std::vector command_interface_types_; std::vector state_interface_types_; - rclcpp::Subscription::SharedPtr state_command_subscriber_; + rclcpp::Subscription::SharedPtr control_input_subscription_; std::unordered_map< std::string, std::vector > *> @@ -87,6 +87,7 @@ namespace unitree_guide_controller { FSMStateList state_list_; std::shared_ptr current_state_; std::shared_ptr next_state_; + std::shared_ptr getNextState(FSMStateName stateName) const; std::unordered_map< std::string, std::vector > *> diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h index 9142ca8..22eed94 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h @@ -8,6 +8,7 @@ #include #include #include +#include struct CtrlComponent { std::vector > @@ -29,7 +30,11 @@ struct CtrlComponent { std::vector > joint_velocity_state_interface_; -public: + control_input_msgs::msg::Inputs default_inputs_; + std::reference_wrapper control_inputs_; + + CtrlComponent() : control_inputs_(default_inputs_) {} + void clear() { joint_effort_command_interface_.clear(); joint_position_command_interface_.clear(); diff --git a/controllers/unitree_guide_controller/package.xml b/controllers/unitree_guide_controller/package.xml index bcb8b1e..8ff9774 100644 --- a/controllers/unitree_guide_controller/package.xml +++ b/controllers/unitree_guide_controller/package.xml @@ -12,6 +12,7 @@ backward_ros controller_interface pluginlib + control_input_msgs ament_lint_auto ament_lint_common diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp index 36a3481..5c02a90 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp @@ -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; } diff --git a/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp b/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp index ec8f12c..f0f870a 100644 --- a/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp @@ -28,7 +28,6 @@ void StatePassive::enter() { for (auto i: ctrlComp_.joint_kd_command_interface_) { i.get().set_value(3.5); } - std::cout<<"passive"<state_name_ == "fixed_stand") { - // return FSMStateName::FIXEDSTAND; - // } + if (ctrlComp_.control_inputs_.get().command == 2) { + return FSMStateName::FIXEDSTAND; + } return FSMStateName::PASSIVE; } diff --git a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp index ffad0ee..2d4b7d3 100644 --- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp +++ b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp @@ -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( - "state_command", 10, [this](const std_msgs::msg::String::SharedPtr msg) { + control_input_subscription_ = get_node()->create_subscription( + "/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 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"