From daa72726c6778a5434a4f7142473d83605fed602 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 23 May 2025 14:04:59 -0400 Subject: [PATCH] up --- TODO.md | 6 +- .../modelec_com/pcb_action_interface.hpp | 10 ++++ src/modelec_com/src/pcb_action_interface.cpp | 57 ++++++++++++++++++- .../include/modelec_strat/action_executor.hpp | 2 + .../modelec_strat/navigation_helper.hpp | 2 + .../include/modelec_strat/strat_fms.hpp | 7 ++- src/modelec_strat/src/action_executor.cpp | 19 +++++-- src/modelec_strat/src/navigation_helper.cpp | 8 +++ src/modelec_strat/src/strat_fms.cpp | 51 ++++++++++++----- 9 files changed, 140 insertions(+), 22 deletions(-) diff --git a/TODO.md b/TODO.md index b51f71c..a5d8687 100644 --- a/TODO.md +++ b/TODO.md @@ -1,2 +1,4 @@ -- rework position system with that https://docs.ros2.org/latest/api/geometry_msgs/index-msg.html -- API (check for the middleware already implemented or fill the API package) \ No newline at end of file +- API (check for the middleware already implemented or fill the API package) + + +- Check every value and after that update the max TIME mission \ No newline at end of file diff --git a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp index 1ab53d0..f317ecf 100644 --- a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -60,6 +61,14 @@ namespace Modelec rclcpp::Publisher::SharedPtr servo_move_res_pub_; rclcpp::Publisher::SharedPtr relay_move_res_pub_; + rclcpp::Publisher::SharedPtr tir_start_pub_; + rclcpp::Publisher::SharedPtr tir_arm_pub_; + rclcpp::Publisher::SharedPtr tir_disarm_pub_; + + rclcpp::Subscription::SharedPtr tir_start_sub_; + rclcpp::Subscription::SharedPtr tir_arm_sub_; + rclcpp::Subscription::SharedPtr tir_disarm_sub_; + bool isOk = false; public: @@ -70,5 +79,6 @@ namespace Modelec void GetData(const std::string& elem, const std::vector& data = {}) const; void SendOrder(const std::string& elem, const std::vector& data = {}) const; void SendMove(const std::string& elem, const std::vector& data = {}) const; + void RespondEvent(const std::string& elem, const std::vector& data = {}) const; }; } \ No newline at end of file diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 3ec3792..8de80ac 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -156,6 +156,36 @@ namespace Modelec relay_move_res_pub_ = this->create_publisher( "action/move/relay/res", 10); + tir_start_pub_ = this->create_publisher( + "action/tir/start", 10); + + tir_start_sub_ = this->create_subscription( + "action/tir/start/res", 10, + [this](const std_msgs::msg::Empty::SharedPtr) + { + RespondEvent("TIR", {"START"}); + }); + + tir_arm_pub_ = this->create_publisher( + "action/tir/arm", 10); + + tir_arm_sub_ = this->create_subscription( + "action/tir/arm/res", 10, + [this](const std_msgs::msg::Empty::SharedPtr) + { + RespondEvent("TIR", {"ARM"}); + }); + + tir_disarm_pub_ = this->create_publisher( + "action/tir/disarm", 10); + + tir_disarm_sub_ = this->create_subscription( + "action/tir/disarm/res", 10, + [this](const std_msgs::msg::Empty::SharedPtr) + { + RespondEvent("TIR", {"DIS"}); + }); + // TODO : check for real value there asc_value_mapper_ = { @@ -203,7 +233,6 @@ namespace Modelec } relay_value_ = { - {0, false}, {1, false}, {2, false}, }; @@ -383,6 +412,27 @@ namespace Modelec relay_move_res_pub_->publish(relay_msg); } } + else if (tokens[0] == "EVENT") + { + if (tokens[1] == "TIR") + { + if (tokens[2] == "START") + { + tir_start_pub_->publish(std_msgs::msg::Empty()); + RespondEvent(tokens[1], {tokens[2]}); + } + else if (tokens[2] == "ARM") + { + tir_arm_pub_->publish(std_msgs::msg::Empty()); + RespondEvent(tokens[1], {tokens[2]}); + } + else if (tokens[2] == "DIS") + { + tir_disarm_pub_->publish(std_msgs::msg::Empty()); + RespondEvent(tokens[1], {tokens[2]}); + } + } + } else { RCLCPP_WARN(this->get_logger(), "Unknown message format"); @@ -427,6 +477,11 @@ namespace Modelec { SendToPCB("MOV", elem, data); } + + void PCBActionInterface::RespondEvent(const std::string& elem, const std::vector& data) const + { + SendToPCB("ACK", elem, data); + } } int main(int argc, char** argv) diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 1338b9c..9da96db 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -56,6 +56,8 @@ namespace Modelec void PlacePot(); + void ReInit(); + protected: /* rclcpp::Publisher::SharedPtr asc_get_pub_; diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 1c3f749..3b5dd02 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -26,6 +26,8 @@ namespace Modelec NavigationHelper(const rclcpp::Node::SharedPtr& node); + void ReInit(); + rclcpp::Node::SharedPtr GetNode() const; std::shared_ptr GetPathfinding() const; diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index bc2c05b..1544e7d 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -42,8 +42,12 @@ namespace Modelec void Init(); + void ReInit(); + void Reset(); + void ResetStrat(); + protected: void Transition(State next, const std::string& reason); @@ -61,12 +65,13 @@ namespace Modelec std::shared_ptr nav_; std::shared_ptr action_executor_; - rclcpp::Subscription::SharedPtr tirette_sub_; + rclcpp::Subscription::SharedPtr tir_sub_; rclcpp::Publisher::SharedPtr state_pub_; rclcpp::Publisher::SharedPtr start_time_pub_; rclcpp::Subscription::SharedPtr team_id_sub_; rclcpp::Subscription::SharedPtr spawn_id_sub_; rclcpp::Subscription::SharedPtr reset_strat_sub_; + rclcpp::Subscription::SharedPtr tir_arm_sub_; rclcpp::Client::SharedPtr client_start_; }; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 4cece68..0c805aa 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -120,12 +120,12 @@ namespace Modelec { modelec_interfaces::msg::ActionRelayState relay_top_msg; relay_top_msg.state = true; // TODO : check that - relay_top_msg.id = 0; // TODO : to define + relay_top_msg.id = 2; relay_move_pub_->publish(relay_top_msg); modelec_interfaces::msg::ActionRelayState relay_bottom_msg; relay_bottom_msg.state = true; // TODO : check that - relay_bottom_msg.id = 1; // TODO : to define + relay_bottom_msg.id = 1; relay_move_pub_->publish(relay_bottom_msg); modelec_interfaces::msg::ActionServoPos first_pot_msg; @@ -224,12 +224,12 @@ namespace Modelec { modelec_interfaces::msg::ActionRelayState relay_top_msg; relay_top_msg.state = false; // TODO : check that - relay_top_msg.id = 0; // TODO : to define + relay_top_msg.id = 2; relay_move_pub_->publish(relay_top_msg); modelec_interfaces::msg::ActionRelayState relay_bottom_msg; relay_bottom_msg.state = false; // TODO : check that - relay_bottom_msg.id = 1; // TODO : to define + relay_bottom_msg.id = 1; relay_move_pub_->publish(relay_bottom_msg); modelec_interfaces::msg::ActionServoPos first_pot_msg; @@ -324,4 +324,15 @@ namespace Modelec Update(); } } + + void ActionExecutor::ReInit() + { + action_done_ = true; + step_running_ = 0; + while (!step_.empty()) + { + step_.pop(); + } + action_ = NONE; + } } diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 2e8b6a7..c5c1b55 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -95,6 +95,11 @@ namespace Modelec }); } + void NavigationHelper::ReInit() + { + SetPos(spawn_); + } + rclcpp::Node::SharedPtr NavigationHelper::GetNode() const { return node_; @@ -584,12 +589,15 @@ namespace Modelec { case YELLOW: SetPos(spawn_yellow_[name]); + spawn_ = spawn_yellow_[name]; break; case BLUE: SetPos(spawn_blue_[name]); + spawn_ = spawn_blue_[name]; break; default: SetPos(spawn_yellow_[name]); + spawn_ = spawn_yellow_[name]; break; } } diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index ee68ea3..25c69c7 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -7,10 +7,12 @@ namespace Modelec StratFMS::StratFMS() : Node("start_fms") { - tirette_sub_ = create_subscription( - "/tirette", 10, [this](const std_msgs::msg::Bool::SharedPtr msg) + tir_sub_ = create_subscription( + "/action/tir/start", 10, [this](const std_msgs::msg::Empty::SharedPtr) { - if (setup_ && !started_ && msg->data) + RCLCPP_INFO(get_logger(), "TIR started"); + + if (setup_ && !started_) { started_ = true; } @@ -30,7 +32,6 @@ namespace Modelec spawn_id_sub_ = create_subscription( "/strat/spawn", 10, [this](const modelec_interfaces::msg::Spawn::SharedPtr msg) { - setup_ = true; team_id_ = msg->team_id; nav_->SetTeamId(team_id_); nav_->SetSpawn(msg->name); @@ -43,6 +44,13 @@ namespace Modelec Reset(); }); + tir_arm_sub_ = create_subscription( + "/action/tir/arm", 10, [this](const std_msgs::msg::Empty::SharedPtr) + { + RCLCPP_INFO(get_logger(), "TIR armed"); + setup_ = true; + }); + client_start_ = create_client("/odometry/start"); while (!client_start_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { @@ -74,10 +82,30 @@ namespace Modelec action_executor_ = std::make_unique(shared_from_this()); RCLCPP_INFO(this->get_logger(), "StratFMS fully initialized"); + } + + void StratFMS::ReInit() + { + nav_->ReInit(); + action_executor_->ReInit(); + setup_ = false; + } + + void StratFMS::Reset() + { + ReInit(); + ResetStrat(); + } + + void StratFMS::ResetStrat() + { + if (timer_) + { + timer_->cancel(); + } state_ = State::INIT; started_ = false; - setup_ = false; current_mission_.reset(); match_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); @@ -87,13 +115,6 @@ namespace Modelec }); } - void StratFMS::Reset() - { - timer_->cancel(); - - Init(); - } - void StratFMS::Transition(State next, const std::string& reason) { RCLCPP_INFO(get_logger(), "Transition %d -> %d: %s", static_cast(state_), static_cast(next), @@ -112,8 +133,10 @@ namespace Modelec switch (state_) { case State::INIT: - RCLCPP_INFO_ONCE(get_logger(), "State: INIT"); - Transition(State::WAIT_START, "System ready"); + if (setup_) + { + Transition(State::WAIT_START, "System ready"); + } break; case State::WAIT_START: if (started_)