action page to the IHM

src/modelec_com/src/pcb_action_interface.cpp

@@ -258,7 +258,7 @@ namespace Modelec
     void PCBActionInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg)
     {
         RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg->data.c_str());
-        std::vector<std::string> tokens = split(msg->data, ';');
+        std::vector<std::string> tokens = split(trim(msg->data), ';');
 
         if (tokens.size() < 3)
         {
@@ -274,6 +274,7 @@ namespace Modelec
 
                 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);
             }
@@ -286,6 +287,7 @@ namespace Modelec
                 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);
             }

src/modelec_com/src/pcb_alim_interface.cpp

@@ -146,7 +146,7 @@ namespace Modelec
     void PCBAlimInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg)
     {
         RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg->data.c_str());
-        std::vector<std::string> tokens = split(msg->data, ';');
+        std::vector<std::string> tokens = split(trim(msg->data), ';');
         if (tokens.size() < 2)
         {
             RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg->data.c_str());

src/modelec_com/src/pcb_odo_interface.cpp

@@ -187,7 +187,7 @@ namespace Modelec
     void PCBOdoInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg)
     {
         RCLCPP_DEBUG(this->get_logger(), "Received from PCB: %s", msg->data.c_str());
-        std::vector<std::string> tokens = split(msg->data, ';');
+        std::vector<std::string> tokens = split(trim(msg->data), ';');
         if (tokens.size() < 2)
         {
             RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg->data.c_str());

src/modelec_gui/CMakeLists.txt

@@ -37,12 +37,18 @@ add_executable(modelec_gui
         src/pages/home_page.cpp
         src/pages/test_map_page.cpp
         src/pages/map_page.cpp
+        src/pages/action_page.cpp
+        src/pages/alim_page.cpp
+        src/widget/action_widget.cpp
 
         include/modelec_gui/modelec_gui.hpp
         include/modelec_gui/pages/odo_page.hpp
         include/modelec_gui/pages/home_page.hpp
         include/modelec_gui/pages/test_map_page.hpp
         include/modelec_gui/pages/map_page.hpp
+        include/modelec_gui/pages/action_page.hpp
+        include/modelec_gui/pages/alim_page.hpp
+        include/modelec_gui/widget/action_widget.hpp
 )
 
 ament_target_dependencies(modelec_gui

src/modelec_gui/include/modelec_gui/modelec_gui.hpp

@@ -8,6 +8,9 @@
 #include <modelec_gui/pages/test_map_page.hpp>
 #include <modelec_gui/pages/odo_page.hpp>
 #include <modelec_gui/pages/map_page.hpp>
+#include <modelec_gui/pages/action_page.hpp>
+#include <modelec_gui/pages/alim_page.hpp>
+
 
 namespace ModelecGUI
 {
@@ -31,10 +34,14 @@ namespace ModelecGUI
         TestMapPage* test_map_page_;
         OdoPage* odo_page_;
         MapPage* map_page_;
+        ActionPage* action_page_;
+        AlimPage* alim_page_;
 
         QAction* home_action_;
         QAction* odo_action_;
         QAction* map_action_;
+        QAction* action_action_;
+        QAction* alim_action_;
         QAction* exit_action_;
 
         QMenu* playmat_map_menu_;

src/modelec_gui/include/modelec_gui/pages/action_page.hpp

@@ -0,0 +1,81 @@
+#pragma once
+
+#include <QWidget>
+#include <QPushButton>
+#include <QLineEdit>
+#include <QVBoxLayout>
+#include <QSpinBox>
+
+#include <modelec_gui/widget/action_widget.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <modelec_interfaces/msg/action_asc_pos.hpp>
+#include <modelec_interfaces/msg/action_servo_pos.hpp>
+#include <modelec_interfaces/msg/action_relay_state.hpp>
+
+namespace ModelecGUI
+{
+    class ActionPage : public QWidget
+    {
+        Q_OBJECT
+
+    public:
+
+        using ActionAscPos = modelec_interfaces::msg::ActionAscPos;
+        using ActionServoPos = modelec_interfaces::msg::ActionServoPos;
+        using ActionRelayState = modelec_interfaces::msg::ActionRelayState;
+
+        ActionPage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr);
+        ~ActionPage() override;
+
+        rclcpp::Node::SharedPtr get_node() const { return node_; }
+
+    private:
+
+        QVBoxLayout* layout_;
+        QHBoxLayout* asc_layout_;
+
+        ActionWidget* asc_action_;
+        bool waiting_for_move_asc_;
+
+        QGridLayout* servo_layout_;
+
+        ActionWidget* servo0_action_;
+        ActionWidget* servo1_action_;
+        ActionWidget* servo2_action_;
+        ActionWidget* servo3_action_;
+        ActionWidget* servo4_action_;
+        ActionWidget* servo5_action_;
+        ActionWidget* servo6_action_;
+        ActionWidget* servo7_action_;
+        ActionWidget* servo8_action_;
+        std::vector<ActionWidget*> servo_actions_;
+        std::vector<bool> waiting_for_move_servo_;
+
+        QHBoxLayout* relay_layout_;
+
+        QPushButton* relay_top_button_;
+        QPushButton* relay_bottom_button_;
+        std::vector<QPushButton*> relay_buttons_;
+        std::vector<bool> relay_values_;
+
+        rclcpp::Node::SharedPtr node_;
+
+        rclcpp::Subscription<ActionAscPos>::SharedPtr asc_get_sub_;
+        rclcpp::Publisher<ActionAscPos>::SharedPtr asc_set_pub_;
+        rclcpp::Subscription<ActionAscPos>::SharedPtr asc_set_res_sub_;
+        rclcpp::Publisher<ActionAscPos>::SharedPtr asc_move_pub_;
+        rclcpp::Subscription<ActionAscPos>::SharedPtr asc_move_res_sub_;
+
+        rclcpp::Subscription<ActionServoPos>::SharedPtr servo_get_sub_;
+        rclcpp::Publisher<ActionServoPos>::SharedPtr servo_set_pub_;
+        rclcpp::Subscription<ActionServoPos>::SharedPtr servo_set_res_sub_;
+        rclcpp::Publisher<ActionServoPos>::SharedPtr servo_move_pub_;
+        rclcpp::Subscription<ActionServoPos>::SharedPtr servo_move_res_sub_;
+
+        rclcpp::Subscription<ActionRelayState>::SharedPtr relay_get_sub_;
+        rclcpp::Publisher<ActionRelayState>::SharedPtr relay_move_pub_;
+        rclcpp::Subscription<ActionRelayState>::SharedPtr relay_move_res_sub_;
+    };
+} // namespace Modelec

src/modelec_gui/include/modelec_gui/pages/alim_page.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <QWidget>
+#include <QPushButton>
+#include <QLineEdit>
+#include <QVBoxLayout>
+#include <QSpinBox>
+
+#include <rclcpp/rclcpp.hpp>
+
+namespace ModelecGUI
+{
+    class AlimPage : public QWidget
+    {
+        Q_OBJECT
+
+    public:
+        AlimPage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr);
+        ~AlimPage() override;
+
+        rclcpp::Node::SharedPtr get_node() const { return node_; }
+
+    private:
+
+        rclcpp::Node::SharedPtr node_;
+    };
+} // namespace Modelec

src/modelec_gui/include/modelec_gui/pages/odo_page.hpp

@@ -55,4 +55,4 @@ namespace ModelecGUI
 
         void PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
     };
