Develop strat 7 (#32)

Co-authored-by: modelec <modelec-isen@gmail.com>

Desktop/cam.desktop

@@ -0,0 +1,8 @@
+[Desktop Entry]
+Type=Application
+Name=Cam
+Comment=Cam
+Exec=/home/modelec/Modelec-ROS2/start_cam_ros2.sh
+Icon=utilities-terminal
+Terminal=true
+Categories=Utility;

build.rasp.sh

@@ -1,4 +1,4 @@
 #!/bin/bash
-MAKEFLAGS="-j1" colcon build --symlink-install --executor sequential # --cmake-args -DCMAKE_BUILD_TYPE=Debug
+MAKEFLAGS="-j1" colcon build --symlink-install --executor sequential --cmake-args -DBUILD_FOR_RPI=ON --no-warn-unused-cli
 
 source install/setup.bash

install.sh

@@ -21,7 +21,7 @@ sudo apt upgrade -y
 
 sudo apt install ros-dev-tools ros-jazzy-desktop -y
 
-sudo apt-get install qt6-base-dev qt6-svg-dev libxml2-dev socat -y
+sudo apt-get install qt6-base-dev qt6-svg-dev qt6-multimedia-dev libxml2-dev socat -y
 
 git submodule init
 git submodule update
@@ -45,4 +45,4 @@ gio set ~/Desktop/no_lidar.joy.ros2_launch_marcel.desktop "metadata::trusted" tr
 gio set ~/Desktop/no_lidar.ros2_launch_marcel.desktop "metadata::trusted" true
 gio set ~/Desktop/joy.ros2_launch_marcel.desktop "metadata::trusted" true
 gio set ~/Desktop/ros2_launch_marcel.desktop "metadata::trusted" true
-gio set ~/Desktop/no-gui.ros2_launch_marcel.desktop "metadata::trusted" true
+gio set ~/Desktop/no-gui.ros2_launch_marcel.desktop "metadata::trusted" true

simulated_pcb/serial.modelec.py

@@ -35,16 +35,19 @@ def read_serial(ser, log_lines, log_lock):
                     log_lines.append(f"[Serial Error] {e}")
         time.sleep(0.05)
 
-def curses_main(stdscr, port, baudrate):
+def curses_main(stdscr, port, baudrate, filter_start=None):
     global stop_thread
     curses.curs_set(1)
     stdscr.nodelay(True)
     stdscr.timeout(100)
 
+    if filter_start is None:
+        filter_start = ["SET;POS"]
+
     log_lines = []
     log_lock = threading.Lock()
     cmd_history = []
-    history_index = -1  # For navigating command history
+    history_index = -1
 
     with serial.Serial(port, baudrate, timeout=1) as ser:
         time.sleep(2)
@@ -57,24 +60,36 @@ def curses_main(stdscr, port, baudrate):
             stdscr.clear()
             h, w = stdscr.getmaxyx()
 
-            # Split screen horizontally (70% logs, 30% command history)
+            # --- Split into 3 vertical columns ---
-            split_x = int(w * 0.7)
+            col_width = w // 3
+            col1_x = 0
+            col2_x = col_width
+            col3_x = 2 * col_width
+
             log_height = h - 2
 
-            # --- Left panel: logs ---
+            # --- Separate logs into filtered and others ---
             with log_lock:
-                visible_logs = log_lines[-log_height:]
+                filtered_logs = [line for line in log_lines if any(line.startswith(p) for p in filter_start)]
-            for i, line in enumerate(visible_logs):
+                other_logs = [line for line in log_lines if not any(line.startswith(p) for p in filter_start)]
-                stdscr.addnstr(i, 0, line, split_x - 1)
+                visible_filtered = filtered_logs[-log_height:]
+                visible_other = other_logs[-log_height:]
+                visible_history = cmd_history[-log_height:]
 
-            # --- Right panel: command history ---
+            # --- Column 1: Filtered serial output ---
-            stdscr.vline(0, split_x, "|", log_height)
+            stdscr.addstr(0, col1_x, f"Filtered ({', '.join(filter_start)}):")
-            history_start_x = split_x + 2
+            for i, line in enumerate(visible_filtered):
-            stdscr.addstr(0, history_start_x, "Command History:")
+                stdscr.addnstr(i + 1, col1_x, line, col_width - 1)
 
-            visible_history = cmd_history[-(log_height - 2):]
+            # --- Column 2: Other serial output ---
+            stdscr.addstr(0, col2_x, "Other Logs:")
+            for i, line in enumerate(visible_other):
+                stdscr.addnstr(i + 1, col2_x, line, col_width - 1)
+
+            # --- Column 3: Command history ---
+            stdscr.addstr(0, col3_x, "Command History:")
             for i, cmd in enumerate(visible_history):
-                stdscr.addnstr(i + 1, history_start_x, cmd, w - history_start_x - 1)
+                stdscr.addnstr(i + 1, col3_x, cmd, col_width - 1)
 
             # --- Input line ---
             stdscr.addstr(log_height, 0, "-" * (w - 1))

src/modelec_com/CMakeLists.txt

@@ -5,6 +5,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+option(BUILD_FOR_RPI "Build with Raspberry Pi libcamera support" OFF)
+
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -16,6 +18,18 @@ find_package(ament_index_cpp REQUIRED)
 find_package(fmt)
 
 find_package(OpenCV REQUIRED)
+find_package(PkgConfig REQUIRED)
+
+if(BUILD_FOR_RPI)
+  message(STATUS "Configuring for Raspberry Pi Hardware...")
+  add_definitions(-DRPI_BUILD)
+  pkg_check_modules(LIBCAMERA REQUIRED libcamera libcamera-base)
+  include_directories(${LIBCAMERA_INCLUDE_DIRS})
+  link_directories(${LIBCAMERA_LIBRARY_DIRS} /usr/local/lib/aarch64-linux-gnu)
+  add_definitions(${LIBCAMERA_CFLAGS_OTHER})
+else()
+  message(STATUS "Configuring for PC (standard OpenCV)...")
+endif()
 
 find_package(modelec_interfaces REQUIRED)
 find_package(modelec_utils REQUIRED)
@@ -57,6 +71,16 @@ target_link_libraries(color_detector
   modelec_utils::config
 )
 
+if(BUILD_FOR_RPI)
+  target_link_libraries(color_detector
+          -Wl,--no-as-needed
+          "/usr/local/lib/aarch64-linux-gnu/libcamera.so"
+          "/usr/local/lib/aarch64-linux-gnu/libcamera-base.so"
+          -Wl,--as-needed
+          cam2opencv
+  )
+endif()
+
 target_include_directories(color_detector PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>

src/modelec_com/include/modelec_com/color_detector.hpp

@@ -5,9 +5,18 @@
 #include <std_srvs/srv/trigger.hpp>
 #include <std_msgs/msg/empty.hpp>
 #include <std_msgs/msg/string.hpp>
+#include <modelec_utils/config.hpp>
+#include <mutex>
+#include <memory>
+
+#ifdef RPI_BUILD
+#include <libcam2opencv.h>
+#endif
 
 namespace Modelec
 {
+    struct CamCallback;
+
     struct ColorSetting
     {
         std::string name;
@@ -34,6 +43,17 @@ namespace Modelec
 
         std::string generateImagePath() const;
 
+        #ifdef RPI_BUILD
+                Libcam2OpenCV camera_;
+                std::unique_ptr<CamCallback> my_callback_;
+        #else
+                cv::VideoCapture pc_cap_;
+        #endif
+
+        cv::Mat latest_frame_;
+
+        std::mutex frame_mutex_;
+
         rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr service_;
 
         rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr ask_sub_;
@@ -49,4 +69,45 @@ namespace Modelec
 
         std::vector<ColorSetting> color_configs_;
     };
+
+    template<>
+    inline std::vector<ColorSetting>
+    Config::get<std::vector<ColorSetting>>(
+        const std::string& prefix,
+        const std::vector<ColorSetting>& default_value, bool)
+    {
+        auto result = Config::getArray<ColorSetting>(
+            prefix,
+            [](const std::string& base)
+            {
+                return ColorSetting{
+                    Config::get<std::string>(base + "@name"),
+                    Config::get<double>(base + "@hue_min"),
+                    Config::get<double>(base + "@hue_max")
+                };
+            });
+
+        return result.empty() ? default_value : result;
+    }
+
+    template<>
+    inline std::vector<cv::Rect>
+    Config::get<std::vector<cv::Rect>>(
+        const std::string& prefix,
+        const std::vector<cv::Rect>& default_value, bool)
+    {
+        auto result = Config::getArray<cv::Rect>(
+            prefix,
+            [](const std::string& base)
+            {
+                return cv::Rect(
+                    Config::get<int>(base + "@x", 0),
+                    Config::get<int>(base + "@y", 0),
+                    Config::get<int>(base + "@w", 100),
+                    Config::get<int>(base + "@h", 100)
+                );
+            });
+
+        return result.empty() ? default_value : result;
+    }
 }

src/modelec_com/include/modelec_com/pcb_action_interface.hpp

@@ -12,6 +12,8 @@
 #include <modelec_interfaces/msg/action_servo_timed_array.hpp>
 #include <modelec_interfaces/msg/action_servo_timed.hpp>
 
+#include "modelec_utils/config.hpp"
+
 #define TIMER_SERVO_TIMED_MS 10 // 100 Hz
 
 namespace Modelec
@@ -37,10 +39,10 @@ namespace Modelec
         std::map<int, int> asc_value_mapper_;
 
         int asc_state_ = 0;
-        std::map<int, double> servo_value_;
+        std::unordered_map<int, double> servo_value_;
-        std::map<int, bool> relay_value_;
+        std::unordered_map<int, bool> relay_value_;
 
-        std::map<int, ServoTimedSet> servo_timed_buffer_;
+        std::vector<ServoTimedSet> servo_timed_buffer_;
 
         rclcpp::TimerBase::SharedPtr servo_timed_timer_;
 
@@ -82,10 +84,29 @@ namespace Modelec
         void SendToPCB(const std::string& order, const std::string& elem,
                        const std::vector<std::string>& data = {});
 
-        // TODO redo thos func to accept arrays, poc without them atm
         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 = {});
     };
+
+    template<>
+    inline std::unordered_map<int, double>
+    Config::get<std::unordered_map<int, double>>(
+        const std::string& prefix,
+        const std::unordered_map<int, double>& default_value, bool)
+    {
+        auto result = Config::getMap<int, double>(
+            prefix,
+            [](const std::string& base)
+            {
+                return std::make_pair(
+                    Config::get<int>(base + "@id"),
+                    Config::get<double>(base + "@angle")
+                );
+            });
+
+        return result.empty() ? default_value : result;
+    }
+
 }

src/modelec_com/include/modelec_com/pcb_odo_interface.hpp

@@ -19,6 +19,8 @@
 
 #include <sensor_msgs/msg/joy.hpp>
 
+#include "modelec_utils/config.hpp"
+
 namespace Modelec
 {
     class PCBOdoInterface : public rclcpp::Node, public SerialListener
@@ -37,9 +39,11 @@ namespace Modelec
 
         struct PIDData
         {
-            float p;
+            float p = 0.0f;
-            float i;
+            float i = 0.0f;
-            float d;
+            float d = 0.0f;
+            std::optional<float> min;
+            std::optional<float> max;
         };
 
     private:
@@ -95,7 +99,26 @@ namespace Modelec
         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);
+        void SetPID(std::string name, const PIDData& pid_data);
 
         void SetMotor(int left, int right);
     };
+
+    template<>
+    inline PCBOdoInterface::PIDData
+    Config::get<PCBOdoInterface::PIDData>(
+        const std::string& prefix,
+        const PCBOdoInterface::PIDData& default_value, bool)
+    {
+        PCBOdoInterface::PIDData result;
+        result.p = Config::get<float>(prefix + "@p", default_value.p);
+        result.i = Config::get<float>(prefix + "@i", default_value.i);
+        result.d = Config::get<float>(prefix + "@d", default_value.d);
+        if (Config::has(prefix + "@min"))
+            result.min = Config::get<float>(prefix + "@min");
+        if (Config::has(prefix + "@max"))
+            result.max = Config::get<float>(prefix + "@max");
+        return result;
+    }
+
 } // namespace Modelec

src/modelec_com/src/color_detector.cpp

@@ -1,48 +1,42 @@
 #include <modelec_com/color_detector.hpp>
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <modelec_utils/utils.hpp>
+#include <mutex>
+
+#ifdef RPI_BUILD
+#include <libcam2opencv.h>
+#endif
 
 namespace Modelec
 {
+#ifdef RPI_BUILD
+    struct CamCallback : Libcam2OpenCV::Callback {
+        cv::Mat* target_frame;
+        std::mutex* frame_mutex;
+        CamCallback(cv::Mat& frame, std::mutex& mtx) : target_frame(&frame), frame_mutex(&mtx) {}
+        void hasFrame(const cv::Mat &frame, const libcamera::ControlList &) override {
+            std::lock_guard<std::mutex> lock(*frame_mutex);
+            frame.copyTo(*target_frame);
+        }
+    };
+#endif
     ColorDetector::ColorDetector()
-        : Node("color_detector")
+    : Node("color_detector")
     {
-        this->declare_parameter("enabled", true);
+        std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
-        this->declare_parameter("link", "");
+         if (!Config::load(config_path))
-        this->declare_parameter("headless", true);
+         {
-        this->declare_parameter("save_to_file.enabled", true);
+             RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str());
-        this->declare_parameter("save_to_file.path", "./");
+         }
-        this->declare_parameter("rois", rclcpp::PARAMETER_INTEGER_ARRAY);
-        this->declare_parameter("colors.blue", rclcpp::PARAMETER_INTEGER_ARRAY);
-        this->declare_parameter("colors.yellow", rclcpp::PARAMETER_INTEGER_ARRAY);
 
-        enable_ = this->get_parameter("enabled").as_bool();
+        enable_ = Config::get<bool>("config.cam.enabled", false);
-        link_ = this->get_parameter("link").as_string();
+        link_ = Config::get<std::string>("config.cam.link", "/dev/video0");
-        headless_ = this->get_parameter("headless").as_bool();
+        headless_ = Config::get<bool>("config.cam.headless", true);
-        save_to_file_ = this->get_parameter("save_to_file.enabled").as_bool();
+        save_to_file_ = Config::get<bool>("config.cam.save_to_file.enabled", false);
-        save_directory_ = this->get_parameter("save_to_file.path").as_string();
+        save_directory_ = Config::get<std::string>("config.cam.save_to_file.directory", "./");
 
-        auto rois_param = this->get_parameter("rois").as_integer_array();
+        rois_ = Config::get<std::vector<cv::Rect>>("config.cam.rois.roi", {});
-        for (size_t i = 0; i + 3 < rois_param.size(); i += 4)
+        color_configs_ = Config::get<std::vector<ColorSetting>>("config.cam.colors.color", {});
-        {
-            rois_.emplace_back(rois_param[i], rois_param[i + 1], rois_param[i + 2], rois_param[i + 3]);
-        }
-
-        auto blue_param = this->get_parameter("colors.blue").as_integer_array();
-        if (blue_param.size() >= 2)
-        {
-            color_configs_.push_back({
-                "blue", static_cast<double>(blue_param[0]), static_cast<double>(blue_param[1])
-            });
-        }
-
-        auto yellow_param = this->get_parameter("colors.yellow").as_integer_array();
-        if (yellow_param.size() >= 2)
-        {
-            color_configs_.push_back({
-                "yellow", static_cast<double>(yellow_param[0]), static_cast<double>(yellow_param[1])
-            });
-        }
 
         if (!enable_)
         {
@@ -51,6 +45,22 @@ namespace Modelec
             return;
         }
 
+#ifdef RPI_BUILD
+        RCLCPP_INFO(get_logger(), "Starting RPi Libcamera (Full FOV mode)...");
+        Libcam2OpenCVSettings settings;
+        settings.width = 2304;
+        settings.height = 1296;
+        settings.framerate = 30;
+        settings.cameraIndex = 0;
+
+        my_callback_ = std::make_unique<CamCallback>(latest_frame_, frame_mutex_);
+        camera_.registerCallback(my_callback_.get());
+        camera_.start(settings);
+#else
+        RCLCPP_INFO(get_logger(), "Starting PC Webcam (OpenCV)...");
+        pc_cap_.open(link_);
+#endif
+
         service_ = create_service<std_srvs::srv::Trigger>(
             "action/detect_color",
             std::bind(&ColorDetector::onRequest, this,
@@ -85,6 +95,8 @@ namespace Modelec
         if (!headless_)
         {
             cv::namedWindow("color_detector", cv::WINDOW_NORMAL);
+
+            cv::setWindowProperty("color_detector", cv::WND_PROP_AUTOSIZE, cv::WINDOW_GUI_EXPANDED);
         }
 
         RCLCPP_INFO(get_logger(), "Color detector service ready");
@@ -92,6 +104,12 @@ namespace Modelec
 
     ColorDetector::~ColorDetector()
     {
+#ifdef RPI_BUILD
+        camera_.stop();
+#else
+        if(pc_cap_.isOpened()) pc_cap_.release();
+#endif
+
         if (!headless_)
         {
             cv::destroyAllWindows();
@@ -100,21 +118,21 @@ namespace Modelec
 
     bool ColorDetector::processSnapshot(std::vector<std::string>& colors, std::string& error)
     {
-        cv::VideoCapture cap(link_);
-
-        if (!cap.isOpened())
-        {
-            RCLCPP_ERROR(get_logger(), "Failed to open camera at %s", link_.c_str());
-            error = "Failed to open camera";
-            return false;
-        }
-
-        cap.set(cv::CAP_PROP_BUFFERSIZE, 1);
-
         cv::Mat frame;
-
+#ifdef RPI_BUILD
-        for (int i = 0; i < 3; ++i)
+        {
-            cap >> frame;
+            std::lock_guard<std::mutex> lock(frame_mutex_);
+            if (latest_frame_.empty()) {
+                RCLCPP_WARN(get_logger(), "No frame received from libcamera yet");
+                error = "Empty frame";
+                return false;
+            }
+            latest_frame_.copyTo(frame);
+        }
+#else
+if(!pc_cap_.isOpened()) { error = "PC Cam not open"; return false; }
+pc_cap_ >> frame;
+#endif
 
         if (frame.empty())
         {
@@ -139,11 +157,9 @@ namespace Modelec
         {
             std::string path = save_directory_ + generateImagePath();
             cv::imwrite(path, frame);
-            RCLCPP_DEBUG(get_logger(), "Saved snapshot to %s", path.c_str());
+            RCLCPP_INFO(get_logger(), "Saved snapshot to %s", path.c_str());
         }
 
-        cap.release();
-
         return true;
     }
 

src/modelec_com/src/pcb_action_interface.cpp

@@ -1,19 +1,31 @@
 #include <modelec_com/pcb_action_interface.hpp>
 #include <modelec_utils/utils.hpp>
 #include <fmt/core.h>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <modelec_utils/config.hpp>
+#include <algorithm>
 
 namespace Modelec
 {
     PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface")
     {
-        // Service to create a new serial listener
+        std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
-        declare_parameter<std::string>("serial_port", "/dev/USB_ACTION");
+        if (!Config::load(config_path))
-        declare_parameter<int>("baudrate", 115200);
+        {
+            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str());
+        }
 
-        auto serial_port = get_parameter("serial_port").as_string();
+        std::string action_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/action.xml";
-        auto baudrate = get_parameter("baudrate").as_int();
+        if (!Config::load(action_path))
+        {
+            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", action_path.c_str());
+        }
 
-        RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %ld", serial_port.c_str(), baudrate);
+        auto serial_port = Config::get<std::string>("config.usb.action.port", "/dev/ttyUSB0");
+        auto baudrate = Config::get<int>("config.usb.action.baudrate", 115200);
+        auto timed_servo_timer_ms = Config::get<int>("config.timer.action.timed_servo.ms", TIMER_SERVO_TIMED_MS);
+
+        RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %d", serial_port.c_str(), baudrate);
 
         asc_get_sub_ = this->create_subscription<modelec_interfaces::msg::ActionAscPos>(
             "action/get/asc", 10,
@@ -157,14 +169,14 @@ namespace Modelec
                 {
                     ServoTimedSet servo_timed_set;
                     servo_timed_set.servo_timed = item;
-                    servo_timed_set.start_time = now;
+                    servo_timed_set.start_time = now + rclcpp::Duration::from_seconds(item.delay_s);
-                    servo_timed_set.end_time = now + rclcpp::Duration::from_seconds(item.duration_s);
+                    servo_timed_set.end_time = now + rclcpp::Duration::from_seconds(item.duration_s + item.delay_s);
                     servo_timed_set.active = true;
 
                     RCLCPP_DEBUG(this->get_logger(), "Scheduled timed move for Servo ID %d from %.3f to %.3f over %.3f seconds",
                                  item.id, item.start_angle, item.end_angle, item.duration_s);
 
-                    servo_timed_buffer_[item.id] = servo_timed_set;
+                    servo_timed_buffer_.push_back(servo_timed_set);
                 }
             });
 
@@ -172,23 +184,32 @@ namespace Modelec
             "action/move/servo/timed/res", 10);
 
 		servo_timed_timer_ = this->create_wall_timer(
-            std::chrono::milliseconds(TIMER_SERVO_TIMED_MS),
+            std::chrono::milliseconds(timed_servo_timer_ms),
             [this]()
             {
+                if (servo_timed_buffer_.empty()) {
+                    return;
+                }
+
                 rclcpp::Time now = this->now();
 
                 modelec_interfaces::msg::ActionServoTimedArray servo_timed_msg;
 
-				std::map<int, double> to_send;
+				std::vector<std::pair<int, double>> to_send;
 
-                for (auto& [id, servo_timed_set] : servo_timed_buffer_)
+                for (auto& servo_timed_set : servo_timed_buffer_)
                 {
+                    if (servo_timed_set.active && now.nanoseconds() < servo_timed_set.start_time.nanoseconds())
+                    {
+                        continue;
+                    }
                     if (servo_timed_set.active && now.nanoseconds() >= servo_timed_set.end_time.nanoseconds())
                     {
                         RCLCPP_DEBUG(this->get_logger(), "Timed move for Servo ID %d completed. Setting to final angle %.3f",
-                                     id, servo_timed_set.servo_timed.end_angle);
+                                     servo_timed_set.servo_timed.id, servo_timed_set.servo_timed.end_angle);
+
+                        to_send.emplace_back(servo_timed_set.servo_timed.id, servo_timed_set.servo_timed.end_angle);
 
-                        to_send[id] = servo_timed_set.servo_timed.end_angle;
                         servo_timed_set.active = false;
 
                         servo_timed_msg.items.push_back(servo_timed_set.servo_timed);
@@ -199,12 +220,13 @@ namespace Modelec
                         double duration = (servo_timed_set.end_time - servo_timed_set.start_time).seconds();
                         double progress = elapsed / duration;
 
-                        RCLCPP_DEBUG(this->get_logger(), "Servo ID %d progress: %.3f | %.3f %.3f", id, progress, elapsed, duration);
+                        RCLCPP_DEBUG(this->get_logger(), "Servo ID %d progress: %.3f | %.3f %.3f",
+                            servo_timed_set.servo_timed.id, progress, elapsed, duration);
 
                         double intermediate_angle = servo_timed_set.servo_timed.start_angle +
-                                                    progress * (servo_timed_set.servo_timed.end_angle - servo_timed_set.servo_timed.start_angle);
+                            progress * (servo_timed_set.servo_timed.end_angle - servo_timed_set.servo_timed.start_angle);
 
-						to_send[id] = intermediate_angle;
+						to_send.emplace_back(servo_timed_set.servo_timed.id, intermediate_angle);
                     }
                 }
 
@@ -228,47 +250,22 @@ namespace Modelec
                     servo_move_timed_res_pub_->publish(servo_timed_msg);
                 }
 
+                servo_timed_buffer_.erase(
+                std::remove_if(
+                    servo_timed_buffer_.begin(),
+                    servo_timed_buffer_.end(),
+                    [&](const ServoTimedSet& s)
+                    {
+                        return s.active == false;
+                    }),
+                servo_timed_buffer_.end());
+
+
 			});
 
         this->open(baudrate, serial_port, MAX_MESSAGE_LEN);
 
-        // TODO : check for real value there
+        servo_value_ = Config::get<std::unordered_map<int, double>>("action.init.servo");
-        /*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_)});
-
-        // rclcpp::sleep_for(std::chrono::milliseconds(100));
-
-        /*servo_value_ = {
-            {0, 1},
-            {1, 1},
-            {2, 0},
-            {3, 0},
-            {4, 1},
-            {5, 0}
-        };*/
-
-        servo_value_ = {
-            {0, 2.93},
-            {1, 0.91},
-            {2, 3.05},
-            {3, 0.3},
-            {4, 1},
-            {5, 1},
-            {6, 1},
-            {7, 1},
-        };
 
         std::string data = "MOV;SERVO;" + std::to_string(servo_value_.size()) + ";";
 
@@ -280,31 +277,6 @@ namespace Modelec
 		data += "\n";
 
         SendToPCB(data);
-
-        /*relay_value_ = {
-            {1, false},
-            {2, false},
-            {3, false},
-        };*/
-
-        /*rclcpp::sleep_for(std::chrono::milliseconds(100));
-
-        data = "SET;RELAY;STATE;";
-        data += std::to_string(relay_value_.size()) + ";";
-
-        for (auto & [id, v] : relay_value_)
-        {
-            data += std::to_string(id) + ";" + std::to_string(v) + ";";
-        }
-
-        SendToPCB(data);*/
-
-        /*for (auto & [id, v] : relay_value_)
-        {
-            rclcpp::sleep_for(std::chrono::milliseconds(100));
-
-            SendMove("RELAY" + std::to_string(id), {std::to_string(v)});
-        }*/
     }
 
     PCBActionInterface::~PCBActionInterface()
@@ -533,7 +505,6 @@ namespace Modelec
         SendToPCB(command);
     }
 
-    // TODO CHANGE TO AAA;BBB;N;X;Y;X;Y;X;Y where N is number of elem you want to send
     void PCBActionInterface::GetData(const std::string& elem, const std::vector<std::string>& data)
     {
         SendToPCB("GET", elem, data);

src/modelec_com/src/pcb_odo_interface.cpp

@@ -1,17 +1,22 @@
 #include <modelec_com/pcb_odo_interface.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")
     {
-        declare_parameter<std::string>("serial_port", "/dev/USB_ODO");
+        std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
-        declare_parameter<int>("baudrate", 115200);
+        if (!Config::load(config_path))
+        {
+            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str());
+        }
 
-        auto serial_port = get_parameter("serial_port").as_string();
+        auto serial_port = Config::get<std::string>("config.usb.odo.port", "/dev/ttyUSB0");
-        auto baudrate = get_parameter("baudrate").as_int();
+        auto baudrate = Config::get<int>("config.usb.odo.baudrate", 115200);
 
-        RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %ld", serial_port.c_str(), baudrate);
+        RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %d", serial_port.c_str(), baudrate);
 
         odo_pos_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
             "odometry/position", 10);
@@ -104,10 +109,10 @@ namespace Modelec
 
         this->open(baudrate, serial_port, MAX_MESSAGE_LEN);
 
-        SetPID("THETA", 14, 0, 0);
+        SetPID("THETA", Config::get<PIDData>("config.odo.pid.theta"));
-        SetPID("POS", 5, 0, 0);
+        SetPID("POS", Config::get<PIDData>("config.odo.pid.pos"));
-        SetPID("LEFT", 5, 0, 0);
+        SetPID("LEFT", Config::get<PIDData>("config.odo.pid.left"));
-        SetPID("RIGHT", 5, 0, 0);
+        SetPID("RIGHT", Config::get<PIDData>("config.odo.pid.right"));
     }
 
     PCBOdoInterface::~PCBOdoInterface()
