Merge pull request #9 from modelec/develop-new-comm

New serial communication

simulated_pcb/action.py

@@ -133,7 +133,7 @@ if __name__ == '__main__':
             sim.stop()
         print("Simulation stopped")
 
-# socat -d -d pty,raw,echo=0,link=/tmp/MARCEL_ACTION_SIM pty,raw,echo=0,link=/tmp/MARCEL_ACTION
+# socat -d -d pty,raw,echo=0,link=/tmp/ACTION_SIM pty,raw,echo=0,link=/tmp/ACTION_USB
-# python3 simulated_pcb/action.py --port /tmp/MARCEL_ACTION_SIM
+# python3 simulated_pcb/action.py --port /tmp/ACTION_SIM
-# socat -d -d pty,raw,echo=0,link=/tmp/ENEMY_ACTION_SIM pty,raw,echo=0,link=/tmp/ENEMY_ACTION
+# socat -d -d pty,raw,echo=0,link=/tmp/ODO_SIM pty,raw,echo=0,link=/tmp/ODO_USB
-# python3 simulated_pcb/action.py --port /tmp/ENEMY_ACTION_SIM
+# python3 simulated_pcb/action.py --port /tmp/ODO_SIM

src/modelec_com/CMakeLists.txt

@@ -35,6 +35,24 @@ target_include_directories(pcb_odo_interface PUBLIC
         $<INSTALL_INTERFACE:include>
 )
 
+# PCB Odometry Node
+add_executable(pcb_odo_interface_new src/pcb_odo_interface.new.cpp src/serial_listener.cpp)
+ament_target_dependencies(pcb_odo_interface_new rclcpp std_msgs modelec_interfaces ament_index_cpp)
+target_link_libraries(pcb_odo_interface_new modelec_utils::utils modelec_utils::config)
+target_include_directories(pcb_odo_interface_new PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
+# PCB Action Node
+add_executable(pcb_action_interface_new src/pcb_action_interface.new.cpp src/serial_listener.cpp)
+ament_target_dependencies(pcb_action_interface_new rclcpp std_msgs modelec_interfaces ament_index_cpp)
+target_link_libraries(pcb_action_interface_new modelec_utils::utils modelec_utils::config)
+target_include_directories(pcb_action_interface_new PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
 # PCB Alim Node
 add_executable(pcb_alim_interface src/pcb_alim_interface.cpp)
 ament_target_dependencies(pcb_alim_interface rclcpp std_msgs modelec_interfaces ament_index_cpp)
@@ -67,7 +85,9 @@ install(TARGETS
         serial_listener
         pcb_alim_interface
         pcb_odo_interface
+        pcb_odo_interface_new
         pcb_action_interface
+        pcb_action_interface_new
         DESTINATION lib/${PROJECT_NAME}
 )
 

src/modelec_com/include/modelec_com/pcb_action_interface.new.hpp

@@ -0,0 +1,76 @@
+#pragma once
+
+#include <modelec_com/serial_listener.hpp>
+
+#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>
+#include <modelec_interfaces/msg/action_servo_pos.hpp>
+
+namespace Modelec
+{
+    class PCBActionInterface : public rclcpp::Node, public SerialListener
+    {
+    public:
+
+        PCBActionInterface();
+
+        ~PCBActionInterface() override;
+
+    protected:
+        std::map<int, int> asc_value_mapper_;
+        std::map<int, std::map<int, double>> servo_pos_mapper_;
+
+        int asc_state_ = 0;
+        std::map<int, int> servo_value_;
+        std::map<int, bool> relay_value_;
+
+    private:
+        void read(const std::string& msg) override;
+
+        rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_get_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_get_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_get_sub_;
+
+        rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_get_res_pub_;
+        rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_get_res_pub_;
+        rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_get_res_pub_;
+
+        rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_set_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_set_sub_;
+
+        rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_set_res_pub_;
+        rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_set_res_pub_;
+
+        rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_sub_;
+
+        rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_res_pub_;
+        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_;
+        rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr tir_arm_set_sub_;
+
+    public:
+        void SendToPCB(const std::string& data);
+        void SendToPCB(const std::string& order, const std::string& elem,
+                       const std::vector<std::string>& data = {});
+
+        void GetData(const std::string& elem, const std::vector<std::string>& data = {});
+        void SendOrder(const std::string& elem, const std::vector<std::string>& data = {});
+        void SendMove(const std::string& elem, const std::vector<std::string>& data = {});
+        void RespondEvent(const std::string& elem, const std::vector<std::string>& data = {});
+    };
+}

src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp

@@ -0,0 +1,108 @@
+#pragma once
+
+#include <modelec_com/serial_listener.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <queue>
+#include <mutex>
+#include <future>
+
+#include <std_msgs/msg/string.hpp>
+
+#include <modelec_interfaces/msg/odometry_pos.hpp>
+#include <modelec_interfaces/msg/odometry_speed.hpp>
+#include <modelec_interfaces/msg/odometry_to_f.hpp>
+#include <modelec_interfaces/msg/odometry_waypoint.hpp>
+#include <modelec_interfaces/msg/odometry_waypoints.hpp>
+#include <modelec_interfaces/msg/odometry_start.hpp>
+#include <modelec_interfaces/msg/odometry_pid.hpp>
+
+#include <modelec_interfaces/srv/odometry_position.hpp>
+#include <modelec_interfaces/srv/odometry_speed.hpp>
+#include <modelec_interfaces/srv/odometry_to_f.hpp>
+#include <modelec_interfaces/srv/add_serial_listener.hpp>
+#include <modelec_interfaces/srv/odometry_start.hpp>
+#include <modelec_interfaces/srv/odometry_get_pid.hpp>
+#include <modelec_interfaces/srv/odometry_set_pid.hpp>
+#include <modelec_interfaces/srv/odometry_add_waypoint.hpp>
+
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/empty.hpp>
+
+namespace Modelec
+{
+    class PCBOdoInterface : public rclcpp::Node, public SerialListener
+    {
+    public:
+        PCBOdoInterface();
+
+        ~PCBOdoInterface() override;
+
+        struct OdometryData
+        {
+            long x;
+            long y;
+            double theta;
+        };
+
+        struct PIDData
+        {
+            float p;
+            float i;
+            float d;
+        };
+
+    private:
+        void read(const std::string& msg) override;
+
+        rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_pos_publisher_;
+        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr odo_get_pos_sub_;
+        rclcpp::Publisher<modelec_interfaces::msg::OdometrySpeed>::SharedPtr odo_speed_publisher_;
+        rclcpp::Publisher<modelec_interfaces::msg::OdometryToF>::SharedPtr odo_tof_publisher_;
+        rclcpp::Publisher<modelec_interfaces::msg::OdometryWaypoint>::SharedPtr odo_waypoint_reach_publisher_;
+        rclcpp::Publisher<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_pid_publisher_;
+
+        rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoint>::SharedPtr odo_add_waypoint_subscriber_;
+        rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoints>::SharedPtr odo_add_waypoints_subscriber_;
+        rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_set_pos_subscriber_;
+        rclcpp::Subscription<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_set_pid_subscriber_;
+
+        void AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg);
+        void AddWaypointsCallback(const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg);
+        void SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
+        void SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg);
+
+        rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr start_odo_sub_;
+        bool start_odo_ = false;
+
+        int timeout_ms = 1000;
+        int attempt = 5;
+
+    public:
+        void SendToPCB(const std::string& data);
+        void SendToPCB(const std::string& order, const std::string& elem,
+                       const std::vector<std::string>& data = {});
+
+        void GetData(const std::string& elem, const std::vector<std::string>& data = {});
+        void SendOrder(const std::string& elem, const std::vector<std::string>& data = {});
+
+        void GetPos();
+        void GetSpeed();
+        void GetToF(const int& tof);
+
+        void SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
+        void SetRobotPos(long x, long y, double theta);
+
+        void AddWaypoints(modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg);
+        void AddWaypoint(modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg);
+        void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta);
+
+        void SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg);
+        void SetStart(bool start);
+
+        void GetPID();
+        void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg);
+        void SetPID(std::string name, float p, float i, float d, std::optional<float> min = std::nullopt, std::optional<float> max = std::nullopt);
+    };
+} // namespace Modelec

