From 79e687c46d3c17ca3916548152c3ee9967e148d8 Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Wed, 28 May 2025 23:25:26 -0400
Subject: [PATCH] init

---
 .../include/modelec_com/pcb_action_interface.hpp  |  2 ++
 src/modelec_com/src/pcb_action_interface.cpp      | 15 ++++++++++-----
 .../include/modelec_strat/strat_fms.hpp           |  1 +
 src/modelec_strat/src/strat_fms.cpp               | 15 +++++++++++----
 4 files changed, 24 insertions(+), 9 deletions(-)

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 0e94436..494ab8e 100644
--- a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp
+++ b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp
@@ -3,6 +3,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <std_msgs/msg/empty.hpp>
+#include <std_msgs/msg/bool.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>
@@ -64,6 +65,7 @@ namespace Modelec
         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_;
+        rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr tir_arm_set_sub_;
 
 
         bool isOk = false;
diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp
index 9d50230..a7fbd80 100644
--- a/src/modelec_com/src/pcb_action_interface.cpp
+++ b/src/modelec_com/src/pcb_action_interface.cpp
@@ -186,6 +186,13 @@ namespace Modelec
                 RespondEvent("TIR", {"DIS"});
             });
 
+        tir_arm_set_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+            "action/tir/arm/set", 10,
+            [this](const std_msgs::msg::Bool::SharedPtr msg)
+            {
+                SendOrder("TIR", {"ARM", msg->data ? "1" : "0"});
+            });
+
 
         // TODO : check for real value there
         asc_value_mapper_ = {
@@ -194,14 +201,14 @@ namespace Modelec
             {2, 200},
             {3, 300}
         };
-        for (auto & [id, v] : asc_value_mapper_)
+        /*for (auto & [id, v] : asc_value_mapper_)
         {
             SendOrder("ASC", {std::to_string(id), std::to_string(v)});
-        }
+        }*/
 
         asc_state_ = 3;
 
-        SendMove("ASC", {std::to_string(asc_state_)});
+        // SendMove("ASC", {std::to_string(asc_state_)});
 
         servo_pos_mapper_ = {
             {0, {{0, M_PI_2}}},
@@ -244,8 +251,6 @@ namespace Modelec
         {
             SendMove("RELAY" + std::to_string(id), {std::to_string(v)});
         }
-
-        SendOrder("TIR", {"ARM", "1"});
     }
 
     PCBActionInterface::~PCBActionInterface()
diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp
index da2594a..d564e42 100644
--- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp
+++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp
@@ -74,6 +74,7 @@ namespace Modelec
         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::Publisher<std_msgs::msg::Bool>::SharedPtr tir_arm_set_pub_;
 
         rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr start_odo_pub_;
     };
diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp
index 3d10d2c..9026c2b 100644
--- a/src/modelec_strat/src/strat_fms.cpp
+++ b/src/modelec_strat/src/strat_fms.cpp
@@ -30,6 +30,14 @@ namespace Modelec
         spawn_id_sub_ = create_subscription<modelec_interfaces::msg::Spawn>(
             "/strat/spawn", 10, [this](const modelec_interfaces::msg::Spawn::SharedPtr msg)
             {
+                std_msgs::msg::Bool start_odo_msg;
+                start_odo_msg.data = true;
+                start_odo_pub_->publish(start_odo_msg);
+
+                std_msgs::msg::Bool tir_msg;
+                tir_msg.data = true;
+                tir_arm_set_pub_->publish(tir_msg);
+
                 team_selected_ = true;
                 team_id_ = msg->team_id;
                 nav_->SetTeamId(team_id_);
@@ -48,6 +56,9 @@ namespace Modelec
                 setup_ = true;
             });
 
+        tir_arm_set_pub_ = create_publisher<std_msgs::msg::Bool>(
+            "/action/tir/arm/set", 10);
+
         start_odo_pub_ = create_publisher<std_msgs::msg::Bool>("/odometry/start", 10);
 
         std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
@@ -118,10 +129,6 @@ namespace Modelec
         case State::INIT:
             if (team_selected_)
             {
-                std_msgs::msg::Bool start_odo_msg;
-                start_odo_msg.data = true;
-                start_odo_pub_->publish(start_odo_msg);
-
                 Transition(State::WAIT_START, "System ready");
             }
             break;