@@ -431,6 +436,11 @@ namespace Modelec
         SendOrder("PID", data);
     }
 
+    void PCBOdoInterface::SetPID(std::string name, const PIDData& pid_data)
+    {
+        SetPID(name, pid_data.p, pid_data.i, pid_data.d, pid_data.min, pid_data.max);
+    }
+
     void PCBOdoInterface::SetMotor(int left, int right)
     {
         std::vector<std::string> data = {

src/modelec_core/launch/modelec.launch.py

@@ -14,7 +14,6 @@ from launch_ros.actions import Node
 from ament_index_python.packages import get_package_share_directory
 
 def generate_launch_description():
-    pkg_name = 'modelec_core'
     # -------------------------------------------------
     # Launch arguments
     # -------------------------------------------------
@@ -50,12 +49,6 @@ def generate_launch_description():
     angle_compensate = LaunchConfiguration('angle_compensate', default='false')
     scan_mode = LaunchConfiguration('scan_mode', default='Standard')
 
-    param_file = os.path.join(
-        get_package_share_directory(pkg_name),
-        'config',
-        'config.yml'
-    )
-
     # -------------------------------------------------
     # RPLIDAR with auto-restart
     # -------------------------------------------------
@@ -64,7 +57,7 @@ def generate_launch_description():
             package='rplidar_ros',
             executable='rplidar_node',
             name='rplidar_node',
-            parameters=[param_file, {
+            parameters=[{
                 'channel_type': channel_type,
                 'serial_port': serial_port,
                 'serial_baudrate': serial_baudrate,
@@ -113,7 +106,6 @@ def generate_launch_description():
                 package='modelec_gui',
                 executable='modelec_gui',
                 name='modelec_gui',
-                parameters=[param_file],
             )
 
             shutdown = RegisterEventHandler(
@@ -135,19 +127,17 @@ def generate_launch_description():
                     package='modelec_com',
                     executable='pcb_odo_interface',
                     name='pcb_odo_interface',
-                    parameters=[param_file],
                 ),
                 Node(
                     package='modelec_com',
                     executable='pcb_action_interface',
                     name='pcb_action_interface',
-                    parameters=[param_file],
                 ),
             ]
         return []
 
     # -------------------------------------------------
-    # Strategy nodes (WITHOUT enemy_manager)
+    # Strategy nodes
     # -------------------------------------------------
     def launch_strat(context, *args, **kwargs):
         if context.launch_configurations.get('with_strat') == 'true':
@@ -156,20 +146,18 @@ def generate_launch_description():
                     package='modelec_strat',
                     executable='strat_fsm',
                     name='strat_fsm',
-                    parameters=[param_file],
                     # prefix=['xterm -e gdb -ex run --args'],
                 ),
                 Node(
                     package='modelec_strat',
                     executable='pami_manager',
                     name='pami_manager',
-                    parameters=[param_file],
                 ),
             ]
         return []
 
     # -------------------------------------------------
-    # Enemy manager (standalone)
+    # Enemy manager
     # -------------------------------------------------
     def launch_enemy_manager(context, *args, **kwargs):
         if context.launch_configurations.get('with_enemy_manager') == 'true':
@@ -178,7 +166,6 @@ def generate_launch_description():
                     package='modelec_strat',
                     executable='enemy_manager',
                     name='enemy_manager',
-                    parameters=[param_file],
                 )
             ]
         return []
@@ -193,7 +180,6 @@ def generate_launch_description():
                     package='joy',
                     executable='joy_node',
                     name='joy_node',
-                    parameters=[param_file],
                 )
             ]
         return []
@@ -205,7 +191,6 @@ def generate_launch_description():
                     package='modelec_com',
                     executable='color_detector',
                     name='color_detector',
-                    parameters=[param_file],
                 )
             ]
         return []

src/modelec_gui/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(Qt6 COMPONENTS
         Gui
         Widgets
         Svg
+        Multimedia
         REQUIRED)
 
 # Add the executable
@@ -63,6 +64,7 @@ target_link_libraries(modelec_gui
         Qt6::Gui
         Qt6::Widgets
         Qt6::Svg
+        Qt6::Multimedia
         modelec_utils::utils
         modelec_utils::config
 )

src/modelec_gui/include/modelec_gui/modelec_gui.hpp

@@ -10,7 +10,8 @@
 #include <modelec_gui/pages/map_page.hpp>
 #include <modelec_gui/pages/action_page.hpp>
 #include <modelec_gui/pages/alim_page.hpp>
-
+#include <QMediaPlayer>
+#include <QAudioOutput>
 
 namespace ModelecGUI
 {
@@ -50,5 +51,11 @@ namespace ModelecGUI
 
         QAction* toggle_show_obstacle_action_;
 
+        QMediaPlayer* media_player_;
+        QAudioOutput* audio_output_;
+
+        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr audio_sub_;
+        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr quit_sub_;
+
     };
 }

src/modelec_gui/resource.qrc

@@ -9,5 +9,6 @@
         <file>img/logo/ISEN-Nantes-fond-blanc.png</file>
         <file>img/logo/modelec.png</file>
         <file>img/wood.jpg</file>
+        <file>sound/test.mp3</file>
     </qresource>
 </RCC>

src/modelec_gui/src/main.cpp

