init

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;

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()

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

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;