up

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)
-- API (check for the middleware already implemented or fill the API package)
+
+
+- Check every value and after that update the max TIME mission

src/modelec_com/include/modelec_com/pcb_action_interface.hpp

@@ -2,6 +2,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/string.hpp>
+#include <std_msgs/msg/empty.hpp>
 #include <modelec_interfaces/srv/add_serial_listener.hpp>
 #include <modelec_interfaces/msg/action_asc_pos.hpp>
 #include <modelec_interfaces/msg/action_relay_state.hpp>
@@ -60,6 +61,14 @@ namespace Modelec
         rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_res_pub_;
         rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_res_pub_;
 
+        rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr tir_start_pub_;
+        rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr tir_arm_pub_;
+        rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr tir_disarm_pub_;
+
+        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_start_sub_;
+        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_arm_sub_;
+        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_disarm_sub_;
+
 
         bool isOk = false;
     public:
@@ -70,5 +79,6 @@ namespace Modelec
         void GetData(const std::string& elem, const std::vector<std::string>& data = {}) const;
         void SendOrder(const std::string& elem, const std::vector<std::string>& data = {}) const;
         void SendMove(const std::string& elem, const std::vector<std::string>& data = {}) const;
+        void RespondEvent(const std::string& elem, const std::vector<std::string>& data = {}) const;
     };
 }

src/modelec_com/src/pcb_action_interface.cpp

@@ -156,6 +156,36 @@ namespace Modelec
         relay_move_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionRelayState>(
             "action/move/relay/res", 10);
 
+        tir_start_pub_ = this->create_publisher<std_msgs::msg::Empty>(
+            "action/tir/start", 10);
+
+        tir_start_sub_ = this->create_subscription<std_msgs::msg::Empty>(
+            "action/tir/start/res", 10,
+            [this](const std_msgs::msg::Empty::SharedPtr)
+            {
+                RespondEvent("TIR", {"START"});
+            });
+
+        tir_arm_pub_ = this->create_publisher<std_msgs::msg::Empty>(
+            "action/tir/arm", 10);
+
+        tir_arm_sub_ = this->create_subscription<std_msgs::msg::Empty>(
+            "action/tir/arm/res", 10,
+            [this](const std_msgs::msg::Empty::SharedPtr)
+            {
+                RespondEvent("TIR", {"ARM"});
+            });
+
+        tir_disarm_pub_ = this->create_publisher<std_msgs::msg::Empty>(
+            "action/tir/disarm", 10);
+
+        tir_disarm_sub_ = this->create_subscription<std_msgs::msg::Empty>(
+            "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<std::string>& data) const
+    {
+        SendToPCB("ACK", elem, data);
+    }
 }
 
 int main(int argc, char** argv)

src/modelec_strat/include/modelec_strat/action_executor.hpp

@@ -56,6 +56,8 @@ namespace Modelec
 
         void PlacePot();
 
+        void ReInit();
+
     protected:
         /*
         rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_get_pub_;

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<Pathfinding> GetPathfinding() const;

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<NavigationHelper> nav_;
         std::shared_ptr<ActionExecutor> action_executor_;
 
-        rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr tirette_sub_;
+        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_sub_;
         rclcpp::Publisher<modelec_interfaces::msg::StratState>::SharedPtr state_pub_;
         rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr start_time_pub_;
         rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr team_id_sub_;
         rclcpp::Subscription<modelec_interfaces::msg::Spawn>::SharedPtr spawn_id_sub_;
         rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr reset_strat_sub_;
+        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_arm_sub_;
 
         rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;
     };

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

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

src/modelec_strat/src/strat_fms.cpp

@@ -7,10 +7,12 @@ namespace Modelec
 
     StratFMS::StratFMS() : Node("start_fms")
     {
-        tirette_sub_ = create_subscription<std_msgs::msg::Bool>(
+        tir_sub_ = create_subscription<std_msgs::msg::Empty>(
-            "/tirette", 10, [this](const std_msgs::msg::Bool::SharedPtr msg)
+            "/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<modelec_interfaces::msg::Spawn>(
             "/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<std_msgs::msg::Empty>(
+            "/action/tir/arm", 10, [this](const std_msgs::msg::Empty::SharedPtr)
+            {
+                RCLCPP_INFO(get_logger(), "TIR armed");
+                setup_ = true;
+            });
+
         client_start_ = create_client<modelec_interfaces::srv::OdometryStart>("/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<ActionExecutor>(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<int>(state_), static_cast<int>(next),
@@ -112,8 +133,10 @@ namespace Modelec
         switch (state_)
         {
         case State::INIT:
-            RCLCPP_INFO_ONCE(get_logger(), "State: INIT");
+            if (setup_)
-            Transition(State::WAIT_START, "System ready");
+            {
+                Transition(State::WAIT_START, "System ready");
+            }
             break;
         case State::WAIT_START:
             if (started_)