src/modelec_com/include/modelec_com/serial_listener.hpp

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <boost/asio.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <modelec_interfaces/srv/add_serial_listener.hpp>
+#include <deque>
+#include <thread>
+#include <mutex>
+
+#define MAX_MESSAGE_LEN 1048
+
+namespace Modelec
+{
+    class SerialListener
+    {
+    protected:
+        bool status_;
+        int bauds_;
+        std::string serial_port_;
+        int max_message_len_;
+
+        boost::asio::io_service io_;
+        std::vector<char> read_buffer_;
+        std::deque<std::string> write_queue_;
+        std::mutex write_mutex_;
+        std::thread io_thread_;
+
+        void start_async_read();
+        void start_async_write();
+
+    public:
+        std::string name_;
+        boost::asio::serial_port port_;
+
+        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
+
+        SerialListener();
+
+        SerialListener(const std::string& name, int bauds, const std::string& serial_port,
+                       int max_message_len);
+
+        virtual ~SerialListener();
+
+        void close();
+        void open(const std::string& name, int bauds, const std::string& serial_port,
+                       int max_message_len);
+
+        void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; }
+        bool IsOk() const { return status_; }
+
+        void SetOk() { status_ = true; }
+
+        virtual void write(const std::string& msg);
+
+        virtual void read(const std::string& msg);
+    };
+} // namespace Modelec

src/modelec_com/src/pcb_action_interface.new.cpp