@@ -4,6 +4,7 @@
 #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)
 {
@@ -13,14 +14,11 @@ int main(int argc, char **argv)
 
     auto node = rclcpp::Node::make_shared("qt_gui_node");
 
-    node->declare_parameter("map.size.map_width_mm", 3000);
+    std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
-    node->declare_parameter("map.size.map_height_mm", 2000);
+    if (!Modelec::Config::load(config_path))
-
+    {
-    node->declare_parameter("robot.size.length_mm", 200);
+        RCLCPP_ERROR(node->get_logger(), "Failed to load config file: %s", config_path.c_str());
-    node->declare_parameter("robot.size.width_mm", 300);
+    }
-
-    node->declare_parameter("enemy.size.length_mm", 200);
-    node->declare_parameter("enemy.size.width_mm", 300);
 
     ModelecGUI::ROS2QtGUI window(node);
     window.show();

src/modelec_gui/src/modelec_gui.cpp

@@ -2,7 +2,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <QMenuBar>
 #include <utility>
-
+#include <modelec_utils/config.hpp>
 
 namespace ModelecGUI {
 
@@ -34,6 +34,32 @@ namespace ModelecGUI {
         {
             stackedWidget->setCurrentWidget(map_page_);
         });
+
+        if (Modelec::Config::get<bool>("config.ihm.sound.enabled", false))
+        {
+            RCLCPP_INFO(get_node()->get_logger(), "Sound enabled, setting up audio player");
+
+            media_player_ = new QMediaPlayer(this);
+            audio_output_ = new QAudioOutput(this);
+            media_player_->setAudioOutput(audio_output_);
+
+            audio_sub_ = node_->create_subscription<std_msgs::msg::String>(
+                "audio/play", 10, [this](const std_msgs::msg::String::SharedPtr msg)
+                {
+                    if (Modelec::Config::has("config.ihm.sound." + msg->data + "@path"))
+                    {
+                        QString audio_file = QString::fromStdString(Modelec::Config::get<std::string>("config.ihm.sound." + msg->data + "@path"));
+                        media_player_->setSource(QUrl::fromLocalFile(audio_file));
+                        media_player_->play();
+                    }
+                });
+        }
+
+        quit_sub_ = node_->create_subscription<std_msgs::msg::Empty>(
+            "gui/quit", 10, [this](const std_msgs::msg::Empty::SharedPtr)
+            {
+                close();
+            });
     }
 
     void ROS2QtGUI::setupMenu() {

src/modelec_gui/src/pages/map_page.cpp

@@ -5,6 +5,8 @@
 #include <utility>
 #include <cmath>
 
+#include <modelec_utils/config.hpp>
+
 namespace ModelecGUI
 {
     MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent),
@@ -48,14 +50,14 @@ namespace ModelecGUI
 
         qpoints = {};
 
-        map_height_ = node_->get_parameter("map.size.map_height_mm").as_int();
+        map_height_ = Modelec::Config::get<int>("config.map.size.map_height_mm", 2000);
-        map_width_ = node_->get_parameter("map.size.map_width_mm").as_int();
+        map_width_ = Modelec::Config::get<int>("config.map.size.map_width_mm", 3000);
 
-        robot_length_ = node_->get_parameter("robot.size.length_mm").as_int();
+        robot_length_ = Modelec::Config::get<int>("config.robot.size.length_mm", 300);
-        robot_width_ = node_->get_parameter("robot.size.width_mm").as_int();
+        robot_width_ = Modelec::Config::get<int>("config.robot.size.width_mm", 200);
 
-        enemy_length_ = node_->get_parameter("enemy.size.length_mm").as_int();
+        enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
-        enemy_width_ = node_->get_parameter("enemy.size.width_mm").as_int();
+        enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
 
         add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryWaypoint>(
             "odometry/add_waypoint", 100,

src/modelec_gui/src/pages/test_map_page.cpp

@@ -1,5 +1,6 @@
 #include <modelec_gui/pages/test_map_page.hpp>
 #include <modelec_utils/utils.hpp>
+#include <modelec_utils/config.hpp>
 
 #include <QMouseEvent>
 #include <utility>
@@ -22,14 +23,14 @@ namespace ModelecGUI
 
         qpoints = {};
 
-        map_height_ = node_->get_parameter("map.size.map_height_mm").as_int();
+        map_height_ = Modelec::Config::get<int>("config.map.size.map_height_mm", 2000);
-        map_width_ = node_->get_parameter("map.size.map_width_mm").as_int();
+        map_width_ = Modelec::Config::get<int>("config.map.size.map_width_mm", 3000);
 
-        robot_length_ = node_->get_parameter("robot.size.length_mm").as_int();
+        robot_length_ = Modelec::Config::get<int>("config.robot.size.length_mm", 300);
-        robot_width_ = node_->get_parameter("robot.size.width_mm").as_int();
+        robot_width_ = Modelec::Config::get<int>("config.robot.size.width_mm", 200);
 
-        enemy_length_ = node_->get_parameter("enemy.size.length_mm").as_int();
+        enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
-        enemy_width_ = node_->get_parameter("enemy.size.width_mm").as_int();
+        enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
 
         add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryWaypoint>(
             "odometry/add_waypoint", 100,

src/modelec_interfaces/msg/Action/ActionServoTimed.msg

@@ -2,4 +2,5 @@ int32 id
 float64 start_angle
 float64 end_angle
 float64 duration_s
-bool success
+float64 delay_s
+bool success

src/modelec_strat/CMakeLists.txt

@@ -26,13 +26,16 @@ set(strat_fsm_sources
         src/action/free_action.cpp
         src/action/take_action.cpp
         src/action/toggle_servo_action.cpp
-        src/action/look_on_action.cpp
         src/action/thermo_action.cpp
 
         src/missions/go_home_mission.cpp
         src/missions/take_mission.cpp
         src/missions/free_mission.cpp
         src/missions/thermo_mission.cpp
+        src/missions/action_mission.cpp
+        src/missions/move_mission.cpp
+        src/missions/mine_time_mission.cpp
+        src/missions/mission_base.cpp
 
         src/obstacle/obstacle.cpp
         src/obstacle/box.cpp

src/modelec_strat/data/action.xml

@@ -0,0 +1,118 @@
+<action>
+    <init>
+        <servo id="0" angle="3.12" />
+        <servo id="1" angle="0.91" />
+        <servo id="2" angle="3.45" />
+        <servo id="3" angle="0" />
+        <servo id="4" angle="1" />
+        <servo id="5" angle="1" />
+        <servo id="6" angle="1" />
+        <servo id="7" angle="1" />
+        <!--<servo id="8" angle="3.1" />-->
+        <!--<servo id="9" angle="0.96" />-->
+        <!--<servo id="10" angle="1" />-->
+        <!--<servo id="11" angle="1" />-->
+        <!--<servo id="12" angle="1" />-->
+        <!--<servo id="13" angle="1" />-->
+        <!--<servo id="14" angle="1" />-->
+        <!--<servo id="15" angle="1" />-->
+        <!--<servo id="16" angle="1" />-->
+        <!--<servo id="17" angle="1" />-->
+        <!--<servo id="18" angle="1" />-->
+    </init>
+    <down>
+        <front>
+            <unrotated>
+                <msg id="0" start_angle="1.84" end_angle="3.12" duration_s="2" />
+                <msg id="1" start_angle="2.15" end_angle="0.91" duration_s="2" />
+                <msg id="2" start_angle="0.5" end_angle="2.85" duration_s="0.9" delay_s="0.4" />
+                <msg id="2" start_angle="2.85" end_angle="3.45" duration_s="0.7" delay_s="1.3" />
+                <msg id="3" start_angle="2.9" end_angle="0.9" duration_s="0.9" delay_s="0.4" />
+                <msg id="3" start_angle="0.9" end_angle="0" duration_s="0.7" delay_s="1.3" />
+            </unrotated>
+            <rotated>
+                <msg id="0" start_angle="1.84" end_angle="2.98" duration_s="2" />
+                <msg id="1" start_angle="2.15" end_angle="1.07" duration_s="2" />
+                <msg id="2" start_angle="0.5" end_angle="0" duration_s="1" />
+                <msg id="3" start_angle="2.9" end_angle="3.3" duration_s="1" />
+            </rotated>
+        </front>
+        <back>
+            <unrotated>
+                <msg id="8" start_angle="1.88" end_angle="2.93" duration_s="2" />
+                <msg id="9" start_angle="2.2" end_angle="0.91" duration_s="2" />
+                <msg id="10" start_angle="0.5" end_angle="2.45" duration_s="1.3" />
+                <msg id="10" start_angle="2.45" end_angle="3.05" duration_s="0.7" delay_s="1.3" />
+                <msg id="11" start_angle="2.6" end_angle="0.9" duration_s="1.3" />
+                <msg id="11" start_angle="0.9" end_angle="0.3" duration_s="0.7" delay_s="1.3" />
+            </unrotated>
+            <rotated>
+                <msg id="8" start_angle="1.88" end_angle="2.93" duration_s="2" />
+                <msg id="9" start_angle="2.2" end_angle="0.91" duration_s="2" />
+                <msg id="10" start_angle="0.5" end_angle="0" duration_s="2" />
+                <msg id="11" start_angle="2.6" end_angle="3.4" duration_s="2" />
+            </rotated>
+        </back>
+    </down>
+    <up>
+        <front>
+            <unrotated>
+                <msg id="0" start_angle="3.12" end_angle="1.84" duration_s="2" />
+                <msg id="1" start_angle="0.91" end_angle="2.15" duration_s="2" />
+                <msg id="2" start_angle="3.45" end_angle="2.95" duration_s="0.7" />
+                <msg id="2" start_angle="2.95" end_angle="0.5" duration_s="0.7" delay_s="0.7" />
+                <msg id="3" start_angle="0" end_angle="0.6" duration_s="0.7" />
+                <msg id="3" start_angle="0.6" end_angle="2.9" duration_s="0.7" delay_s="0.7" />
+            </unrotated>
+            <rotated>
+                <msg id="0" start_angle="2.98" end_angle="1.84" duration_s="2" />
+                <msg id="1" start_angle="1.07" end_angle="2.15" duration_s="2" />
+                <msg id="2" start_angle="0" end_angle="0.5" duration_s="2" />
+                <msg id="3" start_angle="3.3" end_angle="2.9" duration_s="2" />
+            </rotated>
+        </front>
+        <back>
+            <rotated>
+                <msg id="8" start_angle="2.93" end_angle="1.76" duration_s="2" />
+                <msg id="9" start_angle="0.91" end_angle="2.06" duration_s="2" />
+                <msg id="10" start_angle="0" end_angle="0.5" duration_s="2" />
+                <msg id="11" start_angle="3.1" end_angle="2.6" duration_s="2" />
+            </rotated>
+            <unrotated>
+                <msg id="8" start_angle="2.93" end_angle="1.76" duration_s="2" />
+                <msg id="9" start_angle="0.91" end_angle="2.06" duration_s="2" />
+                <msg id="10" start_angle="3.2" end_angle="0.5" duration_s="2" />
+                <msg id="11" start_angle="0" end_angle="2.6" duration_s="2" />
+            </unrotated>
+        </back>
+    </up>
+    <free>
+        <id first="4" second="12" />
+        <msg start_angle="3" end_angle="1" duration_s="0.5" />
+    </free>
+    <take>
+        <id first="4" second="12" />
+        <msg start_angle="1" end_angle="3" duration_s="0.5" />
+    </take>
+    <toggle>
+        <id first="4" second="12" />
+        <msg start_angle="1" end_angle="3" duration_s="0.5" />
+    </toggle>
+    <look_on>
+        <id>18</id>
+        <center>1</center>
+        <front>0</front>
+        <back>2</back>
+        <duration_s>0.5</duration_s>
+    </look_on>
+    <thermo>
+        <left>
+            <deploy id="16" start_angle="1" end_angle="2" duration_s="0.5" />
+            <undeploy id="16" start_angle="2" end_angle="1" duration_s="0.5" />
+        </left>
+        <right>
+            <deploy id="17" start_angle="0" end_angle="1.5" duration_s="0.5" />
+            <undeploy id="17" start_angle="1.5" end_angle="0" duration_s="0.5" />
+        </right>
+    </thermo>
+</action>

src/modelec_strat/data/config.xml

@@ -3,7 +3,7 @@
         <size>
             <width_mm>300</width_mm>
             <length_mm>300</length_mm>
-            <margin_mm>50</margin_mm>
+            <margin_mm>75</margin_mm>
         </size>
         <detection>
             <min_move_threshold_mm>50</min_move_threshold_mm>
@@ -12,21 +12,20 @@
             <min_emergency_distance_mm>300</min_emergency_distance_mm>
             <margin_detection_table_mm>50</margin_detection_table_mm>
         </detection>
-        <factor_close_enemy>-2</factor_close_enemy>
     </enemy>
 
     <robot>
         <size>
             <width_mm>300</width_mm>
             <length_mm>300</length_mm>
-            <margin_mm>100</margin_mm>
+            <margin_mm>20</margin_mm>
         </size>
     </robot>
 
     <map>
         <size>
-            <grid_width>600</grid_width>
+            <grid_width>3000</grid_width>
-            <grid_height>400</grid_height>
+            <grid_height>2000</grid_height>
 
             <map_width_mm>3000</map_width_mm>
             <map_height_mm>2000</map_height_mm>
@@ -50,18 +49,61 @@
         <height_mm>450</height_mm>
     </spawn>
     <mission_score>
+        <go_home>10</go_home>
     </mission_score>
+    <mission>
+        <action>
+            <timeout>
+                <s>10</s>
+            </timeout>
+        </action>
+        <min_time>
+            <min_time_duration>
+                <s>0.1</s>
+            </min_time_duration>
+        </min_time>
+        <move>
+            <min_ask_waypoint>
+                <s>3</s>
+            </min_ask_waypoint>
+            <ask_waypoint_interval>
+                <s>2</s>
+            </ask_waypoint_interval>
+            <go_timeout>
+                <s>10</s>
+            </go_timeout>
+        </move>
+    </mission>
     <pami>
         <time_to_put_zone>77</time_to_put_zone>
         <time_to_remove_top_pot>65</time_to_remove_top_pot>
     </pami>
-    <static_strat>false</static_strat>
+    <static_strat>
+        <enabled>false</enabled>
+        <missions>
+            <mission name="TAKE_MISSION" />
+            <mission name="FREE_MISSION" />
+            <mission name="THERMO_MISSION" />
+        </missions>
+    </static_strat>
     <factor>
         <theta>20</theta>
         <obs>0.6</obs>
         <thermo>0.7</thermo>
+        <close_enemy>-2</close_enemy>
     </factor>
-    <timer_period_ms>20</timer_period_ms>
+    <timer>
+        <strat>
+            <ms>20</ms>
+        </strat>
+        <action>
+            <timed_servo>
+                <ms>
+                    10
+                </ms>
+            </timed_servo>
+        </action>
+    </timer>
     <thermo>
         <yellow>
             <start x="200" y="175" theta="0" />
@@ -75,21 +117,56 @@
     <cam>
         <save_to_file>
             <enabled>true</enabled>
-            <path>./cam/</path>
+            <directory>./cam/</directory>
         </save_to_file>
         <enabled>true</enabled>
         <!--<link>/dev/video0</link>-->
-        <link>http://192.168.1.21:8080/video</link>
+        <link>http://192.168.1.10:8080/video</link>
-        <headless>true</headless>
+        <headless>false</headless>
         <rois>
-            <roi x="300" y="700" w="50" h="50" />
+            <roi x="400" y="200" w="150" h="150" />
-            <roi x="700" y="700" w="50" h="50" />
+            <roi x="400" y="500" w="150" h="150" />
-            <roi x="1200" y="700" w="50" h="50" />
+            <roi x="400" y="700" w="150" h="150" />
-            <roi x="1500" y="700" w="50" h="50" />
+            <roi x="400" y="1000" w="150" h="150" />
+
+            <roi x="1900" y="200" w="150" h="150" />
+            <roi x="1900" y="500" w="150" h="150" />
+            <roi x="1900" y="700" w="150" h="150" />
+            <roi x="1900" y="1000" w="150" h="150" />
         </rois>
         <colors>
             <color name="blue" hue_min="90" hue_max="130" />
             <color name="yellow" hue_min="20" hue_max="40" />
+
+            <color name="red" hue_min="0" hue_max="10" />
+
+            <color name="red" hue_min="160" hue_max="180" />
         </colors>
     </cam>
-</config>
+    <usb>
+        <odo>
+            <port>/dev/USB_ODO</port>
+            <baudrate>115200</baudrate>
+        </odo>
+        <action>
+            <port>/dev/USB_ACTION</port>
+            <baudrate>115200</baudrate>
+        </action>
+        <timeout_ms>300</timeout_ms>
+        <attempt>5</attempt>
+    </usb>
+    <ihm>
+        <sound>
+            <enabled>false</enabled>
+            <test path=":/sound/test.mp3" />
+        </sound>
+    </ihm>
+    <odo>
+        <pid>
+            <left p="4" i="0.1" d="0" />
+            <right p="4" i="0.1" d="0" />
+            <pos p="6" i="0" d="0.1" />
+            <theta p="10" i="0" d="0.1" />
+        </pid>
+    </odo>
+</config>

src/modelec_strat/data/deposite_zone.xml

@@ -1,23 +1,23 @@
 <Map>
-    <DepositeZone id="0">
+    <epositeZone id="0">
         <Pos x="1250" y="1450" theta="0" w="200" h="200" />
         <TakeAngle>
             <Angle>-1.5708</Angle>
         </TakeAngle>
-    </DepositeZone>
+    </epositeZone>
     <DepositeZone id="1">
         <Pos x="1750" y="1450" theta="0" w="200" h="200" />
         <TakeAngle>
             <Angle>-1.5708</Angle>
         </TakeAngle>
     </DepositeZone>
-    <DepositeZone id="2">
+    <epositeZone id="2">
         <Pos x="100" y="800" theta="0" w="200" h="200" />
         <TakeAngle>
             <Angle>0</Angle>
         </TakeAngle>
-    </DepositeZone>
+    </epositeZone>
-    <DepositeZone id="3">
+    <epositeZone id="3">
         <Pos x="800" y="800" theta="0" w="200" h="200" />
         <TakeAngle>
             <Angle>0</Angle>
@@ -25,7 +25,7 @@
             <Angle>-1.5708</Angle>
             <Angle>3.1415</Angle>
         </TakeAngle>
-    </DepositeZone>
+    </epositeZone>
     <DepositeZone id="4">
         <Pos x="1500" y="800" theta="0" w="200" h="200" />
         <TakeAngle>
@@ -50,12 +50,12 @@
             <Angle>3.1415</Angle>
         </TakeAngle>
     </DepositeZone>
-    <DepositeZone id="7">
+    <epositeZone id="7">
         <Pos x="700" y="100" theta="0" w="200" h="200" />
         <TakeAngle>
             <Angle>1.5708</Angle>
         </TakeAngle>
-    </DepositeZone>
+    </epositeZone>
     <DepositeZone id="8">
         <Pos x="1500" y="100" theta="0" w="200" h="200" />
         <TakeAngle>

src/modelec_strat/data/obstacles.xml

@@ -3,20 +3,20 @@
     <Obstacle id="2" x="1500" y="1775" theta="1.5708" width="1800" height="450" type="estrade"/>
 
     <!-- Box TOP TO BOTTOM    LEFT -->
-    <Box id="10" x="175" y="1200" theta="0" width="200" height="150" type="box" />
+    <!--<Obstacle id="10" x="175" y="1200" theta="0" width="200" height="150" type="box" />
-    <Box id="11" x="1150" y="800" theta="1.5708" width="200" height="150" type="box" >
+    <Obstacle id="11" x="1150" y="800" theta="1.5708" width="200" height="150" type="box" >
         <possible-angle theta="-1.5708" />
-    </Box>
+    </Obstacle>
-    <Box id="12" x="175" y="400" theta="0" width="200" height="150" type="box" />
+    <Obstacle id="12" x="175" y="400" theta="0" width="200" height="150" type="box" />
-    <Box id="13" x="1100" y="175" theta="1.5708" width="200" height="150" type="box" />
+    <Obstacle id="13" x="1100" y="175" theta="1.5708" width="200" height="150" type="box" />-->
 
     <!-- Box TOP TO BOTTOM    RIGHT -->
-    <Box id="20" x="2825" y="1200" theta="3.1415" width="200" height="150" type="box" />
+    <Obstacle id="20" x="2825" y="1200" theta="3.1415" width="200" height="150" type="box" />
-    <Box id="21" x="1850" y="800" theta="1.5708" width="200" height="150" type="box">
+    <Obstacle id="21" x="1850" y="800" theta="1.5708" width="200" height="150" type="box">
         <possible-angle theta="-1.5708" />
-    </Box>
+    </Obstacle>
-    <Box id="22" x="2825" y="400" theta="3.1415" width="200" height="150" type="box"/>
+    <Obstacle id="22" x="2825" y="400" theta="3.1415" width="200" height="150" type="box"/>
-    <Box id="23" x="1900" y="175" theta="1.5708" width="200" height="150" type="box"/>
+    <Obstacle id="23" x="1900" y="175" theta="1.5708" width="200" height="150" type="box"/>
 
     <!-- PAMI Start -->
     <Obstacle id="30" x="75" y="1750" theta="1.5708" width="150" height="450" type="pami-start"/>

src/modelec_strat/include/modelec_strat/action/base_action.hpp

@@ -12,6 +12,8 @@
 #include <modelec_interfaces/msg/action_servo_pos_array.hpp>
 #include <modelec_interfaces/msg/action_servo_pos.hpp>
 
+#include <modelec_utils/config.hpp>
+
 namespace Modelec
 {
     class ActionExecutor;
@@ -79,4 +81,30 @@ namespace Modelec
 
         return nullptr;
     }
+
+    template<>
+    inline ActionServoTimed
+    Config::get<ActionServoTimed>(
+        const std::string& prefix,
+        const ActionServoTimed&, bool)
+    {
+        ActionServoTimed msg;
+        msg.id = get<int>(prefix + "@id", 0);
+        msg.start_angle = get<double>(prefix + "@start_angle", 0);
+        msg.end_angle = get<double>(prefix + "@end_angle", 0);
+        msg.duration_s = get<double>(prefix + "@duration_s", 0);
+        msg.delay_s = get<double>(prefix + "@delay_s", 0, true);
+        return msg;
+    }
+
+    template<>
+    inline std::vector<ActionServoTimed>
+    Config::get<std::vector<ActionServoTimed>>(
+        const std::string& prefix,
+        const std::vector<ActionServoTimed>& default_value, bool)
+    {
+        auto result = getArray<ActionServoTimed>(prefix);
+
+        return result.empty() ? default_value : result;
+    }
 }

src/modelec_strat/include/modelec_strat/action/down_action.hpp

@@ -15,6 +15,7 @@ namespace Modelec
         void Init(const std::vector<std::string>& params) override;
         void SetSide(Side side);
         void SetInverted(bool inverted);
+        static void InitConfig();
 
         void End() override;
 
@@ -27,6 +28,13 @@ namespace Modelec
 
         std::queue<int> steps_;
 
+        static inline std::vector<ActionServoTimed> front_unrotated_msg_;
+        static inline std::vector<ActionServoTimed> front_rotated_msg_;
+        static inline std::vector<ActionServoTimed> back_unrotated_msg_;
+        static inline std::vector<ActionServoTimed> back_rotated_msg_;
+
+        static inline bool isConfigInit_ = false;
+
         inline static const bool registered_ =
         []()
         {

src/modelec_strat/include/modelec_strat/action/free_action.hpp

@@ -18,6 +18,7 @@ namespace Modelec
         void AddServo(int id, Side side);
         void AddServo(std::pair<int, Side> servo);
         void AddServos(const std::vector<std::pair<int, Side>>& servos);
+        static void InitConfig();
 
         void End() override;
 
@@ -28,6 +29,14 @@ namespace Modelec
 
         std::queue<int> steps_;
 
+        static inline int first_servo_;
+        static inline int second_servo_;
+        static inline double start_angle_;
+        static inline double end_angle_;
+        static inline double duration_s_;
+
+        static inline bool isConfigInit_ = false;
+
         inline static const bool registered_ =
         []()
         {

src/modelec_strat/include/modelec_strat/action/look_on_action.hpp

@@ -1,38 +0,0 @@
-#pragma once
-
-#include <queue>
-#include <modelec_strat/action/base_action.hpp>
-
-namespace Modelec
-{
-    class LookOnAction : public BaseAction
-    {
-    public:
-        LookOnAction(const std::shared_ptr<ActionExecutor>& action_executor);
-        LookOnAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side);
-
-        void Next() override;
-        void Init(const std::vector<std::string>& params) override;
-        void SetSide(Side side);
-
-        void End() override;
-
-        inline static const std::string Name = ActionExec::LOOK_ON;
-
-    private:
-        Side side_ = CENTER;
-
-        std::queue<int> steps_;
-
-        inline static const bool registered_ =
-        []()
-        {
-            BaseAction::Registry()[Name] =
-                [](const std::shared_ptr<ActionExecutor>& exec)
-                {
-                    return std::make_shared<LookOnAction>(exec);
-                };
-            return true;
-        }();
-    };
-}

src/modelec_strat/include/modelec_strat/action/take_action.hpp

@@ -18,6 +18,7 @@ namespace Modelec
         void AddServo(int id, Side side);
         void AddServo(std::pair<int, Side> servo);
         void AddServos(const std::vector<std::pair<int, Side>>& servos);
+        static void InitConfig();
 
         void End() override;
 
@@ -28,6 +29,14 @@ namespace Modelec
 
         std::queue<int> steps_;
 
+        static inline int first_servo_;
+        static inline int second_servo_;
+        static inline double start_angle_;
+        static inline double end_angle_;
+        static inline double duration_s_;
+
+        static inline bool isConfigInit_ = false;
+
         inline static const bool registered_ =
         []()
         {

src/modelec_strat/include/modelec_strat/action/thermo_action.hpp

@@ -15,6 +15,7 @@ namespace Modelec
         void Init(const std::vector<std::string>& params) override;
         void SetSide(Side side);
         void SetDeploy(bool deploy);
+        static void InitConfig();
 
         void End() override;
 
@@ -26,6 +27,13 @@ namespace Modelec
 
         std::queue<int> steps_;
 
+        static inline std::vector<ActionServoTimed> left_deploy_msg_;
+        static inline std::vector<ActionServoTimed> left_undeploy_msg_;
+        static inline std::vector<ActionServoTimed> right_deploy_msg_;
+        static inline std::vector<ActionServoTimed> right_undeploy_msg_;
+
+        static inline bool isConfigInit_ = false;
+
         inline static const bool registered_ =
         []()
         {

src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp

@@ -18,6 +18,7 @@ namespace Modelec
         void AddServo(int id, Side side);
         void AddServo(std::pair<int, Side> servo);
         void AddServos(const std::vector<std::pair<int, Side>>& servos);
+        static void InitConfig();
 
         void End() override;
 
@@ -28,6 +29,14 @@ namespace Modelec
 
         std::queue<int> steps_;
 
+        static inline int first_servo_;
+        static inline int second_servo_;
+        static inline double start_angle_;
+        static inline double end_angle_;
+        static inline double duration_s_;
+
+        static inline bool isConfigInit_ = false;
+
         inline static const bool registered_ =
         []()
         {

src/modelec_strat/include/modelec_strat/action/up_action.hpp

@@ -14,6 +14,7 @@ namespace Modelec
         void Next() override;
         void Init(const std::vector<std::string>& params) override;
         void SetSide(Side side);
+        static void InitConfig();
 
         void End() override;
 
@@ -24,6 +25,13 @@ namespace Modelec
 
         std::queue<int> steps_;
 
+        static inline std::vector<ActionServoTimed> front_rotated_msg_;
+        static inline std::vector<ActionServoTimed> front_unrotated_msg_;
+        static inline std::vector<ActionServoTimed> back_rotated_msg_;
+        static inline std::vector<ActionServoTimed> back_unrotated_msg_;
+
+        static inline bool isConfigInit_ = false;
+
         inline static const bool registered_ =
         []()
         {

src/modelec_strat/include/modelec_strat/action_executor.hpp

@@ -9,11 +9,11 @@
 
 #include <sensor_msgs/msg/joy.hpp>
 #include <std_msgs/msg/int64.hpp>
+#include <std_msgs/msg/empty.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <std_srvs/srv/trigger.hpp>
 
 #include "action/base_action.hpp"
-#include "missions/thermo_mission.hpp"
 #include "obstacle/box.hpp"
 
 namespace Modelec
@@ -55,8 +55,6 @@ namespace Modelec
 
         void ActivateThermo(BaseAction::Side side, bool deploy, bool force = false);
 
-        void LookOn(BaseAction::Side side, bool force = false);
-
         void ActionFinished(const std::shared_ptr<BaseAction>& action);
 
         std::array<std::shared_ptr<BoxObstacle>, 2> box_obstacles_;
@@ -68,7 +66,10 @@ namespace Modelec
             bool down;
             bool rotated;
         };
-        std::array<ArmState, 2> arm_pos_;
+        std::array<ArmState, 2> arm_pos_ = {
+            ArmState{false, false},
+            ArmState{false, false},
+        };
 
         std::array<float, 16> servo_value_ = {
             2.91,
@@ -86,10 +87,6 @@ namespace Modelec
             {BaseAction::RIGHT, false},
         };
 
-        BaseAction::Side cam_side_ = BaseAction::Side::CENTER;
-
-        bool looking_on_front_ = true;
-
         bool IsEmpty() const;
 
         bool IsFull() const;
@@ -143,6 +140,9 @@ namespace Modelec
         float last_left_trig = 1.0f;
         float last_right_trig = 1.0f;
 
+        bool last_l3 = false;
+        bool last_r3 = false;
+
     private:
         rclcpp::Node::SharedPtr node_;
     };

src/modelec_strat/include/modelec_strat/deposite_zone.hpp

@@ -1,7 +1,6 @@
 #pragma once
 
-#include <queue>
+#include <modelec_utils/config.hpp>
-#include <tinyxml2.h>
 #include <modelec_utils/point.hpp>
 
 namespace Modelec
@@ -9,7 +8,7 @@ namespace Modelec
     class DepositeZone
     {
     public:
-        DepositeZone(tinyxml2::XMLElement* obstacleElem);
+        DepositeZone(int id);
 
         Point GetPosition() const;
 
@@ -20,13 +19,12 @@ namespace Modelec
         bool Validate() const;
 
         int GetId() const;
-        int GetMaxPot() const;
 
         int GetWidth() const;
         int GetHeight() const;
 
     protected:
-        int id_, max_pot_;
+        int id_;
         int w_, h_;
         Point position_;
 

src/modelec_strat/include/modelec_strat/missions/action_mission.hpp

@@ -0,0 +1,26 @@
+#pragma once
+
+#include <modelec_strat/missions/mission_base.hpp>
+#include <modelec_strat/action_executor.hpp>
+
+namespace Modelec {
+    class ActionMission : virtual public Mission {
+    public:
+        ActionMission(const std::shared_ptr<ActionExecutor>& action_executor);
+
+        static void InitConfig();
+
+        void Start(const rclcpp::Node::SharedPtr& node) override;
+
+        bool Update() override;
+
+        std::string GetName() const override { return "Action"; }
+
+    protected:
+        std::shared_ptr<ActionExecutor> action_executor_;
+        rclcpp::Time deploy_time_;
+
+        static double TIMEOUT;
+        static bool IsInit;
+    };
+}

src/modelec_strat/include/modelec_strat/missions/free_mission.hpp

@@ -1,20 +1,19 @@
 #pragma once
 
-#include <modelec_strat/missions/mission_base.hpp>
+#include <modelec_strat/missions/action_mission.hpp>
-#include <modelec_strat/navigation_helper.hpp>
+#include <modelec_strat/missions/move_mission.hpp>
-#include <modelec_strat/action_executor.hpp>
+#include <modelec_strat/missions/min_time_mission.hpp>
 
 namespace Modelec {
-    class FreeMission : public Mission {
+    class FreeMission : public ActionMission, public MoveMission, public MinTimeMission {
     public:
         FreeMission(const std::shared_ptr<NavigationHelper>& nav,
                       const std::shared_ptr<ActionExecutor>& action_executor,
                       BaseAction::Side side = BaseAction::FRONT);
 
-        void Start(rclcpp::Node::SharedPtr node) override;
+        void Start(const rclcpp::Node::SharedPtr& node) override;
-        void Update() override;
+        bool Update() override;
         void Clear() override;
-        MissionStatus GetStatus() const override;
         std::string GetName() const override { return "Free"; }
 
     private:
@@ -33,20 +32,8 @@ namespace Modelec {
 
         BaseAction::Side side_;
 
-        MissionStatus status_;
-        std::shared_ptr<NavigationHelper> nav_;
-        std::shared_ptr<ActionExecutor> action_executor_;
-        rclcpp::Node::SharedPtr node_;
-
         std::shared_ptr<DepositeZone> target_deposite_zone_;
 
         double angle_;
-
-        rclcpp::Time go_timeout_;
-        rclcpp::Time deploy_time_;
-
-        std::optional<rclcpp::Time> min_time_;
-
-        rclcpp::Time last_ask_waypoint_time_;
     };
 }

src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp

@@ -1,20 +1,18 @@
 #pragma once
 
-#include <modelec_strat/missions/mission_base.hpp>
+#include <modelec_strat/missions/move_mission.hpp>
-#include <modelec_strat/navigation_helper.hpp>
 #include <std_msgs/msg/int64.hpp>
 
 namespace Modelec
 {
-    class GoHomeMission : public Mission
+    class GoHomeMission : public MoveMission
     {
     public:
         GoHomeMission(const std::shared_ptr<NavigationHelper>& nav, const rclcpp::Time& start_time);
 
-        void Start(rclcpp::Node::SharedPtr node) override;
+        void Start(const rclcpp::Node::SharedPtr& node) override;
-        void Update() override;
+        bool Update() override;
         void Clear() override;
-        MissionStatus GetStatus() const override;
         std::string GetName() const override { return "GoHome"; }
 
     private:
@@ -26,18 +24,10 @@ namespace Modelec
             DONE,
         };
 
-        MissionStatus status_;
-        std::shared_ptr<NavigationHelper> nav_;
         rclcpp::Time go_home_time_;
-        rclcpp::Node::SharedPtr node_;
         rclcpp::Time start_time_;
         Point home_point_;
         rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
         int mission_score_ = 0;
-        rclcpp::Time go_timeout_;
-
-        std::optional<rclcpp::Time> min_time_;
-
-        rclcpp::Time last_ask_waypoint_time_;
     };
 }

src/modelec_strat/include/modelec_strat/missions/min_time_mission.hpp

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <modelec_strat/missions/mission_base.hpp>
+#include <optional>
+
+namespace Modelec {
+    class MinTimeMission : virtual public Mission {
+    public:
+        MinTimeMission();
+
+        static void InitConfig();
+
+        void Start(const rclcpp::Node::SharedPtr& node) override;
+
+        bool Update() override;
+
+        std::string GetName() const override { return "MinTime"; }
+
+    protected:
+        std::optional<rclcpp::Time> min_time_;
+
+        static double MIN_TIME_DURATION;
+        static bool IsInit;
+    };
+}

src/modelec_strat/include/modelec_strat/missions/mission_base.hpp

@@ -2,7 +2,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <string>
-
+#include <queue>
 
 namespace Modelec
 {
@@ -18,14 +18,19 @@ namespace Modelec
     class Mission
     {
     public:
+        Mission(MissionStatus status = MissionStatus::READY);
         virtual ~Mission() = default;
-        virtual void Start(rclcpp::Node::SharedPtr node) = 0;
+
-        virtual void Update() = 0;
+        virtual void Start(const rclcpp::Node::SharedPtr& node);
+        virtual bool Update() = 0;
         virtual void Clear() = 0;
-        virtual MissionStatus GetStatus() const = 0;
+
+        virtual MissionStatus GetStatus() const;
         virtual std::string GetName() const = 0;
 
     protected:
         std::queue<int> steps_;
+        rclcpp::Node::SharedPtr node_;
+        MissionStatus status_;
     };
-}
+}

src/modelec_strat/include/modelec_strat/missions/move_mission.hpp

@@ -0,0 +1,30 @@
+#pragma once
+
+#include <modelec_strat/missions/mission_base.hpp>
+#include <modelec_strat/navigation_helper.hpp>
+
+namespace Modelec {
+    class MoveMission : virtual public Mission {
+    public:
+        MoveMission(const std::shared_ptr<NavigationHelper>& nav);
+
+        static void InitConfig();
+
+        void Start(const rclcpp::Node::SharedPtr& node) override;
+
+        bool Update() override;
+
+        std::string GetName() const override { return "Move"; }
+
+    protected:
+        std::shared_ptr<NavigationHelper> nav_;
+
+        rclcpp::Time go_timeout_;
+        rclcpp::Time last_ask_waypoint_time_;
+
+        static double MIN_ASK_WAYPOINT;
+        static double ASK_WAYPOINT_INTERVAL;
+        static double GO_TIMEOUT;
+        static bool IsInit;
+    };
+}

src/modelec_strat/include/modelec_strat/missions/take_mission.hpp

@@ -1,20 +1,19 @@
 #pragma once
 
-#include <modelec_strat/missions/mission_base.hpp>
+#include <modelec_strat/missions/action_mission.hpp>
-#include <modelec_strat/navigation_helper.hpp>
+#include <modelec_strat/missions/min_time_mission.hpp>
-#include <modelec_strat/action_executor.hpp>
+#include <modelec_strat/missions/move_mission.hpp>
 
 namespace Modelec {
-    class TakeMission : public Mission {
+    class TakeMission : public MinTimeMission, public ActionMission, public MoveMission {
     public:
         TakeMission(const std::shared_ptr<NavigationHelper>& nav,
                       const std::shared_ptr<ActionExecutor>& action_executor,
                       BaseAction::Side side = BaseAction::FRONT);
 
-        void Start(rclcpp::Node::SharedPtr node) override;
+        void Start(const rclcpp::Node::SharedPtr& node) override;
-        void Update() override;
+        bool Update() override;
         void Clear() override;
-        MissionStatus GetStatus() const override;
         std::string GetName() const override { return "Take"; }
 
     private:
@@ -31,16 +30,5 @@ namespace Modelec {
         BaseAction::Side side_;
 
         std::shared_ptr<BoxObstacle> closestBox;
-        MissionStatus status_;
-        std::shared_ptr<NavigationHelper> nav_;
-        std::shared_ptr<ActionExecutor> action_executor_;
-        rclcpp::Node::SharedPtr node_;
-
-        rclcpp::Time go_timeout_;
-        rclcpp::Time deploy_time_;
-
-        std::optional<rclcpp::Time> min_time_;
-
-        rclcpp::Time last_ask_waypoint_time_;
     };
-}
+}

src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp

@@ -1,19 +1,17 @@
 #pragma once
 
-#include <modelec_strat/missions/mission_base.hpp>
+#include <modelec_strat/missions/action_mission.hpp>
-#include <modelec_strat/navigation_helper.hpp>
+#include <modelec_strat/missions/move_mission.hpp>
-#include <modelec_strat/action_executor.hpp>
 
 namespace Modelec {
-    class ThermoMission : public Mission {
+    class ThermoMission : public ActionMission, public MoveMission {
     public:
         ThermoMission(const std::shared_ptr<NavigationHelper>& nav,
                       const std::shared_ptr<ActionExecutor>& action_executor);
 
-        void Start(rclcpp::Node::SharedPtr node) override;
+        void Start(const rclcpp::Node::SharedPtr& node) override;
-        void Update() override;
+        bool Update() override;
         void Clear() override;
-        MissionStatus GetStatus() const override;
         std::string GetName() const override { return "Thermo"; }
 
         static bool IsThermoDone;
@@ -32,16 +30,5 @@ namespace Modelec {
         std::array<Point, 2> thermo_positions_;
 
         std::shared_ptr<BoxObstacle> closestBox;
-        MissionStatus status_;
-        std::shared_ptr<NavigationHelper> nav_;
-        std::shared_ptr<ActionExecutor> action_executor_;
-        rclcpp::Node::SharedPtr node_;
-
-        rclcpp::Time go_timeout_;
-        rclcpp::Time deploy_time_;
-
-        std::optional<rclcpp::Time> min_time_;
-
-        rclcpp::Time last_ask_waypoint_time_;
     };
 }

src/modelec_strat/include/modelec_strat/navigation_helper.hpp

@@ -85,26 +85,26 @@ namespace Modelec
 
         PosMsg::SharedPtr GetCurrentPos() const;
 
-        bool LoadDepositeZoneFromXML(const std::string& filename);
+        void LoadDepositeZoneFromXML();
 
         std::shared_ptr<DepositeZone> GetClosestDepositeZone(const PosMsg::SharedPtr& pos,
                                                              const std::vector<int>& blacklistedId = {}, bool only_free = false);
 
         template <typename T,
           typename = std::enable_if_t<std::is_base_of<Obstacle, T>::value>>
-        std::shared_ptr<T> GetClosestObstacle(const PosMsg::SharedPtr& pos) const;
+        std::shared_ptr<T> GetClosestObstacle(const PosMsg::SharedPtr& pos, bool countObjective = false) const;
 
         PosMsg::SharedPtr GetHomePosition();
         std::array<Point, 2> GetThermoPositions();
 
-        void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
+        void OnEnemyPosition(modelec_interfaces::msg::OdometryPos::SharedPtr msg);
 
-        void OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
+        void OnEnemyPositionLongTime(modelec_interfaces::msg::OdometryPos::SharedPtr msg);
 
         bool DoesLineIntersectCircle(const Point& start, const Point& end,
                                      const Point& center, float radius);
 
-        bool EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg);
+        bool EnemyOnPath(modelec_interfaces::msg::OdometryPos msg);
 
         bool Replan(bool force = false);
 
@@ -152,7 +152,7 @@ namespace Modelec
 
         PosMsg::SharedPtr current_pos_;
 
-        std::map<int, std::shared_ptr<DepositeZone>> deposite_zones_;
+        std::vector<std::shared_ptr<DepositeZone>> deposite_zones_;
 
         rclcpp::Subscription<WaypointMsg>::SharedPtr waypoint_reach_sub_;
         rclcpp::Publisher<WaypointMsg>::SharedPtr waypoint_pub_;
@@ -181,7 +181,7 @@ namespace Modelec
     };
 
     template <typename T, typename>
-    std::shared_ptr<T> NavigationHelper::GetClosestObstacle(const PosMsg::SharedPtr& pos) const
+    std::shared_ptr<T> NavigationHelper::GetClosestObstacle(const PosMsg::SharedPtr& pos, bool countObjective) const
     {
         std::shared_ptr<T> closest_obstacle = nullptr;
         auto robotPos = Point(pos->x, pos->y, pos->theta);
@@ -190,9 +190,9 @@ namespace Modelec
 
         for (const auto& obstacle : GetPathfinding()->GetObstacles())
         {
-            if (auto obs = std::dynamic_pointer_cast<T>(obstacle.second))
+            if (auto obs = std::dynamic_pointer_cast<T>(obstacle))
             {
-                if (!obs->IsAtObjective())
+                if (!obs->IsAtObjective() || countObjective)
                 {
                     auto obsPoint = obs->GetPosition();
                     double distance = Point::distance(robotPos, obsPoint);

src/modelec_strat/include/modelec_strat/obstacle/box.hpp

@@ -2,6 +2,7 @@
 
 #include "obstacle.hpp"
 #include <modelec_interfaces/msg/odometry_pos.hpp>
+#include <modelec_utils/config.hpp>
 
 namespace Modelec
 {
@@ -18,7 +19,7 @@ namespace Modelec
         BoxObstacle(const BoxObstacle&) = default;
 
         BoxObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type);
-        BoxObstacle(tinyxml2::XMLElement* obstacleElem);
+        BoxObstacle(int id);
         BoxObstacle(const modelec_interfaces::msg::Obstacle& msg);
 
         Point GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const;
@@ -42,8 +43,8 @@ namespace Modelec
         std::array<Team, 4> colors_ = {
             BLUE,
             BLUE,
-            BLUE,
+            YELLOW,
-            BLUE
+            YELLOW
         };
     };
 }

src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp

@@ -18,8 +18,8 @@ namespace Modelec
         }
 
         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(int id);
         Obstacle(const Obstacle& other) = default;
 
         virtual ~Obstacle() = default;
@@ -32,7 +32,7 @@ namespace Modelec
         double GetTheta() const;
         int GetWidth() const;
         int GetHeight() const;
-        const std::string& Type() const;
+        const std::string& GetType() const;
         Point GetPosition() const;
 
         void SetId(int id);
@@ -52,11 +52,15 @@ namespace Modelec
         bool IsAtObjective() const;
         void SetAtObjective(bool atObjective);
 
+        bool ShouldTakeCountInPathfinding() const;
+        void SetTakeCountInPathfinding(bool takeCount);
+
     protected:
         int id_, x_, y_, w_, h_;
         double theta_;
         std::string type_;
 
         bool isAtObjective = false;
+        bool takeCountInPathfinding = true;
     };
 }

src/modelec_strat/include/modelec_strat/pami_manager.hpp

@@ -2,39 +2,11 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <modelec_utils/point.hpp>
-
-#include <modelec_strat/obstacle/obstacle.hpp>
-
-#include <tinyxml2.h>
-
-#include <std_msgs/msg/int64.hpp>
-#include <std_msgs/msg/empty.hpp>
-
 namespace Modelec
 {
     class PamiManger : public rclcpp::Node
     {
     public:
         PamiManger();
-
-        bool ReadFromXML(const std::string& filename);
-
-    protected:
-        std::vector<Obstacle> zones_;
-
-        int time_to_put_zone_ = 0;
-        int time_to_remove_top_pot_ = 0;
-
-        int score_to_add_ = 0;
-
-        rclcpp::TimerBase::SharedPtr timer_add_;
-        // rclcpp::TimerBase::SharedPtr timer_remove_;
-
-        rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr start_time_sub_;
-        rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr add_obs_pub_;
-        rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr remove_obs_pub_;
-        rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
-        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr reset_strat_sub_;
     };
 }

src/modelec_strat/include/modelec_strat/pathfinding.hpp

@@ -73,14 +73,14 @@ namespace Modelec
 
         //void SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal);
 
-        bool LoadObstaclesFromXML(const std::string& filename);
+        void LoadObstaclesFromXML();
 
         void SetCurrentPos(const PosMsg::SharedPtr& pos);
 
         [[nodiscard]] std::shared_ptr<Obstacle> GetObstacle(int id) const;
 
-        std::map<int, std::shared_ptr<Obstacle>> GetObstacles() const {
+        std::vector<std::shared_ptr<Obstacle>> GetObstacles() const {
-            return obstacle_map_;
+            return obstacles_;
         }
 
         void RemoveObstacle(int id);
@@ -114,7 +114,7 @@ namespace Modelec
 
         std::vector<std::vector<int>> grid_;
 
-        std::map<int, std::shared_ptr<Obstacle>> obstacle_map_;
+        std::vector<std::shared_ptr<Obstacle>> obstacles_;
 
         PosMsg::SharedPtr current_start_;
         PosMsg::SharedPtr current_goal_;

src/modelec_strat/src/action/down_action.cpp

@@ -2,13 +2,17 @@
 
 #include "modelec_strat/action_executor.hpp"
 
-Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor) : BaseAction(action_executor), side_(BOTH), inverted_(false)
+Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor) : BaseAction(action_executor),
+    side_(BOTH), inverted_(false)
 {
     steps_.push(ActionExec::DOWN_STEP);
     steps_.push(ActionExec::DONE_STEP);
+
+    InitConfig();
 }
 
-Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, bool inverted) : DownAction(action_executor)
+Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side,
+                                bool inverted) : DownAction(action_executor)
 {
     side_ = side;
     inverted_ = inverted;
@@ -30,53 +34,27 @@ void Modelec::DownAction::Next()
     case ActionExec::DOWN_STEP:
         {
             ActionServoTimedArray msg;
-            msg.items.resize(side_ == BOTH ? 8 : 4);
 
             if (side_ == FRONT || side_ == BOTH)
             {
-                msg.items[0].id = 0;
+                if (inverted_)
-                msg.items[0].start_angle = 1.76;
+                {
-                msg.items[0].end_angle = 2.93;
+                    msg.items.insert(msg.items.end(), front_rotated_msg_.begin(), front_rotated_msg_.end());
-                msg.items[0].duration_s = 2;
+                } else
-
+                {
-                msg.items[1].id = 1;
+                    msg.items.insert(msg.items.end(), front_unrotated_msg_.begin(), front_unrotated_msg_.end());
-                msg.items[1].start_angle = 2.06;
+                }
-                msg.items[1].end_angle = 0.91;
-                msg.items[1].duration_s = 2;
-
-                msg.items[2].id = 2;
-                msg.items[2].start_angle = 0.5;
-                msg.items[2].end_angle = inverted_ ? 0 : 3.05;
-                msg.items[2].duration_s = 1.7;
-
-                msg.items[3].id = 3;
-                msg.items[3].start_angle = 2.6;
-                msg.items[3].end_angle = inverted_ ? 3.1 : 0.3;
-                msg.items[3].duration_s = 1.7;
             }
 
             if (side_ == BACK || side_ == BOTH)
             {
-                int i = side_ == BOTH ? 4 : 0;
+                if (inverted_)
-                msg.items[i].id = 8;
+                {
-                msg.items[i].start_angle = 0;
+                    msg.items.insert(msg.items.end(), back_rotated_msg_.begin(), back_rotated_msg_.end());
-                msg.items[i].end_angle = 0;
+                } else
-                msg.items[i].duration_s = 0.5;
+                {
-
+                    msg.items.insert(msg.items.end(), back_unrotated_msg_.begin(), back_unrotated_msg_.end());
-                msg.items[i+1].id = 9;
+                }
-                msg.items[i+1].start_angle = 0;
-                msg.items[i+1].end_angle = 0;
-                msg.items[i+1].duration_s = 0.5;
-
-                msg.items[i+2].id = 10;
-                msg.items[i+2].start_angle = 0;
-                msg.items[i+2].end_angle = 0;
-                msg.items[i+2].duration_s = 0.5;
-
-                msg.items[i+3].id = 11;
-                msg.items[i+3].start_angle = 0;
-                msg.items[i+3].end_angle = 0;
-                msg.items[i+3].duration_s = 0.5;
             }
 
             action_executor_->MoveServoTimed(msg);
@@ -111,6 +89,17 @@ void Modelec::DownAction::SetInverted(bool inverted)
     inverted_ = inverted;
 }
 
+void Modelec::DownAction::InitConfig()
+{
+    if (isConfigInit_) return;
+    isConfigInit_ = true;
+
+    front_unrotated_msg_ = Config::get<std::vector<ActionServoTimed>>("action.down.front.unrotated.msg");
+    front_rotated_msg_ = Config::get<std::vector<ActionServoTimed>>("action.down.front.rotated.msg");
+    back_unrotated_msg_ = Config::get<std::vector<ActionServoTimed>>("action.down.back.unrotated.msg");
+    back_rotated_msg_ = Config::get<std::vector<ActionServoTimed>>("action.down.back.rotated.msg");
+}
+
 void Modelec::DownAction::End()
 {
     if (side_ == BOTH)

src/modelec_strat/src/action/free_action.cpp

@@ -6,6 +6,8 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_ex
 {
     steps_.push(ActionExec::FREE_STEP);
     steps_.push(ActionExec::DONE_STEP);
+
+    InitConfig();
 }
 
 Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, int n) : FreeAction(action_executor)
@@ -45,9 +47,9 @@ void Modelec::FreeAction::Next()
             for (size_t i = 0; i < servos_.size(); i++)
             {
                 msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12);
-                msg.items[i].start_angle = 3;
+                msg.items[i].start_angle = start_angle_;
-                msg.items[i].end_angle = 1;
+                msg.items[i].end_angle = end_angle_;
-                msg.items[i].duration_s = 0.5;
+                msg.items[i].duration_s = duration_s_;
             }
 
             action_executor_->MoveServoTimed(msg);
@@ -91,6 +93,18 @@ void Modelec::FreeAction::AddServos(const std::vector<std::pair<int, Side>>& ser
     servos_.insert(servos_.end(), servos.begin(), servos.end());
 }
 
+void Modelec::FreeAction::InitConfig()
+{
+    if (isConfigInit_) return;
+    isConfigInit_ = true;
+
+    first_servo_ = Config::get<int>("action.free.id@first", 0);
+    second_servo_ = Config::get<int>("action.free.id@second", 0);
+    start_angle_ = Config::get<double>("action.free.msg@start_angle", 3);
+    end_angle_ = Config::get<double>("action.free.msg@end_angle", 1);
+    duration_s_ = Config::get<double>("action.free.msg@duration_s", 0.5);
+}
+
 void Modelec::FreeAction::End()
 {
     for (auto servo : servos_)

src/modelec_strat/src/action/look_on_action.cpp

@@ -1,69 +0,0 @@
-#include <modelec_strat/action/look_on_action.hpp>
-
-#include "modelec_strat/action_executor.hpp"
-
-Modelec::LookOnAction::LookOnAction(const std::shared_ptr<ActionExecutor>& action_executor) : BaseAction(action_executor)
-{
-    steps_.push(ActionExec::LOOK_ON_STEP);
-    steps_.push(ActionExec::DONE_STEP);
-}
-
-Modelec::LookOnAction::LookOnAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side) : LookOnAction(action_executor)
-{
-    side_ = side;
-}
-
-void Modelec::LookOnAction::Next()
-{
-    if (steps_.empty())
-    {
-        done_ = true;
-        return;
-    }
-
-    auto step = steps_.front();
-    steps_.pop();
-
-    switch (step)
-    {
-    case ActionExec::LOOK_ON_STEP:
-        {
-            modelec_interfaces::msg::ActionServoTimedArray msg;
-
-            msg.items.resize(1);
-
-            msg.items[0].id = 18;
-            msg.items[0].start_angle = action_executor_->cam_side_ == CENTER ? 1 : action_executor_->cam_side_ == FRONT ? 0 : 2;
-            msg.items[0].end_angle = side_ == CENTER ? 1 : side_ == FRONT ? 0 : 2;
-            msg.items[0].duration_s = 0.5;
-
-            action_executor_->MoveServoTimed(msg);
-        }
-        break;
-    case ActionExec::DONE_STEP:
-        {
-            done_ = true;
-        }
-        break;
-    default:
-        break;
-    }
-}
-
-void Modelec::LookOnAction::Init(const std::vector<std::string>& params)
-{
-    if (!params.empty())
-    {
-        SetSide(static_cast<Side>(std::stoi(params[0])));
-    }
-}
-
-void Modelec::LookOnAction::SetSide(Side side)
-{
-    side_ = side;
-}
-
-void Modelec::LookOnAction::End()
-{
-    action_executor_->cam_side_ = side_;
-}

src/modelec_strat/src/action/take_action.cpp

@@ -6,6 +6,8 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_ex
 {
     steps_.push(ActionExec::TAKE_STEP);
     steps_.push(ActionExec::DONE_STEP);
+
+    InitConfig();
 }
 
 Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, int n) : TakeAction(action_executor)
@@ -44,10 +46,10 @@ void Modelec::TakeAction::Next()
 
             for (size_t i = 0; i < servos_.size(); i++)
             {
-                msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12);
+                msg.items[i].id = servos_[i].first + (servos_[i].second ? first_servo_ : second_servo_);
-                msg.items[i].start_angle = 0;
+                msg.items[i].start_angle = start_angle_;
-                msg.items[i].end_angle = 3;
+                msg.items[i].end_angle = end_angle_;
-                msg.items[i].duration_s = 0.5;
+                msg.items[i].duration_s = duration_s_;
             }
 
             action_executor_->MoveServoTimed(msg);
@@ -91,6 +93,18 @@ void Modelec::TakeAction::AddServos(const std::vector<std::pair<int, Side>>& ser
     servos_.insert(servos_.end(), servos.begin(), servos.end());
 }
 
+void Modelec::TakeAction::InitConfig()
+{
+    if (isConfigInit_) return;
+    isConfigInit_ = true;
+
+    first_servo_ = Config::get<int>("action.take.id@first", 0);
+    second_servo_ = Config::get<int>("action.take.id@second", 0);
+    start_angle_ = Config::get<double>("action.take.msg@start_angle", 3);
+    end_angle_ = Config::get<double>("action.take.msg@end_angle", 1);
+    duration_s_ = Config::get<double>("action.take.msg@duration_s", 0.5);
+}
+
 void Modelec::TakeAction::End()
 {
     for (auto servo : servos_)

src/modelec_strat/src/action/thermo_action.cpp

@@ -6,6 +6,8 @@ Modelec::ThermoAction::ThermoAction(const std::shared_ptr<ActionExecutor>& actio
 {
     steps_.push(ActionExec::THERMO_STEP);
     steps_.push(ActionExec::DONE_STEP);
+
+    InitConfig();
 }
 
 Modelec::ThermoAction::ThermoAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, bool deploy) : ThermoAction(action_executor)
@@ -31,22 +33,26 @@ void Modelec::ThermoAction::Next()
         {
             modelec_interfaces::msg::ActionServoTimedArray msg;
 
-            msg.items.resize(side_ == BOTH ? 2 : 1);
-
             if (side_ == LEFT || side_ == BOTH)
             {
-                msg.items[0].id = 16;
+                if (deploy_)
-                msg.items[0].start_angle = deploy_ ? 1 : 2;
+                {
-                msg.items[0].end_angle = deploy_ ? 2 : 1;
+                    msg.items.insert(msg.items.end(), left_deploy_msg_.begin(), left_deploy_msg_.end());
-                msg.items[0].duration_s = 0.5;
+                } else
+                {
+                    msg.items.insert(msg.items.end(), left_undeploy_msg_.begin(), left_undeploy_msg_.end());
+                }
             }
 
             if (side_ == RIGHT || side_ == BOTH)
             {
-                msg.items[side_ == BOTH ? 1 : 0].id = 17;
+                if (deploy_)
-                msg.items[side_ == BOTH ? 1 : 0].start_angle = deploy_ ? 1 : 2;
+                {
-                msg.items[side_ == BOTH ? 1 : 0].end_angle = deploy_ ? 2 : 1;
+                    msg.items.insert(msg.items.end(), right_deploy_msg_.begin(), right_deploy_msg_.end());
-                msg.items[side_ == BOTH ? 1 : 0].duration_s = 0.5;
+                } else
+                {
+                    msg.items.insert(msg.items.end(), right_undeploy_msg_.begin(), right_undeploy_msg_.end());
+                }
             }
 
             action_executor_->MoveServoTimed(msg);
@@ -81,6 +87,17 @@ void Modelec::ThermoAction::SetDeploy(bool deploy)
     deploy_ = deploy;
 }
 
+void Modelec::ThermoAction::InitConfig()
+{
+    if (isConfigInit_) return;
+    isConfigInit_ = true;
+
+    left_deploy_msg_ = Config::get<std::vector<ActionServoTimed>>("action.thermo.left.deploy");
+    left_undeploy_msg_ = Config::get<std::vector<ActionServoTimed>>("action.thermo.left.undeploy");
+    right_deploy_msg_ = Config::get<std::vector<ActionServoTimed>>("action.thermo.right.deploy");
+    right_undeploy_msg_ = Config::get<std::vector<ActionServoTimed>>("action.thermo.right.undeploy");
+}
+
 void Modelec::ThermoAction::End()
 {
     action_executor_->thermo_state_[side_] = deploy_;

src/modelec_strat/src/action/toggle_servo_action.cpp

@@ -6,6 +6,8 @@ Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr<ActionExecut
 {
     steps_.push(ActionExec::TOGGLE_SERVO_STEP);
     steps_.push(ActionExec::DONE_STEP);
+
+    InitConfig();
 }
 
 Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, int n) : ToggleServoAction(action_executor)
@@ -44,10 +46,10 @@ void Modelec::ToggleServoAction::Next()
 
             for (size_t i = 0; i < servos_.size(); i++)
             {
-                msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12);
+                msg.items[i].id = servos_[i].first + (servos_[i].second ? first_servo_ : second_servo_);
-                msg.items[i].start_angle = action_executor_->servo_pos_[servos_[i].first + (servos_[i].second ? 0 : 4)] ? 1 : 3;
+                msg.items[i].start_angle = action_executor_->servo_pos_[servos_[i].first + (servos_[i].second ? 0 : 4)] ? start_angle_ : end_angle_;
-                msg.items[i].end_angle = action_executor_->servo_pos_[servos_[i].first + (servos_[i].second ? 0 : 4)] ? 3 : 1;
+                msg.items[i].end_angle = action_executor_->servo_pos_[servos_[i].first + (servos_[i].second ? 0 : 4)] ? end_angle_ : start_angle_;
-                msg.items[i].duration_s = 0.5;
+                msg.items[i].duration_s = duration_s_;
             }
 
             action_executor_->MoveServoTimed(msg);
@@ -91,6 +93,18 @@ void Modelec::ToggleServoAction::AddServos(const std::vector<std::pair<int, Side
     servos_.insert(servos_.end(), servos.begin(), servos.end());
 }
 
+void Modelec::ToggleServoAction::InitConfig()
+{
+    if (isConfigInit_) return;
+    isConfigInit_ = true;
+
+    first_servo_ = Config::get<int>("action.toggle.id@first", 0);
+    second_servo_ = Config::get<int>("action.toggle.id@second", 0);
+    start_angle_ = Config::get<double>("action.toggle.msg@start_angle", 3);
+    end_angle_ = Config::get<double>("action.toggle.msg@end_angle", 1);
+    duration_s_ = Config::get<double>("action.toggle.msg@duration_s", 0.5);
+}
+
 void Modelec::ToggleServoAction::End()
 {
     for (auto servo : servos_)

src/modelec_strat/src/action/up_action.cpp

@@ -6,6 +6,8 @@ Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_execut
 {
     steps_.push(ActionExec::UP_STEP);
     steps_.push(ActionExec::DONE_STEP);
+
+    InitConfig();
 }
 
 Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side) : UPAction(action_executor)
@@ -29,53 +31,27 @@ void Modelec::UPAction::Next()
     case ActionExec::UP_STEP:
         {
             ActionServoTimedArray msg;
-            msg.items.resize(side_ == BOTH ? 8 : 4);
 
             if (side_ == FRONT || side_ == BOTH)
             {
-                msg.items[0].id = 0;
+                if (action_executor_->arm_pos_[FRONT].rotated)
-                msg.items[0].start_angle = 2.93;
+                {
-                msg.items[0].end_angle = 1.76;
+                    msg.items.insert(msg.items.end(), front_rotated_msg_.begin(), front_rotated_msg_.end());
-                msg.items[0].duration_s = 2;
+                } else
-
+                {
-                msg.items[1].id = 1;
+                    msg.items.insert(msg.items.end(), front_unrotated_msg_.begin(), front_unrotated_msg_.end());
-                msg.items[1].start_angle = 0.91;
+                }
-                msg.items[1].end_angle = 2.06;
-                msg.items[1].duration_s = 2;
-
-                msg.items[2].id = 2;
-                msg.items[2].start_angle = action_executor_->arm_pos_[FRONT].rotated ? 0 : 3.2;
-                msg.items[2].end_angle = 0.5;
-                msg.items[2].duration_s = 2;
-
-                msg.items[3].id = 3;
-                msg.items[3].start_angle = action_executor_->arm_pos_[FRONT].rotated ? 3.1 : 0;
-                msg.items[3].end_angle = 2.6;
-                msg.items[3].duration_s = 2;
             }
 
-            if (side_ == BACK || side_ == BOTH) {
+            if (side_ == BACK || side_ == BOTH)
-                int i = side_ == BOTH ? 4 : 0;
+            {
-
+                if (action_executor_->arm_pos_[BACK].rotated)
-                msg.items[i].id = 8;
+                {
-                msg.items[i].start_angle = 0;
+                    msg.items.insert(msg.items.end(), back_rotated_msg_.begin(), back_rotated_msg_.end());
-                msg.items[i].end_angle = 0;
+                } else
-                msg.items[i].duration_s = 0.5;
+                {
-
+                    msg.items.insert(msg.items.end(), back_unrotated_msg_.begin(), back_unrotated_msg_.end());
-                msg.items[i+1].id = 9;
+                }
-                msg.items[i+1].start_angle = 0;
-                msg.items[i+1].end_angle = 0;
-                msg.items[i+1].duration_s = 0.5;
-
-                msg.items[i+2].id = 10;
-                msg.items[i+2].start_angle = 0;
-                msg.items[i+2].end_angle = 0;
-                msg.items[i+2].duration_s = 0.5;
-
-                msg.items[i+3].id = 11;
-                msg.items[i+3].start_angle = 0;
-                msg.items[i+3].end_angle = 0;
-                msg.items[i+3].duration_s = 0.5;
             }
 
             action_executor_->MoveServoTimed(msg);
@@ -104,6 +80,17 @@ void Modelec::UPAction::SetSide(Side side)
     side_ = side;
 }
 
+void Modelec::UPAction::InitConfig()
+{
+    if (isConfigInit_) return;
+    isConfigInit_ = true;
+
+    front_rotated_msg_ = Config::get<std::vector<ActionServoTimed>>("action.up.front.rotated.msg");
+    front_unrotated_msg_ = Config::get<std::vector<ActionServoTimed>>("action.up.front.unrotated.msg");
+    back_rotated_msg_ = Config::get<std::vector<ActionServoTimed>>("action.up.back.rotated.msg");
+    back_unrotated_msg_ = Config::get<std::vector<ActionServoTimed>>("action.up.back.unrotated.msg");
+}
+
 void Modelec::UPAction::End()
 {
     if (side_ == BOTH)

src/modelec_strat/src/action_executor.cpp

@@ -5,7 +5,6 @@
 #include "modelec_strat/action/free_action.hpp"
 #include "modelec_strat/action/take_action.hpp"
 #include "modelec_strat/action/toggle_servo_action.hpp"
-#include "modelec_strat/action/look_on_action.hpp"
 #include "modelec_strat/action/thermo_action.hpp"
 #include "modelec_utils/utils.hpp"
 
@@ -87,7 +86,6 @@ namespace Modelec
         joy_sub_ = node_->create_subscription<sensor_msgs::msg::Joy>(
             "/joy", 10, [this](const sensor_msgs::msg::Joy::SharedPtr msg)
             {
-                // use game controller to manually control all the action. make it carefully
                 if (msg->buttons.size() >= 15)
                 {
                     if (msg->buttons[0] == 1) // A button
@@ -132,13 +130,19 @@ namespace Modelec
                             RotateArm(BaseAction::FRONT, false, !arm_pos_[BaseAction::FRONT].rotated);
                         }
                     }
-                    else if (msg->buttons[14] == 1) // LT button
+                    else if (msg->buttons[13] == 1) // LT button
                     {
-                        ActivateThermo(BaseAction::Side::LEFT, !thermo_state_[BaseAction::Side::LEFT]);
+                        if (action_done_)
+                        {
+                            ActivateThermo(BaseAction::Side::LEFT, !thermo_state_[BaseAction::Side::LEFT]);
+                        }
                     }
-                    else if (msg->buttons[15] == 1) // LR button
+                    else if (msg->buttons[14] == 1) // LR button
                     {
-                        ActivateThermo(BaseAction::Side::RIGHT, !thermo_state_[BaseAction::Side::RIGHT]);
+                        if (action_done_)
+                        {
+                            ActivateThermo(BaseAction::Side::RIGHT, !thermo_state_[BaseAction::Side::RIGHT]);
+                        }
                     }
                 }
                 if (msg->axes.size() == 8)
@@ -223,10 +227,18 @@ namespace Modelec
 
                 RCLCPP_DEBUG(node_->get_logger(), "Color detection succeeded: %s", res[1].c_str());
 
-                if (box_obstacles_[cam_side_] != nullptr) {
+                auto color = split(res[1], ';');
-                    box_obstacles_[cam_side_]->ParseColor(res[1]);
+
+                if (box_obstacles_[BaseAction::FRONT] != nullptr) {
+                    box_obstacles_[BaseAction::FRONT]->ParseColor(std::vector(color.begin(), color.begin() + 4));
                 } else {
-                    RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box in the cam side");
+                    RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box obstacle found to update");
+                }
+
+                if (box_obstacles_[BaseAction::BACK] != nullptr) {
+                    box_obstacles_[BaseAction::BACK]->ParseColor(std::vector(color.begin() + 4, color.begin() + 8));
+                } else {
+                    RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box obstacle found to update");
                 }
             });
 
@@ -476,22 +488,6 @@ namespace Modelec
         }
     }
 
-    void ActionExecutor::LookOn(BaseAction::Side side, bool force)
-    {
-        if (cam_side_ != side || force)
-        {
-            auto action = std::make_shared<LookOnAction>(shared_from_this(), side);
-            action_.push(action);
-            if (action_done_)
-            {
-                step_running_ = 0;
-            }
-            action_done_ = false;
-
-            Update();
-        }
-    }
-
     void ActionExecutor::ActionFinished(const std::shared_ptr<BaseAction>& action)
     {
         RCLCPP_DEBUG(
@@ -539,27 +535,6 @@ namespace Modelec
 
     void ActionExecutor::AskColor() const
     {
-        /*if (!ask_color_client_->service_is_ready()) {
-            RCLCPP_ERROR(node_->get_logger(), "Color detection service not ready");
-            return;
-        }
-
-        auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
-
-        ask_color_client_->async_send_request(request,
-            [this](rclcpp::Client<std_srvs::srv::Trigger>::SharedFuture future) {
-                auto response = future.get();
-                if (response->success) {
-                    RCLCPP_INFO(node_->get_logger(), "Color detection succeeded: %s", response->message.c_str());
-                    if (box_obstacles_[cam_side_] != nullptr) {
-                        box_obstacles_[cam_side_]->ParseColor(response->message);
-                    } else {
-                        RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box in the cam side");
-                    }
-                } else {
-                    RCLCPP_ERROR(node_->get_logger(), "Color detection failed: %s", response->message.c_str());
-                }
-            });*/
         ask_pub_->publish(std_msgs::msg::Empty());
     }
 }

src/modelec_strat/src/deposite_zone.cpp

@@ -4,27 +4,24 @@
 
 namespace Modelec
 {
-    DepositeZone::DepositeZone(tinyxml2::XMLElement* obstacleElem)
+    DepositeZone::DepositeZone(int id)
     {
-        obstacleElem->QueryIntAttribute("id", &id_);
+        id_ = Config::get<int>("Map.DepositeZone[" + std::to_string(id) + "]@id", id);
+        w_ = Config::get<int>("Map.DepositeZone[" + std::to_string(id) + "].Pos@w");
+        h_ = Config::get<int>("Map.DepositeZone[" + std::to_string(id) + "].Pos@h");
+        position_ = Point(
+            Config::get<int>("Map.DepositeZone[" + std::to_string(id) + "].Pos@x"),
+            Config::get<int>("Map.DepositeZone[" + std::to_string(id) + "].Pos@y"),
+            Config::get<double>("Map.DepositeZone[" + std::to_string(id) + "].Pos@theta")
+            );
 
-        auto posElem = obstacleElem->FirstChildElement("Pos");
+        if (Config::count("Map.DepositeZone[" + std::to_string(id) + "].TakeAngle.Angle") > 0)
-        if (posElem)
         {
-            posElem->QueryIntAttribute("x", &position_.x);
+            take_angle_ = Config::getArray<double>("Map.DepositeZone[" + std::to_string(id) + "].TakeAngle.Angle");
-            posElem->QueryIntAttribute("y", &position_.y);
+        } else
-            posElem->QueryDoubleAttribute("theta", &position_.theta);
+        {
-            posElem->QueryIntAttribute("w", &w_);
+            auto take_angle = Config::get<double>("Map.DepositeZone[" + std::to_string(id) + "].TakeAngle.Angle", position_.theta);
-            posElem->QueryIntAttribute("h", &h_);
+            take_angle_ = {take_angle};
-
-            if (auto TakeAngleElem = obstacleElem->FirstChildElement("TakeAngle"))
-            {
-                for (auto takePos = TakeAngleElem->FirstChildElement("Angle"); takePos; takePos = takePos->NextSiblingElement("Angle"))
-                {
-                    double angle = takePos->DoubleText(0);
-                    take_angle_.push_back(angle);
-                }
-            }
         }
     }
 
@@ -76,9 +73,6 @@ namespace Modelec
     int DepositeZone::GetId() const
     { return id_; }
 
-    int DepositeZone::GetMaxPot() const
-    { return max_pot_; }
-
     int DepositeZone::GetWidth() const
     { return w_; }
 

src/modelec_strat/src/enemy_manager.cpp

@@ -7,29 +7,22 @@ namespace Modelec
 {
     EnemyManager::EnemyManager() : Node("enemy_manager")
     {
-        this->declare_parameter("enemy.detection.min_move_threshold_mm", 50.0);
+        if (!Config::load(ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"))
-        this->declare_parameter("enemy.detection.refresh_rate", 1.0);
+        {
-        this->declare_parameter("enemy.detection.max_stationary_time_s", 10.0);
+            RCLCPP_ERROR(get_logger(), "Failed to load config file");
-        this->declare_parameter("enemy.detection.margin_detection_table_mm", 50.0);
+        }
-        this->declare_parameter("enemy.detection.min_emergency_distance_mm", 500.0);
 
-        this->declare_parameter("map.size.map_width_mm", 3000.0);
+        min_move_threshold_mm_ = Config::get<double>("config.enemy.detection.min_move_threshold_mm");
-        this->declare_parameter("map.size.map_height_mm", 2000.0);
+        refresh_rate_s_ = Config::get<double>("config.enemy.detection.refresh_rate");
+        max_stationary_time_s_ = Config::get<double>("config.enemy.detection.max_stationary_time_s");
+        margin_detection_table_ = Config::get<double>("config.enemy.detection.margin_detection_table_mm");
+        min_emergency_distance_ = Config::get<double>("config.enemy.detection.min_emergency_distance_mm");
 
-        this->declare_parameter("robot.size.width_mm", 500.0);
+        map_width_ = Config::get<double>("config.map.size.map_width_mm");
-        this->declare_parameter("robot.size.length_mm", 500.0);
+        map_height_ = Config::get<double>("config.map.size.map_height_mm");
 
-        min_move_threshold_mm_ = this->get_parameter("enemy.detection.min_move_threshold_mm").as_double();
+        robot_width_ = Config::get<double>("config.robot.size.width_mm");
-        refresh_rate_s_ = this->get_parameter("enemy.detection.refresh_rate").as_double();
+        robot_length_ = Config::get<double>("config.robot.size.length_mm");
-        max_stationary_time_s_ = this->get_parameter("enemy.detection.max_stationary_time_s").as_double();
-        margin_detection_table_ = this->get_parameter("enemy.detection.margin_detection_table_mm").as_double();
-        min_emergency_distance_ = this->get_parameter("enemy.detection.min_emergency_distance_mm").as_double();
-
-        map_width_ = this->get_parameter("map.size.map_width_mm").as_double();
-        map_height_ = this->get_parameter("map.size.map_height_mm").as_double();
-
-        robot_width_ = this->get_parameter("robot.size.width_mm").as_double();
-        robot_length_ = this->get_parameter("robot.size.length_mm").as_double();
 
         current_pos_sub_ = this->create_subscription<modelec_interfaces::msg::OdometryPos>(
             "odometry/position", 10,

src/modelec_strat/src/missions/action_mission.cpp

@@ -0,0 +1,47 @@
+#include <modelec_strat/missions/action_mission.hpp>
+#include <modelec_strat/action_executor.hpp>
+#include <modelec_utils/config.hpp>
+
+namespace Modelec {
+
+    double ActionMission::TIMEOUT = 5.0;
+    bool ActionMission::IsInit = false;
+
+    ActionMission::ActionMission(const std::shared_ptr<ActionExecutor>& action_executor)
+        : action_executor_(action_executor)
+    {
+    }
+
+    void ActionMission::InitConfig()
+    {
+        TIMEOUT = Config::get<double>("config.mission.action.timeout.s", 5.0);
+        IsInit = true;
+    }
+
+    void ActionMission::Start(const rclcpp::Node::SharedPtr& node)
+    {
+        Mission::Start(node);
+
+        if (!IsInit)
+        {
+            InitConfig();
+        }
+
+        deploy_time_ = node->now();
+    }
+
+    bool ActionMission::Update()
+    {
+        if (!action_executor_->IsActionDone())
+        {
+            if ((node_->now() - deploy_time_).seconds() > TIMEOUT)
+            {
+                RCLCPP_WARN(node_->get_logger(), "ActionMission Update TIMEOUT");
+                return true;
+            }
+            return false;
+        }
+
+        return true;
+    }
+}

src/modelec_strat/src/missions/free_mission.cpp

@@ -4,19 +4,17 @@
 
 namespace Modelec {
     FreeMission::FreeMission(const std::shared_ptr<NavigationHelper>& nav,
-        const std::shared_ptr<ActionExecutor>& action_executor,
+        const std::shared_ptr<ActionExecutor>& action_executor, BaseAction::Side side) :
-        BaseAction::Side side)
+        Mission(MissionStatus::READY), ActionMission(action_executor), MoveMission(nav),
-     : side_(side), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
+        side_(side)
     {
     }
 
-    void FreeMission::Start(rclcpp::Node::SharedPtr node)
+    void FreeMission::Start(const rclcpp::Node::SharedPtr& node)
     {
-        node_ = node;
+        ActionMission::Start(node);
-
+        MoveMission::Start(node);
-        go_timeout_ = node_->now();
+        MinTimeMission::Start(node);
-        deploy_time_ = node_->now();
-        last_ask_waypoint_time_ = node_->now();
 
         status_ = MissionStatus::RUNNING;
 
@@ -34,40 +32,13 @@ namespace Modelec {
         steps_.push(DONE);
     }
 
-    void FreeMission::Update()
+    bool FreeMission::Update()
     {
-        if (!action_executor_->IsActionDone())
+        if (!ActionMission::Update() || !MoveMission::Update() || !MinTimeMission::Update())
         {
-            return;
+            return false;
         }
 
-        if (!nav_->HasArrived())
-        {
-            if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1)
-            {
-                nav_->AskWaypoint();
-                last_ask_waypoint_time_ = node_->now();
-                return;
-            }
-            if ((node_->now() - go_timeout_).seconds() < 10)
-            {
-                return;
-            }
-        }
-
-        if (min_time_.has_value())
-        {
-            if ((node_->now() - min_time_.value()).seconds() < 0.1)
-            {
-                return;
-            }
-            else
-            {
-                min_time_.reset();
-            }
-        }
-
-
 		auto step_ = steps_.front();
 		steps_.pop();
 
@@ -82,7 +53,7 @@ namespace Modelec {
                 if (target_deposite_zone_ == nullptr)
                 {
                     status_ = MissionStatus::FAILED;
-                    return;
+                    return false;
                 }
 
                 auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta));
@@ -96,12 +67,10 @@ namespace Modelec {
                     if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE)
                     {
                         status_ = MissionStatus::FAILED;
-                        return;
+                        return false;
                     }
                 }
 
-                action_executor_->LookOn(BaseAction::Side::CENTER);
-
                 go_timeout_ = node_->now();
             }
             break;
@@ -195,7 +164,7 @@ namespace Modelec {
                 if (nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE) != Pathfinding::FREE)
                 {
                     status_ = MissionStatus::FAILED;
-                    return;
+                    return false;
                 }
 
                 go_timeout_ = node_->now();
@@ -210,8 +179,8 @@ namespace Modelec {
                 auto pos = nav_->GetCurrentPos();
 
                 obs->SetPosition(
-                    pos->x + 300 * cos(pos->theta + (side_ == BaseAction::FRONT ? 0 : M_PI)),
+                    target_deposite_zone_->GetPosition().x,
-                    pos->y + 300 * sin(pos->theta + (side_ == BaseAction::FRONT ? 0 : M_PI)),
+                    target_deposite_zone_->GetPosition().y,
                     pos->theta);
 
                 obs->SetAtObjective(true);
@@ -226,15 +195,12 @@ namespace Modelec {
         default:
             break;
         }
+
+        return true;
     }
 
     void FreeMission::Clear()
     {
     }
 
-    MissionStatus FreeMission::GetStatus() const
-    {
-        return status_;
-    }
-
 }

src/modelec_strat/src/missions/go_home_mission.cpp

@@ -5,23 +5,19 @@
 namespace Modelec
 {
     GoHomeMission::GoHomeMission(const std::shared_ptr<NavigationHelper>& nav, const rclcpp::Time& start_time) :
-        status_(MissionStatus::READY), nav_(nav), start_time_(start_time)
+        Mission(MissionStatus::READY), MoveMission(nav),
+        start_time_(start_time)
     {
     }
 
-    void GoHomeMission::Start(rclcpp::Node::SharedPtr node)
+    void GoHomeMission::Start(const rclcpp::Node::SharedPtr& node)
     {
-        node_ = node;
+        MoveMission::Start(node);
 
-        node->declare_parameter("mission_score.go_home", 0);
+        mission_score_ = Config::get<int>("config.mission_score.go_home", 0);
-
-        mission_score_ = node->get_parameter("mission_score.go_home").as_int();
 
         score_pub_ = node_->create_publisher<std_msgs::msg::Int64>("/strat/score", 10);
 
-        go_timeout_ = node_->now();
-        last_ask_waypoint_time_ = node_->now();
-
         status_ = MissionStatus::RUNNING;
 
         std::queue<int> empty;
@@ -33,32 +29,11 @@ namespace Modelec
         steps_.push(DONE);
     }
 
-    void GoHomeMission::Update()
+    bool GoHomeMission::Update()
     {
-        if (!nav_->HasArrived())
+        if (!MoveMission::Update())
         {
-            if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1)
+            return false;
-            {
-                nav_->AskWaypoint();
-                last_ask_waypoint_time_ = node_->now();
-                return;
-            }
-            if ((node_->now() - go_timeout_).seconds() < 10)
-            {
-                return;
-            }
-        }
-
-        if (min_time_.has_value())
-        {
-            if ((node_->now() - min_time_.value()).seconds() < 0.1)
-            {
-                return;
-            }
-            else
-            {
-                min_time_.reset();
-            }
         }
 
         auto step_ = steps_.front();
@@ -75,7 +50,7 @@ namespace Modelec
                     if (nav_->CanGoTo(home_point_.GetTakeBasePosition(), true) != Pathfinding::FREE)
                     {
                         status_ = MissionStatus::FAILED;
-                        return;
+                        return false;
                     }
                 }
                 nav_->RotateTo(home_point_);
@@ -90,7 +65,7 @@ namespace Modelec
                     if (nav_->GoTo(home_point_.GetTakeBasePosition(), true) != Pathfinding::FREE)
                     {
                         status_ = MissionStatus::FAILED;
-                        return;
+                        return false;
                     }
                 }
 
@@ -99,11 +74,6 @@ namespace Modelec
             break;
         case GO_CLOSE:
             {
-                /*if ((node_->now() - start_time_).seconds() < 94)
-                {
-                    break;
-                }*/
-
                 nav_->GoTo(home_point_, true);
 
                 go_timeout_ = node_->now();
@@ -121,14 +91,12 @@ namespace Modelec
         default:
             break;
         }
+
+        return true;
     }
 
     void GoHomeMission::Clear()
     {
     }
 
-    MissionStatus GoHomeMission::GetStatus() const
-    {
-        return status_;
-    }
 }

src/modelec_strat/src/missions/mine_time_mission.cpp

@@ -0,0 +1,44 @@
+#include <modelec_strat/missions/min_time_mission.hpp>
+#include <modelec_utils/config.hpp>
+
+namespace Modelec {
+
+    double MinTimeMission::MIN_TIME_DURATION = 0.1;
+    bool MinTimeMission::IsInit = false;
+
+    MinTimeMission::MinTimeMission() : min_time_(std::nullopt)
+    {
+    }
+
+    void MinTimeMission::InitConfig()
+    {
+        MIN_TIME_DURATION = Config::get<double>("config.mission.min_time.min_time_duration.s", 0.1);
+        IsInit = true;
+    }
+
+    void MinTimeMission::Start(const rclcpp::Node::SharedPtr& node)
+    {
+        Mission::Start(node);
+
+        if (!IsInit)
+        {
+            InitConfig();
+        }
+    }
+
+    bool MinTimeMission::Update()
+    {
+        if (min_time_.has_value())
+        {
+            if ((node_->now() - min_time_.value()).seconds() < MIN_TIME_DURATION)
+            {
+                return false;
+            }
+            else
+            {
+                min_time_.reset();
+            }
+        }
+        return true;
+    }
+}

src/modelec_strat/src/missions/mission_base.cpp

@@ -0,0 +1,19 @@
+#include <modelec_strat/missions/mission_base.hpp>
+
+namespace Modelec
+{
+    Mission::Mission(MissionStatus status) 
+        : status_(status) 
+    {
+    }
+
+    void Mission::Start(const rclcpp::Node::SharedPtr& node)
+    {
+        node_ = node;
+    }
+
+    MissionStatus Mission::GetStatus() const
+    {
+        return status_;
+    }
+}

src/modelec_strat/src/missions/move_mission.cpp

@@ -0,0 +1,65 @@
+#include <modelec_strat/missions/move_mission.hpp>
+#include <modelec_strat/navigation_helper.hpp>
+#include <modelec_utils/config.hpp>
+
+namespace Modelec {
+
+    double MoveMission::MIN_ASK_WAYPOINT = 3.0;
+    double MoveMission::ASK_WAYPOINT_INTERVAL = 2.0;
+    double MoveMission::GO_TIMEOUT = 10.0;
+    bool MoveMission::IsInit = false;
+
+    MoveMission::MoveMission(const std::shared_ptr<NavigationHelper>& nav)
+        : nav_(nav)
+    {
+    }
+
+    void MoveMission::InitConfig()
+    {
+        MIN_ASK_WAYPOINT = Config::get<double>("config.mission.move.min_ask_waypoint.s", 3.0);
+        ASK_WAYPOINT_INTERVAL = Config::get<double>("config.mission.move.ask_waypoint_interval.s", 2.0);
+        GO_TIMEOUT = Config::get<double>("config.mission.move.go_timeout.s", 10.0);
+
+        IsInit = true;
+    }
+
+    void MoveMission::Start(const rclcpp::Node::SharedPtr& node)
+    {
+        Mission::Start(node);
+
+        if (!IsInit)
+        {
+            InitConfig();
+        }
+
+        go_timeout_ = node->now();
+        last_ask_waypoint_time_ = node->now();
+    }
+
+    bool MoveMission::Update()
+    {
+        if (!nav_->HasArrived())
+        {
+            auto now = node_->now();
+            double elapsed_total = (now - go_timeout_).seconds();
+            double elapsed_since_last_ask = (now - last_ask_waypoint_time_).seconds();
+
+            if (elapsed_total > MIN_ASK_WAYPOINT && elapsed_since_last_ask > ASK_WAYPOINT_INTERVAL)
+            {
+                nav_->AskWaypoint();
+                last_ask_waypoint_time_ = now;
+                return false;
+            }
+
+            if (elapsed_total > GO_TIMEOUT)
+            {
+                RCLCPP_WARN(node_->get_logger(), "Go Timeout");
+                return true;
+            }
+
+            return false;
+        }
+
+        return true;
+    }
+}

src/modelec_strat/src/missions/take_mission.cpp

@@ -4,19 +4,17 @@
 
 namespace Modelec {
     TakeMission::TakeMission(const std::shared_ptr<NavigationHelper>& nav,
-        const std::shared_ptr<ActionExecutor>& action_executor,
+        const std::shared_ptr<ActionExecutor>& action_executor, BaseAction::Side side) :
-        BaseAction::Side side)
+    Mission(MissionStatus::READY), ActionMission(action_executor), MoveMission(nav),
-     : side_(side), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
+    side_(side)
     {
     }
 
-    void TakeMission::Start(rclcpp::Node::SharedPtr node)
+    void TakeMission::Start(const rclcpp::Node::SharedPtr& node)
     {
-        node_ = node;
+        ActionMission::Start(node);
-
+        MoveMission::Start(node);
-        go_timeout_ = node_->now();
+        MinTimeMission::Start(node);
-        deploy_time_ = node_->now();
-        last_ask_waypoint_time_ = node_->now();
 
         status_ = MissionStatus::RUNNING;
 
@@ -31,37 +29,11 @@ namespace Modelec {
         steps_.push(DONE);
     }
 
-    void TakeMission::Update()
+    bool TakeMission::Update()
     {
-        if (!action_executor_->IsActionDone())
+        if (!ActionMission::Update() || !MoveMission::Update() || !MinTimeMission::Update())
         {
-            return;
+            return false;
-        }
-
-        if (!nav_->HasArrived())
-        {
-            if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1)
-            {
-                nav_->AskWaypoint();
-                last_ask_waypoint_time_ = node_->now();
-                return;
-            }
-            if ((node_->now() - go_timeout_).seconds() < 10)
-            {
-                return;
-            }
-        }
-
-        if (min_time_.has_value())
-        {
-            if ((node_->now() - min_time_.value()).seconds() < 0.1)
-            {
-                return;
-            }
-            else
-            {
-                min_time_.reset();
-            }
         }
 
         auto step_ = steps_.front();
@@ -85,6 +57,8 @@ namespace Modelec {
                 auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
                 pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI));
 
+                // TODO the robot do not go to the exact position i would like it to go to the exact so he can go forward after to take the box in a good way
+
                 if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE)
                 {
                     if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE)
@@ -92,7 +66,7 @@ namespace Modelec {
                         if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE)
                         {
                             status_ = MissionStatus::FAILED;
-                            break;
+                            return false;
                         }
                     }
                 }
