add state switch

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

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;
     }
 }

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 ()

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;
 };
 
 

controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h

@@ -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> > *>

controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h

@@ -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();

controllers/unitree_guide_controller/package.xml

@@ -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>

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;
 }

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"<<std::endl;
 }
 
 void StatePassive::run() {
@@ -38,8 +37,8 @@ void StatePassive::exit() {
 }
 
 FSMStateName StatePassive::checkChange() {
-    // if (controller_->state_name_ == "fixed_stand") {
+    if (ctrlComp_.control_inputs_.get().command == 2) {
-    //     return FSMStateName::FIXEDSTAND;
+        return FSMStateName::FIXEDSTAND;
-    // }
+    }
     return FSMStateName::PASSIVE;
 }

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<std_msgs::msg::String>(
+        control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
-            "state_command", 10, [this](const std_msgs::msg::String::SharedPtr msg) {
+            "/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());
+                ctrl_comp_.control_inputs_.get().command = msg->command;
-                state_name_ = msg->data;
+                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"