HUGE update :

- more dynamic strat (not everything work unfortunately) - beginning of an easy static strat - some refactoring - implementation of some basic mission
2026-01-18 16:47:43 +01:00 · 2025-05-03 00:30:40 -04:00
parent 7ffe157982
commit 1818adc0b5
33 changed files with 519 additions and 226 deletions
--- a/simulated_pcb/odo.py
+++ b/simulated_pcb/odo.py
@@ -4,13 +4,13 @@ import math
 import threading

 class SimulatedPCB:
-    def __init__(self, port='/dev/pts/6', baud=115200):
+    def __init__(self, port='/dev/pts/8', baud=115200):
        self.ser = serial.Serial(port, baud, timeout=1)
        self.running = True
        self.start = False

        self.x = 1225.0
-        self.y = 200.0
+        self.y = 300.0
        self.theta = math.pi / 2
        self.vx = 0.0
        self.vy = 0.0
@@ -27,6 +27,9 @@ class SimulatedPCB:
        self.thread = threading.Thread(target=self.run)
        self.thread.start()

+    def normalize_angle(self, angle):
+        return (angle + math.pi) % (2 * math.pi) - math.pi
+
    def update_position(self):
        now = time.time()
        dt = now - self.last_update
@@ -39,13 +42,13 @@ class SimulatedPCB:
                dy = wp['y'] - self.y
                distance = math.hypot(dx, dy)
                angle = math.atan2(dy, dx)
+                angle_diff = self.normalize_angle(wp['theta'] - self.theta)

-                # Behavior differs by type
                if wp['type'] == 1:
-                    # Final waypoint: stop only when 100% accurate
-                    if distance < 5.0:
+                    if distance < 5.0 and abs(angle_diff) < 0.05:
                        self.vx = 0.0
                        self.vy = 0.0
+                        self.vtheta = 0.0
                        self.ser.write(f'SET;WAYPOINT;{wp["id"]}\n'.encode('utf-8'))
                        self.current_wp_id = wp['id']
                        self.waypoint_order.pop(0)
@@ -54,11 +57,12 @@ class SimulatedPCB:
                        speed = min(200.0, distance * 1.5)
                        self.vx = speed * math.cos(angle)
                        self.vy = speed * math.sin(angle)
+                        self.vtheta = max(-1.0, min(1.0, angle_diff))
                else:
-                    # Normal pass-through waypoint
                    speed = min(300.0, distance * 2)
                    self.vx = speed * math.cos(angle)
                    self.vy = speed * math.sin(angle)
+                    self.vtheta = 0.0
                    if distance < 100:
                        self.ser.write(f'SET;WAYPOINT;{wp["id"]}\n'.encode('utf-8'))
                        self.current_wp_id = wp['id']
@@ -67,14 +71,15 @@ class SimulatedPCB:
            else:
                self.vx = 0.0
                self.vy = 0.0
+                self.vtheta = 0.0

            self.x += self.vx * dt
            self.y += self.vy * dt
            self.theta += self.vtheta * dt
-            self.theta = (self.theta + math.pi) % (2 * math.pi) - math.pi
+            self.theta = self.normalize_angle(self.theta)

            if now - self.last_send > 0.1:
-                self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}\n'.encode())
+                self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n'.encode())
                self.last_send = now

    def run(self):
@@ -144,4 +149,4 @@ if __name__ == '__main__':
        sim = SimulatedPCB()
    except KeyboardInterrupt:
        sim.stop()
-        print("Simulation stopped")
+        print("Simulation stopped")
--- a/src/modelec_com/CMakeLists.txt
+++ b/src/modelec_com/CMakeLists.txt
@@ -20,7 +20,7 @@ find_library(WIRINGPI_LIB wiringPi)
 # USB Arduino Listener Node
 add_executable(serial_listener src/multiple_serial_listener.cpp)
 ament_target_dependencies(serial_listener rclcpp std_msgs modelec_interfaces)