-} // namespace Modelec
+} // namespace Modelec GUI

src/modelec_gui/include/modelec_gui/widget/action_widget.hpp

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <QHBoxLayout>
+#include <QPushButton>
+#include <qspinbox.h>
+#include <QWidget>
+
+namespace ModelecGUI
+{
+    class ActionWidget : public QWidget
+    {
+        Q_OBJECT
+    public:
+        ActionWidget(QWidget *parent = 0);
+
+        ~ActionWidget();
+
+        void SetButtonText(const QString& text);
+        void SetSpinBoxValue(double value);
+        void SetSpinBoxRange(double min, double max);
+        void SetSpinBoxStep(double step);
+
+        double GetSpinBoxValue();
+
+    protected slots:
+        void OnButtonClicked();
+
+    signals:
+        void ButtonClicked(double value);
+
+    private:
+
+        QHBoxLayout* layout_;
+        QDoubleSpinBox* spinBox_;
+        QPushButton* pushButton_;
+    };
+}

src/modelec_gui/src/modelec_gui.cpp

@@ -16,11 +16,15 @@ namespace ModelecGUI {
         odo_page_ = new OdoPage(get_node(), this);
         test_map_page_ = new TestMapPage(get_node(), this);
         map_page_ = new MapPage(get_node(), this);
+        action_page_ = new ActionPage(get_node(), this);
+        alim_page_ = new AlimPage(get_node(), this);
 
         stackedWidget->addWidget(home_page_);
         stackedWidget->addWidget(odo_page_);
         stackedWidget->addWidget(test_map_page_);
         stackedWidget->addWidget(map_page_);
+        stackedWidget->addWidget(action_page_);
+        stackedWidget->addWidget(alim_page_);
         setCentralWidget(stackedWidget);
 
         setupMenu();
@@ -38,6 +42,8 @@ namespace ModelecGUI {
 
         home_action_ = new QAction("Home", this);
         odo_action_ = new QAction("Odometrie", this);
+        action_action_ = new QAction("Action", this);
+        alim_action_ = new QAction("Alim", this);
         map_action_ = new QAction("Map", this);
 
         playmat_map_menu_ = new QMenu("playmat");
@@ -50,6 +56,8 @@ namespace ModelecGUI {
 
         optionsMenu->addAction(home_action_);
         optionsMenu->addAction(odo_action_);
+        optionsMenu->addAction(action_action_);
+        optionsMenu->addAction(alim_action_);
         optionsMenu->addSeparator();
         optionsMenu->addAction(map_action_);
         optionsMenu->addMenu(playmat_map_menu_);
@@ -69,6 +77,14 @@ namespace ModelecGUI {
             stackedWidget->setCurrentIndex(1);
         });
 
+        connect(action_action_, &QAction::triggered, this, [this]() {
+            stackedWidget->setCurrentIndex(4);
+        });
+
+        connect(alim_action_, &QAction::triggered, this, [this]() {
+            stackedWidget->setCurrentIndex(5);
+        });
+
         connect(map_action_, &QAction::triggered, this, [this]() {
             stackedWidget->setCurrentIndex(2);
         });

src/modelec_gui/src/pages/action_page.cpp

@@ -0,0 +1,254 @@
+#include <modelec_gui/pages/action_page.hpp>
+
+namespace ModelecGUI
+{
+    ActionPage::ActionPage(rclcpp::Node::SharedPtr node, QWidget* parent) :
+        QWidget(parent),
+        node_(node)
+    {
+        layout_ = new QVBoxLayout(this);
+        setLayout(layout_);
+        setWindowTitle("Action Page");
+
+        asc_layout_ = new QHBoxLayout;
+
+        asc_action_ = new ActionWidget(this);
+        asc_action_->SetSpinBoxRange(0, 1000);
+        asc_action_->SetSpinBoxStep(1);
+        asc_action_->SetSpinBoxValue(0);
+        asc_action_->SetButtonText("Send ASC");
+
+        waiting_for_move_asc_ = false;
+
+        connect(asc_action_, &ActionWidget::ButtonClicked, this,
+            [this](double value)
+            {
+                ActionAscPos asc_move;
+                asc_move.value = value;
+                asc_move.pos = 128;
+                asc_set_pub_->publish(asc_move);
+                asc_action_->setDisabled(true);
+
+                waiting_for_move_asc_ = true;
+            });
+
+        asc_layout_->addWidget(asc_action_);
+
+        servo_layout_ = new QGridLayout;
+
+        servo0_action_ = new ActionWidget(this);
+        servo0_action_->SetSpinBoxRange(0, 180);
+        servo0_action_->SetSpinBoxStep(.1);
+        servo0_action_->SetSpinBoxValue(90);
+        servo0_action_->SetButtonText("Servo 0");
+
+        servo1_action_ = new ActionWidget(this);
+        servo1_action_->SetSpinBoxRange(0, 180);
+        servo1_action_->SetSpinBoxStep(.1);
+        servo1_action_->SetSpinBoxValue(90);
+        servo1_action_->SetButtonText("Servo 1");
+
+        servo2_action_ = new ActionWidget(this);
+        servo2_action_->SetSpinBoxRange(0, 180);
+        servo2_action_->SetSpinBoxStep(.1);
+        servo2_action_->SetSpinBoxValue(90);
+        servo2_action_->SetButtonText("Servo 2");
+
+        servo3_action_ = new ActionWidget(this);
+        servo3_action_->SetSpinBoxRange(0, 180);
+        servo3_action_->SetSpinBoxStep(.1);
+        servo3_action_->SetSpinBoxValue(90);
+        servo3_action_->SetButtonText("Servo 3");
+
+        servo4_action_ = new ActionWidget(this);
+        servo4_action_->SetSpinBoxRange(0, 180);
+        servo4_action_->SetSpinBoxStep(.1);
+        servo4_action_->SetSpinBoxValue(90);
+        servo4_action_->SetButtonText("Servo 4");
+
+        servo5_action_ = new ActionWidget(this);
+        servo5_action_->SetSpinBoxRange(0, 180);
+        servo5_action_->SetSpinBoxStep(.1);
+        servo5_action_->SetSpinBoxValue(90);
+        servo5_action_->SetButtonText("Servo 5");
+
+        servo6_action_ = new ActionWidget(this);
+        servo6_action_->SetSpinBoxRange(0, 180);
+        servo6_action_->SetSpinBoxStep(.1);
+        servo6_action_->SetSpinBoxValue(90);
+        servo6_action_->SetButtonText("Servo 6");
+
+        servo7_action_ = new ActionWidget(this);
+        servo7_action_->SetSpinBoxRange(0, 180);
+        servo7_action_->SetSpinBoxStep(.1);
+        servo7_action_->SetSpinBoxValue(90);
+        servo7_action_->SetButtonText("Servo 7");
+
+        servo8_action_ = new ActionWidget(this);
+        servo8_action_->SetSpinBoxRange(0, 180);
+        servo8_action_->SetSpinBoxStep(.1);
+        servo8_action_->SetSpinBoxValue(90);
+        servo8_action_->SetButtonText("Servo 8");
+
+        servo_layout_->addWidget(servo0_action_, 0, 0);
+        servo_layout_->addWidget(servo1_action_, 0, 1);
+        servo_layout_->addWidget(servo2_action_, 0, 2);
+        servo_layout_->addWidget(servo3_action_, 1, 0);
+        servo_layout_->addWidget(servo4_action_, 1, 1);
+        servo_layout_->addWidget(servo5_action_, 1, 2);
+        servo_layout_->addWidget(servo6_action_, 2, 0);
+        servo_layout_->addWidget(servo7_action_, 2, 1);
+        servo_layout_->addWidget(servo8_action_, 2, 2);
+
+        servo_actions_.push_back(servo0_action_);
+        servo_actions_.push_back(servo1_action_);
+        servo_actions_.push_back(servo2_action_);
+        servo_actions_.push_back(servo3_action_);
+        servo_actions_.push_back(servo4_action_);
+        servo_actions_.push_back(servo5_action_);
+        servo_actions_.push_back(servo6_action_);
+        servo_actions_.push_back(servo7_action_);
+        servo_actions_.push_back(servo8_action_);
+        waiting_for_move_servo_ = std::vector<bool>(servo_actions_.size(), false);
+
+        for (size_t i = 0; i < servo_actions_.size(); ++i)
+        {
+            connect(servo_actions_[i], &ActionWidget::ButtonClicked, this,
+                [this, i](double value)
+                {
+                    ActionServoPos servo_move;
+                    servo_move.id = i;
+                    servo_move.pos = 128;
+                    servo_move.angle = value;
+                    servo_set_pub_->publish(servo_move);
+                    servo_actions_[i]->setDisabled(true);
+
+                    waiting_for_move_servo_[i] = true;
+                });
+        }
+
+        relay_layout_ = new QHBoxLayout;
+
+        relay_top_button_ = new QPushButton(this);
+        relay_top_button_->setText("Toggle Relay Top");
+
+        relay_bottom_button_ = new QPushButton(this);
+        relay_bottom_button_->setText("Toggle Relay Bottom");
+
+        relay_layout_->addWidget(relay_top_button_);
+        relay_layout_->addWidget(relay_bottom_button_);
+
+        relay_buttons_.push_back(relay_top_button_);
+        relay_buttons_.push_back(relay_bottom_button_);
+        relay_values_ = {false, false};
+
+        for (size_t i = 0; i < relay_buttons_.size(); ++i)
+        {
+            connect(relay_buttons_[i], &QPushButton::clicked, this,
+                [this, i]()
+                {
+                    ActionRelayState relay_state;
+                    relay_state.id = i;
+                    relay_state.state = !relay_values_[i];
+                    relay_move_pub_->publish(relay_state);
+                    relay_buttons_[i]->setDisabled(true);
+                });
+        }
+
+        layout_->addLayout(asc_layout_);
+        layout_->addLayout(servo_layout_);
+        layout_->addLayout(relay_layout_);
+
+        asc_get_sub_ = node_->create_subscription<ActionAscPos>(
+            "action/get/asc", 10, [this](const ActionAscPos::SharedPtr msg)
+            {
+                asc_action_->SetSpinBoxValue(msg->value);
+            });
+
+        asc_set_pub_ = node_->create_publisher<ActionAscPos>(
+            "action/set/asc", 10);
+
+        asc_set_res_sub_ = node_->create_subscription<ActionAscPos>(
+            "action/set/asc/res", 10, [this](const ActionAscPos::SharedPtr msg)
+            {
+                if (waiting_for_move_asc_)
+                {
+                    ActionAscPos asc_move;
+                    asc_move.pos = msg->pos;
+                    asc_move_pub_->publish(asc_move);
+                    waiting_for_move_asc_ = false;
+                }
+            });
+
+        asc_move_pub_ = node_->create_publisher<ActionAscPos>(
+            "action/move/asc", 10);
+
+        asc_move_res_sub_ = node_->create_subscription<ActionAscPos>(
+            "action/move/asc/res", 10, [this](const ActionAscPos::SharedPtr)
+            {
+                asc_action_->setDisabled(false);
+            });
+
+        servo_get_sub_ = node_->create_subscription<ActionServoPos>(
+            "action/get/servo", 10, [this](const ActionServoPos::SharedPtr msg)
+            {
+                if (static_cast<int>(servo_actions_.size()) > msg->id && servo_actions_[msg->id] != nullptr)
+                {
+                    servo_actions_[msg->id]->SetSpinBoxValue(msg->angle);
+                }
+            });
+
+        servo_set_pub_ = node_->create_publisher<ActionServoPos>(
+            "action/set/servo", 10);
+
+        servo_set_res_sub_ = node_->create_subscription<ActionServoPos>(
+            "action/set/servo/res", 10, [this](const ActionServoPos::SharedPtr msg)
+            {
+                if (waiting_for_move_servo_[msg->id])
+                {
+                    ActionServoPos servo_move;
+                    servo_move.id = msg->id;
+                    servo_move.pos = msg->pos;
+                    servo_move_pub_->publish(servo_move);
+                    waiting_for_move_servo_[msg->id] = false;
+                }
+            });
+
+        servo_move_pub_ = node_->create_publisher<ActionServoPos>(
+            "action/move/servo", 10);
+
+        servo_move_res_sub_ = node_->create_subscription<ActionServoPos>(
+            "action/move/servo/res", 10, [this](const ActionServoPos::SharedPtr msg)
+            {
+                if (static_cast<int>(servo_actions_.size()) > msg->id && servo_actions_[msg->id] != nullptr)
+                {
+                    servo_actions_[msg->id]->setDisabled(false);
+                }
+            });
+
+        relay_get_sub_ = node_->create_subscription<ActionRelayState>(
+            "action/get/relay", 10, [this](const ActionRelayState::SharedPtr msg)
+            {
+                if (static_cast<int>(relay_values_.size()) > msg->id)
+                {
+                    relay_values_[msg->id] = msg->state;
+                }
+            });
+
+        relay_move_pub_ = node_->create_publisher<ActionRelayState>(
+            "action/move/relay", 10);
+
+        relay_move_res_sub_ = node_->create_subscription<ActionRelayState>(
+            "action/move/relay/res", 10, [this](const ActionRelayState::SharedPtr msg)
+            {
+                if (static_cast<int>(relay_buttons_.size()) > msg->id && relay_buttons_[msg->id] != nullptr && static_cast<int>(relay_values_.size()) > msg->id)
+                {
+                    relay_buttons_[msg->id]->setDisabled(false);
+                    relay_values_[msg->id] = msg->state;
+                }
+            });
+    }
+
+    ActionPage::~ActionPage()
+    = default;
+}

src/modelec_gui/src/pages/alim_page.cpp

@@ -0,0 +1,13 @@
+#include <modelec_gui/pages/alim_page.hpp>
+
+namespace ModelecGUI
+{
+    AlimPage::AlimPage(rclcpp::Node::SharedPtr node, QWidget* parent) :
+        QWidget(parent),
+        node_(node)
+    {
+    }
+
+    AlimPage::~AlimPage()
+    = default;
+}

src/modelec_gui/src/pages/map_page.cpp

@@ -27,7 +27,7 @@ namespace ModelecGUI
         score_label_->setFont(QFont("Arial", 26));
         score_label_->setStyleSheet("QLabel { color: black; }");
 
-        h_layout = new QHBoxLayout(this);
+        h_layout = new QHBoxLayout();
         h_layout->addStretch();
         h_layout->addStretch();
         h_layout->addWidget(score_label_);

src/modelec_gui/src/widget/action_widget.cpp

@@ -0,0 +1,69 @@
+#include <modelec_gui/widget/action_widget.hpp>
+
+namespace ModelecGUI
+{
+    ActionWidget::ActionWidget(QWidget* parent) : QWidget(parent)
+    {
+        layout_ = new QHBoxLayout(this);
+        setLayout(layout_);
+
+        spinBox_ = new QDoubleSpinBox(this);
+        pushButton_ = new QPushButton(this);
+        connect(pushButton_, &QPushButton::clicked, this, &ActionWidget::OnButtonClicked);
+
+        layout_->addWidget(spinBox_);
+        layout_->addWidget(pushButton_);
+
+        layout_->setStretch(0, 4);
+        layout_->setStretch(1, 1);
+    }
+
+    ActionWidget::~ActionWidget()
+    {
+        delete layout_;
+        delete spinBox_;
+        delete pushButton_;
+    }
+
+    void ActionWidget::SetButtonText(const QString& text)
+    {
+        if (pushButton_)
+        {
+            pushButton_->setText(text);
+        }
+    }
+
+    void ActionWidget::SetSpinBoxValue(double value)
+    {
+        if (spinBox_)
+        {
+            spinBox_->setValue(value);
+        }
+    }
+
+    void ActionWidget::SetSpinBoxRange(double min, double max)
+    {
+        if (spinBox_)
+        {
+            spinBox_->setRange(min, max);
+        }
+    }
+
+    void ActionWidget::SetSpinBoxStep(double step)
+    {
+        if (spinBox_)
+        {
+            spinBox_->setSingleStep(step);
+        }
+    }
+
+    double ActionWidget::GetSpinBoxValue()
+    {
+        return spinBox_ ? spinBox_->value() : 0.0;
+    }
+
+    void ActionWidget::OnButtonClicked()
+    {
+        emit ButtonClicked(GetSpinBoxValue());
+    }
+}

src/modelec_interfaces/msg/Action/ActionAscPos.msg

@@ -1,3 +1,3 @@
 int32 pos
-int32 value
+float64 value
 bool success

src/modelec_interfaces/msg/Action/ActionServoPos.msg

@@ -1,4 +1,4 @@
 int32 id
 int32 pos
-float32 angle
+float64 angle
 bool success

src/modelec_strat/data/config.xml

@@ -53,17 +53,17 @@
         <pcb>
             <pcb_odo>
                 <name>pcb_odo</name>
-                <port>/dev/pts/4</port>
+                <port>/tmp/USB_ODO</port>
                 <baud_rate>115200</baud_rate>
             </pcb_odo>
             <pcb_alim>
                 <name>pcb_alim</name>
-                <port>/dev/pts/12</port>
+                <port>/tmp/USB_ALIM</port>
                 <baud_rate>115200</baud_rate>
             </pcb_alim>
             <pcb_action>
                 <name>pcb_action</name>
-                <port>/dev/pts/9</port>
+                <port>/tmp/USB_ACTION</port>
                 <baud_rate>115200</baud_rate>
             </pcb_action>
         </pcb>

src/modelec_strat/include/modelec_strat/strat_fms.hpp

@@ -59,6 +59,7 @@ namespace Modelec
         rclcpp::Time match_start_time_;
         bool started_ = false;
         bool setup_ = false;
+        bool team_selected_ = false;
         std::unique_ptr<Mission> current_mission_;
         int team_id_ = 0;
         bool is_banner_done_ = false;

src/modelec_strat/src/strat_fms.cpp

@@ -30,6 +30,7 @@ namespace Modelec
         spawn_id_sub_ = create_subscription<modelec_interfaces::msg::Spawn>(
             "/strat/spawn", 10, [this](const modelec_interfaces::msg::Spawn::SharedPtr msg)
             {
+                team_selected_ = true;
                 team_id_ = msg->team_id;
                 nav_->SetTeamId(team_id_);
                 nav_->SetSpawn(msg->name);
@@ -103,6 +104,7 @@ namespace Modelec
 
         state_ = State::INIT;
         started_ = false;
+        team_selected_ = false;
         current_mission_.reset();
         match_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
 
@@ -130,7 +132,7 @@ namespace Modelec
         switch (state_)
         {
         case State::INIT:
-            if (setup_)
+            if (setup_ && team_selected_)
             {
                 Transition(State::WAIT_START, "System ready");
             }

src/modelec_utils/include/modelec_utils/utils.hpp

@@ -21,4 +21,6 @@ namespace Modelec {
     T mapValue(T v, T v_min, T v_max, T v_min_prime, T v_max_prime) {
         return v_min_prime + (((v - v_min) * (v_max_prime - v_min_prime)) / (v_max - v_min));
     }
+
+    std::string trim(const std::string &s);
 }

src/modelec_utils/src/utils.cpp

@@ -38,4 +38,14 @@ namespace Modelec {
     {
         return s.find(substring) != std::string::npos;
     }
+
+    std::string trim(const std::string& s)
+    {
+        size_t start = s.find_first_not_of(" \t\n\r\f\v");
+        if (start == std::string::npos)
+            return "";
+
+        size_t end = s.find_last_not_of(" \t\n\r\f\v");
+        return s.substr(start, end - start + 1);
+    }
 };