@@ -105,7 +79,7 @@ namespace Modelec {
                 if (action_executor_->box_obstacles_[side_] == nullptr)
                 {
                     status_ = MissionStatus::FAILED;
-                    break;
+                    return false;
                 }
 
                 auto pos = action_executor_->box_obstacles_[side_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition();
@@ -114,7 +88,7 @@ namespace Modelec {
                 if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE)
                 {
                     status_ = MissionStatus::FAILED;
-                    break;
+                    return false;
                 }
 
                 go_timeout_ = node_->now();
@@ -137,7 +111,6 @@ namespace Modelec {
         case UP:
             {
                 action_executor_->Up(side_);
-                action_executor_->LookOn(side_);
                 deploy_time_ = node_->now();
             }
             break;
@@ -146,7 +119,7 @@ namespace Modelec {
                 if (action_executor_->box_obstacles_[side_] == nullptr)
                 {
                     status_ = MissionStatus::FAILED;
-                    break;
+                    return false;
                 }
 
                 nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[side_]->GetId());
@@ -159,15 +132,12 @@ namespace Modelec {
         default:
             break;
         }
+
+        return true;
     }
 
     void TakeMission::Clear()
     {
     }
 
-    MissionStatus TakeMission::GetStatus() const
-    {
-        return status_;
-    }
-
 }

src/modelec_strat/src/missions/thermo_mission.cpp

@@ -7,18 +7,15 @@ namespace Modelec {
     bool ThermoMission::IsThermoDone = false;
 
     ThermoMission::ThermoMission(const std::shared_ptr<NavigationHelper>& nav,
-        const std::shared_ptr<ActionExecutor>& action_executor)
+        const std::shared_ptr<ActionExecutor>& action_executor) :
-     : status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
+        Mission(MissionStatus::READY), ActionMission(action_executor), MoveMission(nav)
     {
     }
 
-    void ThermoMission::Start(rclcpp::Node::SharedPtr node)
+    void ThermoMission::Start(const rclcpp::Node::SharedPtr& node)
     {
-        node_ = node;
+        ActionMission::Start(node);
-
+        MoveMission::Start(node);
-        go_timeout_ = node_->now();
-        deploy_time_ = node_->now();
-        last_ask_waypoint_time_ = node_->now();
 
         status_ = MissionStatus::RUNNING;
 
@@ -35,37 +32,11 @@ namespace Modelec {
         steps_.push(DONE);
     }
 
-    void ThermoMission::Update()
+    bool ThermoMission::Update()
     {
-        if (!action_executor_->IsActionDone())
+        if (!ActionMission::Update() || !MoveMission::Update())
         {
-            return;
+            return false;
-        }
-
-        if (!nav_->HasArrived())
-        {
-            if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1)
-            {
-                nav_->AskWaypoint();
-                last_ask_waypoint_time_ = node_->now();
-                return;
-            }
-            if ((node_->now() - go_timeout_).seconds() < 10)
-            {
-                return;
-            }
-        }
-
-        if (min_time_.has_value())
-        {
-            if ((node_->now() - min_time_.value()).seconds() < 0.1)
-            {
-                return;
-            }
-            else
-            {
-                min_time_.reset();
-            }
         }
 
         auto step_ = steps_.front();
@@ -80,7 +51,7 @@ namespace Modelec {
                 if (nav_->GoToRotateFirst(start, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE)
                 {
                     status_ = MissionStatus::FAILED;
-                    break;
+                    return false;
                 }
 
                 go_timeout_ = node_->now();
@@ -93,7 +64,7 @@ namespace Modelec {
                 if (nav_->GoToRotateFirst(start, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE)
                 {
                     status_ = MissionStatus::FAILED;
-                    break;
+                    return false;
                 }
 
                 go_timeout_ = node_->now();
@@ -101,7 +72,6 @@ namespace Modelec {
             break;
         case ACTIVATE_THERMO:
             {
-                RCLCPP_INFO(node_->get_logger(), "Activating thermo");
                 action_executor_->ActivateThermo(nav_->GetTeamId() == NavigationHelper::YELLOW ? BaseAction::RIGHT : BaseAction::LEFT, true);
                 deploy_time_ = node_->now();
             }
@@ -113,13 +83,12 @@ namespace Modelec {
                 if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE)
                 {
                     status_ = MissionStatus::FAILED;
-                    break;
+                    return false;
                 }
             }
             break;
         case DEACTIVATE_THERMO:
             {
-                RCLCPP_INFO(node_->get_logger(), "Deactivating thermo");
                 action_executor_->ActivateThermo(nav_->GetTeamId() == NavigationHelper::YELLOW ? BaseAction::RIGHT : BaseAction::LEFT, false);
                 deploy_time_ = node_->now();
             }
@@ -135,15 +104,12 @@ namespace Modelec {
         default:
             break;
         }
+
+        return true;
     }
 
     void ThermoMission::Clear()
     {
     }
 
-    MissionStatus ThermoMission::GetStatus() const
-    {
-        return status_;
-    }
-
 }

src/modelec_strat/src/navigation_helper.cpp

@@ -1,6 +1,7 @@
 #include <modelec_strat/navigation_helper.hpp>
 #include <utility>
 #include <ament_index_cpp/get_package_share_directory.hpp>
+#include <modelec_utils/config.hpp>
 
 namespace Modelec
 {
@@ -9,41 +10,9 @@ namespace Modelec
 
     NavigationHelper::NavigationHelper(const rclcpp::Node::SharedPtr& node) : node_(node)
     {
-        node_->declare_parameter("factor_close_enemy", -0.5);
+        factor_close_enemy_ = Config::get<float>("config.factor.close_enemy", 1.0f);
-        node_->declare_parameter("factor.theta", 20.0);
+        factor_theta_ = Config::get<float>("config.factor.theta", 1.0f);
-        node_->declare_parameter("enemy.detection.min_emergency_distance_mm", 390);
+        enemy_emergency_distance_ = Config::get<int>("config.enemy.detection.min_emergency_distance_mm", 300);
-
-        factor_close_enemy_ = node_->get_parameter("factor_close_enemy").as_double();
-        factor_theta_ = node_->get_parameter("factor.theta").as_double();
-        enemy_emergency_distance_ = node_->get_parameter("enemy.detection.min_emergency_distance_mm").as_int();
-
-        node_->declare_parameter("home.yellow.x", 0);
-        node_->declare_parameter("home.yellow.y", 0);
-        node_->declare_parameter("home.yellow.theta", 0.0);
-        node_->declare_parameter("home.blue.x", 0);
-        node_->declare_parameter("home.blue.y", 0);
-        node_->declare_parameter("home.blue.theta", 0.0);
-
-        node_->declare_parameter("thermo.yellow.start.x", 0);
-        node_->declare_parameter("thermo.yellow.start.y", 0);
-        node_->declare_parameter("thermo.yellow.start.theta", 0.0);
-        node_->declare_parameter("thermo.yellow.finish.x", 0);
-        node_->declare_parameter("thermo.yellow.finish.y", 0);
-        node_->declare_parameter("thermo.yellow.finish.theta", 0.0);
-
-        node_->declare_parameter("thermo.blue.start.x", 0);
-        node_->declare_parameter("thermo.blue.start.y", 0);
-        node_->declare_parameter("thermo.blue.start.theta", 0.0);
-        node_->declare_parameter("thermo.blue.finish.x", 0);
-        node_->declare_parameter("thermo.blue.finish.y", 0);
-        node_->declare_parameter("thermo.blue.finish.theta", 0.0);
-
-        node_->declare_parameter("spawn.yellow_top.x", 0);
-        node_->declare_parameter("spawn.yellow_top.y", 0);
-        node_->declare_parameter("spawn.yellow_top.theta", 0.0);
-        node_->declare_parameter("spawn.blue_top.x", 0);
-        node_->declare_parameter("spawn.blue_top.y", 0);
-        node_->declare_parameter("spawn.blue_top.theta", 0.0);
 
         pathfinding_ = std::make_shared<Pathfinding>(node);
 
@@ -90,13 +59,6 @@ namespace Modelec
 
         start_odo_pub_ = node_->create_publisher<std_msgs::msg::Bool>("odometry/start", 10);
 
-        std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") +
-            "/data/deposite_zone.xml";
-        if (!LoadDepositeZoneFromXML(deposite_zone_path))
-        {
-            RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML");
-        }
-
         spawn_pub_ = node_->create_publisher<modelec_interfaces::msg::Spawn>("nav/spawn", 10);
 
         ask_spawn_srv_ = node->create_service<std_srvs::srv::Empty>(
@@ -130,6 +92,8 @@ namespace Modelec
 
         odo_ask_waypoint_pub_ = node_->create_publisher<std_msgs::msg::Empty>(
             "odometry/ask_active_waypoint", 30);
+
+        LoadDepositeZoneFromXML();
     }
 
     void NavigationHelper::ReInit()
@@ -333,7 +297,7 @@ namespace Modelec
     {
         double angle = std::atan2(pos.y - current_pos_->y, pos.x - current_pos_->x);
 
-        if (Point::angleDiff(angle, (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4)
+        if (std::abs(Point::angleDiff(angle, (current_pos_->theta + (front ? 0 : M_PI)))) > M_PI / 4)
         {
             Rotate(angle);
             return true;
@@ -505,32 +469,14 @@ namespace Modelec
         return current_pos_;
     }
 
-    bool NavigationHelper::LoadDepositeZoneFromXML(const std::string& filename)
+    void NavigationHelper::LoadDepositeZoneFromXML()
     {
-        tinyxml2::XMLDocument doc;
+        for (size_t i = 0; i < Config::count("Map.DepositeZone"); ++i)
-        if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS)
         {
-            RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacle XML file: %s", filename.c_str());
+            deposite_zones_.push_back(std::make_shared<DepositeZone>(i));
-            return false;
         }
 
-        tinyxml2::XMLElement* root = doc.FirstChildElement("Map");
+        RCLCPP_INFO(node_->get_logger(), "Loaded %zu deposite zones from XML", deposite_zones_.size());
-        if (!root)
-        {
-            RCLCPP_ERROR(node_->get_logger(), "No <Obstacles> root element in file");
-            return false;
-        }
-
-        for (tinyxml2::XMLElement* elem = root->FirstChildElement("DepositeZone");
-             elem != nullptr;
-             elem = elem->NextSiblingElement("DepositeZone"))
-        {
-            std::shared_ptr<DepositeZone> obs = std::make_shared<DepositeZone>(elem);
-            deposite_zones_[obs->GetId()] = obs;
-        }
-
-        RCLCPP_INFO(node_->get_logger(), "Loaded %zu zone from XML", deposite_zones_.size());
-        return true;
     }
 
     std::shared_ptr<DepositeZone> NavigationHelper::GetClosestDepositeZone(
@@ -541,10 +487,10 @@ namespace Modelec
         auto posPoint = Point(pos->x, pos->y, pos->theta);
         auto enemyPos = Point(last_enemy_pos_.x, last_enemy_pos_.y, last_enemy_pos_.theta);
 
-        for (const auto& [id, zone] : deposite_zones_)
+        for (const auto& zone : deposite_zones_)
         {
             if (blacklistedId.end() == std::find(
-                blacklistedId.begin(), blacklistedId.end(), id)
+                blacklistedId.begin(), blacklistedId.end(), zone->GetId())
                 && (!only_free || !zone->Validate()))
             {
                 auto zonePoint = zone->GetPosition();
@@ -567,29 +513,29 @@ namespace Modelec
     {
         PosMsg::SharedPtr home = std::make_shared<PosMsg>();
 
-        std::string prefix = (team_id_ == YELLOW) ? "home.yellow" : "home.blue";
+        std::string prefix = (team_id_ == YELLOW) ? "config.home.yellow" : "config.home.blue";
 
-        home->x = node_->get_parameter(prefix + ".x").as_int();
+        home->x = Config::get<int>(prefix + "@x");
-        home->y = node_->get_parameter(prefix + ".y").as_int();
+        home->y = Config::get<int>(prefix + "@y");
-        home->theta = node_->get_parameter(prefix + ".theta").as_double();
+        home->theta = Config::get<double>(prefix + "@theta");
 
         return home;
     }
 
     std::array<Point, 2> NavigationHelper::GetThermoPositions()
     {
-        std::string prefix = (team_id_ == YELLOW) ? "thermo.yellow" : "thermo.blue";
+        std::string prefix = (team_id_ == YELLOW) ? "config.thermo.yellow" : "config.thermo.blue";
 
         Point start(
-            node_->get_parameter(prefix + ".start.x").as_int(),
+            Config::get<int>(prefix + ".start@x"),
-            node_->get_parameter(prefix + ".start.y").as_int(),
+            Config::get<int>(prefix + ".start@y"),
-            node_->get_parameter(prefix + ".start.theta").as_double()
+            Config::get<double>(prefix + ".start@theta")
         );
 
         Point finish(
-            node_->get_parameter(prefix + ".finish.x").as_int(),
+            Config::get<int>(prefix + ".finish@x"),
-            node_->get_parameter(prefix + ".finish.y").as_int(),
+            Config::get<int>(prefix + ".finish@y"),
-            node_->get_parameter(prefix + ".finish.theta").as_double()
+            Config::get<double>(prefix + ".finish@theta")
         );
 
         return {start, finish};
@@ -653,13 +599,13 @@ namespace Modelec
         pathfinding_->OnEnemyPositionLongTime(msg);
 
         Point enemy_pos(msg->x, msg->y, msg->theta);
-        for (auto& [id, zone] : deposite_zones_)
+        for (auto& zone : deposite_zones_)
         {
             auto zonePos = zone->GetPosition();
             if (Point::distance(enemy_pos, zonePos) < (zone->GetWidth() / 2) + pathfinding_->enemy_margin_mm_)
             {
                 std::shared_ptr<Obstacle> obs = std::make_shared<Obstacle>(
-                    id, zonePos.x, zonePos.y, zonePos.theta, zone->GetWidth(), zone->GetHeight(), "enemy-zone");
+                    zone->GetId(), zonePos.x, zonePos.y, zonePos.theta, zone->GetWidth(), zone->GetHeight(), "enemy-zone");
                 pathfinding_->AddObstacle(obs);
             }
         }
@@ -804,15 +750,15 @@ namespace Modelec
     void NavigationHelper::SetupSpawn()
     {
         spawn_yellow_["top"] = Point(
-            node_->get_parameter("spawn.yellow_top.x").as_int(),
+            Config::get<int>("config.spawn.yellow.top@x"),
-            node_->get_parameter("spawn.yellow_top.y").as_int(),
+            Config::get<int>("config.spawn.yellow.top@y"),
-            node_->get_parameter("spawn.yellow_top.theta").as_double()
+            Config::get<double>("config.spawn.yellow.top@theta")
         );
 
         spawn_blue_["top"] = Point(
-            node_->get_parameter("spawn.blue_top.x").as_int(),
+            Config::get<int>("config.spawn.blue.top@x"),
-            node_->get_parameter("spawn.blue_top.y").as_int(),
+            Config::get<int>("config.spawn.blue.top@y"),
-            node_->get_parameter("spawn.blue_top.theta").as_double()
+            Config::get<double>("config.spawn.blue.top@theta")
         );
     }
 }

src/modelec_strat/src/obstacle/box.cpp

@@ -9,15 +9,17 @@ namespace Modelec
     {
     }
 
-    BoxObstacle::BoxObstacle(tinyxml2::XMLElement* obstacleElem)
+    BoxObstacle::BoxObstacle(int id) : Obstacle(id)
-        : Obstacle(obstacleElem)
     {
-        possible_angles_.push_back(theta_);
+        if (Config::count("Obstacles.Obstacle[" + std::to_string(id) + "].possible-angle") > 0)
-        for (auto elem = obstacleElem->FirstChildElement("possible-angle");
-             elem;
-             elem = elem->NextSiblingElement("possible-angle"))
         {
-            possible_angles_.push_back(elem->DoubleAttribute("theta"));
+            possible_angles_ = Config::getArray<double>("Obstacles.Obstacle[" + std::to_string(id) + "].possible-angle", [](const std::string& base)
+            {
+                return Config::get<double>(base + "@theta");
+            });
+            possible_angles_.push_back(theta_);
+        } else {
+            possible_angles_ = {theta_};
         }
     }
 

src/modelec_strat/src/obstacle/obstacle.cpp

@@ -1,5 +1,7 @@
 #include <modelec_strat/obstacle/obstacle.hpp>
 
+#include "modelec_utils/config.hpp"
+
 namespace Modelec
 {
     Obstacle::Obstacle(int id, int x, int y, double theta, int w, int h, const std::string& type)
@@ -7,28 +9,22 @@ namespace Modelec
     {
     }
 
-    Obstacle::Obstacle(tinyxml2::XMLElement* obstacleElem)
-    {
-        const char* type = nullptr;
-        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)
-        {
-            RCLCPP_WARN(rclcpp::get_logger("Obstacle"), "Failed to parse obstacle element");
-            return;
-        }
-        type_ = type;
-    }
-
     Obstacle::Obstacle(const modelec_interfaces::msg::Obstacle& msg)
         : id_(msg.id), x_(msg.x), y_(msg.y), w_(msg.width), h_(msg.height), theta_(msg.theta), type_("unknown")
     {
     }
 
+    Obstacle::Obstacle(int id)
+    {
+        id_ = Config::get<int>("Obstacles.Obstacle[" + std::to_string(id) + "]@id", id);
+        x_ = Config::get<int>("Obstacles.Obstacle[" + std::to_string(id) + "]@x");
+        y_ = Config::get<int>("Obstacles.Obstacle[" + std::to_string(id) + "]@y");
+        theta_ = Config::get<double>("Obstacles.Obstacle[" + std::to_string(id) + "]@theta");
+        w_ = Config::get<int>("Obstacles.Obstacle[" + std::to_string(id) + "]@width");
+        h_ = Config::get<int>("Obstacles.Obstacle[" + std::to_string(id) + "]@height");
+        type_ = Config::get<std::string>("Obstacles.Obstacle[" + std::to_string(id) + "]@type", "box");
+    }
+
     modelec_interfaces::msg::Obstacle Obstacle::toMsg() const
     {
         modelec_interfaces::msg::Obstacle msg;
@@ -62,7 +58,7 @@ namespace Modelec
     int Obstacle::GetHeight() const
     { return h_; }
 
-    const std::string& Obstacle::Type() const
+    const std::string& Obstacle::GetType() const
     { return type_; }
 
     Point Obstacle::GetPosition() const
@@ -118,4 +114,14 @@ namespace Modelec
     {
         isAtObjective = atObjective;
     }
+
+    bool Obstacle::ShouldTakeCountInPathfinding() const
+    {
+        return takeCountInPathfinding;
+    }
+
+    void Obstacle::SetTakeCountInPathfinding(bool takeCount)
+    {
+        takeCountInPathfinding = takeCount;
+    }
 }

src/modelec_strat/src/pami_manager.cpp

@@ -5,92 +5,6 @@ namespace Modelec
 {
     PamiManger::PamiManger() : Node("pami_manager")
     {
-        this->declare_parameter("time_to_put_zone", 80);
-        this->declare_parameter("time_to_remove_top_pot", 70);
-
-        time_to_put_zone_ = this->get_parameter("time_to_put_zone").as_int();
-        time_to_remove_top_pot_ = this->get_parameter("time_to_remove_top_pot").as_int();
-
-        score_to_add_ = 0;
-
-        std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") +
-            "/data/pami_zone.xml";
-        if (!ReadFromXML(obstacles_path))
-        {
-            RCLCPP_ERROR(get_logger(), "Failed to load obstacles from XML");
-        }
-
-        start_time_sub_ = create_subscription<std_msgs::msg::Int64>(
-            "/strat/start_time", 10, [this](const std_msgs::msg::Int64::SharedPtr)
-            {
-                /*auto now = std::chrono::system_clock::now().time_since_epoch();
-                auto goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_remove_top_pot_);
-                auto second_goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_put_zone_);
-
-                timer_add_ = create_wall_timer(
-                    second_goal - now,
-                    [this]()
-                    {
-                        for (const auto& obs : zones_)
-                        {
-                            add_obs_pub_->publish(obs.toMsg());
-
-                            std_msgs::msg::Int64 msg;
-                            msg.data = score_to_add_;
-                            score_pub_->publish(msg);
-                        }
-
-                        timer_add_->cancel();
-                    });*/
-            });
-
-        add_obs_pub_ = create_publisher<modelec_interfaces::msg::Obstacle>(
-            "obstacle/add", 10);
-
-        remove_obs_pub_ = create_publisher<modelec_interfaces::msg::Obstacle>(
-            "obstacle/remove", 10);
-
-        score_pub_ = create_publisher<std_msgs::msg::Int64>("/strat/score", 10);
-
-        reset_strat_sub_ = create_subscription<std_msgs::msg::Empty>(
-            "/strat/reset", 10, [this](const std_msgs::msg::Empty::SharedPtr)
-            {
-                /*if (timer_add_)
-                {
-                    timer_add_->cancel();
-                }*/
-                /*if (timer_remove_)
-                {
-                    timer_remove_->cancel();
-                }*/
-            });
-    }
-
-    bool PamiManger::ReadFromXML(const std::string& filename)
-    {
-        tinyxml2::XMLDocument doc;
-        if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS)
-        {
-            RCLCPP_ERROR(get_logger(), "Failed to load obstacle XML file: %s", filename.c_str());
-            return false;
-        }
-
-        tinyxml2::XMLElement* root = doc.FirstChildElement("pami_zone");
-        if (!root)
-        {
-            RCLCPP_ERROR(get_logger(), "No <pami-zone> root element in file");
-            return false;
-        }
-
-        for (tinyxml2::XMLElement* elem = root->FirstChildElement("zone");
-             elem;
-             elem = elem->NextSiblingElement("zone"))
-        {
-            zones_.push_back(Obstacle(elem));
-        }
-
-        RCLCPP_INFO(get_logger(), "Loaded zone obstacles from XML");
-        return true;
     }
 }
 

src/modelec_strat/src/pathfinding.cpp

@@ -1,5 +1,6 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <modelec_strat/pathfinding.hpp>
+#include <modelec_utils/config.hpp>
 
 namespace Modelec {
     struct AStarNode {
@@ -22,39 +23,22 @@ namespace Modelec {
     }
 
     Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr &node) : node_(node) {
-        node_->declare_parameter("map.size.map_width_mm", 3000);
-        node_->declare_parameter("map.size.map_height_mm", 2000);
-        node_->declare_parameter("map.size.grid_width", 300);
-        node_->declare_parameter("map.size.grid_height", 200);
 
-        map_width_mm_ = node_->get_parameter("map.size.map_width_mm").as_int();
+        map_width_mm_ = Config::get<int>("config.map.size.map_width_mm", 3000);
-        map_height_mm_ = node_->get_parameter("map.size.map_height_mm").as_int();
+        map_height_mm_ = Config::get<int>("config.map.size.map_height_mm", 2000);
-        grid_width_ = node_->get_parameter("map.size.grid_width").as_int();
+        grid_width_ = Config::get<int>("config.map.size.grid_width", 300);
-        grid_height_ = node_->get_parameter("map.size.grid_height").as_int();
+        grid_height_ = Config::get<int>("config.map.size.grid_height", 200);
 
-        node_->declare_parameter("robot.size.length_mm", 300);
+        robot_length_mm_ = Config::get<int>("config.robot.size.length_mm", 300);
-        node_->declare_parameter("robot.size.width_mm", 300);
+        robot_width_mm_ = Config::get<int>("config.robot.size.width_mm", 200);
-        node_->declare_parameter("robot.size.margin_mm", 100);
+        margin_mm_ = Config::get<int>("config.robot.size.margin_mm", 50);
 
-        robot_length_mm_ = node_->get_parameter("robot.size.length_mm").as_int();
+        enemy_length_mm_ = Config::get<int>("config.enemy.size.length_mm", 300);
-        robot_width_mm_ = node_->get_parameter("robot.size.width_mm").as_int();
+        enemy_width_mm_ = Config::get<int>("config.enemy.size.width_mm", 300);
-        margin_mm_ = node_->get_parameter("robot.size.margin_mm").as_int();
+        enemy_margin_mm_ = Config::get<int>("config.enemy.size.margin_mm", 0);
+        factor_close_enemy_ = Config::get<float>("config.factor.close_enemy", 1.0f);
 
-        node_->declare_parameter("enemy.size.length_mm", 300);
+        LoadObstaclesFromXML();
-        node_->declare_parameter("enemy.size.width_mm", 300);
-        node_->declare_parameter("enemy.size.margin_mm", 50);
-        node_->declare_parameter("enemy.factor_close_enemy", -0.5);
-
-        enemy_length_mm_ = node_->get_parameter("enemy.size.length_mm").as_int();
-        enemy_width_mm_ = node_->get_parameter("enemy.size.width_mm").as_int();
-        enemy_margin_mm_ = node_->get_parameter("enemy.size.margin_mm").as_int();
-        factor_close_enemy_ = node_->get_parameter("enemy.factor_close_enemy").as_double();
-
-        std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") +
-                                     "/data/obstacles.xml";
-        if (!LoadObstaclesFromXML(obstacles_path)) {
-            RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML");
-        }
 
         obstacle_add_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>(
             "obstacle/add", 10,
@@ -201,7 +185,11 @@ namespace Modelec {
 
         // 2. Fill obstacles with inflation
         // TODO some bug exist with the inflate
-        for (const auto &[id, obstacle]: obstacle_map_) {
+        for (const auto &obstacle: obstacles_) {
+            if (!obstacle->ShouldTakeCountInPathfinding())
+            {
+                continue;
+            }
             float cx = obstacle->GetX();
             float cy = obstacle->GetY();
             float width = obstacle->GetWidth() + inflate_x * 2 * cell_size_mm_x;
@@ -286,10 +274,10 @@ namespace Modelec {
 
         if (!TestCollision(start_x, start_y, collisionMask) || !TestCollision(goal_x, goal_y, collisionMask)) {
             if (!TestCollision(start_x, start_y, collisionMask)) {
-                RCLCPP_WARN(node_->get_logger(), "Start inside an obstacle");
+                RCLCPP_WARN(node_->get_logger(), "Start inside an obstacle x=%d y=%d mask=%d | %d", start_x, start_y, collisionMask, grid_[start_y][start_x]);
                 return {grid_[start_y][start_x], waypoints};
             } else {
-                RCLCPP_WARN(node_->get_logger(), "Goal inside an obstacle");
+                RCLCPP_WARN(node_->get_logger(), "Goal inside an obstacle x=%d y=%d mask=%d | %d", goal_x, goal_y, collisionMask, grid_[goal_y][goal_x]);
                 return {grid_[goal_y][goal_x], waypoints};
             }
         }
@@ -505,11 +493,13 @@ namespace Modelec {
     }
 
     std::shared_ptr<Obstacle> Pathfinding::GetObstacle(int id) const {
-        return obstacle_map_.at(id);
+        return obstacles_.at(id);
     }
 
     void Pathfinding::RemoveObstacle(int id) {
-        obstacle_map_.erase(id);
+        obstacles_.erase(std::remove_if(obstacles_.begin(), obstacles_.end(),
+                                           [id](const std::shared_ptr<Obstacle> &obs) { return obs->GetId() == id; }),
+                            obstacles_.end());
 
         modelec_interfaces::msg::Obstacle msg;
         msg.id = id;
@@ -517,7 +507,7 @@ namespace Modelec {
     }
 
     void Pathfinding::AddObstacle(const std::shared_ptr<Obstacle> &obstacle) {
-        obstacle_map_[obstacle->GetId()] = obstacle;
+        obstacles_.push_back(obstacle);
         modelec_interfaces::msg::Obstacle msg = obstacle->toMsg();
         obstacle_add_pub_->publish(msg);
     }
@@ -542,7 +532,7 @@ namespace Modelec {
 
     void Pathfinding::HandleAskObstacleRequest(const std::shared_ptr<std_srvs::srv::Empty::Request>,
                                                const std::shared_ptr<std_srvs::srv::Empty::Response>) {
-        for (auto &[index, obs]: obstacle_map_) {
+        for (auto &obs: obstacles_) {
             obstacle_add_pub_->publish(obs->toMsg());
         }
     }
@@ -554,7 +544,7 @@ namespace Modelec {
 
     void Pathfinding::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
         Point enemyPos(msg->x, msg->y, msg->theta);
-        for (auto &[index, obs]: obstacle_map_) {
+        for (auto &obs: obstacles_) {
             if (auto column = std::dynamic_pointer_cast<BoxObstacle>(obs)) {
                 if (Point::distance(enemyPos, column->GetPosition()) < enemy_width_mm_ + (column->GetWidth() / 2) +
                     enemy_margin_mm_) {
@@ -568,41 +558,29 @@ namespace Modelec {
         if (y < 0 || y >= static_cast<int>(grid_.size()) ||
             x < 0 || x >= static_cast<int>(grid_[y].size())) {
             RCLCPP_WARN(node_->get_logger(), "TestCollision: access out of bounds x=%d y=%d", x, y);
-            return false; // ou true, selon ce que tu veux (false = pas de collision)
+            return false;
         }
 
         return (grid_[y][x] & collisionMask) && !(grid_[y][x] & ~collisionMask);
     }
 
-    bool Pathfinding::LoadObstaclesFromXML(const std::string &filename) {
+    void Pathfinding::LoadObstaclesFromXML() {
-        tinyxml2::XMLDocument doc;
+        auto obs = Config::getArray<std::string>("Obstacles.Obstacle", [](const std::string& prefix)
-        if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS) {
+        {
-            RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacle XML file: %s", filename.c_str());
+            return Config::get<std::string>(prefix + "@type", "box");
-            return false;
+        });
+        for (size_t i = 0; i < obs.size(); ++i)
+        {
+            if (obs[i] == "box")
+            {
+                obstacles_.push_back(std::make_shared<BoxObstacle>(i));
+            } else
+            {
+                obstacles_.push_back(std::make_shared<Obstacle>(i));
+            }
         }
 
-        tinyxml2::XMLElement *root = doc.FirstChildElement("Obstacles");
+        RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", obstacles_.size());
-        if (!root) {
-            RCLCPP_ERROR(node_->get_logger(), "No <Obstacles> root element in file");
-            return false;
-        }
-
-        for (tinyxml2::XMLElement *obstacleElem = root->FirstChildElement("Obstacle");
-             obstacleElem != nullptr;
-             obstacleElem = obstacleElem->NextSiblingElement("Obstacle")) {
-            std::shared_ptr<Obstacle> obs = std::make_shared<Obstacle>(obstacleElem);
-            obstacle_map_[obs->GetId()] = obs;
-        }
-
-        for (tinyxml2::XMLElement *obstacleElem = root->FirstChildElement("Box");
-             obstacleElem != nullptr;
-             obstacleElem = obstacleElem->NextSiblingElement("Box")) {
-            std::shared_ptr<BoxObstacle> obs = std::make_shared<BoxObstacle>(obstacleElem);
-            obstacle_map_[obs->GetId()] = obs;
-        }
-
-        RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", obstacle_map_.size());
-        return true;
     }
 
     Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos &pos, int index, bool isLast) {

src/modelec_strat/src/strat_fms.cpp

@@ -15,15 +15,28 @@ namespace Modelec
 
     StratFMS::StratFMS() : Node("start_fms")
     {
-        this->declare_parameter<bool>("static_strat", false);
+        std::string data_dir = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/";
-        this->declare_parameter<double>("factor.obs", 1.0);
+        if (!Config::load(data_dir + "/config.xml"))
-        this->declare_parameter<double>("factor.thermo", 0.5);
+        {
-        this->declare_parameter<int>("timer_period_ms", 100);
+            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", (data_dir + "/config.xml").c_str());
+        }
+        if (!Config::load(data_dir + "/obstacles.xml"))
+        {
+            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", (data_dir + "/obstacles.xml").c_str());
+        }
+        if (!Config::load(data_dir + "/deposite_zone.xml"))
+        {
+            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", (data_dir + "/deposite_zone.xml").c_str());
+        }
+        if (!Config::load(data_dir + "/action.xml"))
+        {
+            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", (data_dir + "/action.xml").c_str());
+        }
 
-        static_strat_ = this->get_parameter("static_strat").as_bool();
+        static_strat_ = Config::get<bool>("config.static_strat.enabled", false);
-        factor_obs_ = this->get_parameter("factor.obs").as_double();
+        factor_obs_ = Config::get<float>("config.factor.obs", 1.0);
-        factor_thermo_ = this->get_parameter("factor.thermo").as_double();
+        factor_thermo_ = Config::get<float>("config.factor.thermo", 1.0);
-        timer_period_ms_ = this->get_parameter("timer_period_ms").as_int();
+        timer_period_ms_ = Config::get<int>("config.timer.strat.ms", 100);
 
         tir_sub_ = create_subscription<std_msgs::msg::Empty>(
             "/action/tir/start", 10, [this](const std_msgs::msg::Empty::SharedPtr)
@@ -153,8 +166,6 @@ namespace Modelec
                 game_action_sequence_.push(State::TAKE_MISSION);
                 game_action_sequence_.push(State::TAKE_MISSION);
                 game_action_sequence_.push(State::FREE_MISSION);
-                game_action_sequence_.push(State::FREE_MISSION);
-                game_action_sequence_.push(State::THERMO_MISSION);
 
                 Transition(State::WAIT_START, "System ready");
             }

src/modelec_utils/include/modelec_utils/config.hpp

@@ -6,6 +6,7 @@
 #include <sstream>
 #include <vector>
 #include <functional>
+#include <iostream>
 
 namespace Modelec
 {
@@ -17,19 +18,33 @@ namespace Modelec
         template<typename T>
         using BuilderFunc = std::function<T(const std::string& base_key)>;
 
+        template<typename K, typename V>
+        using MapBuilderFunc = std::function<std::pair<K, V>(const std::string& base_key)>;
+
         static bool load(const std::string& filepath);
 
         template<typename T>
-        static T get(const std::string& key, const T& default_value = T());
+        static T get(const std::string& key, const T& default_value = T(), bool is_optional = false);
 
         template<typename T>
         static std::vector<T> getArray(const std::string& prefix,
                                     BuilderFunc<T> builder = [](const std::string& base) { return get<T>(base); });
 
+        template<typename K, typename V>
+        static std::unordered_map<K, V> getMap(
+            const std::string& prefix,
+            MapBuilderFunc<K, V> builder);
+
         static size_t count(const std::string& prefix);
 
+        static void clear(const std::string& prefix);
+
+        static bool has(const std::string& prefix);
+
         static void printAll();
 
+        static void print(const std::string& prefix);
+
     private:
         static void parseNode(tinyxml2::XMLElement* element, const std::string& key);
 
@@ -37,25 +52,47 @@ namespace Modelec
     };
 
     template<typename T>
-    T Config::get(const std::string& key, const T& default_value) {
+    T Config::get(const std::string& key, const T& default_value, bool is_optional) {
         auto it = values_.find(key);
-        if (it == values_.end()) return default_value;
+        if (it == values_.end())
+        {
+            if (!is_optional)
+            {
+                std::cerr << "Config key not found: " << key << std::endl;
+            }
+            return default_value;
+        }
 
         std::istringstream iss(it->second);
         T result;
-        if (!(iss >> result)) return default_value;
+        if (!(iss >> result))
+        {
+            if (!is_optional)
+            {
+                std::cerr << "Config key has invalid format: " << key << " = " << it->second << std::endl;
+            }
+            return default_value;
+        }
         return result;
     }
 
     template<>
-    inline std::string Config::get<std::string>(const std::string& key, const std::string& default_value) {
+    inline std::string Config::get<std::string>(const std::string& key, const std::string& default_value, bool is_optional) {
         auto it = values_.find(key);
-        return it != values_.end() ? it->second : default_value;
+        if (it == values_.end())
+        {
+            if (is_optional)
+            {
+                std::cerr << "Config key not found: " << key << std::endl;
+            }
+            return default_value;
+        }
+        return it->second;
     }
 
     template<>
-    inline bool Config::get<bool>(const std::string& key, const bool& default_value) {
+    inline bool Config::get<bool>(const std::string& key, const bool& default_value, bool is_optional) {
-        auto str = get<std::string>(key, default_value ? "true" : "false");
+        auto str = get<std::string>(key, default_value ? "true" : "false", is_optional);
         return str == "true" || str == "1";
     }
 
@@ -65,13 +102,49 @@ namespace Modelec
     {
         std::vector<T> result;
 
-        size_t n = Config::count(prefix);
+        size_t n = count(prefix);
         result.reserve(n);
 
+        if (n > 0)
+        {
+            if (n == 1 && !has(prefix + "[0]"))
+            {
+                result.emplace_back(builder(prefix));
+            } else
+            {
+                for (size_t i = 0; i < n; ++i)
+                {
+                    std::string base = prefix + "[" + std::to_string(i) + "]";
+                    result.emplace_back(builder(base));
+                }
+            }
+        } else
+        {
+            std::cerr << "Config array not found: " << prefix << std::endl;
+        }
+
+        return result;
+    }
+
+    template<typename K, typename V>
+    std::unordered_map<K, V> Config::getMap(
+        const std::string& prefix,
+        MapBuilderFunc<K, V> builder)
+    {
+        std::unordered_map<K, V> result;
+
+        size_t n = count(prefix);
+        if (n == 0)
+        {
+            std::cerr << "Config map not found: " << prefix << std::endl;
+            return result;
+        }
+
         for (size_t i = 0; i < n; ++i)
         {
             std::string base = prefix + "[" + std::to_string(i) + "]";
-            result.emplace_back(builder(base));
+            auto [key, value] = builder(base);
+            result.emplace(key, value);
         }
 
         return result;

src/modelec_utils/include/modelec_utils/point.hpp

@@ -1,7 +1,7 @@
 #pragma once
 
-#define CLOSE_DISTANCE 155
+#define CLOSE_DISTANCE 170
-#define BASE_DISTANCE 310
+#define BASE_DISTANCE 305
 
 namespace Modelec
 {

src/modelec_utils/src/config.cpp

@@ -1,4 +1,3 @@
-#include <iostream>
 #include <modelec_utils/config.hpp>
 
 namespace Modelec
@@ -40,14 +39,49 @@ namespace Modelec
             }
         }
 
-        return found ? max_index + 1 : 0;
+        return found ? max_index + 1 : has(prefix) ? 1 : 0;
+    }
+
+    void Config::clear(const std::string& prefix)
+    {
+        for (auto it = values_.begin(); it != values_.end();)
+        {
+            if (it->first.rfind(prefix, 0) == 0)
+            {
+                it = values_.erase(it);
+            }
+            else
+            {
+                ++it;
+            }
+        }
+    }
+
+    bool Config::has(const std::string& prefix)
+    {
+        return std::any_of(values_.begin(), values_.end(), [&prefix](const auto& pair) {
+            return pair.first.rfind(prefix, 0) == 0;
+        });
     }
 
     void Config::printAll()
     {
+        std::cerr << "Config values:" << std::endl;
         for (const auto& [key, value] : values_)
         {
-            std::cout << key << " = " << value << std::endl;
+            std::cerr << "    " << key << " = " << value << std::endl;
+        }
+    }
+
+    void Config::print(const std::string& prefix)
+    {
+        std::cerr << "Config values with prefix '" << prefix << "':" << std::endl;
+        for (const auto& [key, value] : values_)
+        {
+            if (key.rfind(prefix, 0) == 0)
+            {
+                std::cerr << "    " << key << " = " << value << std::endl;
+            }
         }
     }
 
@@ -60,20 +94,17 @@ namespace Modelec
             }
         }
 
-        // Store attributes
         for (auto* attr = element->FirstAttribute(); attr; attr = attr->Next())
         {
             values_[key + "@" + attr->Name()] = attr->Value();
         }
 
-        // Count children by name
         std::unordered_map<std::string, int> child_count;
         for (auto* child = element->FirstChildElement(); child; child = child->NextSiblingElement())
         {
             child_count[child->Name()]++;
         }
 
-        // Index children
         std::unordered_map<std::string, int> child_index;
 
         for (auto* child = element->FirstChildElement(); child; child = child->NextSiblingElement())

src/modelec_utils/test/config.test.cpp

@@ -34,6 +34,9 @@ TEST(ConfigTest, LoadValidXML)
 
     // Deep nested node
     EXPECT_EQ(Modelec::Config::get<std::string>("root.nested.deep", ""), "hello");
+
+    EXPECT_TRUE(Modelec::Config::has("root.child1"));
+    EXPECT_FALSE(Modelec::Config::has("root.child11T1435TRERGFSRVSPKRMGHJNIUSRJOGFH"));
 }
 
 TEST(ConfigTest, LoadInvalidXML)
@@ -113,4 +116,33 @@ TEST(ConfigTest, GetArray)
     EXPECT_EQ(array2[1].attr, "c");
     EXPECT_EQ(array2[2].value, 3);
     EXPECT_EQ(array2[2].attr, "d");
+}
+
+TEST(ConfigTest, Clear)
+{
+    // Create temporary XML file
+    const std::string filepath = "test_array_config.xml";
+
+    std::ofstream file(filepath);
+    file <<
+        "<root>"
+        "   <item a=\"b\">1</item>"
+        "   <item a=\"c\">2</item>"
+        "   <item a=\"d\">3</item>"
+        "   <test a=\"d\">3</test>"
+        "</root>";
+    file.close();
+
+    EXPECT_TRUE(Modelec::Config::load(filepath));
+
+    EXPECT_TRUE(Modelec::Config::has("root.test"));
+    Modelec::Config::clear("root.test");
+    EXPECT_FALSE(Modelec::Config::has("root.test"));
+
+    EXPECT_EQ(Modelec::Config::count("root.item"), 3);
+    Modelec::Config::clear("root.item");
+    EXPECT_EQ(Modelec::Config::count("root.item"), 0);
+
+    Modelec::Config::clear("");
+    EXPECT_FALSE(Modelec::Config::has("root"));
 }

start_cam_ros2.sh

@@ -0,0 +1,12 @@
+#!/bin/bash
+set -e
+
+source /opt/ros/jazzy/setup.bash
+source /home/modelec/Modelec-ROS2/install/setup.bash
+
+export RCL_LOG_LEVEL=info
+#export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+#export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/Modelec-ROS2/fastdds_setup.xml
+#export ROS_DOMAIN_ID=128
+
+exec ros2 run modelec_com color_detector

start_ros2.sh

@@ -6,7 +6,7 @@ source /home/modelec/Modelec-ROS2/install/setup.bash
 
 export RCL_LOG_LEVEL=info
 #export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
-export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/Modelec-ROS2/fastdds_setup.xml
+#export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/Modelec-ROS2/fastdds_setup.xml
 #export ROS_DOMAIN_ID=128
 
-exec ros2 launch modelec_core modelec.launch.py with_color_detector:=false "$@"
+exec ros2 launch modelec_core modelec.launch.py "$@"

start_ros2.zsh

@@ -0,0 +1,9 @@
+#!/bin/bash
+set -e
+
+source /opt/ros/jazzy/setup.zsh
+source ~/Modelec-ROS2/install/setup.zsh
+
+export RCL_LOG_LEVEL=info
+
+exec ros2 launch modelec_core modelec.launch.py "$@"