-target_link_libraries(serial_listener Boost::system modelec_utils::modelec_utils)
+target_link_libraries(serial_listener Boost::system modelec_utils::utils)
 target_include_directories(serial_listener PUBLIC
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
        $<INSTALL_INTERFACE:include>
@@ -29,7 +29,7 @@ target_include_directories(serial_listener PUBLIC
 # USB Arduino Logic Processor Node
 add_executable(pcb_odo_interface src/pcb_odo_interface.cpp)
 ament_target_dependencies(pcb_odo_interface rclcpp std_msgs modelec_interfaces)
-target_link_libraries(pcb_odo_interface modelec_utils::modelec_utils)
+target_link_libraries(pcb_odo_interface modelec_utils::utils)
 target_include_directories(pcb_odo_interface PUBLIC
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
        $<INSTALL_INTERFACE:include>
@@ -38,7 +38,7 @@ target_include_directories(pcb_odo_interface PUBLIC
 # USB Arduino Logic Processor Node
 add_executable(pcb_alim_interface src/pcb_alim_interface.cpp)
 ament_target_dependencies(pcb_alim_interface rclcpp std_msgs modelec_interfaces)
-target_link_libraries(pcb_alim_interface modelec_utils::modelec_utils)
+target_link_libraries(pcb_alim_interface modelec_utils::utils)
 target_include_directories(pcb_alim_interface PUBLIC
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
        $<INSTALL_INTERFACE:include>
@@ -47,7 +47,7 @@ target_include_directories(pcb_alim_interface PUBLIC
 # PCA9685 Listener Node
 add_executable(pca9685_controller src/pca9685_controller.cpp)
 ament_target_dependencies(pca9685_controller rclcpp std_msgs modelec_interfaces)
-target_link_libraries(pca9685_controller ${WIRINGPI_LIB} modelec_utils::modelec_utils)
+target_link_libraries(pca9685_controller ${WIRINGPI_LIB} modelec_utils::utils)
 target_include_directories(pca9685_controller PUBLIC
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
        $<INSTALL_INTERFACE:include>
--- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
+++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
@@ -39,7 +39,7 @@ public:
    struct OdometryData {
        long x;
        long y;
-        long theta;
+        double theta;
    };

    struct PIDData
--- a/src/modelec_com/src/pcb_odo_interface.cpp
+++ b/src/modelec_com/src/pcb_odo_interface.cpp
@@ -10,7 +10,7 @@ namespace Modelec
        auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
        request->name = "pcb_odo";
        request->bauds = 115200;
-        request->serial_port = "/dev/pts/7"; // TODO : check the real serial port
+        request->serial_port = "/dev/pts/9"; // TODO : check the real serial port
        auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
        while (!client->wait_for_service(std::chrono::seconds(1)))
        {
@@ -186,7 +186,7 @@ namespace Modelec
            {
                long x = std::stol(tokens[2]);
                long y = std::stol(tokens[3]);
-                long theta = std::stol(tokens[4]);
+                double theta = std::stod(tokens[4]);

                auto message = modelec_interfaces::msg::OdometryPos();
                message.x = x;
@@ -201,7 +201,7 @@ namespace Modelec
            {
                long x = std::stol(tokens[2]);
                long y = std::stol(tokens[3]);
-                long theta = std::stol(tokens[4]);
+                double theta = std::stod(tokens[4]);

                auto message = modelec_interfaces::msg::OdometrySpeed();
                message.x = x;
--- a/src/modelec_core/CMakeLists.txt
+++ b/src/modelec_core/CMakeLists.txt
@@ -20,7 +20,7 @@ find_library(WIRINGPI_LIB wiringPi)

 add_executable(gamecontroller_listener src/gamecontroller_listener.cpp)
 ament_target_dependencies(gamecontroller_listener rclcpp std_msgs sensor_msgs modelec_interfaces)
-target_link_libraries(gamecontroller_listener modelec_utils::modelec_utils)
+target_link_libraries(gamecontroller_listener modelec_utils::utils)
 target_include_directories(gamecontroller_listener PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
@@ -28,7 +28,7 @@ target_include_directories(gamecontroller_listener PUBLIC

 add_executable(solenoid_controller src/solenoid_controller.cpp)
 ament_target_dependencies(solenoid_controller rclcpp modelec_interfaces)
-target_link_libraries(solenoid_controller ${WIRINGPI_LIB} modelec_utils::modelec_utils)
+target_link_libraries(solenoid_controller ${WIRINGPI_LIB} modelec_utils::utils)
 target_include_directories(solenoid_controller PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
@@ -36,7 +36,7 @@ target_include_directories(solenoid_controller PUBLIC

 add_executable(arm_controller src/arm_controller.cpp)
 ament_target_dependencies(arm_controller rclcpp std_msgs modelec_interfaces)
-target_link_libraries(arm_controller ${WIRINGPI_LIB} modelec_utils::modelec_utils)
+target_link_libraries(arm_controller ${WIRINGPI_LIB} modelec_utils::utils)
 target_include_directories(arm_controller PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
@@ -44,7 +44,7 @@ target_include_directories(arm_controller PUBLIC

 add_executable(button_gpio_controller src/button_gpio_controller.cpp)
 ament_target_dependencies(button_gpio_controller rclcpp std_msgs modelec_interfaces)
-target_link_libraries(button_gpio_controller ${WIRINGPI_LIB} modelec_utils::modelec_utils)
+target_link_libraries(button_gpio_controller ${WIRINGPI_LIB} modelec_utils::utils)
 target_include_directories(button_gpio_controller PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
@@ -52,7 +52,7 @@ target_include_directories(button_gpio_controller PUBLIC

 add_executable(speed_result src/speed_result.cpp)
 ament_target_dependencies(speed_result rclcpp std_msgs sensor_msgs modelec_interfaces)
-target_link_libraries(speed_result modelec_utils::modelec_utils)
+target_link_libraries(speed_result modelec_utils::utils)
 target_include_directories(speed_result PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
--- a/src/modelec_gui/CMakeLists.txt
+++ b/src/modelec_gui/CMakeLists.txt
@@ -15,6 +15,8 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+
 find_package(modelec_interfaces REQUIRED)
 find_package(modelec_utils REQUIRED)

@@ -45,13 +47,15 @@ ament_target_dependencies(modelec_gui
        std_msgs
        std_srvs
        modelec_interfaces
+        ament_index_cpp
 )
 target_link_libraries(modelec_gui
        Qt6::Core
        Qt6::Gui
        Qt6::Widgets
        Qt6::Svg
-        modelec_utils::modelec_utils
+        modelec_utils::utils
+        modelec_utils::config
 )

 target_include_directories(modelec_gui PUBLIC
--- a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp
+++ b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp
@@ -17,6 +17,9 @@
 #include <modelec_interfaces/srv/map.hpp>
 #include <modelec_interfaces/srv/map_size.hpp>
 #include <modelec_interfaces/msg/obstacle.hpp>
+#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
+
+#include <std_msgs/msg/bool.hpp>

 namespace ModelecGUI {
    class MapPage : public QWidget
@@ -49,12 +52,16 @@ namespace ModelecGUI {

        void OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg);

+        void resizeEvent(QResizeEvent* event) override;
+
        QSvgRenderer* renderer;

        QVBoxLayout* v_layout;
        QHBoxLayout* h_layout;

-        QPoint robotPosPoint = QPoint(0, 0);
+        QPushButton* tirette_button_;
+
+        modelec_interfaces::msg::OdometryPos robotPos;
        std::vector<QPoint> qpoints;
        //std::vector<modelec_interfaces::msg::OdometryAddWaypoint> points;
        modelec_interfaces::msg::OdometryPos go_to_point;
@@ -67,17 +74,27 @@ namespace ModelecGUI {
        int map_width_ = 0;
        int map_height_ = 0;

+        int robot_length_ = 0;
+        int robot_width_ = 0;
+        int enemy_length_ = 0;
+        int enemy_width_ = 0;
+
+        float ratioBetweenMapAndWidgetX_;
+        float ratioBetweenMapAndWidgetY_;
+
        rclcpp::Node::SharedPtr node_;

+        rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr tirette_pub_;
+
        rclcpp::Subscription<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr add_waypoint_sub_;

        rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odometry_sub_;
        rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr go_to_pub_;
        rclcpp::Client<modelec_interfaces::srv::MapSize>::SharedPtr map_client_;
-        rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_added_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_removed_sub_;
        rclcpp::Client<std_srvs::srv::Empty>::SharedPtr ask_map_obstacle_client_;

-
        modelec_interfaces::msg::OdometryPos enemy_pos_;
        rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_pub_;
    };
--- a/src/modelec_gui/src/main.cpp
+++ b/src/modelec_gui/src/main.cpp
@@ -3,6 +3,9 @@
 #include <QThread>
 #include "modelec_gui/modelec_gui.hpp"

+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <modelec_utils/config.hpp>
+
 int main(int argc, char **argv)
 {
    // Initialize the Qt application
@@ -11,6 +14,12 @@ int main(int argc, char **argv)
    // Initialize ROS 2
    rclcpp::init(argc, argv);

+    std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
+    if (!Modelec::Config::load(config_path))
+    {
+        RCLCPP_ERROR(rclcpp::get_logger("modelec_gui"), "Failed to load configuration file: %s", config_path.c_str());
+    }
+
    // Create the node only once
    auto node = rclcpp::Node::make_shared("qt_gui_node");

--- a/src/modelec_gui/src/modelec_gui.cpp
+++ b/src/modelec_gui/src/modelec_gui.cpp
@@ -19,7 +19,7 @@ namespace ModelecGUI {

        setupMenu();

-        resize(1000, 700);
+        resize(1200, 800);
    }

    void ROS2QtGUI::setupMenu() {
--- a/src/modelec_gui/src/pages/map_page.cpp
+++ b/src/modelec_gui/src/pages/map_page.cpp
@@ -3,19 +3,27 @@

 #include <QMouseEvent>
 #include <utility>
+#include <modelec_utils/config.hpp>
+#include <cmath>

 namespace ModelecGUI
 {
    MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), renderer(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this)), node_(node)
    {
+        ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
+        ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
+
        v_layout = new QVBoxLayout(this);
-        v_layout->addStretch();
+
+        tirette_button_ = new QPushButton("Tirette", this);

        h_layout = new QHBoxLayout(this);
        h_layout->addStretch();
+        h_layout->addWidget(tirette_button_);
        h_layout->addStretch();

        v_layout->addLayout(h_layout);
+        v_layout->addStretch();

        this->setLayout(v_layout);

@@ -25,6 +33,12 @@ namespace ModelecGUI
        enemy_pos_.y = 200;
        enemy_pos_.theta = 3.1415/2;

+        robot_length_ = Modelec::Config::get<int>("config.robot.size.length_mm", 200);
+        robot_width_ = Modelec::Config::get<int>("config.robot.size.width_mm", 300);
+
+        enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
+        enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
+
        add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>("odometry/add_waypoint", 100,
            [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) {
                if (lastWapointWasEnd)
@@ -32,7 +46,7 @@ namespace ModelecGUI
                    qpoints.clear();
                    lastWapointWasEnd = false;

-                    qpoints.push_back(QPoint(robotPosPoint.x(), robotPosPoint.y()));
+                    qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_));
                }

                if (msg->is_end)
@@ -40,28 +54,41 @@ namespace ModelecGUI
                    lastWapointWasEnd = true;
                }

-                qpoints.push_back(QPoint(Modelec::mapValue(static_cast<int>(msg->x), 0, 3000, 0, width()),
-                                          height() - Modelec::mapValue(static_cast<int>(msg->y), 0, 2000, 0, height())));
+                qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_, height() - msg->y * ratioBetweenMapAndWidgetY_));
                update();
        });

        // lambda
        odometry_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>("odometry/position", 10,
            [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
-                robotPosPoint.setX(Modelec::mapValue(static_cast<int>(msg->x), 0, 3000, 0, width()));
-                robotPosPoint.setY(height() - Modelec::mapValue(static_cast<int>(msg->y), 0, 2000, 0, height()));
+                robotPos = *msg;
                update();
        });

-        obstacle_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>("nav/obstacle", 40,
+        obstacle_added_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>("nav/obstacle/added", 40,
            [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
                OnObstacleReceived(msg);
        });

+        obstacle_removed_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>("nav/obstacle/removed", 40,
+            [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
+                obstacle_.erase(msg->id);
+        });
+
+
+
        go_to_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("nav/go_to", 10);

        enemy_pos_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("enemy/position", 10);

+        tirette_pub_ = node_->create_publisher<std_msgs::msg::Bool>("tirette", 10);
+
+        connect(tirette_button_, &QPushButton::clicked, this, [this]() {
+            std_msgs::msg::Bool msg;
+            msg.data = true;
+            tirette_pub_->publish(msg);
+        });
+
        // client to nav/map
        map_client_ = node_->create_client<modelec_interfaces::srv::MapSize>("nav/map_size");
        while (!map_client_->wait_for_service(std::chrono::seconds(1))) {
@@ -120,6 +147,7 @@ namespace ModelecGUI

        QPainter painter(this);
        renderer->render(&painter, rect()); // Scales SVG to widget size
+        painter.save();

        painter.setRenderHint(QPainter::Antialiasing);

@@ -129,6 +157,8 @@ namespace ModelecGUI
            painter.drawLine(qpoints[i], qpoints[i + 1]);
        }

+        painter.setPen(Qt::NoPen);
+
        // --- Draw colored points ---
        const int radius = 5;

@@ -138,26 +168,43 @@ namespace ModelecGUI
            else
                painter.setBrush(Qt::red);   // Middle = red

-            painter.setPen(Qt::NoPen);
            painter.drawEllipse(qpoints[i], radius, radius);
        }

-        painter.setBrush(Qt::green);
-        painter.setPen(Qt::NoPen);
-        painter.drawEllipse(robotPosPoint, radius, radius); // Robot position
+        painter.restore();

        if (show_obstacle_)
        {
-            float ratioBetweenMapAndWidget = height() / 2000.0f;
            for (auto & [index, obs] : obstacle_)
            {
+                painter.save();
+
+                QPoint obsPoint(obs.x * ratioBetweenMapAndWidgetX_, height() - obs.y * ratioBetweenMapAndWidgetY_);
+                painter.translate(obsPoint);
+                painter.rotate(90 - obs.theta * (180.0 / M_PI));
                painter.setBrush(Qt::black);
-                painter.drawRect(obs.x * ratioBetweenMapAndWidget, height() - (obs.y + obs.height) * ratioBetweenMapAndWidget, obs.width * ratioBetweenMapAndWidget, obs.height * ratioBetweenMapAndWidget);
+
+                QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), -(obs.height * ratioBetweenMapAndWidgetY_ / 2),
+                       obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_);
+
+                painter.drawRect(toDraw);
+
+                painter.restore();
            }

+            // -- Draw enemy position --
            painter.setBrush(Qt::red);
-            painter.drawRect((enemy_pos_.x - 150.0f) * ratioBetweenMapAndWidget, height() - (enemy_pos_.y + 150.0f) * ratioBetweenMapAndWidget, 300.0f*ratioBetweenMapAndWidget, 300.0f*ratioBetweenMapAndWidget);
+            painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, enemy_width_*ratioBetweenMapAndWidgetX_, enemy_length_*ratioBetweenMapAndWidgetY_);
        }
+
+        // -- Draw robot position --
+        painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_);
+        painter.rotate(90 - robotPos.theta * (180.0 / M_PI));
+
+        QRect rect(-(robot_width_ * ratioBetweenMapAndWidgetX_ / 2), -(robot_length_ * ratioBetweenMapAndWidgetY_ / 2),
+                   robot_width_ * ratioBetweenMapAndWidgetX_, robot_length_ * ratioBetweenMapAndWidgetY_);
+        painter.setBrush(Qt::green);
+        painter.drawRect(rect);
    }

    void MapPage::mousePressEvent(QMouseEvent* event)
@@ -203,4 +250,12 @@ namespace ModelecGUI
    {
        obstacle_.emplace(msg->id, *msg);
    }
+
+    void MapPage::resizeEvent(QResizeEvent* event)
+    {
+        QWidget::resizeEvent(event);
+
+        ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
+        ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
+    }
 }
--- a/src/modelec_interfaces/msg/Map/Obstacle.msg
+++ b/src/modelec_interfaces/msg/Map/Obstacle.msg
@@ -1,5 +1,6 @@
 int32 id
 int32 x
 int32 y
+float32 theta
 int32 width
-int32 height
+int32 height
--- a/src/modelec_sensors/CMakeLists.txt
+++ b/src/modelec_sensors/CMakeLists.txt
@@ -18,7 +18,7 @@ find_library(WIRINGPI_LIB wiringPi)

 add_executable(lidar_controller src/lidar_controller.cpp)
 ament_target_dependencies(lidar_controller rclcpp std_msgs sensor_msgs modelec_interfaces)
-target_link_libraries(lidar_controller modelec_utils::modelec_utils)
+target_link_libraries(lidar_controller modelec_utils::utils)
 target_include_directories(lidar_controller PUBLIC
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
        $<INSTALL_INTERFACE:include>
@@ -26,7 +26,7 @@ target_include_directories(lidar_controller PUBLIC

 add_executable(tirette_controller ../modelec_sensors/src/tirette_controller.cpp)
 ament_target_dependencies(tirette_controller rclcpp std_msgs modelec_interfaces)
-target_link_libraries(tirette_controller ${WIRINGPI_LIB} modelec_utils::modelec_utils)
+target_link_libraries(tirette_controller ${WIRINGPI_LIB} modelec_utils::utils)
 target_include_directories(tirette_controller PUBLIC
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
        $<INSTALL_INTERFACE:include>
--- a/src/modelec_strat/CMakeLists.txt
+++ b/src/modelec_strat/CMakeLists.txt
@@ -9,7 +9,6 @@ find_package(ament_cmake REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
-find_package(tinyxml2 REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(ament_index_cpp REQUIRED)

@@ -18,7 +17,7 @@ find_package(modelec_utils REQUIRED)

 add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/mission_manager.cpp src/missions/prepare_concert_mission.cpp src/missions/promotion_mission.cpp src/missions/go_home_mission.cpp src/obstacle.cpp)
 ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces ament_index_cpp)
-target_link_libraries(strat_fsm modelec_utils::modelec_utils tinyxml2::tinyxml2)
+target_link_libraries(strat_fsm modelec_utils::utils modelec_utils::config)
 target_include_directories(strat_fsm PUBLIC
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
        $<INSTALL_INTERFACE:include>
@@ -26,7 +25,7 @@ target_include_directories(strat_fsm PUBLIC

 add_executable(enemy_manager src/enemy_manager.cpp)
 ament_target_dependencies(enemy_manager rclcpp std_msgs sensor_msgs std_srvs modelec_interfaces ament_index_cpp)
-target_link_libraries(enemy_manager modelec_utils::modelec_utils tinyxml2::tinyxml2)
+target_link_libraries(enemy_manager modelec_utils::utils modelec_utils::config)
 target_include_directories(enemy_manager PUBLIC
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
        $<INSTALL_INTERFACE:include>
--- a/src/modelec_strat/data/config.xml
+++ b/src/modelec_strat/data/config.xml
@@ -16,6 +16,7 @@
        <size>
            <width_mm>300</width_mm>
            <length_mm>300</length_mm>
+            <margin_mm>0</margin_mm>
        </size>
    </robot>

--- a/src/modelec_strat/data/obstacles.xml
+++ b/src/modelec_strat/data/obstacles.xml
@@ -1,25 +1,25 @@
 <Obstacles>
    <!-- TOP -->
-    <Obstacle id="1" x="600" y="1800" width="450" height="200" type="estrade"/>
-    <Obstacle id="2" x="1050" y="1550" width="900" height="450" type="estrade"/>
-    <Obstacle id="3" x="1950" y="1800" width="450" height="200" type="estrade"/>
+    <Obstacle id="1" x="850" y="1900" theta="1.5708" width="400" height="200" type="estrade"/>
+    <Obstacle id="2" x="1500" y="1775" theta="1.5708" width="900" height="450" type="estrade"/>
+    <Obstacle id="3" x="2150" y="1900" theta="1.5708" width="400" height="200" type="estrade"/>

    <!-- Gradin TOP TO BOTTOM    LEFT -->
-    <Obstacle id="10" x="600" y="1650" width="450" height="150" type="gradin"/>
-    <Obstacle id="11" x="0" y="1100" width="150" height="450" type="gradin"/>
-    <Obstacle id="12" x="900" y="900" width="400" height="100" type="gradin"/>
-    <Obstacle id="13" x="0" y="200" width="130" height="400" type="gradin"/>
-    <Obstacle id="14" x="570" y="200" width="400" height="100" type="gradin"/>
+    <Obstacle id="10" x="825" y="1725" theta="1.5708" width="400" height="100" type="gradin"/>
+    <Obstacle id="11" x="75" y="1325" theta="0" width="400" height="100" type="gradin"/>
+    <Obstacle id="12" x="1100" y="950" theta="1.5708" width="400" height="100" type="gradin"/>
+    <Obstacle id="13" x="75" y="400" theta="0" width="400" height="100" type="gradin"/>
+    <Obstacle id="14" x="775" y="250" theta="1.5708" width="400" height="100" type="gradin"/>

    <!-- Gradin TOP TO BOTTOM    RIGHT -->
-    <Obstacle id="20" x="1950" y="1650" width="450" height="150" type="gradin"/>
-    <Obstacle id="21" x="2850" y="1100" width="150" height="450" type="gradin"/>
-    <Obstacle id="22" x="1700" y="900" width="400" height="100" type="gradin"/>
-    <Obstacle id="23" x="2850" y="200" width="130" height="400" type="gradin"/>
-    <Obstacle id="24" x="2000" y="200" width="400" height="100" type="gradin"/>
+    <Obstacle id="20" x="2175" y="1725" theta="1.5708" width="400" height="100" type="gradin"/>
+    <Obstacle id="21" x="2925" y="1325" theta="0" width="400" height="100" type="gradin"/>
+    <Obstacle id="22" x="1900" y="950" theta="1.5708" width="400" height="100" type="gradin"/>
+    <Obstacle id="23" x="2925" y="400" theta="0" width="400" height="100" type="gradin"/>
+    <Obstacle id="24" x="2225" y="250" theta="1.5708" width="400" height="100" type="gradin"/>

    <!-- PAMI Start -->
-    <Obstacle id="30" x="0" y="1550" width="150" height="450" type="pami-start"/>
-    <Obstacle id="31" x="2850" y="1550" width="150" height="450" type="pami-start"/>
+    <Obstacle id="30" x="75" y="1750" theta="1.5708" width="150" height="450" type="pami-start"/>
+    <Obstacle id="31" x="2925" y="1750" theta="1.5708" width="150" height="450" type="pami-start"/>

 </Obstacles>
--- a/src/modelec_strat/include/modelec_strat/config.hpp
+++ b/src/modelec_strat/include/modelec_strat/config.hpp
@@ -1,97 +0,0 @@
-#pragma once
-#include <map>
-#include <string>
-#include <tinyxml2.h>
-
-#include "obstacle.hpp"
-
-namespace Modelec
-{
-    class Config
-    {
-
-    public:
-        static bool load(const std::string& filepath);
-
-        template<typename T>
-        static T get(const std::string& key, const T& default_value = T());
-
-        static void printAll();
-
-    private:
-        static void parseNode(tinyxml2::XMLElement* element, const std::string& prefix);
-
-        static inline std::unordered_map<std::string, std::string> values_;
-    };
-
-    inline bool Config::load(const std::string& filepath)
-    {
-        tinyxml2::XMLDocument doc;
-        if (doc.LoadFile(filepath.c_str()) != tinyxml2::XML_SUCCESS) {
-            RCLCPP_ERROR(rclcpp::get_logger("Config"), "Failed to load XML file: %s", doc.ErrorName());
-            return false;
-        }
-
-        auto* root = doc.RootElement();
-        if (!root)
-        {
-            RCLCPP_ERROR(rclcpp::get_logger("Config"), "No root element found in XML file");
-            return false;
-        }
-
-        parseNode(root, "");
-        return true;
-    }
-
-    inline void Config::parseNode(tinyxml2::XMLElement* element, const std::string& prefix) {
-        std::string key_prefix = prefix.empty() ? element->Name() : prefix + "." + element->Name();
-
-        const char* text = element->GetText();
-        if (text && std::string(text).find_first_not_of(" \n\t") != std::string::npos) {
-            values_[key_prefix] = text;
-        }
-
-        const tinyxml2::XMLAttribute* attr = element->FirstAttribute();
-        while (attr) {
-            std::string attr_key = key_prefix + "@" + attr->Name();
-            values_[attr_key] = attr->Value();
-            attr = attr->Next();
-        }
-
-        for (tinyxml2::XMLElement* child = element->FirstChildElement(); child; child = child->NextSiblingElement()) {
-            parseNode(child, key_prefix);
-        }
-    }
-
-    inline void Config::printAll()
-    {
-        RCLCPP_INFO(rclcpp::get_logger("Config"), "Loaded %zu configuration values", values_.size());
-
-        for (const auto& pair : values_) {
-            RCLCPP_INFO(rclcpp::get_logger("Config"), "%s: %s", pair.first.c_str(), pair.second.c_str());
-        }
-    }
-
-    template<typename T>
-    T Config::get(const std::string& key, const T& default_value) {
-        auto it = values_.find(key);
-        if (it == values_.end()) return default_value;
-
-        std::istringstream iss(it->second);
-        T result;
-        if (!(iss >> result)) return default_value;
-        return result;
-    }
-
-    template<>
-    inline std::string Config::get<std::string>(const std::string& key, const std::string& default_value) {
-        auto it = values_.find(key);
-        return it != values_.end() ? it->second : default_value;
-    }
-
-    template<>
-    inline bool Config::get<bool>(const std::string& key, const bool& default_value) {
-        auto str = get<std::string>(key, default_value ? "true" : "false");
-        return str == "true" || str == "1";
-    }
-}
--- a/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp
+++ b/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp
@@ -17,8 +17,10 @@ namespace Modelec {
    private:
        enum Step {
            GO_TO_COLUMN,
+            GO_CLOSE_TO_COLUMN,
            TAKE_COLUMN,
            GO_TO_PLATFORM,
+            GO_CLOSE_TO_PLATFORM,
            PLACE_PLATFORM,
            DONE
        } step_;
@@ -26,6 +28,8 @@ namespace Modelec {
        MissionStatus status_;
        std::shared_ptr<NavigationHelper> nav_;
        rclcpp::Node::SharedPtr node_;
+
+        Obstacle column_;
    };

 }
--- a/src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp
+++ b/src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp
@@ -23,6 +23,7 @@ namespace Modelec
        enum Step {
            GO_TO_FRONT,
            DEPLOY_BANNER,
+            GO_FORWARD,
            DONE
        } step_;

--- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
+++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
@@ -32,9 +32,10 @@ namespace Modelec {

        bool HasArrived() const;

-        void GoTo(const PosMsg::SharedPtr &goal);
+        void GoTo(const PosMsg::SharedPtr &goal, bool isClose = false);
+        void GoTo(int x, int y, double theta, bool isClose = false);

-        WaypointListMsg FindPath(const PosMsg::SharedPtr &goal);
+        WaypointListMsg FindPath(const PosMsg::SharedPtr &goal, bool isClose = false);

    protected:
        void OnWaypointReach(const WaypointReachMsg::SharedPtr msg);
--- a/src/modelec_strat/include/modelec_strat/obstacle.hpp
+++ b/src/modelec_strat/include/modelec_strat/obstacle.hpp
@@ -9,8 +9,8 @@
 namespace Modelec {
    class Obstacle {
    public:
-        Obstacle() : id_(-1), x_(0), y_(0), w_(0), h_(0), type_("unknown") {}
-        Obstacle(int id, int x, int y, int w, int h, const std::string& type);
+        Obstacle() : id_(-1), x_(0), y_(0), w_(0), h_(0), theta_(0), type_("unknown") {}
+        Obstacle(int id, int x, int y, double theta, int w, int h, const std::string& type);
        Obstacle(tinyxml2::XMLElement * obstacleElem);
        Obstacle(const modelec_interfaces::msg::Obstacle& msg);
        Obstacle(const Obstacle& other) = default;
@@ -20,6 +20,7 @@ namespace Modelec {
        int id() const { return id_; }
        int x() const { return x_; }
        int y() const { return y_; }
+        double theta() const { return theta_; }
        int width() const { return w_; }
        int height() const { return h_; }
        const std::string& type() const { return type_; }
@@ -27,11 +28,24 @@ namespace Modelec {
        void setId(int id) { id_ = id; }
        void setX(int x) { x_ = x; }
        void setY(int y) { y_ = y; }
+        void setTheta(double theta) { theta_ = theta; }
        void setWidth(int w) { w_ = w; }
        void setHeight(int h) { h_ = h; }
        void setType(const std::string& type) { type_ = type; }
+
+        void setPosition(int x, int y, double theta) {
+            x_ = x;
+            y_ = y;
+            theta_ = theta;
+        }
+
+        void setSize(int w, int h) {
+            w_ = w;
+            h_ = h;
+        }
    private:
        int id_, x_, y_, w_, h_;
+        double theta_;
        std::string type_;
    };
 }
--- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp
+++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp
@@ -45,7 +45,7 @@ namespace Modelec {
        rclcpp::Node::SharedPtr getNode() const;

        WaypointListMsg FindPath(const PosMsg::SharedPtr& start,
-                      const PosMsg::SharedPtr& goal);
+                                 const PosMsg::SharedPtr& goal, bool isClose = false);

        //void SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal);

@@ -53,6 +53,12 @@ namespace Modelec {

        void SetCurrentPos(const PosMsg::SharedPtr& pos);

+        Obstacle GetObstacle(int id) const;
+
+        void RemoveObstacle(int id);
+
+        void AddObstacle(const Obstacle& obstacle);
+
    protected:
        void HandleMapRequest(
            const std::shared_ptr<modelec_interfaces::srv::Map::Request> request,
@@ -84,6 +90,7 @@ namespace Modelec {
        int enemy_width_mm_ = 0;
        int enemy_length_mm_ = 0;
        int enemy_margin_mm_ = 0;
+        int margin_mm_ = 0;


        std::map<int, Obstacle> obstacle_map_;
@@ -96,12 +103,14 @@ namespace Modelec {
        bool has_enemy_pos_ = false;

        rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
-        rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr map_pub_;
        rclcpp::Publisher<WaypointMsg>::SharedPtr waypoint_pub_;

        rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_add_sub_;
        rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_remove_sub_;

+        rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_add_pub_;
+        rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_remove_pub_;
+
        rclcpp::Service<modelec_interfaces::srv::Map>::SharedPtr map_srv_;
        rclcpp::Service<modelec_interfaces::srv::MapSize>::SharedPtr map_size_srv_;
        rclcpp::Service<std_srvs::srv::Empty>::SharedPtr ask_obstacle_srv_;
--- a/src/modelec_strat/src/config.cpp
+++ b/src/modelec_strat/src/config.cpp
--- a/src/modelec_strat/src/enemy_manager.cpp
+++ b/src/modelec_strat/src/enemy_manager.cpp
@@ -1,6 +1,6 @@
 #include <modelec_strat/enemy_manager.hpp>
 #include <cmath>
-#include <modelec_strat/config.hpp>
+#include <modelec_utils/config.hpp>
 #include <ament_index_cpp/get_package_share_directory.hpp>

 namespace Modelec
--- a/src/modelec_strat/src/missions/go_home_mission.cpp
+++ b/src/modelec_strat/src/missions/go_home_mission.cpp
@@ -10,7 +10,7 @@ namespace Modelec
    {
        node_ = node;

-        // go home
+        nav_->GoTo(400, 1600.0, -M_PI_2);

        status_ = MissionStatus::RUNNING;
    }
--- a/src/modelec_strat/src/missions/prepare_concert_mission.cpp
+++ b/src/modelec_strat/src/missions/prepare_concert_mission.cpp
@@ -2,15 +2,18 @@

 namespace Modelec
 {
-    PrepareConcertMission::PrepareConcertMission(const std::shared_ptr<NavigationHelper>& nav) : step_(GO_TO_COLUMN), status_(MissionStatus::READY), nav_(nav)
+    PrepareConcertMission::PrepareConcertMission(const std::shared_ptr<NavigationHelper>& nav) : step_(GO_TO_COLUMN),
+        status_(MissionStatus::READY), nav_(nav)
    {
    }

    void PrepareConcertMission::start(rclcpp::Node::SharedPtr node)
    {
+        column_ = nav_->getPathfinding()->GetObstacle(14);
+
        node_ = node;

-        
+        nav_->GoTo(775, 480, -M_PI_2);

        status_ = MissionStatus::RUNNING;
    }
@@ -24,14 +27,39 @@ namespace Modelec
        switch (step_)
        {
        case GO_TO_COLUMN:
+            nav_->GoTo(775, 460, -M_PI_2, true);
+
+            step_ = GO_CLOSE_TO_COLUMN;
+            break;
+        case GO_CLOSE_TO_COLUMN:
+            nav_->getPathfinding()->RemoveObstacle(column_.id());
+
            step_ = TAKE_COLUMN;
            break;
-
        case TAKE_COLUMN:
+            {
+                nav_->GoTo(2500, 875, 0);
+            }
+
            step_ = GO_TO_PLATFORM;
            break;

        case GO_TO_PLATFORM:
+            {
+                nav_->GoTo(2700, 875, 0, true);
+            }
+
+            step_ = GO_CLOSE_TO_PLATFORM;
+            break;
+
+        case GO_CLOSE_TO_PLATFORM:
+            {
+                column_.setX(2925);
+                column_.setY(875);
+                column_.setTheta(0);
+                nav_->getPathfinding()->AddObstacle(column_);
+            }
+
            step_ = PLACE_PLATFORM;
            break;

--- a/src/modelec_strat/src/missions/promotion_mission.cpp
+++ b/src/modelec_strat/src/missions/promotion_mission.cpp
@@ -10,11 +10,39 @@ namespace Modelec
    {
        node_ = node;

+        nav_->GoTo(1225, 152, M_PI_2, true);
+
        status_ = MissionStatus::RUNNING;
    }

    void PromotionMission::update()
    {
+        if (status_ != MissionStatus::RUNNING) return;
+
+        if (!nav_->HasArrived()) return;
+
+        switch (step_)
+        {
+        case GO_TO_FRONT:
+            // TODO deploy the banner here
+
+            step_ = DEPLOY_BANNER;
+            break;
+
+        case DEPLOY_BANNER:
+            nav_->GoTo(1225, 300, M_PI_2, true);
+
+            step_ = GO_FORWARD;
+            break;
+
+        case GO_FORWARD:
+            step_ = DONE;
+            status_ = MissionStatus::DONE;
+            break;
+
+        default:
+            break;
+        }
    }

    MissionStatus PromotionMission::getStatus() const
--- a/src/modelec_strat/src/navigation_helper.cpp
+++ b/src/modelec_strat/src/navigation_helper.cpp
@@ -159,15 +159,32 @@ namespace Modelec {
        return waypoints_.back().reached;
    }

-    void NavigationHelper::GoTo(const PosMsg::SharedPtr& goal)
+    void NavigationHelper::GoTo(const PosMsg::SharedPtr& goal, bool isClose)
    {
-        SendWaypoint(FindPath(goal));
-        //pathfinding_->SetStartAndGoal(current_pos_, goal);
+        waypoints_.clear();
+
+        auto wml = FindPath(goal, isClose);
+
+        for (auto & w : wml)
+        {
+            waypoints_.emplace_back(w);
+        }
+
+        SendWaypoint();
    }

-    WaypointListMsg NavigationHelper::FindPath(const PosMsg::SharedPtr& goal)
+    void NavigationHelper::GoTo(int x, int y, double theta, bool isClose)
    {
-        return pathfinding_->FindPath(current_pos_, goal);
+        PosMsg::SharedPtr goal = std::make_shared<PosMsg>();
+        goal->x = x;
+        goal->y = y;
+        goal->theta = theta;
+        GoTo(goal, isClose);
+    }
+
+    WaypointListMsg NavigationHelper::FindPath(const PosMsg::SharedPtr& goal, bool isClose)
+    {
+        return pathfinding_->FindPath(current_pos_, goal, isClose);
    }

    void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr msg)
--- a/src/modelec_strat/src/obstacle.cpp
+++ b/src/modelec_strat/src/obstacle.cpp
@@ -2,8 +2,8 @@

 namespace Modelec
 {
-    Obstacle::Obstacle(int id, int x, int y, int w, int h, const std::string& type)
-        : id_(id), x_(x), y_(y), w_(w), h_(h), type_(type)
+    Obstacle::Obstacle(int id, int x, int y, double theta, int w, int h, const std::string& type)
+        : id_(id), x_(x), y_(y), w_(w), h_(h), theta_(theta), type_(type)
    {
    }

@@ -13,6 +13,7 @@ namespace Modelec
        if (obstacleElem->QueryIntAttribute("id", &id_) != tinyxml2::XML_SUCCESS ||
            obstacleElem->QueryIntAttribute("x", &x_) != tinyxml2::XML_SUCCESS ||
            obstacleElem->QueryIntAttribute("y", &y_) != tinyxml2::XML_SUCCESS ||
+            obstacleElem->QueryDoubleAttribute("theta", &theta_) != tinyxml2::XML_SUCCESS ||
            obstacleElem->QueryIntAttribute("width", &w_) != tinyxml2::XML_SUCCESS ||
            obstacleElem->QueryIntAttribute("height", &h_) != tinyxml2::XML_SUCCESS ||
            obstacleElem->QueryStringAttribute("type", &type) != tinyxml2::XML_SUCCESS)
@@ -24,7 +25,7 @@ namespace Modelec
    }

    Obstacle::Obstacle(const modelec_interfaces::msg::Obstacle& msg)
-        : id_(msg.id), x_(msg.x), y_(msg.y), w_(msg.width), h_(msg.height), type_("unknown")
+        : id_(msg.id), x_(msg.x), y_(msg.y), w_(msg.width), h_(msg.height), theta_(msg.theta), type_("unknown")
    {
    }

@@ -37,6 +38,7 @@ namespace Modelec
        msg.y = y_;
        msg.width = w_;
        msg.height = h_;
+        msg.theta = theta_;

        return msg;
    }
--- a/src/modelec_strat/src/pathfinding.cpp
+++ b/src/modelec_strat/src/pathfinding.cpp
@@ -1,7 +1,6 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <modelec_strat/pathfinding.hpp>
-
-#include <modelec_strat/config.hpp>
+#include <modelec_utils/config.hpp>

 namespace Modelec {

@@ -35,6 +34,7 @@ namespace Modelec {

        robot_length_mm_ = Config::get<int>("config.robot.size.length_mm", 300);
        robot_width_mm_ = Config::get<int>("config.robot.size.width_mm", 300);
+        margin_mm_ = Config::get<int>("config.robot.size.margin_mm", 100);

        enemy_length_mm_ = Config::get<int>("config.enemy.size.length_mm", 300);
        enemy_width_mm_ = Config::get<int>("config.enemy.size.width_mm", 300);
@@ -50,24 +50,26 @@ namespace Modelec {
            RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML");
        }

-        map_pub_ = node_->create_publisher<modelec_interfaces::msg::Obstacle>(
-            "nav/obstacle", 40);
-
        obstacle_add_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>(
-            "nav/obstacle/add", 10,
+            "obstacle/add", 10,
            [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
                RCLCPP_INFO(node_->get_logger(), "Obstacle add request received");
-                obstacle_map_[msg->id] = Obstacle(*msg);
-                map_pub_->publish(*msg);
+                AddObstacle(Obstacle(*msg));
            });

+        obstacle_add_pub_ = node_->create_publisher<modelec_interfaces::msg::Obstacle>(
+            "nav/obstacle/added", 40);
+
        obstacle_remove_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>(
-            "nav/obstacle/remove", 10,
+            "obstacle/remove", 10,
            [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
                RCLCPP_INFO(node_->get_logger(), "Obstacle remove request received");
-                obstacle_map_.erase(msg->id);
+                RemoveObstacle(msg->id);
            });

+        obstacle_remove_pub_ = node_->create_publisher<modelec_interfaces::msg::Obstacle>(
+            "nav/obstacle/removed", 40);
+
        ask_obstacle_srv_ = node_->create_service<std_srvs::srv::Empty>(
            "nav/ask_map_obstacle",
            [this](const std::shared_ptr<std_srvs::srv::Empty::Request> request,
@@ -136,7 +138,7 @@ namespace Modelec {
        }
    }*/

-    WaypointListMsg Pathfinding::FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal)
+    WaypointListMsg Pathfinding::FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal, bool isClose)
    {
        if (!start || !goal)
        {
@@ -150,8 +152,8 @@ namespace Modelec {
        const float cell_size_mm_y = map_height_mm_ / grid_height_;

        // Robot dimensions
-        const int inflate_x = std::ceil((robot_width_mm_ / 2.0f) / cell_size_mm_x);
-        const int inflate_y = std::ceil((robot_length_mm_ / 2.0f) / cell_size_mm_y);
+        const int inflate_x = isClose ? 0 : std::ceil((robot_width_mm_ + margin_mm_ / 2.0f) / cell_size_mm_x);
+        const int inflate_y = isClose ? 0 : std::ceil((robot_length_mm_ + margin_mm_ / 2.0f) / cell_size_mm_y);

        // 1. Build fresh empty grid
        grid_.clear();
@@ -204,21 +206,71 @@ namespace Modelec {
        // 2. Fill obstacles with inflation
        for (const auto& [id, obstacle] : obstacle_map_)
        {
-            int x_start = std::max(0, (int)(obstacle.x() / cell_size_mm_x) - inflate_x);
-            int y_start = std::max(0, (int)(obstacle.y() / cell_size_mm_y) - inflate_y);
-            int x_end = std::min(grid_width_ - 1, (int)((obstacle.x() + obstacle.width()) / cell_size_mm_x) + inflate_x);
-            int y_end = std::min(grid_height_ - 1, (int)((obstacle.y() + obstacle.height()) / cell_size_mm_y) + inflate_y);
+            float cx = obstacle.x();
+            float cy = obstacle.y();
+            float width = obstacle.width() + inflate_x * cell_size_mm_x;
+            float height = obstacle.height() + inflate_y * cell_size_mm_y;
+            float theta = M_PI_2 - obstacle.theta();

-            // Inverser l'axe Y
-            y_start = (grid_height_ - 1) - y_start;
-            y_end = (grid_height_ - 1) - y_end;
-            if (y_start > y_end) std::swap(y_start, y_end);
+            float dx = width / 2.0f;
+            float dy = height / 2.0f;

+            // Compute corners in local space and rotate+translate to world
+            std::vector<std::pair<float, float>> corners = {
+                {-dx, -dy}, {dx, -dy}, {dx, dy}, {-dx, dy}
+            };
+
+            for (auto& [x, y] : corners)
+            {
+                float rx = x * std::cos(theta) - y * std::sin(theta);
+                float ry = x * std::sin(theta) + y * std::cos(theta);
+                x = rx + cx;
+                y = ry + cy;
+            }
+
+            // Compute bounding box in grid space
+            float min_x = corners[0].first;
+            float max_x = corners[0].first;
+            float min_y = corners[0].second;
+            float max_y = corners[0].second;
+
+            for (const auto& [x, y] : corners)
+            {
+                min_x = std::min(min_x, x);
+                max_x = std::max(max_x, x);
+                min_y = std::min(min_y, y);
+                max_y = std::max(max_y, y);
+            }
+
+            int x_start = std::max(0, (int)(min_x / cell_size_mm_x));
+            int x_end   = std::min(grid_width_ - 1, (int)(max_x / cell_size_mm_x));
+            int y_start = std::max(0, (int)(min_y / cell_size_mm_y));
+            int y_end   = std::min(grid_height_ - 1, (int)(max_y / cell_size_mm_y));
+
+            // Mark cells that fall inside rotated rectangle
            for (int y = y_start; y <= y_end; ++y)
            {
                for (int x = x_start; x <= x_end; ++x)
                {
-                    grid_[y][x] = 1; // mark as obstacle
+                    // Convert cell center to world space
+                    float wx = (x + 0.5f) * cell_size_mm_x;
+                    float wy = (y + 0.5f) * cell_size_mm_y;
+
+                    // Inverse transform: world -> obstacle local space
+                    float dx_ = wx - cx;
+                    float dy_ = wy - cy;
+
+                    float lx = dx_ * std::cos(-theta) - dy_ * std::sin(-theta);
+                    float ly = dx_ * std::sin(-theta) + dy_ * std::cos(-theta);
+
+                    if (std::abs(lx) <= dx + 1 && std::abs(ly) <= dy + 1)
+                    {
+                        int gy = (grid_height_ - 1) - y;
+                        if (x >= 0 && gy >= 0 && x < grid_width_ && gy < grid_height_)
+                        {
+                            grid_[gy][x] = 1;
+                        }
+                    }
                }
            }
        }
@@ -239,7 +291,14 @@ namespace Modelec {

        if (grid_[start_y][start_x] == 1 || grid_[goal_y][goal_x] == 1)
        {
-            RCLCPP_WARN(node_->get_logger(), "Start or Goal inside an obstacle");
+            if (grid_[start_y][start_x] == 1)
+            {
+                RCLCPP_WARN(node_->get_logger(), "Start inside an obstacle");
+            }
+            else
+            {
+                RCLCPP_WARN(node_->get_logger(), "Goal inside an obstacle");
+            }
            return waypoints;
        }

@@ -407,7 +466,7 @@ namespace Modelec {
            WaypointMsg wp;
            wp.x = x * cell_size_mm_x;
            wp.y = (grid_height_ - 1 - y) * cell_size_mm_y;
-            wp.theta = 0;
+            wp.theta = i == smooth_path.size() - 1 ? goal->theta : 0.0f; // last waypoint takes goal theta
            wp.id = id++;
            wp.is_end = false;
            waypoints.push_back(wp);
@@ -437,6 +496,27 @@ namespace Modelec {
        current_start_ = pos;
    }

+    Obstacle Pathfinding::GetObstacle(int id) const
+    {
+        return obstacle_map_.at(id);
+    }
+
+    void Pathfinding::RemoveObstacle(int id)
+    {
+        obstacle_map_.erase(id);
+
+        modelec_interfaces::msg::Obstacle msg;
+        msg.id = id;
+        obstacle_remove_pub_->publish(msg);
+    }
+
+    void Pathfinding::AddObstacle(const Obstacle& obstacle)
+    {
+        obstacle_map_[obstacle.id()] = obstacle;
+        modelec_interfaces::msg::Obstacle msg = obstacle.toMsg();
+        obstacle_add_pub_->publish(msg);
+    }
+
    void Pathfinding::HandleMapRequest(const std::shared_ptr<modelec_interfaces::srv::Map::Request>,
                                       const std::shared_ptr<modelec_interfaces::srv::Map::Response> response)
    {
@@ -464,7 +544,7 @@ namespace Modelec {
    {
        for (auto & [index, obs] : obstacle_map_)
        {
-            map_pub_->publish(obs.toMsg());
+            obstacle_add_pub_->publish(obs.toMsg());
        }
    }

--- a/src/modelec_strat/src/strat_fms.cpp
+++ b/src/modelec_strat/src/strat_fms.cpp
@@ -1,5 +1,5 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
-#include <modelec_strat/config.hpp>
+#include <modelec_utils/config.hpp>
 #include <modelec_strat/strat_fms.hpp>

 namespace Modelec
@@ -74,21 +74,17 @@ namespace Modelec
            {
                auto elapsed = now - match_start_time_;
                // select mission in a good way there.
-                if (elapsed.seconds() < 10)
+                if (elapsed.seconds() < 2)
                {
-                    transition(State::DO_PROMOTION, "Start by preparing the concert");
+                    transition(State::DO_PROMOTION, "Start promotion");
                }
-                else if (elapsed.seconds() < 30)
+                else if (elapsed.seconds() < 20)
                {
-                    transition(State::DO_PROMOTION, "Proceed to promotion");
-                }
-                else if (elapsed.seconds() >= 100.0)
-                {
-                    transition(State::DO_GO_HOME, "Cleanup and finish match");
+                    transition(State::DO_PREPARE_CONCERT, "Proceed to concert");
                }
                else
                {
-                    transition(State::STOP, "Nothing more to do");
+                    transition(State::DO_GO_HOME, "Cleanup and finish match");
                }
                break;
            }
--- a/src/modelec_utils/CMakeLists.txt
+++ b/src/modelec_utils/CMakeLists.txt
@@ -8,37 +8,69 @@ endif()
 # Dependencies
 find_package(ament_cmake REQUIRED)

-# str Lib
-add_library(${PROJECT_NAME}
-        src/utils.cpp
+find_package(tinyxml2 REQUIRED)
+
+# === modelec::config ===
+add_library(config
+        src/config.cpp
 )

-target_include_directories(${PROJECT_NAME} PUBLIC
+target_include_directories(config PUBLIC
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
        $<INSTALL_INTERFACE:include>
 )

+target_link_libraries(config PUBLIC tinyxml2::tinyxml2)
+
+# === modelec::utils ===
+add_library(utils
+        src/utils.cpp
+)
+
+target_include_directories(utils PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
+# Si utils dépend de config, décommente :
+# target_link_libraries(${PROJECT_NAME} PUBLIC modelec_config)
+
 # === Install headers ===
 install(DIRECTORY include/
        DESTINATION include/
 )

-# === Install and export the library ===
-install(TARGETS ${PROJECT_NAME}
-        EXPORT ${PROJECT_NAME}Targets
+# === Install and export config lib ===
+install(TARGETS config
+        EXPORT configTargets
        ARCHIVE DESTINATION lib
        LIBRARY DESTINATION lib
        RUNTIME DESTINATION bin
 )

-install(EXPORT ${PROJECT_NAME}Targets
-        FILE ${PROJECT_NAME}Targets.cmake
+install(EXPORT configTargets
+        FILE configTargets.cmake
        NAMESPACE ${PROJECT_NAME}::
-        DESTINATION lib/cmake/${PROJECT_NAME}
+        DESTINATION lib/cmake/config
 )

-# === Export package metadata ===
-ament_export_targets(${PROJECT_NAME}Targets)
+# === Install and export utils lib ===
+install(TARGETS utils
+        EXPORT utilsTargets
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+)
+
+install(EXPORT utilsTargets
+        FILE utilsTargets.cmake
+        NAMESPACE ${PROJECT_NAME}::
+        DESTINATION lib/cmake/utils
+)
+
+# === Export metadata for ament ===
+ament_export_targets(utilsTargets)
+ament_export_targets(configTargets)
 ament_export_include_directories(include)

-ament_package()
+ament_package()
--- a/src/modelec_utils/include/modelec_utils/config.hpp
+++ b/src/modelec_utils/include/modelec_utils/config.hpp
@@ -0,0 +1,46 @@
+#pragma once
+
+#include <string>
+#include <tinyxml2.h>
+#include <unordered_map>
+
+namespace Modelec
+{
+    class Config
+    {
+
+    public:
+        static bool load(const std::string& filepath);
+
+        template<typename T>
+        static T get(const std::string& key, const T& default_value = T());
+
+    private:
+        static void parseNode(tinyxml2::XMLElement* element, const std::string& prefix);
+
+        static inline std::unordered_map<std::string, std::string> values_;
+    };
+
+    template<typename T>
+    T Config::get(const std::string& key, const T& default_value) {
+        auto it = values_.find(key);
+        if (it == values_.end()) return default_value;
+
+        std::istringstream iss(it->second);
+        T result;
+        if (!(iss >> result)) return default_value;
+        return result;
+    }
+
+    template<>
+    inline std::string Config::get<std::string>(const std::string& key, const std::string& default_value) {
+        auto it = values_.find(key);
+        return it != values_.end() ? it->second : default_value;
+    }
+
+    template<>
+    inline bool Config::get<bool>(const std::string& key, const bool& default_value) {
+        auto str = get<std::string>(key, default_value ? "true" : "false");
+        return str == "true" || str == "1";
+    }
+}
--- a/src/modelec_utils/src/config.cpp
+++ b/src/modelec_utils/src/config.cpp
@@ -0,0 +1,41 @@
+#include <modelec_utils/config.hpp>
+
+namespace Modelec
+{
+    bool Config::load(const std::string& filepath)
+    {
+        tinyxml2::XMLDocument doc;
+        if (doc.LoadFile(filepath.c_str()) != tinyxml2::XML_SUCCESS) {
+            return false;
+        }
+
+        auto* root = doc.RootElement();
+        if (!root)
+        {
+            return false;
+        }
+
+        parseNode(root, "");
+        return true;
+    }
+
+    void Config::parseNode(tinyxml2::XMLElement* element, const std::string& prefix) {
+        std::string key_prefix = prefix.empty() ? element->Name() : prefix + "." + element->Name();
+
+        const char* text = element->GetText();
+        if (text && std::string(text).find_first_not_of(" \n\t") != std::string::npos) {
+            values_[key_prefix] = text;
+        }
+
+        const tinyxml2::XMLAttribute* attr = element->FirstAttribute();
+        while (attr) {
+            std::string attr_key = key_prefix + "@" + attr->Name();
+            values_[attr_key] = attr->Value();
+            attr = attr->Next();
+        }
+
+        for (tinyxml2::XMLElement* child = element->FirstChildElement(); child; child = child->NextSiblingElement()) {
+            parseNode(child, key_prefix);
+        }
+    }
+}