@@ -0,0 +1,461 @@
+#include <modelec_com/pcb_action_interface.new.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <modelec_utils/config.hpp>
+#include <modelec_utils/utils.hpp>
+
+namespace Modelec
+{
+    PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface"), SerialListener()
+    {
+        // Service to create a new serial listener
+        declare_parameter<std::string>("serial_port", "/dev/USB_ACTION");
+        declare_parameter<int>("baudrate", 115200);
+        declare_parameter<std::string>("name", "pcb_action");
+
+        auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
+        request->name = get_parameter("name").as_string();
+        request->bauds = get_parameter("baudrate").as_int();
+        request->serial_port = get_parameter("serial_port").as_string();
+
+        this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN);
+
+        asc_get_sub_ = this->create_subscription<modelec_interfaces::msg::ActionAscPos>(
+            "action/get/asc", 10,
+            [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr)
+            {
+                GetData("ASC", {"POS"});
+            });
+
+        servo_get_sub_ = this->create_subscription<modelec_interfaces::msg::ActionServoPos>(
+            "action/get/servo", 10,
+            [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg)
+            {
+                GetData("SERVO" + std::to_string(msg->id), {"POS"});
+            });
+
+        relay_get_sub_ = this->create_subscription<modelec_interfaces::msg::ActionRelayState>(
+            "action/get/relay", 10,
+            [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg)
+            {
+                GetData("RELAY" + std::to_string(msg->id), {"STATE"});
+            });
+
+        asc_get_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionAscPos>(
+            "action/get/asc/res", 10);
+        servo_get_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionServoPos>(
+            "action/get/servo/res", 10);
+        relay_get_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionRelayState>(
+            "action/get/relay/res", 10);
+
+        asc_set_sub_ = this->create_subscription<modelec_interfaces::msg::ActionAscPos>(
+            "action/set/asc", 10,
+            [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg)
+            {
+                SendOrder("ASC", {std::to_string(msg->pos), std::to_string(msg->value)});
+            });
+
+        servo_set_sub_ = this->create_subscription<modelec_interfaces::msg::ActionServoPos>(
+            "action/set/servo", 10,
+            [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg)
+            {
+                SendOrder("SERVO" + std::to_string(msg->id), {
+                              "POS" + std::to_string(msg->pos), std::to_string(static_cast<int>(msg->angle * 100))
+                          });
+            });
+
+        asc_set_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionAscPos>(
+            "action/set/asc/res", 10);
+
+        servo_set_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionServoPos>(
+            "action/set/servo/res", 10);
+
+        asc_move_sub_ = this->create_subscription<modelec_interfaces::msg::ActionAscPos>(
+            "action/move/asc", 10,
+            [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg)
+            {
+                SendMove("ASC", {std::to_string(msg->pos)});
+            });
+
+        servo_move_sub_ = this->create_subscription<modelec_interfaces::msg::ActionServoPos>(
+            "action/move/servo", 10,
+            [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg)
+            {
+                SendMove("SERVO" + std::to_string(msg->id), {"POS" + std::to_string(msg->pos)});
+            });
+
+        relay_move_sub_ = this->create_subscription<modelec_interfaces::msg::ActionRelayState>(
+            "action/move/relay", 10,
+            [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg)
+            {
+                SendMove("RELAY" + std::to_string(msg->id), {std::to_string(msg->state)});
+            });
+
+        asc_move_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionAscPos>(
+            "action/move/asc/res", 10);
+
+        servo_move_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionServoPos>(
+            "action/move/servo/res", 10);
+
+        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"});
+            });
+
+        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_ = {
+            {0, 0},
+            {1, 100},
+            {2, 200},
+            {3, 300}
+        };
+        /*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_)});
+
+        servo_pos_mapper_ = {
+            {0, {{0, 0.55}, {1, 0}}},
+            {1, {{0, 0}, {1, 0.4}}},
+            {2, {{0, M_PI_2}}},
+            {3, {{0, M_PI_2}}},
+            {4, {{0, 1.25}, {1, 0.45}}},
+            {5, {{0, 0}, {1, M_PI}}},
+        };
+
+        for (auto & [id, v] : servo_pos_mapper_)
+        {
+            if (id == 5) continue;
+
+            for (auto & [key, angle] : v)
+            {
+                rclcpp::sleep_for(std::chrono::milliseconds(100));
+
+                SendOrder("SERVO" + std::to_string(id), {"POS" + std::to_string(key), std::to_string(static_cast<int>(angle * 100))});
+            }
+        }
+
+        servo_value_ = {
+            {0, 1},
+            {1, 1},
+            {2, 0},
+            {3, 0},
+            {4, 1},
+            {5, 0}
+        };
+
+        for (auto & [id, v] : servo_value_)
+        {
+            rclcpp::sleep_for(std::chrono::milliseconds(100));
+
+            SendMove("SERVO" + std::to_string(id), {"POS" + std::to_string(v)});
+        }
+
+        relay_value_ = {
+            {1, false},
+            {2, false},
+            {3, false},
+        };
+
+        for (auto & [id, v] : relay_value_)
+        {
+            rclcpp::sleep_for(std::chrono::milliseconds(100));
+
+            SendMove("RELAY" + std::to_string(id), {std::to_string(v)});
+        }
+
+        rclcpp::sleep_for(std::chrono::milliseconds(100));
+
+        SendOrder("TIR", {"ARM", "1"});
+    }
+
+    PCBActionInterface::~PCBActionInterface()
+    {
+    }
+
+    void PCBActionInterface::read(const std::string& msg)
+    {
+        RCLCPP_INFO_ONCE(this->get_logger(), "Received message: '%s'", msg.c_str());
+        RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received message: '%s'", msg.c_str());
+        std::vector<std::string> tokens = split(trim(msg), ';');
+
+        if (tokens.size() < 2)
+        {
+            RCLCPP_ERROR(this->get_logger(), "Invalid message format");
+            return;
+        }
+
+        if (tokens[0] == "SET")
+        {
+            if (tokens[1] == "ASC")
+            {
+                asc_state_ = std::stoi(tokens[3]);
+
+                modelec_interfaces::msg::ActionAscPos asc_msg;
+                asc_msg.pos = asc_state_;
+                asc_msg.value = asc_value_mapper_[asc_state_];
+                asc_msg.success = true;
+                asc_get_res_pub_->publish(asc_msg);
+            }
+            else if (startsWith(tokens[1], "SERVO"))
+            {
+                int servo_id = std::stoi(tokens[1].substr(5));
+                int v = std::stoi(tokens[3]);
+                servo_value_[servo_id] = v;
+
+                modelec_interfaces::msg::ActionServoPos servo_msg;
+                servo_msg.id = servo_id;
+                servo_msg.pos = v;
+                servo_msg.angle = servo_pos_mapper_[servo_id][v];
+                servo_msg.success = true;
+                servo_get_res_pub_->publish(servo_msg);
+            }
+            else if (startsWith(tokens[1], "RELAY"))
+            {
+                int relay_id = std::stoi(tokens[1].substr(5));
+                bool state = (tokens[3] == "1");
+                relay_value_[relay_id] = state;
+
+                modelec_interfaces::msg::ActionRelayState relay_msg;
+                relay_msg.id = relay_id;
+                relay_msg.state = state;
+                relay_msg.success = true;
+                relay_get_res_pub_->publish(relay_msg);
+            }
+        }
+        else if (tokens[0] == "OK")
+        {
+            if (tokens.size() == 4)
+            {
+                if (tokens[1] == "ASC")
+                {
+                    int pos = std::stoi(tokens[2]);
+                    int v = std::stoi(tokens[3]);
+                    asc_value_mapper_[pos] = v;
+
+                    modelec_interfaces::msg::ActionAscPos asc_msg;
+                    asc_msg.pos = pos;
+                    asc_msg.value = v;
+                    asc_msg.success = true;
+                    asc_set_res_pub_->publish(asc_msg);
+                }
+                else if (startsWith(tokens[1], "SERVO"))
+                {
+                    int servo_id = std::stoi(tokens[1].substr(5));
+                    int key = std::stoi(tokens[2].substr(3));
+                    int v = std::stoi(tokens[3]);
+                    servo_pos_mapper_[servo_id][key] = v;
+
+                    modelec_interfaces::msg::ActionServoPos servo_msg;
+                    servo_msg.id = servo_id;
+                    servo_msg.pos = key;
+                    servo_msg.angle = v;
+                    servo_msg.success = true;
+                    servo_set_res_pub_->publish(servo_msg);
+                }
+            }
+            else if (tokens.size() == 3)
+            {
+                if (tokens[1] == "ASC")
+                {
+                    asc_state_ = std::stoi(tokens[2]);
+
+                    modelec_interfaces::msg::ActionAscPos asc_msg;
+                    asc_msg.pos = asc_state_;
+                    asc_msg.success = true;
+                    asc_move_res_pub_->publish(asc_msg);
+                }
+                else if (startsWith(tokens[1], "SERVO"))
+                {
+                    int servo_id = std::stoi(tokens[1].substr(5));
+                    int key = std::stoi(tokens[2].substr(3));
+                    servo_value_[servo_id] = key;
+
+                    modelec_interfaces::msg::ActionServoPos servo_msg;
+                    servo_msg.id = servo_id;
+                    servo_msg.pos = key;
+                    servo_msg.success = true;
+                    servo_move_res_pub_->publish(servo_msg);
+                }
+                else if (startsWith(tokens[1], "RELAY"))
+                {
+                    int relay_id = std::stoi(tokens[1].substr(5));
+                    bool state = (tokens[2] == "1");
+                    relay_value_[relay_id] = state;
+
+                    modelec_interfaces::msg::ActionRelayState relay_msg;
+                    relay_msg.id = relay_id;
+                    relay_msg.state = state;
+                    relay_msg.success = true;
+                    relay_move_res_pub_->publish(relay_msg);
+                }
+            }
+            else
+            {
+                RCLCPP_WARN(this->get_logger(), "Unknown message format for OK response: '%s'", msg.c_str());
+            }
+        }
+        else if (tokens[0] == "KO")
+        {
+            if (tokens.size() == 4)
+            {
+                if (tokens[1] == "ASC")
+                {
+                    modelec_interfaces::msg::ActionAscPos asc_msg;
+                    asc_msg.success = false;
+                    asc_set_res_pub_->publish(asc_msg);
+                }
+                else if (startsWith(tokens[1], "SERVO"))
+                {
+                    modelec_interfaces::msg::ActionServoPos servo_msg;
+                    servo_msg.success = false;
+                    servo_set_res_pub_->publish(servo_msg);
+                }
+            }
+            else if (tokens.size() == 3)
+            {
+                if (tokens[1] == "ASC")
+                {
+                    modelec_interfaces::msg::ActionAscPos asc_msg;
+                    asc_msg.success = false;
+                    asc_move_res_pub_->publish(asc_msg);
+                }
+                else if (startsWith(tokens[1], "SERVO"))
+                {
+                    modelec_interfaces::msg::ActionServoPos servo_msg;
+                    servo_msg.success = false;
+                    servo_move_res_pub_->publish(servo_msg);
+                }
+                else if (startsWith(tokens[1], "RELAY"))
+                {
+                    modelec_interfaces::msg::ActionRelayState relay_msg;
+                    relay_msg.success = false;
+                    relay_move_res_pub_->publish(relay_msg);
+                }
+            }
+            else
+            {
+                RCLCPP_WARN(this->get_logger(), "Unknown message format: '%s'", msg.c_str());
+            }
+        }
+        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]});
+                }
+            }
+        }
+    }
+
+    void PCBActionInterface::SendToPCB(const std::string& data)
+    {
+        if (IsOk())
+        {
+            RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: %s", data.c_str());
+            this->write(data);
+        }
+    }
+
+    void PCBActionInterface::SendToPCB(const std::string& order, const std::string& elem,
+                                       const std::vector<std::string>& data)
+    {
+        std::string command = order + ";" + elem;
+        for (const auto& d : data)
+        {
+            command += ";" + d;
+        }
+        command += "\n";
+
+        SendToPCB(command);
+    }
+
+    void PCBActionInterface::GetData(const std::string& elem, const std::vector<std::string>& data)
+    {
+        SendToPCB("GET", elem, data);
+    }
+
+    void PCBActionInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data)
+    {
+        SendToPCB("SET", elem, data);
+    }
+
+    void PCBActionInterface::SendMove(const std::string& elem, const std::vector<std::string>& data)
+    {
+        SendToPCB("MOV", elem, data);
+    }
+
+    void PCBActionInterface::RespondEvent(const std::string& elem, const std::vector<std::string>& data)
+    {
+        SendToPCB("ACK", elem, data);
+    }
+}
+
+#ifndef MODELEC_COM_TESTING
+int main(int argc, char** argv)
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<Modelec::PCBActionInterface>();
+
+    // Increase number of threads explicitly!
+    rclcpp::executors::MultiThreadedExecutor executor(
+        rclcpp::ExecutorOptions(), 2 /* or more threads! */);
+
+    executor.add_node(node);
+    executor.spin();
+    rclcpp::shutdown();
+    return 0;
+}
+#endif

