diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h index 6ab4703..790e239 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h @@ -31,6 +31,7 @@ namespace ocs2::legged_robot { std::shared_ptr gait_schedule_ptr_; ModeSequenceTemplate target_gait_; + int last_command_ = 0; bool gait_updated_{false}; bool verbose_{false}; std::vector gait_list_; diff --git a/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp b/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp index 2f97b7f..bc30667 100644 --- a/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp +++ b/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp @@ -42,6 +42,8 @@ namespace ocs2::legged_robot { void GaitManager::getTargetGait() { if (ctrl_component_.control_inputs_.command == 0) return; + if (ctrl_component_.control_inputs_.command == last_command_) return; + last_command_ = ctrl_component_.control_inputs_.command; target_gait_ = gait_list_[ctrl_component_.control_inputs_.command - 1]; RCLCPP_INFO(rclcpp::get_logger("GaitManager"), "Switch to gait: %s", gait_name_list_[ctrl_component_.control_inputs_.command - 1].c_str());