diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index 2110d8e..04ba3fb 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(ament_index_cpp REQUIRED) find_package(modelec_interfaces REQUIRED) find_package(modelec_utils REQUIRED) -add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/mission_manager.cpp src/missions/prepare_concert_mission.cpp src/missions/banner_mission.cpp src/missions/go_home_mission.cpp src/obstacle/obstacle.cpp src/obstacle/column.cpp src/deposite_zone.cpp) +add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/missions/prepare_concert_mission.cpp src/missions/banner_mission.cpp src/missions/go_home_mission.cpp src/obstacle/obstacle.cpp src/obstacle/column.cpp src/deposite_zone.cpp) ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces ament_index_cpp) target_link_libraries(strat_fsm modelec_utils::utils modelec_utils::config) target_include_directories(strat_fsm PUBLIC diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 4b6715c..fee9fda 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -1,6 +1,9 @@ #pragma once #include +#include +#include +#include namespace Modelec { @@ -8,11 +11,64 @@ namespace Modelec class ActionExecutor { public: + + enum Action + { + NONE, + DEPLOY_BANNER, + TAKE_POT, + PLACE_POT, + }; + + enum Step + { + }; + ActionExecutor(); ActionExecutor(const rclcpp::Node::SharedPtr& node); - rclcpp::Node::SharedPtr getNode() const; + rclcpp::Node::SharedPtr GetNode() const; + + bool IsActionDone() const; + + void Update(); + + void DeployBanner(); + + void TakePot(); + + void PlacePot(); + + protected: + rclcpp::Publisher::SharedPtr asc_get_pub_; + rclcpp::Publisher::SharedPtr servo_get_pub_; + rclcpp::Publisher::SharedPtr relay_get_pub_; + + rclcpp::Subscription::SharedPtr asc_get_res_sub_; + rclcpp::Subscription::SharedPtr servo_get_res_sub_; + rclcpp::Subscription::SharedPtr relay_get_res_sub_; + + rclcpp::Publisher::SharedPtr asc_set_pub_; + rclcpp::Publisher::SharedPtr servo_set_pub_; + + rclcpp::Subscription::SharedPtr asc_set_res_sub_; + rclcpp::Subscription::SharedPtr servo_set_res_sub_; + + rclcpp::Publisher::SharedPtr asc_move_pub_; + rclcpp::Publisher::SharedPtr servo_move_pub_; + rclcpp::Publisher::SharedPtr relay_move_pub_; + + rclcpp::Subscription::SharedPtr asc_move_res_sub_; + rclcpp::Subscription::SharedPtr servo_move_res_sub_; + rclcpp::Subscription::SharedPtr relay_move_res_sub_; + + Action action_ = NONE; + + std::queue step_; + + bool action_done_ = true; + bool step_done_ = true; private: rclcpp::Node::SharedPtr node_; diff --git a/src/modelec_strat/include/modelec_strat/mission_manager.hpp b/src/modelec_strat/include/modelec_strat/mission_manager.hpp deleted file mode 100644 index fbb3c38..0000000 --- a/src/modelec_strat/include/modelec_strat/mission_manager.hpp +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once - -#include - -namespace Modelec -{ - - class MissionManager - { - public: - MissionManager(); - - MissionManager(const rclcpp::Node::SharedPtr& node); - - rclcpp::Node::SharedPtr getNode() const; - - private: - rclcpp::Node::SharedPtr node_; - }; - -} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp index 473f6b6..2d00b61 100644 --- a/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -13,7 +14,8 @@ namespace Modelec class BannerMission : public Mission { public: - BannerMission(const std::shared_ptr& nav); + BannerMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor); void start(rclcpp::Node::SharedPtr node) override; void update() override; @@ -30,6 +32,7 @@ namespace Modelec MissionStatus status_; std::shared_ptr nav_; + std::shared_ptr action_executor_; rclcpp::Node::SharedPtr node_; rclcpp::Publisher::SharedPtr score_pub_; int mission_score_ = 0; diff --git a/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp index eb7b59b..464aa02 100644 --- a/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -9,7 +10,8 @@ namespace Modelec { class PrepareConcertMission : public Mission { public: - PrepareConcertMission(const std::shared_ptr& nav); + PrepareConcertMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor); void start(rclcpp::Node::SharedPtr node) override; void update() override; @@ -32,6 +34,7 @@ namespace Modelec { MissionStatus status_; std::shared_ptr nav_; + std::shared_ptr action_executor_; rclcpp::Node::SharedPtr node_; std::shared_ptr column_; diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index 21f54ed..6102638 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -3,7 +3,6 @@ #include #include "action_executor.hpp" -#include "mission_manager.hpp" #include "navigation_helper.hpp" #include "missions/mission_base.hpp" #include "missions/banner_mission.hpp" @@ -59,8 +58,7 @@ namespace Modelec int team_id_ = 0; std::shared_ptr nav_; - std::unique_ptr mission_manager_; - std::unique_ptr action_executor_; + std::shared_ptr action_executor_; rclcpp::Subscription::SharedPtr tirette_sub_; rclcpp::Publisher::SharedPtr state_pub_; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 4f2ebd4..187fb6e 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -8,10 +8,108 @@ namespace Modelec ActionExecutor::ActionExecutor(const rclcpp::Node::SharedPtr& node) : node_(node) { + asc_get_pub_ = node_->create_publisher("/action/get/asc", 10); + servo_get_pub_ = node_->create_publisher("/action/get/servo", 10); + relay_get_pub_ = node_->create_publisher("/action/get/relay", 10); + + asc_get_res_sub_ = node_->create_subscription( + "/action/get/asc/res", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg) + { + }); + + servo_get_res_sub_ = node_->create_subscription( + "/action/get/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg) + { + }); + + relay_get_res_sub_ = node_->create_subscription( + "/action/get/relay/res", 10, [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg) + { + }); + + asc_set_pub_ = node_->create_publisher("/action/set/asc", 10); + servo_set_pub_ = node_->create_publisher("/action/set/servo", 10); + + asc_set_res_sub_ = node_->create_subscription( + "/action/set/asc/res", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg) + { + }); + + servo_set_res_sub_ = node_->create_subscription( + "/action/set/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg) + { + }); + + asc_move_pub_ = node_->create_publisher("/action/move/asc", 10); + servo_move_pub_ = node_->create_publisher("/action/move/servo", 10); + relay_move_pub_ = node_->create_publisher("/action/move/relay", 10); + + asc_move_res_sub_ = node_->create_subscription( + "/action/move/asc/res", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg) + { + step_done_ = true; + Update(); + }); + + servo_move_res_sub_ = node_->create_subscription( + "/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg) + { + step_done_ = true; + Update(); + }); + + relay_move_res_sub_ = node_->create_subscription( + "/action/move/relay/res", 10, [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg) + { + step_done_ = true; + Update(); + }); } - rclcpp::Node::SharedPtr ActionExecutor::getNode() const + rclcpp::Node::SharedPtr ActionExecutor::GetNode() const { return node_; } + + bool ActionExecutor::IsActionDone() const + { + return action_done_; + } + + void ActionExecutor::Update() + { + if (step_.empty()) + { + action_ = NONE; + action_done_ = true; + return; + } + + step_.pop(); + step_done_ = false; + } + + void ActionExecutor::DeployBanner() + { + action_ = DEPLOY_BANNER; + action_done_ = false; + + Update(); + } + + void ActionExecutor::TakePot() + { + action_ = TAKE_POT; + action_done_ = false; + + Update(); + } + + void ActionExecutor::PlacePot() + { + action_ = PLACE_POT; + action_done_ = false; + + Update(); + } } diff --git a/src/modelec_strat/src/mission_manager.cpp b/src/modelec_strat/src/mission_manager.cpp deleted file mode 100644 index cb43054..0000000 --- a/src/modelec_strat/src/mission_manager.cpp +++ /dev/null @@ -1,30 +0,0 @@ -#include - -namespace Modelec -{ - enum class State { - INIT, // Initialisation des capteurs, timers, etc. - WAIT_START, // Attente du feu vert - SELECT_MISSION, // Choix intelligent de la prochaine mission - - RETURN_HOME, // Retour zone de fin - FAILSAFE, // En cas d'erreur bloquante - STOP, // Fin du match - - GO_TO_OBJECTIVE, // Navigation vers la cible - EXECUTE_MISSION, // Exécution de l'action - }; - - MissionManager::MissionManager() - { - } - - MissionManager::MissionManager(const rclcpp::Node::SharedPtr& node) : node_(node) - { - } - - rclcpp::Node::SharedPtr MissionManager::getNode() const - { - return node_; - } -} diff --git a/src/modelec_strat/src/missions/banner_mission.cpp b/src/modelec_strat/src/missions/banner_mission.cpp index 0bbb199..7a77a3b 100644 --- a/src/modelec_strat/src/missions/banner_mission.cpp +++ b/src/modelec_strat/src/missions/banner_mission.cpp @@ -4,8 +4,8 @@ namespace Modelec { - BannerMission::BannerMission(const std::shared_ptr& nav) : step_(GO_TO_FRONT), - status_(MissionStatus::READY), nav_(nav) + BannerMission::BannerMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) : step_(GO_TO_FRONT), + status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { } @@ -30,10 +30,12 @@ namespace Modelec if (!nav_->HasArrived()) return; + if (!action_executor_->IsActionDone()) return; + switch (step_) { case GO_TO_FRONT: - // TODO deploy the banner here + action_executor_->DeployBanner(); step_ = DEPLOY_BANNER; break; diff --git a/src/modelec_strat/src/missions/prepare_concert_mission.cpp b/src/modelec_strat/src/missions/prepare_concert_mission.cpp index ff99799..258327f 100644 --- a/src/modelec_strat/src/missions/prepare_concert_mission.cpp +++ b/src/modelec_strat/src/missions/prepare_concert_mission.cpp @@ -4,8 +4,8 @@ namespace Modelec { - PrepareConcertMission::PrepareConcertMission(const std::shared_ptr& nav) : step_(GO_TO_COLUMN), - status_(MissionStatus::READY), nav_(nav) + PrepareConcertMission::PrepareConcertMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) : step_(GO_TO_COLUMN), + status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { } @@ -64,6 +64,8 @@ namespace Modelec if (!nav_->HasArrived()) return; + if (!action_executor_->IsActionDone()) return; + switch (step_) { case GO_TO_COLUMN: @@ -75,6 +77,7 @@ namespace Modelec step_ = GO_CLOSE_TO_COLUMN; break; case GO_CLOSE_TO_COLUMN: + action_executor_->TakePot(); nav_->GetPathfinding()->RemoveObstacle(column_->id()); step_ = TAKE_COLUMN; @@ -170,6 +173,8 @@ namespace Modelec case GO_CLOSE_TO_PLATFORM: { + action_executor_->PlacePot(); + column_->setX(closestDepoZonePoint_.x); column_->setY(closestDepoZonePoint_.y); column_->setTheta(closestDepoZonePoint_.theta); diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index cf6f259..2f9f06c 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -64,7 +64,6 @@ namespace Modelec void StratFMS::Init() { nav_ = std::make_shared(shared_from_this()); - mission_manager_ = std::make_unique(shared_from_this()); action_executor_ = std::make_unique(shared_from_this()); RCLCPP_INFO(this->get_logger(), "StratFMS fully initialized"); @@ -146,7 +145,7 @@ namespace Modelec case State::DO_PREPARE_CONCERT: if (!current_mission_) { - current_mission_ = std::make_unique(nav_); + current_mission_ = std::make_unique(nav_, action_executor_); current_mission_->start(shared_from_this()); } current_mission_->update(); @@ -171,7 +170,7 @@ namespace Modelec case State::DO_PROMOTION: if (!current_mission_) { - current_mission_ = std::make_unique(nav_); + current_mission_ = std::make_unique(nav_, action_executor_); current_mission_->start(shared_from_this()); } current_mission_->update();