src/modelec_com/src/pcb_odo_interface.new.cpp

@@ -0,0 +1,398 @@
+#include <modelec_com/pcb_odo_interface.new.hpp>
+#include <modelec_utils/utils.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <modelec_utils/config.hpp>
+
+namespace Modelec
+{
+    PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface"), SerialListener()
+    {
+        declare_parameter<std::string>("serial_port", "/dev/USB_ODO");
+        declare_parameter<int>("baudrate", 115200);
+        declare_parameter<std::string>("name", "pcb_odo");
+
+        // Service to create a new serial listener
+        auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
+        request->name = get_parameter("name").as_string();
+        request->bauds = get_parameter("baudrate").as_int();
+        request->serial_port = get_parameter("serial_port").as_string();
+
+        this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN);
+
+        odo_pos_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
+            "odometry/position", 10);
+
+        odo_get_pos_sub_ = this->create_subscription<std_msgs::msg::Empty>(
+            "odometry/get/pos", 30, [this](const std_msgs::msg::Empty::SharedPtr)
+            {
+                if (IsOk())
+                {
+                    GetPos();
+                }
+            });
+
+        odo_speed_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometrySpeed>(
+            "odometry/speed", 10);
+
+        odo_tof_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryToF>(
+            "odometry/tof", 10);
+
+        odo_waypoint_reach_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryWaypoint>(
+            "odometry/waypoint_reach", 10);
+
+        odo_pid_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryPid>(
+            "odometry/get_pid", 10);
+
+        odo_add_waypoint_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryWaypoint>(
+            "odometry/add_waypoint", 30,
+            [this](const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg)
+            {
+                AddWaypointCallback(msg);
+            });
+
+        odo_add_waypoints_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryWaypoints>(
+            "odometry/add_waypoints", 30,
+            [this](const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg)
+            {
+                AddWaypointsCallback(msg);
+            });
+
+        odo_set_pos_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryPos>(
+            "odometry/set_position", 10,
+            [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
+            {
+                SetPosCallback(msg);
+            });
+
+        odo_set_pid_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryPid>(
+            "odometry/set_pid", 10,
+            [this](const modelec_interfaces::msg::OdometryPid::SharedPtr msg)
+            {
+                SetPIDCallback(msg);
+            });
+
+        start_odo_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+            "odometry/start", 10,
+            [this](const std_msgs::msg::Bool::SharedPtr msg)
+            {
+                if (msg->data != start_odo_)
+                {
+                    start_odo_ = msg->data;
+                    SendOrder("START", {std::to_string(msg->data)});
+                }
+            });
+
+
+        SetPID("THETA", 14, 0, 0);
+        SetPID("POS", 10, 0, 0);
+        SetPID("LEFT", 5, 0, 0);
+        SetPID("RIGHT", 5, 0, 0);
+    }
+
+    PCBOdoInterface::~PCBOdoInterface()
+    {
+        SetStart(false);
+    }
+
+    void PCBOdoInterface::read(const std::string& msg)
+    {
+        RCLCPP_INFO_ONCE(this->get_logger(), "Received from PCB: %s", msg.c_str());
+        RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received from PCB: %s", msg.c_str());
+        std::vector<std::string> tokens = split(trim(msg), ';');
+        if (tokens.size() < 2)
+        {
+            RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg.c_str());
+            return;
+        }
+
+        if (tokens[0] == "SET")
+        {
+            if (tokens[1] == "POS")
+            {
+                long x = std::stol(tokens[2]);
+                long y = std::stol(tokens[3]);
+                double theta = std::stod(tokens[4]);
+
+                auto message = modelec_interfaces::msg::OdometryPos();
+                message.x = x;
+                message.y = y;
+                message.theta = theta;
+
+                if (odo_pos_publisher_) {
+                    odo_pos_publisher_->publish(message);
+                }
+            }
+            else if (tokens[1] == "SPEED")
+            {
+                long x = std::stol(tokens[2]);
+                long y = std::stol(tokens[3]);
+                double theta = std::stod(tokens[4]);
+
+                auto message = modelec_interfaces::msg::OdometrySpeed();
+                message.x = x;
+                message.y = y;
+                message.theta = theta;
+
+                odo_speed_publisher_->publish(message);
+            }
+            else if (tokens[1] == "DIST")
+            {
+                int n = std::stoi(tokens[2]);
+                long dist = std::stol(tokens[3]);
+
+                auto message = modelec_interfaces::msg::OdometryToF();
+                message.n = n;
+                message.distance = dist;
+
+                odo_tof_publisher_->publish(message);
+            }
+            else if (tokens[1] == "WAYPOINT")
+            {
+                int id = std::stoi(tokens[2]);
+
+                auto message = modelec_interfaces::msg::OdometryWaypoint();
+                message.id = id;
+
+                odo_waypoint_reach_publisher_->publish(message);
+            }
+            else if (tokens[1] == "PID")
+            {
+                std::string name = tokens[2];
+                float p = std::stof(tokens[3]);
+                float i = std::stof(tokens[4]);
+                float d = std::stof(tokens[5]);
+
+                auto message = modelec_interfaces::msg::OdometryPid();
+                message.name = name;
+                message.p = p;
+                message.i = i;
+                message.d = d;
+
+                odo_pid_publisher_->publish(message);
+            }
+        }
+        else if (tokens[0] == "OK")
+        {
+            if (tokens[1] == "START")
+            {
+                start_odo_ = tokens[2] == "1";
+            }
+            else if (tokens[1] == "WAYPOINT")
+            {
+            }
+            else if (tokens[1] == "PID")
+            {
+            }
+            else if (tokens[1] == "POS")
+            {
+            }
+            else
+            {
+                RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg.c_str());
+            }
+        }
+        else if (tokens[0] == "KO")
+        {
+            if (tokens[1] == "START")
+            {
+            }
+            else if (tokens[1] == "WAYPOINT")
+            {
+            }
+
+            else if (tokens[1] == "PID")
+            {
+            }
+            else
+            {
+                RCLCPP_WARN(this->get_logger(), "PCB error: %s", msg.c_str());
+            }
+        }
+    }
+
+    void PCBOdoInterface::AddWaypointsCallback(const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg) {
+        AddWaypoints(msg);
+    }
+
+    void PCBOdoInterface::AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg)
+    {
+        AddWaypoint(msg);
+    }
+
+    void PCBOdoInterface::SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
+    {
+        SetRobotPos(msg);
+    }
+
+    void PCBOdoInterface::SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg)
+    {
+        SetPID(msg);
+    }
+
+    void PCBOdoInterface::SendToPCB(const std::string& data)
+    {
+        if (IsOk())
+        {
+            RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: %s", data.c_str());
+            this->write(data);
+        }
+    }
+
+    void PCBOdoInterface::SendToPCB(const std::string& order, const std::string& elem,
+                                    const std::vector<std::string>& data)
+    {
+        std::string command = order + ";" + elem;
+        for (const auto& d : data)
+        {
+            command += ";" + d;
+        }
+        command += "\n";
+
+        SendToPCB(command);
+    }
+
+    void PCBOdoInterface::GetData(const std::string& elem, const std::vector<std::string>& data)
+    {
+        SendToPCB("GET", elem, data);
+    }
+
+    void PCBOdoInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data)
+    {
+        SendToPCB("SET", elem, data);
+    }
+
+    void PCBOdoInterface::GetPos()
+    {
+        GetData("POS");
+    }
+
+    void PCBOdoInterface::GetSpeed()
+    {
+        GetData("SPEED");
+    }
+
+    void PCBOdoInterface::GetToF(const int& tof)
+    {
+        GetData("DIST", {std::to_string(tof)});
+    }
+
+    void PCBOdoInterface::SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
+    {
+        SetRobotPos(msg->x, msg->y, msg->theta);
+    }
+
+    void PCBOdoInterface::SetRobotPos(const long x, const long y, const double theta)
+    {
+        std::vector<std::string> data = {
+            std::to_string(x),
+            std::to_string(y),
+            std::to_string(theta)
+        };
+
+        SendOrder("POS", data);
+    }
+
+    void PCBOdoInterface::AddWaypoints(modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg)
+    {
+        if (!start_odo_)
+        {
+            SendOrder("START", {std::to_string(true)});
+        }
+
+        std::vector<std::string> data;
+
+        for (const auto& wp : msg->waypoints)
+        {
+            data.push_back(std::to_string(wp.id));
+            data.push_back(std::to_string(wp.is_end));
+            data.push_back(std::to_string(wp.x));
+            data.push_back(std::to_string(wp.y));
+            data.push_back(std::to_string(wp.theta));
+       }
+
+        SendOrder("WAYPOINT", data);
+    }
+
+    void PCBOdoInterface::AddWaypoint(
+        const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg)
+    {
+        AddWaypoint(msg->id, msg->is_end, msg->x, msg->y, msg->theta);
+    }
+
+    void PCBOdoInterface::AddWaypoint(const int index, const bool IsStopPoint, const long x, const long y,
+                                      const double theta)
+    {
+        if (!start_odo_)
+        {
+            SendOrder("START", {std::to_string(true)});
+        }
+
+        std::vector<std::string> data = {
+            std::to_string(index),
+            std::to_string(IsStopPoint),
+            std::to_string(x),
+            std::to_string(y),
+            std::to_string(theta)
+        };
+
+        SendOrder("WAYPOINT", data);
+    }
+
+    void PCBOdoInterface::SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg)
+    {
+        SetStart(msg->start);
+    }
+
+    void PCBOdoInterface::SetStart(bool start)
+    {
+        SendOrder("START", {std::to_string(start)});
+    }
+
+    void PCBOdoInterface::GetPID()
+    {
+        GetData("PID");
+    }
+
+    void PCBOdoInterface::SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg)
+    {
+        SetPID(msg->name, msg->p, msg->i, msg->d);
+    }
+
+    void PCBOdoInterface::SetPID(std::string name, float p, float i, float d, std::optional<float> min, std::optional<float> max)
+    {
+        std::vector<std::string> data = {
+            name,
+            std::to_string(p),
+            std::to_string(i),
+            std::to_string(d)
+        };
+
+        if (min.has_value())
+        {
+            data.push_back(std::to_string(min.value()));
+        }
+
+        if (max.has_value())
+        {
+            data.push_back(std::to_string(max.value()));
+        }
+
+        SendOrder("PID", data);
+    }
+} // Modelec
+
+#ifndef MODELEC_COM_TESTING
+int main(int argc, char** argv)
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<Modelec::PCBOdoInterface>();
+
+    rclcpp::executors::MultiThreadedExecutor executor(
+        rclcpp::ExecutorOptions(), 2);
+
+    executor.add_node(node);
+    executor.spin();
+    rclcpp::shutdown();
+    return 0;
+}
+#endif

src/modelec_com/src/serial_listener.cpp

@@ -0,0 +1,149 @@
+#include <modelec_com/serial_listener.hpp>
+#include <modelec_utils/utils.hpp>
+
+namespace Modelec
+{
+    SerialListener::SerialListener() : io_(), port_(io_)
+    {
+    }
+
+    SerialListener::SerialListener(const std::string& name, int bauds, const std::string& serial_port,
+                                   int max_message_len) : io_(), port_(io_)
+    {
+        open(name, bauds, serial_port, max_message_len);
+    }
+
+    SerialListener::~SerialListener()
+    {
+        if (status_)
+        {
+            close();
+        }
+    }
+
+    void SerialListener::close()
+    {
+        if (status_)
+        {
+            if (port_.is_open()) port_.close();
+            io_.stop();
+            if (io_thread_.joinable()) io_thread_.join();
+            status_ = false;
+        }
+    }
+
+    void SerialListener::open(const std::string& name, int bauds, const std::string& serial_port,
+               int max_message_len) {
+       this->name_ = name;
+       this->bauds_ = bauds;
+       this->serial_port_ = serial_port;
+       this->max_message_len_ = max_message_len;
+
+        try
+        {
+            port_.open(serial_port_);
+            port_.set_option(boost::asio::serial_port_base::baud_rate(bauds_));
+            status_ = true;
+        }
+        catch (boost::system::system_error& e)
+        {
+            RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Failed to open serial port: %s", e.what());
+            status_ = false;
+            return;
+        }
+
+        read_buffer_.resize(max_message_len_);
+        start_async_read();
+
+        io_thread_ = std::thread([this]()
+        {
+            try
+            {
+                io_.run();
+            }
+            catch (const std::exception& e)
+            {
+                RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "IO thread exception: %s", e.what());
+            }
+        });
+    }
+
+
+    void SerialListener::start_async_read()
+    {
+        if (!status_) start_async_read();
+
+        port_.async_read_some(
+            boost::asio::buffer(read_buffer_),
+            [this](const boost::system::error_code& ec, std::size_t bytes_transferred)
+            {
+                if (!ec && bytes_transferred > 0)
+                {
+                    std::string d = std::string(read_buffer_.begin(), read_buffer_.begin() + bytes_transferred);
+
+                    auto allMess = Modelec::split(d, '\n');
+                    for (const auto& mess : allMess)
+                    {
+                        if (!mess.empty())
+                        {
+                            // RCLCPP_INFO(rclcpp::get_logger("SerialListener"), "Received message: %s", mess.c_str());
+                            /*auto msg = std_msgs::msg::String();
+                            msg.data = mess;
+                            if (publisher_)
+                            {
+                                publisher_->publish(msg);
+                            }*/
+                            read(mess);
+                        }
+                    }
+
+                    start_async_read(); // continue reading
+                }
+                else
+                {
+                    RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Async read error: %s", ec.message().c_str());
+                }
+            });
+    }
+
+    void SerialListener::write(const std::string& msg)
+    {
+        std::lock_guard<std::mutex> lock(write_mutex_);
+        bool write_in_progress = !write_queue_.empty();
+        write_queue_.push_back(msg);
+
+        if (!write_in_progress)
+        {
+            start_async_write();
+        }
+    }
+
+    void SerialListener::start_async_write()
+    {
+        if (write_queue_.empty()) return;
+
+        boost::asio::async_write(
+            port_,
+            boost::asio::buffer(write_queue_.front()),
+            [this](const boost::system::error_code& ec, std::size_t /*length*/)
+            {
+                std::lock_guard<std::mutex> lock(write_mutex_);
+                if (!ec)
+                {
+                    write_queue_.pop_front();
+                    if (!write_queue_.empty())
+                    {
+                        start_async_write(); // continue writing
+                    }
+                }
+                else
+                {
+                    RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Async write error: %s", ec.message().c_str());
+                }
+            });
+    }
+
+    void SerialListener::read(const std::string&) {
+        // Default implementation does nothing
+    }
+} // namespace Modelec

src/modelec_core/launch/modelec.launch.py

@@ -96,7 +96,7 @@ def generate_launch_description():
                 Node(package='modelec_com', executable='serial_listener', name='serial_listener'),
                 Node(
                     package='modelec_com',
-                    executable='pcb_odo_interface',
+                    executable='pcb_odo_interface_new',
                     name='pcb_odo_interface',
                     parameters=[{
                         'serial_port': "/dev/USB_ODO",
@@ -106,10 +106,10 @@ def generate_launch_description():
                 ),
                 Node(
                     package='modelec_com',
-                    executable='pcb_action_interface',
+                    executable='pcb_action_interface_new',
                     name='pcb_action_interface',
                     parameters=[{
-                        'serial_port': "/tmp/USB_ACTION",
+                        'serial_port': "/dev/USB_ACTION",
                         'baudrate': 115200,
                         'name': "pcb_action",
                     }]

src/modelec_strat/src/strat_fms.cpp

@@ -126,6 +126,8 @@ namespace Modelec
         case State::INIT:
             if (team_selected_)
             {
+                started_ = false;
+
                 Transition(State::WAIT_START, "System ready");
             }
             break;