Lidar + new action + rewrite action executor + cpu usage (gui & enemy manager) (#28)

.gitignore

@@ -106,4 +106,5 @@ AMENT_IGNORE
 .vscode
 .cache
 
-.idea
+.idea
+cam

Desktop/no-gui.ros2_launch_marcel.desktop

@@ -0,0 +1,8 @@
+[Desktop Entry]
+Type=Application
+Name=No GUI
+Comment=Lance le système ROS 2 Sans GUI
+Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false with_gui:=false
+Icon=utilities-terminal
+Terminal=true
+Categories=Utility;

Desktop/no_lidar.joy.ros2_launch_marcel.desktop

@@ -2,7 +2,7 @@
 Type=Application
 Name=Joy no Lidar
 Comment=Lance le système ROS 2 avec GUI
-Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false
+Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false with_enemy_manager:=false
 Icon=utilities-terminal
 Terminal=true
 Categories=Utility;

Desktop/no_lidar.ros2_launch_marcel.desktop

@@ -2,9 +2,7 @@
 Type=Application
 Name=No Lidar
 Comment=Lance le système ROS 2 avec GUI
-
-Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_joy:=false with_rplidar:=false
-
+Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_joy:=false with_rplidar:=false with_enemy_manager:=false
 Icon=utilities-terminal
 Terminal=true
 Categories=Utility;

TODO.md

@@ -1,4 +0,0 @@
-- API (check for the middleware already implemented or fill the API package)
-
-
-- Check every value and after that update the max TIME mission

build.rasp.sh

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

build.sh

@@ -1,4 +1,4 @@
 #!/bin/bash
-colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
+colcon build --symlink-install --cmake-args # -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
 
 source install/setup.bash

fastdds_setup.xml

@@ -28,7 +28,7 @@
                     </locator>
                     <locator>
                         <udpv4>
-                            <address>100.73.69.106</address>
+                            <address>100.91.204.77</address>
                         </udpv4>
                     </locator>
                 </initialPeersList>

install.sh

@@ -27,9 +27,9 @@ git submodule init
 git submodule update
 
 echo "source /opt/ros/jazzy/setup.bash
-source ~/Modelec-ROS/install/setup.bash
+source ~/Modelec-ROS2/install/setup.bash
 export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
-export FASTRTPS_DEFAULT_PROFILES_FILE=~/Modelec-ROS/fastdds_setup.xml
+export FASTRTPS_DEFAULT_PROFILES_FILE=~/Modelec-ROS2/fastdds_setup.xml
 export ROS_DOMAIN_ID=128" >> ~/.bashrc
 
 source ~/.bashrc
@@ -39,9 +39,10 @@ source src/rplidar_ros/scripts/create_udev_rules.sh
 
 cd ../..
 
-cp ./*.desktop ~/Desktop
+cp ./Desktop/*.desktop ~/Desktop
 chmod +x ~/Desktop/*.desktop
 gio set ~/Desktop/no_lidar.joy.ros2_launch_marcel.desktop "metadata::trusted" true
 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/ros2_launch_marcel.desktop "metadata::trusted" true
+gio set ~/Desktop/no-gui.ros2_launch_marcel.desktop "metadata::trusted" true

simulated_pcb/odo.py

@@ -125,6 +125,9 @@ class SimulatedPCB:
 
         elif msg.startswith("SET;START;"):
             self.start = msg.split(';')[2] == '1'
+            if not self.start:
+                self.waypoints.clear()
+                self.waypoint_queue.clear()
             self.ser.write(b"OK;START;1\n")
 
         # --- MULTI WAYPOINT (clears previous) ---

src/modelec_com/CMakeLists.txt

@@ -10,10 +10,13 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system)
 find_package(ament_index_cpp REQUIRED)
 find_package(fmt)
 
+find_package(OpenCV REQUIRED)
+
 find_package(modelec_interfaces REQUIRED)
 find_package(modelec_utils REQUIRED)
 
@@ -35,10 +38,35 @@ target_include_directories(pcb_action_interface PUBLIC
         $<INSTALL_INTERFACE:include>
 )
 
+# Color Detector Node
+add_executable(color_detector
+  src/color_detector.cpp
+)
+
+ament_target_dependencies(color_detector
+  rclcpp
+  std_msgs
+  std_srvs
+  modelec_interfaces
+  ament_index_cpp
+)
+
+target_link_libraries(color_detector
+  ${OpenCV_LIBS}
+  modelec_utils::utils
+  modelec_utils::config
+)
+
+target_include_directories(color_detector PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
 # Install targets
 install(TARGETS
         pcb_odo_interface
         pcb_action_interface
+        color_detector
         DESTINATION lib/${PROJECT_NAME}
 )
 

src/modelec_com/include/modelec_com/color_detector.hpp

@@ -0,0 +1,52 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <opencv2/opencv.hpp>
+#include <std_srvs/srv/trigger.hpp>
+#include <std_msgs/msg/empty.hpp>
+#include <std_msgs/msg/string.hpp>
+
+namespace Modelec
+{
+    struct ColorSetting
+    {
+        std::string name;
+        double h_min;
+        double h_max;
+    };
+
+    class ColorDetector : public rclcpp::Node
+    {
+    public:
+        ColorDetector();
+
+        ~ColorDetector() override;
+    private:
+        bool processSnapshot(std::vector<std::string>& colors, std::string& error);
+
+        void onRequest(
+            const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+            std::shared_ptr<std_srvs::srv::Trigger::Response> response);
+
+        std::vector<std::string> classifyROIs(const cv::Mat& hsv, cv::Mat& debug_img) const;
+
+        std::string classify(const cv::Vec3d& hsv_roi) const;
+
+        std::string generateImagePath() const;
+
+        rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr service_;
+
+        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr ask_sub_;
+        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr color_pub_;
+
+        std::vector<cv::Rect> rois_;
+
+        std::string link_;
+        bool save_to_file_ = true;
+        std::string save_directory_ = "./";
+        bool enable_ = false;
+        bool headless_ = true;
+
+        std::vector<ColorSetting> color_configs_;
+    };
+}

src/modelec_com/include/modelec_com/serial_listener.hpp

@@ -30,7 +30,6 @@ namespace Modelec
         void start_async_write();
 
     public:
-        std::string name_;
         boost::asio::serial_port port_;
 
         rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
@@ -38,13 +37,13 @@ namespace Modelec
 
         SerialListener();
 
-        SerialListener(const std::string& name, int bauds, const std::string& serial_port,
+        SerialListener(int bauds, const std::string& serial_port,
                        int max_message_len);
 
         virtual ~SerialListener();
 
         void close();
-        void open(const std::string& name, int bauds, const std::string& serial_port,
+        void open(int bauds, const std::string& serial_port,
                        int max_message_len);
 
         void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; }

src/modelec_com/src/color_detector.cpp

@@ -0,0 +1,238 @@
+#include <modelec_com/color_detector.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <modelec_utils/utils.hpp>
+
+namespace Modelec
+{
+    ColorDetector::ColorDetector()
+        : Node("color_detector")
+    {
+        this->declare_parameter("enabled", true);
+        this->declare_parameter("link", "");
+        this->declare_parameter("headless", true);
+        this->declare_parameter("save_to_file.enabled", true);
+        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();
+        link_ = this->get_parameter("link").as_string();
+        headless_ = this->get_parameter("headless").as_bool();
+        save_to_file_ = this->get_parameter("save_to_file.enabled").as_bool();
+        save_directory_ = this->get_parameter("save_to_file.path").as_string();
+
+        auto rois_param = this->get_parameter("rois").as_integer_array();
+        for (size_t i = 0; i + 3 < rois_param.size(); i += 4)
+        {
+            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_)
+        {
+            RCLCPP_INFO(get_logger(), "Camera disabled by config");
+            rclcpp::shutdown();
+            return;
+        }
+
+        service_ = create_service<std_srvs::srv::Trigger>(
+            "action/detect_color",
+            std::bind(&ColorDetector::onRequest, this,
+                      std::placeholders::_1,
+                      std::placeholders::_2));
+
+        rclcpp::QoS qos(rclcpp::KeepLast(10));
+        qos.reliable();
+
+        ask_sub_ = create_subscription<std_msgs::msg::Empty>(
+            "action/detect_color/ask", qos,
+            [this](const std_msgs::msg::Empty::SharedPtr)
+            {
+                std_msgs::msg::String res;
+                std::vector<std::string> colors;
+                std::string error;
+
+                if (!processSnapshot(colors, error))
+                {
+                    res.data = "0|" + error;
+                }
+                else
+                {
+                    res.data = "1|" + join(colors, ";");
+                }
+
+                color_pub_->publish(res);
+            });
+
+        color_pub_ = create_publisher<std_msgs::msg::String>("action/detect_color/res", qos);
+
+        if (!headless_)
+        {
+            cv::namedWindow("color_detector", cv::WINDOW_NORMAL);
+        }
+
+        RCLCPP_INFO(get_logger(), "Color detector service ready");
+    }
+
+    ColorDetector::~ColorDetector()
+    {
+        if (!headless_)
+        {
+            cv::destroyAllWindows();
+        }
+    }
+
+    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;
+
+        for (int i = 0; i < 3; ++i)
+            cap >> frame;
+
+        if (frame.empty())
+        {
+            RCLCPP_WARN(get_logger(), "Captured empty frame from camera");
+            error = "Empty frame";
+            return false;
+        }
+
+        cv::Mat hsv;
+        cv::GaussianBlur(frame, frame, cv::Size(5, 5), 0);
+        cv::cvtColor(frame, hsv, cv::COLOR_BGR2HSV);
+
+        colors = classifyROIs(hsv, frame);
+
+        if (!headless_)
+        {
+            cv::imshow("color_detector", frame);
+            cv::waitKey(1);
+        }
+
+        if (save_to_file_)
+        {
+            std::string path = save_directory_ + generateImagePath();
+            cv::imwrite(path, frame);
+            RCLCPP_DEBUG(get_logger(), "Saved snapshot to %s", path.c_str());
+        }
+
+        cap.release();
+
+        return true;
+    }
+
+    void ColorDetector::onRequest(
+        const std::shared_ptr<std_srvs::srv::Trigger::Request>,
+        std::shared_ptr<std_srvs::srv::Trigger::Response> response)
+    {
+        std::vector<std::string> colors;
+        std::string error;
+
+        if (!processSnapshot(colors, error))
+        {
+            response->success = false;
+            response->message = error;
+            return;
+        }
+
+        response->success = true;
+        response->message = join(colors, ";");
+    }
+
+    std::vector<std::string> ColorDetector::classifyROIs(const cv::Mat& hsv, cv::Mat& debug_img) const
+    {
+        std::vector<std::string> results;
+
+        for (auto r : rois_)
+        {
+            cv::Rect roi = r & cv::Rect(0, 0, hsv.cols, hsv.rows);
+            cv::Scalar mean = cv::mean(hsv(roi));
+
+            std::string color = classify(cv::Vec3d(mean[0], mean[1], mean[2]));
+
+            RCLCPP_DEBUG(get_logger(), "ROI at (%d, %d, %d, %d) has mean HSV (%.2f, %.2f, %.2f) classified as %s",
+                         roi.x, roi.y, roi.width, roi.height,
+                         mean[0], mean[1], mean[2],
+                         color.c_str());
+
+            results.push_back(color);
+
+            if (save_to_file_)
+            {
+                cv::rectangle(debug_img, roi, {0, 255, 0}, 1);
+                cv::putText(
+                    debug_img,
+                    color,
+                    roi.tl() + cv::Point(0, -5),
+                    cv::FONT_HERSHEY_SIMPLEX,
+                    0.4,
+                    {0, 255, 0},
+                    1);
+            }
+        }
+
+        return results;
+    }
+
+    std::string ColorDetector::classify(const cv::Vec3d& hsv_roi) const
+    {
+        double h = hsv_roi[0];
+
+        for (const auto& color_config : color_configs_)
+        {
+            if (h >= color_config.h_min && h <= color_config.h_max)
+            {
+                return color_config.name;
+            }
+        }
+
+        return "unknown";
+    }
+
+    std::string ColorDetector::generateImagePath() const
+    {
+        auto now = std::chrono::system_clock::now();
+        auto in_time_t = std::chrono::system_clock::to_time_t(now);
+        std::stringstream ss;
+        ss << "snapshot_"
+            << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S")
+            << ".png";
+        return ss.str();
+    }
+}
+
+
+int main(int argc, char* argv[])
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<Modelec::ColorDetector>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

src/modelec_com/src/pcb_action_interface.cpp

@@ -1,6 +1,4 @@
 #include <modelec_com/pcb_action_interface.hpp>
-#include <ament_index_cpp/get_package_share_directory.hpp>
-#include <modelec_utils/config.hpp>
 #include <modelec_utils/utils.hpp>
 #include <fmt/core.h>
 
@@ -11,12 +9,11 @@ namespace Modelec
         // Service to create a new serial listener
         declare_parameter<std::string>("serial_port", "/dev/USB_ACTION");
         declare_parameter<int>("baudrate", 115200);
-        declare_parameter<std::string>("name", "pcb_action");
 
-        auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
-        request->name = get_parameter("name").as_string();
-        request->bauds = get_parameter("baudrate").as_int();
-        request->serial_port = get_parameter("serial_port").as_string();
+        auto serial_port = get_parameter("serial_port").as_string();
+        auto baudrate = get_parameter("baudrate").as_int();
+
+        RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %ld", serial_port.c_str(), baudrate);
 
         asc_get_sub_ = this->create_subscription<modelec_interfaces::msg::ActionAscPos>(
             "action/get/asc", 10,
@@ -233,7 +230,7 @@ namespace Modelec
 
 			});
 
-        this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN);
+        this->open(baudrate, serial_port, MAX_MESSAGE_LEN);
 
         // TODO : check for real value there
         /*asc_value_mapper_ = {
@@ -262,15 +259,15 @@ namespace Modelec
             {5, 0}
         };*/
 
-        /*servo_value_ = {
-            {0, 2.95},
-            {1, 0.93},
-            {2, 0},
-            {3, 3},
-            {4, 0.8},
-            {5, 0.8},
-            {6, 0.8},
-            {7, 0.8},
+        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()) + ";";
@@ -282,7 +279,7 @@ namespace Modelec
 
 		data += "\n";
 
-        SendToPCB(data);*/
+        SendToPCB(data);
 
         /*relay_value_ = {
             {1, false},

src/modelec_com/src/pcb_odo_interface.cpp

@@ -1,7 +1,5 @@
 #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
 {
@@ -9,13 +7,11 @@ namespace Modelec
     {
         declare_parameter<std::string>("serial_port", "/dev/USB_ODO");
         declare_parameter<int>("baudrate", 115200);
-        declare_parameter<std::string>("name", "pcb_odo");
 
-        // Service to create a new serial listener
-        auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
-        request->name = get_parameter("name").as_string();
-        request->bauds = get_parameter("baudrate").as_int();
-        request->serial_port = get_parameter("serial_port").as_string();
+        auto serial_port = get_parameter("serial_port").as_string();
+        auto baudrate = get_parameter("baudrate").as_int();
+
+        RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %ld", serial_port.c_str(), baudrate);
 
         odo_pos_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
             "odometry/position", 10);
@@ -102,17 +98,14 @@ namespace Modelec
             "odometry/start", 10,
             [this](const std_msgs::msg::Bool::SharedPtr msg)
             {
-                if (msg->data != start_odo_)
-                {
-                    start_odo_ = msg->data;
-                    SendOrder("START", {std::to_string(msg->data)});
-                }
+                start_odo_ = msg->data;
+                SendOrder("START", {std::to_string(msg->data)});
             });
 
-        this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN);
+        this->open(baudrate, serial_port, MAX_MESSAGE_LEN);
 
         SetPID("THETA", 14, 0, 0);
-        SetPID("POS", 10, 0, 0);
+        SetPID("POS", 5, 0, 0);
         SetPID("LEFT", 5, 0, 0);
         SetPID("RIGHT", 5, 0, 0);
     }
@@ -179,7 +172,7 @@ namespace Modelec
             {
                 if (tokens[2] == "REACH")
                 {
-                    // RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[3].c_str());
+                    RCLCPP_DEBUG(this->get_logger(), "Waypoint reached: ID %s", tokens[3].c_str());
 
                     int id = std::stoi(tokens[3]);
 

src/modelec_com/src/serial_listener.cpp

@@ -7,10 +7,10 @@ namespace Modelec
     {
     }
 
-    SerialListener::SerialListener(const std::string& name, int bauds, const std::string& serial_port,
+    SerialListener::SerialListener(int bauds, const std::string& serial_port,
                                    int max_message_len) : io_(), port_(io_)
     {
-        open(name, bauds, serial_port, max_message_len);
+        open(bauds, serial_port, max_message_len);
     }
 
     SerialListener::~SerialListener()
@@ -32,9 +32,8 @@ namespace Modelec
         }
     }
 
-    void SerialListener::open(const std::string& name, int bauds, const std::string& serial_port,
+    void SerialListener::open(int bauds, const std::string& serial_port,
                int max_message_len) {
-       this->name_ = name;
        this->bauds_ = bauds;
        this->serial_port_ = serial_port;
        this->max_message_len_ = max_message_len;

src/modelec_core/config/config.yml

@@ -0,0 +1,134 @@
+enemy_manager:
+  ros__parameters:
+    size:
+      width_mm: 300
+      length_mm: 300
+      margin_mm: 50
+    detection:
+      min_move_threshold_mm: 50
+      refresh_rate: 3
+      max_stationary_time_s: 0.5
+      min_emergency_distance_mm: 300
+      margin_detection_table_mm: 50
+    factor_close_enemy: -2
+
+pami_manager:
+  ros__parameters:
+    time_to_put_zone: 77
+    time_to_remove_top_pot: 65
+
+modelec_gui:
+  ros__parameters:
+    robot:
+      size:
+        width_mm: 300
+        length_mm: 300
+        margin_mm: 100
+    map:
+      size:
+        grid_width: 600
+        grid_height: 400
+        map_width_mm: 3000
+        map_height_mm: 2000
+    enemy:
+      size:
+        width_mm: 300
+        length_mm: 300
+        margin_mm: 50
+
+strat_fsm:
+  ros__parameters:
+    robot:
+      size:
+        width_mm: 300
+        length_mm: 300
+        margin_mm: 100
+
+    enemy:
+      size:
+        width_mm: 300
+        length_mm: 300
+        margin_mm: 50
+
+    map:
+      size:
+        grid_width: 600
+        grid_height: 400
+        map_width_mm: 3000
+        map_height_mm: 2000
+
+    spawn:
+      yellow_top:
+        x: 350
+        y: 1700
+        theta: -1.5708
+      blue_top:
+        x: 2650
+        y: 1700
+        theta: -1.5708
+      width_mm: 450
+      height_mm: 450
+
+    home:
+      blue:
+        x: 2650
+        y: 1700
+        theta: -1.5708
+      yellow:
+        x: 350
+        y: 1700
+        theta: -1.5708
+
+    static_strat: false
+    factor:
+      theta: 20.0
+      obs: 0.6
+      thermo: 0.7
+    timer_period_ms: 20
+
+    thermo:
+      yellow:
+        start:
+          x: 200
+          y: 175
+          theta: 0.0
+        finish:
+          x: 700
+          y: 175
+          theta: 0.0
+      blue:
+        start:
+          x: 2800
+          y: 175
+          theta: 3.14
+        finish:
+          x: 2300
+          y: 175
+          theta: 3.14
+
+    mission_score:
+      go_home: 10
+
+color_detector:
+  ros__parameters:
+    save_to_file:
+      enabled: true
+      path: "./cam/"
+    enabled: true
+    # link: "/dev/video0"
+    link: "http://192.168.1.21:8080/video"
+    headless: true
+    rois: [300, 700, 50, 50, 700, 700, 50, 50, 1200, 700, 50, 50, 1500, 700, 50, 50]
+    colors:
+      blue: [90, 130]
+      yellow: [20, 40]
+
+pcb_odo_interface:
+  ros__parameters:
+    serial_port: '/dev/USB_ODO'
+    baudrate: 115200
+
+pcb_action_interface:
+  ros__parameters:
+    serial_port: '/dev/USB_ACTION'
+    baudrate: 115200

src/modelec_core/launch/modelec.launch.py

@@ -1,36 +1,70 @@
+import os
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown, RegisterEventHandler, TimerAction, LogInfo
+from launch.actions import (
+    DeclareLaunchArgument,
+    OpaqueFunction,
+    Shutdown,
+    RegisterEventHandler,
+    TimerAction,
+    LogInfo,
+)
 from launch.event_handlers import OnProcessExit
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
-
+from ament_index_python.packages import get_package_share_directory
 
 def generate_launch_description():
-    # Declare launch arguments
-    with_gui_arg = DeclareLaunchArgument('with_gui', default_value='true', description='Launch GUI?')
-    with_rplidar_arg = DeclareLaunchArgument('with_rplidar', default_value='true', description='Launch RPLIDAR?')
-    with_com_arg = DeclareLaunchArgument('with_com', default_value='true', description='Launch COM nodes?')
-    with_strat_arg = DeclareLaunchArgument('with_strat', default_value='true', description='Launch strategy nodes?')
-    with_joy_arg = DeclareLaunchArgument('with_joy', default_value='true', description='Launch joystick node?')
+    pkg_name = 'modelec_core'
+    # -------------------------------------------------
+    # Launch arguments
+    # -------------------------------------------------
+    with_gui_arg = DeclareLaunchArgument(
+        'with_gui', default_value='true', description='Launch GUI?'
+    )
+    with_rplidar_arg = DeclareLaunchArgument(
+        'with_rplidar', default_value='true', description='Launch RPLIDAR?'
+    )
+    with_com_arg = DeclareLaunchArgument(
+        'with_com', default_value='true', description='Launch COM nodes?'
+    )
+    with_strat_arg = DeclareLaunchArgument(
+        'with_strat', default_value='true', description='Launch strategy nodes (except enemy_manager)?'
+    )
+    with_enemy_manager_arg = DeclareLaunchArgument(
+        'with_enemy_manager',
+        default_value='true',
+        description='Launch enemy_manager node?',
+    )
+    with_joy_arg = DeclareLaunchArgument(
+        'with_joy', default_value='true', description='Launch joystick node?'
+    )
+    with_color_detector_arg = DeclareLaunchArgument(
+        'with_color_detector', default_value='true', description='Launch color detector node?'
+    )
 
     channel_type = LaunchConfiguration('channel_type', default='serial')
     serial_port = LaunchConfiguration('serial_port', default='/dev/rplidar')
     serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200')
     frame_id = LaunchConfiguration('frame_id', default='laser')
     inverted = LaunchConfiguration('inverted', default='false')
-    angle_compensate = LaunchConfiguration('angle_compensate', default='true')
-    scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity')
+    angle_compensate = LaunchConfiguration('angle_compensate', default='false')
+    scan_mode = LaunchConfiguration('scan_mode', default='Standard')
 
-    # ----------------------------
-    # Infinite LIDAR restart logic
-    # ----------------------------
+    param_file = os.path.join(
+        get_package_share_directory(pkg_name),
+        'config',
+        'config.yml'
+    )
+
+    # -------------------------------------------------
+    # RPLIDAR with auto-restart
+    # -------------------------------------------------
     def create_lidar_with_restart():
-        """Create a lidar node and its auto-restart handler."""
         lidar_node = Node(
             package='rplidar_ros',
             executable='rplidar_node',
             name='rplidar_node',
-            parameters=[{
+            parameters=[param_file, {
                 'channel_type': channel_type,
                 'serial_port': serial_port,
                 'serial_baudrate': serial_baudrate,
@@ -39,20 +73,22 @@ def generate_launch_description():
                 'angle_compensate': angle_compensate,
                 'scan_mode': scan_mode,
             }],
-            output='screen'
         )
 
-        # Instead of recursion at definition time, we delay the re-creation using a lambda
         restart_handler = RegisterEventHandler(
             OnProcessExit(
                 target_action=lidar_node,
                 on_exit=[
                     LogInfo(msg='[Launch] RPLIDAR crashed — restarting in 2s...'),
                     TimerAction(
-                        period=0.3,
-                        actions=[OpaqueFunction(function=lambda *_: create_lidar_with_restart())]
-                    )
-                ]
+                        period=2.0,
+                        actions=[
+                            OpaqueFunction(
+                                function=lambda *_: create_lidar_with_restart()
+                            )
+                        ],
+                    ),
+                ],
             )
         )
 
@@ -60,37 +96,38 @@ def generate_launch_description():
 
     def launch_rplidar_restart_if_needed(context, *args, **kwargs):
         if context.launch_configurations.get('with_rplidar') == 'true':
-            # Start after 3 seconds
             return [
                 TimerAction(
                     period=3.0,
-                    actions=create_lidar_with_restart()
+                    actions=create_lidar_with_restart(),
                 )
             ]
         return []
 
-    # ----------------------------
-    # GUI node
-    # ----------------------------
+    # -------------------------------------------------
+    # GUI
+    # -------------------------------------------------
     def launch_gui(context, *args, **kwargs):
         if context.launch_configurations.get('with_gui') == 'true':
             gui = Node(
                 package='modelec_gui',
                 executable='modelec_gui',
-                name='modelec_gui'
+                name='modelec_gui',
+                parameters=[param_file],
             )
+
             shutdown = RegisterEventHandler(
                 OnProcessExit(
                     target_action=gui,
-                    on_exit=[Shutdown()]
+                    on_exit=[Shutdown()],
                 )
             )
             return [gui, shutdown]
         return []
 
-    # ----------------------------
+    # -------------------------------------------------
     # COM nodes
-    # ----------------------------
+    # -------------------------------------------------
     def launch_com(context, *args, **kwargs):
         if context.launch_configurations.get('with_com') == 'true':
             return [
@@ -98,74 +135,108 @@ def generate_launch_description():
                     package='modelec_com',
                     executable='pcb_odo_interface',
                     name='pcb_odo_interface',
-                    parameters=[{
-                        'serial_port': "/dev/USB_ODO",
-                        'baudrate': 115200,
-                        'name': "pcb_odo",
-                    }]
+                    parameters=[param_file],
                 ),
                 Node(
                     package='modelec_com',
                     executable='pcb_action_interface',
                     name='pcb_action_interface',
-                    parameters=[{
-                        'serial_port': "/dev/USB_ACTION",
-                        'baudrate': 115200,
-                        'name': "pcb_action",
-                    }]
+                    parameters=[param_file],
                 ),
             ]
         return []
 
-    # ----------------------------
-    # Strategy nodes
-    # ----------------------------
+    # -------------------------------------------------
+    # Strategy nodes (WITHOUT enemy_manager)
+    # -------------------------------------------------
     def launch_strat(context, *args, **kwargs):
         if context.launch_configurations.get('with_strat') == 'true':
             return [
-                Node(package='modelec_strat', executable='strat_fsm', name='strat_fsm'),
-                Node(package='modelec_strat', executable='pami_manager', name='pami_manager'),
-                Node(package='modelec_strat', executable='enemy_manager', name='enemy_manager'),
+                Node(
+                    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 []
 
-    # ----------------------------
-    # Joy node
-    # ----------------------------
-    def launch_joy(context, *args, **kwargs):
-        if context.launch_configurations.get('with_joy', 'true') == 'true':
-            joy = Node(
-                package='joy',
-                executable='joy_node',
-                name='joy_node',
-                output='screen'
-            )
-            return [joy]
+    # -------------------------------------------------
+    # Enemy manager (standalone)
+    # -------------------------------------------------
+    def launch_enemy_manager(context, *args, **kwargs):
+        if context.launch_configurations.get('with_enemy_manager') == 'true':
+            return [
+                Node(
+                    package='modelec_strat',
+                    executable='enemy_manager',
+                    name='enemy_manager',
+                    parameters=[param_file],
+                )
+            ]
         return []
 
-    # ----------------------------
-    # Final launch description
-    # ----------------------------
+    # -------------------------------------------------
+    # Joystick
+    # -------------------------------------------------
+    def launch_joy(context, *args, **kwargs):
+        if context.launch_configurations.get('with_joy') == 'true':
+            return [
+                Node(
+                    package='joy',
+                    executable='joy_node',
+                    name='joy_node',
+                    parameters=[param_file],
+                )
+            ]
+        return []
+
+    def launch_color_detector(context, *args, **kwargs):
+        if context.launch_configurations.get('with_color_detector') == 'true':
+            return [
+                Node(
+                    package='modelec_com',
+                    executable='color_detector',
+                    name='color_detector',
+                    parameters=[param_file],
+                )
+            ]
+        return []
+
+    # -------------------------------------------------
+    # Final LaunchDescription
+    # -------------------------------------------------
     return LaunchDescription([
-        DeclareLaunchArgument('channel_type', default_value=channel_type, description='Specifying channel type of lidar'),
-        DeclareLaunchArgument('serial_port', default_value=serial_port, description='Specifying usb port to connected lidar'),
-        DeclareLaunchArgument('serial_baudrate', default_value=serial_baudrate, description='Specifying usb port baudrate to connected lidar'),
-        DeclareLaunchArgument('frame_id', default_value=frame_id, description='Specifying frame_id of lidar'),
-        DeclareLaunchArgument('inverted', default_value=inverted, description='Specifying whether or not to invert scan data'),
-        DeclareLaunchArgument('angle_compensate', default_value=angle_compensate, description='Specifying whether or not to enable angle_compensate of scan data'),
-        DeclareLaunchArgument('scan_mode', default_value=scan_mode, description='Specifying scan mode of lidar'),
+        DeclareLaunchArgument('channel_type', default_value=channel_type),
+        DeclareLaunchArgument('serial_port', default_value=serial_port),
+        DeclareLaunchArgument('serial_baudrate', default_value=serial_baudrate),
+        DeclareLaunchArgument('frame_id', default_value=frame_id),
+        DeclareLaunchArgument('inverted', default_value=inverted),
+        DeclareLaunchArgument('angle_compensate', default_value=angle_compensate),
+        DeclareLaunchArgument('scan_mode', default_value=scan_mode),
 
         with_gui_arg,
         with_rplidar_arg,
         with_com_arg,
         with_strat_arg,
+        with_enemy_manager_arg,
         with_joy_arg,
+        with_color_detector_arg,
 
         OpaqueFunction(function=launch_rplidar_restart_if_needed),
         OpaqueFunction(function=launch_gui),
         OpaqueFunction(function=launch_com),
         OpaqueFunction(function=launch_strat),
+        OpaqueFunction(function=launch_enemy_manager),
         OpaqueFunction(function=launch_joy),
+        OpaqueFunction(function=launch_color_detector),
     ])
 
-# to run in debug : , prefix=['xterm -e gdb -ex run --args']
+#  prefix = ['xterm -e gdb -ex run --args']

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

@@ -55,12 +55,8 @@ namespace ModelecGUI
             *deploy_action1_front_servo3_button_,
             *deploy_action1_front_servo4_button_;
 
-        bool deploy_action1_front_servo1_state_,
-            deploy_action1_front_servo2_state_,
-            deploy_action1_front_servo3_state_,
-            deploy_action1_front_servo4_state_;
-
         QPushButton* deploy_action1_front_down_button_;
+        QPushButton* deploy_action1_front_rotate_button_;
 
         // ---- Action1 back ----
         QVBoxLayout* deploy_action1_back_layout_;
@@ -73,12 +69,8 @@ namespace ModelecGUI
             *deploy_action1_back_servo3_button_,
             *deploy_action1_back_servo4_button_;
 
-        bool deploy_action1_back_servo1_state_,
-            deploy_action1_back_servo2_state_,
-            deploy_action1_back_servo3_state_,
-            deploy_action1_back_servo4_state_;
-
         QPushButton* deploy_action1_back_down_button_;
+        QPushButton* deploy_action1_back_rotate_button_;
 
         // ---- Action2 ----
         QVBoxLayout* deploy_action2_layout_;

src/modelec_gui/include/modelec_gui/pages/map_page.hpp

@@ -7,6 +7,7 @@
 #include <QSvgRenderer>
 #include <QVBoxLayout>
 #include <QHBoxLayout>
+#include <QTimer>
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -40,15 +41,19 @@ namespace ModelecGUI {
     protected:
         void paintEvent(QPaintEvent*) override;
 
-        void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
-
         void OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg);
 
         void resizeEvent(QResizeEvent* event) override;
 
+        void updateBackgroundCache();
+
+        void updateObstaclesCache();
+
+        void updateWaypointsCache();
+
         rclcpp::TimerBase::SharedPtr reset_timer_;
 
-        QSvgRenderer* renderer;
+        QSvgRenderer* renderer_;
 
         QVBoxLayout* v_layout;
         QHBoxLayout* h_layout;
@@ -97,5 +102,17 @@ namespace ModelecGUI {
         long int start_time_ = 0;
 
         rclcpp::Subscription<modelec_interfaces::msg::StratState>::SharedPtr strat_state_sub_;
+
+        QPixmap background_cache_;
+        QPixmap obstacles_cache_;
+        QPixmap waypoints_cache_;
+
+        bool bg_dirty_ = true;
+        bool obstacles_dirty_ = true;
+        bool waypoints_dirty_ = true;
+
+        QPixmap robot_texture_;
+        QPixmap top_texture_;
+        QPixmap obs_texture_;
     };
 }

src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp

@@ -54,7 +54,13 @@ namespace ModelecGUI
 
         void resizeEvent(QResizeEvent* event) override;
 
-        QSvgRenderer* renderer;
+        void updateBackgroundCache();
+
+        void updateObstaclesCache();
+
+        void updateWaypointsCache();
+
+        QSvgRenderer* renderer_;
 
         modelec_interfaces::msg::OdometryPos robotPos;
         std::vector<QPoint> qpoints;
@@ -95,5 +101,18 @@ namespace ModelecGUI
         bool hasEnemy = false;
         rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_pub_;
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
+
+        QPixmap background_cache_;
+        QPixmap obstacles_cache_;
+        QPixmap waypoints_cache_;
+
+        bool bg_dirty_ = true;
+        bool obstacles_dirty_ = true;
+        bool waypoints_dirty_ = true;
+
+        QPixmap robot_texture_;
+        QPixmap top_texture_;
+        QPixmap obs_texture_;
+
     };
 }

src/modelec_gui/src/main.cpp

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

@@ -49,6 +49,7 @@ namespace ModelecGUI
 
         deploy_action1_front_up_button_ = new QPushButton("Up");
         deploy_action1_front_down_button_ = new QPushButton("Down");
+        deploy_action1_front_rotate_button_ = new QPushButton("DownRota");
 
         connect(deploy_action1_front_up_button_, &QPushButton::clicked, this,
             [this]()
@@ -62,7 +63,15 @@ namespace ModelecGUI
             [this]()
             {
                 ActionExec action_exec;
-                action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "1";
+                action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "0";
+                action_exec_pub_->publish(action_exec);
+            });
+
+        connect(deploy_action1_front_rotate_button_, &QPushButton::clicked, this,
+            [this]()
+            {
+                ActionExec action_exec;
+                action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "1";
                 action_exec_pub_->publish(action_exec);
             });
 
@@ -76,12 +85,9 @@ namespace ModelecGUI
         connect(deploy_action1_front_servo1_button_, &QPushButton::clicked, this,
             [this]()
             {
-                deploy_action1_front_servo1_state_ = !deploy_action1_front_servo1_state_;
-
                 ActionExec action_exec;
 
-                action_exec.action = (deploy_action1_front_servo1_state_
-                    ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "1";
+                action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "1";
 
                 action_exec_pub_->publish(action_exec);
             });
@@ -89,12 +95,9 @@ namespace ModelecGUI
         connect(deploy_action1_front_servo2_button_, &QPushButton::clicked, this,
             [this]()
             {
-                deploy_action1_front_servo2_state_ = !deploy_action1_front_servo2_state_;
-
                 ActionExec action_exec;
 
-                action_exec.action = (deploy_action1_front_servo2_state_
-                    ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "2";
+                action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "1";
 
                 action_exec_pub_->publish(action_exec);
             });
@@ -102,12 +105,9 @@ namespace ModelecGUI
         connect(deploy_action1_front_servo3_button_, &QPushButton::clicked, this,
             [this]()
             {
-                deploy_action1_front_servo3_state_ = !deploy_action1_front_servo3_state_;
-
                 ActionExec action_exec;
 
-                action_exec.action = (deploy_action1_front_servo3_state_
-                    ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "3";
+                action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "2" + ActionExec::DELIMITER + "1";
 
                 action_exec_pub_->publish(action_exec);
             });
@@ -115,12 +115,9 @@ namespace ModelecGUI
         connect(deploy_action1_front_servo4_button_, &QPushButton::clicked, this,
             [this]()
             {
-                deploy_action1_front_servo4_state_ = !deploy_action1_front_servo4_state_;
-
                 ActionExec action_exec;
 
-                action_exec.action = (deploy_action1_front_servo4_state_
-                    ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "4";
+                action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "3" + ActionExec::DELIMITER + "1";
 
                 action_exec_pub_->publish(action_exec);
             });
@@ -134,12 +131,14 @@ namespace ModelecGUI
         deploy_action1_front_layout_->addWidget(deploy_action1_front_up_button_);
         deploy_action1_front_layout_->addLayout(deploy_action1_front_servos_layout_);
         deploy_action1_front_layout_->addWidget(deploy_action1_front_down_button_);
+        deploy_action1_front_layout_->addWidget(deploy_action1_front_rotate_button_);
 
         deploy_action1_back_layout_ = new QVBoxLayout;
         deploy_action1_back_label_ = new QLabel("Back Action 1");
 
         deploy_action1_back_up_button_ = new QPushButton("Up");
         deploy_action1_back_down_button_ = new QPushButton("Down");
+        deploy_action1_back_rotate_button_ = new QPushButton("DownRota");
 
         connect(deploy_action1_back_up_button_, &QPushButton::clicked, this,
             [this]()
@@ -153,7 +152,15 @@ namespace ModelecGUI
             [this]()
             {
                 ActionExec action_exec;
-                action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "0";
+                action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "0";
+                action_exec_pub_->publish(action_exec);
+            });
+
+        connect(deploy_action1_back_rotate_button_, &QPushButton::clicked, this,
+            [this]()
+            {
+                ActionExec action_exec;
+                action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "1";
                 action_exec_pub_->publish(action_exec);
             });
 
@@ -167,12 +174,9 @@ namespace ModelecGUI
         connect(deploy_action1_back_servo1_button_, &QPushButton::clicked, this,
             [this]()
             {
-                deploy_action1_back_servo1_state_ = !deploy_action1_back_servo1_state_;
-
                 ActionExec action_exec;
 
-                action_exec.action = (deploy_action1_back_servo1_state_
-                    ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "1";
+                action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "0";
 
                 action_exec_pub_->publish(action_exec);
             });
@@ -180,12 +184,9 @@ namespace ModelecGUI
         connect(deploy_action1_back_servo2_button_, &QPushButton::clicked, this,
             [this]()
             {
-                deploy_action1_back_servo2_state_ = !deploy_action1_back_servo2_state_;
-
                 ActionExec action_exec;
 
-                action_exec.action = (deploy_action1_back_servo2_state_
-                    ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "2";
+                action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "0";
 
                 action_exec_pub_->publish(action_exec);
             });
@@ -193,12 +194,9 @@ namespace ModelecGUI
         connect(deploy_action1_back_servo3_button_, &QPushButton::clicked, this,
             [this]()
             {
-                deploy_action1_back_servo3_state_ = !deploy_action1_back_servo3_state_;
-
                 ActionExec action_exec;
 
-                action_exec.action = (deploy_action1_back_servo3_state_
-                    ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "3";
+                action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "2" + ActionExec::DELIMITER + "0";
 
                 action_exec_pub_->publish(action_exec);
             });
@@ -206,12 +204,9 @@ namespace ModelecGUI
         connect(deploy_action1_back_servo4_button_, &QPushButton::clicked, this,
             [this]()
             {
-                deploy_action1_back_servo4_state_ = !deploy_action1_back_servo4_state_;
-
                 ActionExec action_exec;
 
-                action_exec.action = (deploy_action1_back_servo4_state_
-                    ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "4";
+                action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "3" + ActionExec::DELIMITER + "0";
 
                 action_exec_pub_->publish(action_exec);
             });
@@ -225,6 +220,7 @@ namespace ModelecGUI
         deploy_action1_back_layout_->addWidget(deploy_action1_back_up_button_);
         deploy_action1_back_layout_->addLayout(deploy_action1_back_servos_layout_);
         deploy_action1_back_layout_->addWidget(deploy_action1_back_down_button_);
+        deploy_action1_back_layout_->addWidget(deploy_action1_back_rotate_button_);
 
         deploy_action1_layout_->addLayout(deploy_action1_front_layout_);
         deploy_action1_layout_->addLayout(deploy_action1_back_layout_);
@@ -238,7 +234,7 @@ namespace ModelecGUI
             [this]()
             {
                 ActionExec action_exec;
-                action_exec.action = ActionExec::THERMO_DEPLOY;
+                action_exec.action = ActionExec::THERMO + ActionExec::DELIMITER + "-1" + ActionExec::DELIMITER + "1";
                 action_exec_pub_->publish(action_exec);
             });
 
@@ -246,7 +242,7 @@ namespace ModelecGUI
             [this]()
             {
                 ActionExec action_exec;
-                action_exec.action = ActionExec::THERMO_UNDEPLOY;
+                action_exec.action = ActionExec::THERMO + ActionExec::DELIMITER + "-1" + ActionExec::DELIMITER + "0";
                 action_exec_pub_->publish(action_exec);
             });
 

src/modelec_gui/src/pages/home_page.cpp

@@ -2,7 +2,6 @@
 #include <modelec_gui/pages/home_page.hpp>
 
 #include <QVBoxLayout>
-#include <modelec_utils/config.hpp>
 
 namespace ModelecGUI
 {

src/modelec_gui/src/pages/map_page.cpp

@@ -3,15 +3,18 @@
 
 #include <QMouseEvent>
 #include <utility>
-#include <modelec_utils/config.hpp>
 #include <cmath>
 
 namespace ModelecGUI
 {
     MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent),
-                                                                      renderer(new QSvgRenderer(
+                                                                      renderer_(new QSvgRenderer(
                                                                           QString(":/img/playmat/2026_FINAL.svg"),
-                                                                          this)), node_(node)
+                                                                          this)),
+                                                                      node_(node),
+                                                                      robot_texture_(":/img/logo/modelec.png"),
+                                                                      top_texture_(":/img/logo/ISEN-Nantes.png"),
+                                                                      obs_texture_(":/img/wood.jpg")
     {
         ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
         ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
@@ -45,11 +48,14 @@ namespace ModelecGUI
 
         qpoints = {};
 
-        robot_length_ = Modelec::Config::get<int>("config.robot.size.length_mm", 200);
-        robot_width_ = Modelec::Config::get<int>("config.robot.size.width_mm", 300);
+        map_height_ = node_->get_parameter("map.size.map_height_mm").as_int();
+        map_width_ = node_->get_parameter("map.size.map_width_mm").as_int();
 
-        enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
-        enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
+        robot_length_ = node_->get_parameter("robot.size.length_mm").as_int();
+        robot_width_ = node_->get_parameter("robot.size.width_mm").as_int();
+
+        enemy_length_ = node_->get_parameter("enemy.size.length_mm").as_int();
+        enemy_width_ = node_->get_parameter("enemy.size.width_mm").as_int();
 
         add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryWaypoint>(
             "odometry/add_waypoint", 100,
@@ -71,6 +77,8 @@ namespace ModelecGUI
 
                 qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_,
                                          height() - msg->y * ratioBetweenMapAndWidgetY_));
+
+                waypoints_dirty_ = true;
                 update();
             });
 
@@ -90,6 +98,7 @@ namespace ModelecGUI
                                          height() - point.y * ratioBetweenMapAndWidgetY_));
             }
 
+            waypoints_dirty_ = true;
             update();
         });
 
@@ -113,6 +122,8 @@ namespace ModelecGUI
             [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg)
             {
                 OnObstacleReceived(msg);
+                obstacles_dirty_ = true;
+                update();
             });
 
         obstacle_removed_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>(
@@ -120,6 +131,8 @@ namespace ModelecGUI
             [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg)
             {
                 obstacle_.erase(msg->id);
+                obstacles_dirty_ = true;
+                update();
             });
 
         enemy_pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>("enemy/position", 10,
@@ -185,6 +198,14 @@ namespace ModelecGUI
 
         auto result2 = ask_map_obstacle_client_->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
         rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result2);
+
+        auto timer = new QTimer(this);
+        connect(timer, &QTimer::timeout, this, [this]() {
+            if (!isGameStarted_) return;
+            auto elapsed = (std::chrono::system_clock::now().time_since_epoch().count() - start_time_) / 1e9;
+            timer_label_->setText(QString("%1 s").arg(elapsed));
+        });
+        timer->start(1000);
     }
 
     void MapPage::AskMap()
@@ -228,136 +249,49 @@ namespace ModelecGUI
     {
         QWidget::paintEvent(paint_event);
 
-        if (isGameStarted_)
-        {
-            auto now = std::chrono::system_clock::now().time_since_epoch();
-            auto start = std::chrono::nanoseconds(start_time_);
-            auto elapsed = now - start;
-            auto elapsed_s = std::chrono::duration_cast<std::chrono::seconds>(elapsed).count();
-            timer_label_->setText(QString::number(elapsed_s) + " s");
-        }
-
         QPainter painter(this);
-        renderer->render(&painter, rect()); // Scales SVG to widget size
-        painter.save();
 
-        painter.setRenderHint(QPainter::Antialiasing);
-
-        // --- Draw lines ---
-        painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide
-        for (size_t i = 0; i + 1 < qpoints.size(); ++i)
-        {
-            painter.drawLine(qpoints[i], qpoints[i + 1]);
-        }
-
-        painter.setPen(Qt::NoPen);
-
-        // --- Draw colored points ---
-        const int radius = 5;
-
-        for (size_t i = 0; i < qpoints.size(); ++i)
-        {
-            if (i == qpoints.size() - 1)
-                painter.setBrush(Qt::blue); // Last = blue
-            else
-                painter.setBrush(Qt::red); // Middle = red
-
-            painter.drawEllipse(qpoints[i], radius, radius);
-        }
-
-        painter.restore();
+        if (bg_dirty_) updateBackgroundCache();
+        painter.drawPixmap(0, 0, background_cache_);
 
         if (show_obstacle_)
         {
-            for (auto& [index, obs] : obstacle_)
-            {
-                painter.save();
-
-                QPoint obsPoint(obs.x * ratioBetweenMapAndWidgetX_, height() - obs.y * ratioBetweenMapAndWidgetY_);
-                painter.translate(obsPoint);
-                painter.rotate(90 - obs.theta * (180.0 / M_PI));
-
-                if (obs.type == modelec_interfaces::msg::Obstacle::GRADIN)
-                {
-                    QPixmap texture(":/img/wood.jpg");
-                    painter.setBrush(QBrush(texture));
-                }
-                else if (obs.id == 2)
-                {
-                    QPixmap texture(":/img/logo/ISEN-Nantes.png");
-                    texture = texture.scaled(obs.width * ratioBetweenMapAndWidgetX_,
-                                           obs.height * ratioBetweenMapAndWidgetY_, Qt::KeepAspectRatio);
-
-                    QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height());
-
-                    QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2),
-                         -(obs.height * ratioBetweenMapAndWidgetY_ / 2),
-                         obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_);
-
-                    painter.setBrush(Qt::white);
-                    painter.setPen(Qt::NoPen);
-                    painter.drawRect(toDraw);
-
-                    painter.drawPixmap(imageRect.topLeft(), texture);
-
-                    painter.restore();
-
-                    continue;
-                }
-                else if (obs.type == modelec_interfaces::msg::Obstacle::ESTRADE)
-                {
-                    painter.setBrush(Qt::white);
-                    painter.setPen(Qt::NoPen);
-                }
-                else
-                {
-                    painter.setBrush(Qt::red);
-                    painter.setOpacity(0.5);
-                    painter.setPen(QPen(Qt::red, 5));
-                }
-
-                QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2),
-                             -(obs.height * ratioBetweenMapAndWidgetY_ / 2),
-                             obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_);
-
-                painter.drawRect(toDraw);
-
-                painter.restore();
-            }
-
-            // -- Draw enemy position --
-            if (hasEnemy)
-            {
-                painter.setBrush(Qt::red);
-                painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_,
-                                 height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_,
-                                 enemy_width_ * ratioBetweenMapAndWidgetX_, enemy_length_ * ratioBetweenMapAndWidgetY_);
-            }
+            if (obstacles_dirty_) updateObstaclesCache();
+            painter.drawPixmap(0, 0, obstacles_cache_);
         }
 
-        // -- Draw robot position --
-        painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_);
-        painter.rotate(90 - robotPos.theta * (180.0 / M_PI));
+        if (waypoints_dirty_) updateWaypointsCache();
+        painter.drawPixmap(0, 0, waypoints_cache_);
 
-        QPixmap texture(":/img/logo/modelec.png");
-        texture = texture.scaled(robot_width_ * ratioBetweenMapAndWidgetX_ * 0.9,
-                               robot_length_ * ratioBetweenMapAndWidgetY_ * 0.9, Qt::KeepAspectRatio);
+        // --- robot ---
+        painter.save();
+        painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_,
+                          height() - robotPos.y * ratioBetweenMapAndWidgetY_);
+        painter.rotate(90 - robotPos.theta * 180.0 / M_PI);
 
-        QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height());
+        QRect r(-(robot_width_*ratioBetweenMapAndWidgetX_)/2, -(robot_length_*ratioBetweenMapAndWidgetY_)/2,
+                (robot_width_*ratioBetweenMapAndWidgetX_), (robot_length_*ratioBetweenMapAndWidgetY_));
 
-        QRect rect(-(robot_width_ * ratioBetweenMapAndWidgetX_ / 2), -(robot_length_ * ratioBetweenMapAndWidgetY_ / 2),
-                           robot_width_ * ratioBetweenMapAndWidgetX_, robot_length_ * ratioBetweenMapAndWidgetY_);
+        painter.drawPixmap(r, robot_texture_);
+        painter.restore();
 
-        painter.setBrush(Qt::white);
-        painter.setPen(QPen(Qt::black, 5));
-        painter.drawRect(rect);
-
-        painter.drawPixmap(imageRect.topLeft(), texture);
+        // --- Enemy ---
+        if (hasEnemy)
+        {
+            painter.setBrush(Qt::red);
+            painter.drawRect(
+                (enemy_pos_.x - enemy_width_/2) * ratioBetweenMapAndWidgetX_,
+                height() - (enemy_pos_.y + enemy_length_/2) * ratioBetweenMapAndWidgetY_,
+                enemy_width_ * ratioBetweenMapAndWidgetX_,
+                enemy_length_ * ratioBetweenMapAndWidgetY_);
+        }
     }
 
     void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg)
     {
         obstacle_[msg->id] = *msg;
+        obstacles_dirty_ = true;
+        update();
     }
 
     void MapPage::resizeEvent(QResizeEvent* event)
@@ -366,5 +300,109 @@ namespace ModelecGUI
 
         ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
         ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
+
+        bg_dirty_ = true;
+        obstacles_dirty_ = true;
+        waypoints_dirty_ = true;
+        update();
+    }
+
+    void MapPage::updateBackgroundCache()
+    {
+        background_cache_ = QPixmap(size());
+        background_cache_.fill(Qt::transparent);
+
+        QPainter p(&background_cache_);
+        renderer_->render(&p, rect());
+        bg_dirty_ = false;
+    }
+
+    void MapPage::updateObstaclesCache()
+    {
+        obstacles_cache_ = QPixmap(size());
+        obstacles_cache_.fill(Qt::transparent);
+
+        QPainter painter(&obstacles_cache_);
+        painter.setRenderHint(QPainter::Antialiasing);
+
+        for (auto& [index, obs] : obstacle_)
+        {
+            painter.save();
+
+            QPoint pos(obs.x * ratioBetweenMapAndWidgetX_,
+                       height() - obs.y * ratioBetweenMapAndWidgetY_);
+            painter.translate(pos);
+            painter.rotate(90 - obs.theta * 180.0 / M_PI);
+
+            if (obs.type == modelec_interfaces::msg::Obstacle::GRADIN)
+            {
+                painter.setBrush(QBrush(obs_texture_));
+            }
+            else if (obs.id == 2)
+            {
+
+                auto texture = top_texture_.scaled(obs.width * ratioBetweenMapAndWidgetX_,
+                                       obs.height * ratioBetweenMapAndWidgetY_, Qt::KeepAspectRatio);
+
+                QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height());
+
+                QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2),
+                     -(obs.height * ratioBetweenMapAndWidgetY_ / 2),
+                     obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_);
+
+                painter.setBrush(Qt::white);
+                painter.setPen(Qt::NoPen);
+                painter.drawRect(toDraw);
+
+                painter.drawPixmap(imageRect.topLeft(), texture);
+
+                painter.restore();
+
+                continue;
+            }
+            else if (obs.type == modelec_interfaces::msg::Obstacle::ESTRADE)
+            {
+                painter.setBrush(Qt::white);
+                painter.setPen(Qt::NoPen);
+            }
+            else
+            {
+                painter.setBrush(Qt::red);
+                painter.setOpacity(0.5);
+                painter.setPen(QPen(Qt::red, 5));
+            }
+
+            QRect r(-(obs.width * ratioBetweenMapAndWidgetX_ / 2),
+                    -(obs.height * ratioBetweenMapAndWidgetY_ / 2),
+                     obs.width * ratioBetweenMapAndWidgetX_,
+                     obs.height * ratioBetweenMapAndWidgetY_);
+
+            painter.drawRect(r);
+
+            painter.restore();
+        }
+
+        obstacles_dirty_ = false;
+    }
+
+    void MapPage::updateWaypointsCache()
+    {
+        waypoints_cache_ = QPixmap(size());
+        waypoints_cache_.fill(Qt::transparent);
+
+        QPainter painter(&waypoints_cache_);
+        painter.setRenderHint(QPainter::Antialiasing);
+        painter.setPen(QPen(Qt::red, 2));
+
+        for (size_t i = 0; i + 1 < qpoints.size(); ++i)
+            painter.drawLine(qpoints[i], qpoints[i + 1]);
+
+        painter.setPen(Qt::NoPen);
+        painter.setBrush(Qt::red);
+
+        for (auto& p : qpoints)
+            painter.drawEllipse(p, 5, 5);
+
+        waypoints_dirty_ = false;
     }
 }

src/modelec_gui/src/pages/test_map_page.cpp

@@ -3,27 +3,33 @@
 
 #include <QMouseEvent>
 #include <utility>
-#include <modelec_utils/config.hpp>
 #include <cmath>
 
 namespace ModelecGUI
 {
     TestMapPage::TestMapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent),
-                                                                              renderer(new QSvgRenderer(
+                                                                              renderer_(new QSvgRenderer(
                                                                                   QString(
                                                                                       ":/img/playmat/2026_FINAL.svg"),
-                                                                                  this)), node_(node)
+                                                                                  this)),
+                                                                              node_(node),
+                                                                              robot_texture_(":/img/logo/modelec.png"),
+                                                                              top_texture_(":/img/logo/ISEN-Nantes.png"),
+                                                                              obs_texture_(":/img/wood.jpg")
     {
         ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
         ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
 
         qpoints = {};
 
-        robot_length_ = Modelec::Config::get<int>("config.robot.size.length_mm", 200);
-        robot_width_ = Modelec::Config::get<int>("config.robot.size.width_mm", 300);
+        map_height_ = node_->get_parameter("map.size.map_height_mm").as_int();
+        map_width_ = node_->get_parameter("map.size.map_width_mm").as_int();
 
-        enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
-        enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
+        robot_length_ = node_->get_parameter("robot.size.length_mm").as_int();
+        robot_width_ = node_->get_parameter("robot.size.width_mm").as_int();
+
+        enemy_length_ = node_->get_parameter("enemy.size.length_mm").as_int();
+        enemy_width_ = node_->get_parameter("enemy.size.width_mm").as_int();
 
         add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryWaypoint>(
             "odometry/add_waypoint", 100,
@@ -45,6 +51,8 @@ namespace ModelecGUI
 
                 qpoints.emplace_back(msg->x * ratioBetweenMapAndWidgetX_,
                                          height() - msg->y * ratioBetweenMapAndWidgetY_);
+
+                waypoints_dirty_ = true;
                 update();
             });
 
@@ -64,6 +72,7 @@ namespace ModelecGUI
                                              height() - point.y * ratioBetweenMapAndWidgetY_);
                 }
 
+                waypoints_dirty_ = true;
                 update();
             });
 
@@ -85,6 +94,8 @@ namespace ModelecGUI
             [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg)
             {
                 obstacle_.erase(msg->id);
+                obstacles_dirty_ = true;
+                update();
             });
 
         go_to_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryGoTo>("nav/go_to", 10);
@@ -142,13 +153,15 @@ namespace ModelecGUI
 
     void TestMapPage::setPlaymatGrid()
     {
-        renderer->load(QString(":/img/playmat/grid_v1.svg"));
+        renderer_->load(QString(":/img/playmat/grid_v1.svg"));
+        bg_dirty_ = true;
         update();
     }
 
     void TestMapPage::setPlaymatMap()
     {
-        renderer->load(QString(":/img/playmat/2026_FINAL.svg"));
+        renderer_->load(QString(":/img/playmat/2026_FINAL.svg"));
+        bg_dirty_ = true;
         update();
     }
 
@@ -173,73 +186,41 @@ namespace ModelecGUI
         QWidget::paintEvent(paint_event);
 
         QPainter painter(this);
-        renderer->render(&painter, rect()); // Scales SVG to widget size
-        painter.save();
 
-        painter.setRenderHint(QPainter::Antialiasing);
-
-        // --- Draw lines ---
-        painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide
-        for (size_t i = 0; i + 1 < qpoints.size(); ++i)
-        {
-            painter.drawLine(qpoints[i], qpoints[i + 1]);
-        }
-
-        painter.setPen(Qt::NoPen);
-
-        // --- Draw colored points ---
-        const int radius = 5;
-
-        for (size_t i = 0; i < qpoints.size(); ++i)
-        {
-            if (i == qpoints.size() - 1)
-                painter.setBrush(Qt::blue); // Last = blue
-            else
-                painter.setBrush(Qt::red); // Middle = red
-
-            painter.drawEllipse(qpoints[i], radius, radius);
-        }
-
-        painter.restore();
+        if (bg_dirty_) updateBackgroundCache();
+        painter.drawPixmap(0, 0, background_cache_);
 
         if (show_obstacle_)
         {
-            for (auto& [index, obs] : obstacle_)
-            {
-                painter.save();
-
-                QPoint obsPoint(obs.x * ratioBetweenMapAndWidgetX_, height() - obs.y * ratioBetweenMapAndWidgetY_);
-                painter.translate(obsPoint);
-                painter.rotate(90 - obs.theta * (180.0 / M_PI));
-                painter.setBrush(Qt::black);
-
-                QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2),
-                             -(obs.height * ratioBetweenMapAndWidgetY_ / 2),
-                             obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_);
-
-                painter.drawRect(toDraw);
-
-                painter.restore();
-            }
-
-            // -- Draw enemy position --
-            if (hasEnemy)
-            {
-                painter.setBrush(Qt::red);
-                painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_,
-                                 height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_,
-                                 enemy_width_ * ratioBetweenMapAndWidgetX_, enemy_length_ * ratioBetweenMapAndWidgetY_);
-            }
+            if (obstacles_dirty_) updateObstaclesCache();
+            painter.drawPixmap(0, 0, obstacles_cache_);
         }
 
-        // -- Draw robot position --
-        painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_);
-        painter.rotate(90 - robotPos.theta * (180.0 / M_PI));
+        if (waypoints_dirty_) updateWaypointsCache();
+        painter.drawPixmap(0, 0, waypoints_cache_);
 
-        QRect rect(-(robot_width_ * ratioBetweenMapAndWidgetX_ / 2), -(robot_length_ * ratioBetweenMapAndWidgetY_ / 2),
-                   robot_width_ * ratioBetweenMapAndWidgetX_, robot_length_ * ratioBetweenMapAndWidgetY_);
-        painter.setBrush(Qt::green);
-        painter.drawRect(rect);
+        // --- robot ---
+        painter.save();
+        painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_,
+                          height() - robotPos.y * ratioBetweenMapAndWidgetY_);
+        painter.rotate(90 - robotPos.theta * 180.0 / M_PI);
+
+        QRect r(-(robot_width_*ratioBetweenMapAndWidgetX_)/2, -(robot_length_*ratioBetweenMapAndWidgetY_)/2,
+                (robot_width_*ratioBetweenMapAndWidgetX_), (robot_length_*ratioBetweenMapAndWidgetY_));
+
+        painter.drawPixmap(r, robot_texture_);
+        painter.restore();
+
+        // --- Enemy ---
+        if (hasEnemy)
+        {
+            painter.setBrush(Qt::red);
+            painter.drawRect(
+                (enemy_pos_.x - enemy_width_/2) * ratioBetweenMapAndWidgetX_,
+                height() - (enemy_pos_.y + enemy_length_/2) * ratioBetweenMapAndWidgetY_,
+                enemy_width_ * ratioBetweenMapAndWidgetX_,
+                enemy_length_ * ratioBetweenMapAndWidgetY_);
+        }
     }
 
     void TestMapPage::mousePressEvent(QMouseEvent* event)
@@ -279,6 +260,8 @@ namespace ModelecGUI
     void TestMapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg)
     {
         obstacle_.emplace(msg->id, *msg);
+        obstacles_dirty_ = true;
+        update();
     }
 
     void TestMapPage::resizeEvent(QResizeEvent* event)
@@ -287,5 +270,109 @@ namespace ModelecGUI
 
         ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
         ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
+
+        bg_dirty_ = true;
+        obstacles_dirty_ = true;
+        waypoints_dirty_ = true;
+        update();
+    }
+
+    void TestMapPage::updateBackgroundCache()
+    {
+        background_cache_ = QPixmap(size());
+        background_cache_.fill(Qt::transparent);
+
+        QPainter p(&background_cache_);
+        renderer_->render(&p, rect());
+        bg_dirty_ = false;
+    }
+
+    void TestMapPage::updateObstaclesCache()
+    {
+        obstacles_cache_ = QPixmap(size());
+        obstacles_cache_.fill(Qt::transparent);
+
+        QPainter painter(&obstacles_cache_);
+        painter.setRenderHint(QPainter::Antialiasing);
+
+        for (auto& [index, obs] : obstacle_)
+        {
+            painter.save();
+
+            QPoint pos(obs.x * ratioBetweenMapAndWidgetX_,
+                       height() - obs.y * ratioBetweenMapAndWidgetY_);
+            painter.translate(pos);
+            painter.rotate(90 - obs.theta * 180.0 / M_PI);
+
+            if (obs.type == modelec_interfaces::msg::Obstacle::GRADIN)
+            {
+                painter.setBrush(QBrush(obs_texture_));
+            }
+            else if (obs.id == 2)
+            {
+
+                auto texture = top_texture_.scaled(obs.width * ratioBetweenMapAndWidgetX_,
+                                       obs.height * ratioBetweenMapAndWidgetY_, Qt::KeepAspectRatio);
+
+                QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height());
+
+                QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2),
+                     -(obs.height * ratioBetweenMapAndWidgetY_ / 2),
+                     obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_);
+
+                painter.setBrush(Qt::white);
+                painter.setPen(Qt::NoPen);
+                painter.drawRect(toDraw);
+
+                painter.drawPixmap(imageRect.topLeft(), texture);
+
+                painter.restore();
+
+                continue;
+            }
+            else if (obs.type == modelec_interfaces::msg::Obstacle::ESTRADE)
+            {
+                painter.setBrush(Qt::white);
+                painter.setPen(Qt::NoPen);
+            }
+            else
+            {
+                painter.setBrush(Qt::red);
+                painter.setOpacity(0.5);
+                painter.setPen(QPen(Qt::red, 5));
+            }
+
+            QRect r(-(obs.width * ratioBetweenMapAndWidgetX_ / 2),
+                    -(obs.height * ratioBetweenMapAndWidgetY_ / 2),
+                     obs.width * ratioBetweenMapAndWidgetX_,
+                     obs.height * ratioBetweenMapAndWidgetY_);
+
+            painter.drawRect(r);
+
+            painter.restore();
+        }
+
+        obstacles_dirty_ = false;
+    }
+
+    void TestMapPage::updateWaypointsCache()
+    {
+        waypoints_cache_ = QPixmap(size());
+        waypoints_cache_.fill(Qt::transparent);
+
+        QPainter painter(&waypoints_cache_);
+        painter.setRenderHint(QPainter::Antialiasing);
+        painter.setPen(QPen(Qt::red, 2));
+
+        for (size_t i = 0; i + 1 < qpoints.size(); ++i)
+            painter.drawLine(qpoints[i], qpoints[i + 1]);
+
+        painter.setPen(Qt::NoPen);
+        painter.setBrush(Qt::red);
+
+        for (auto& p : qpoints)
+            painter.drawEllipse(p, 5, 5);
+
+        waypoints_dirty_ = false;
     }
 }

src/modelec_interfaces/msg/Action/ActionExec.msg

@@ -3,10 +3,13 @@ string DELIMITER = ";"
 # Action
 string UP = "UP"
 string DOWN = "DOWN"
+string TOGGLE_ARM = "TOGGLE_ARM"
+string ROTATE_ARM = "ROTATE_ARM"
 string TAKE = "TAKE"
 string FREE = "FREE"
-string THERMO_DEPLOY = "THERMO_DEPLOY"
-string THERMO_UNDEPLOY = "THERMO_UNDEPLOY"
+string TOGGLE_SERVO = "TOGGLE_SERVO"
+string THERMO = "THERMO"
+string LOOK_ON = "LOOK_ON"
 string MAX_DEPLOY = "MAX_DEPLOY"
 
 # Step
@@ -15,8 +18,11 @@ int32 UP_STEP = 1
 int32 DOWN_STEP = 2
 int32 TAKE_STEP = 3
 int32 FREE_STEP = 4
-int32 THERMO_DEPLOY_STEP = 5
-int32 THERMO_UNDEPLOY_STEP = 6
+int32 THERMO_STEP = 5
+int32 TOGGLE_SERVO_STEP = 7
+int32 TOGGLE_ARM_STEP = 8
+int32 ROTATE_ARM_STEP = 9
+int32 LOOK_ON_STEP = 10
 int32 MAX_DEPLOY_STEP = 99
 
 int32[] steps

src/modelec_interfaces/msg/Strat/StratState.msg

@@ -5,6 +5,7 @@ int32 SELECT_GAME_ACTION=3
 
 int32 TAKE_MISSION=10
 int32 FREE_MISSION=11
+int32 THERMO_MISSION=12
 
 int32 DO_GO_HOME=20
 int32 STOP=21

src/modelec_strat/CMakeLists.txt

@@ -25,10 +25,14 @@ set(strat_fsm_sources
         src/action/down_action.cpp
         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/obstacle/obstacle.cpp
         src/obstacle/box.cpp

src/modelec_strat/data/config.xml

@@ -12,7 +12,7 @@
             <min_emergency_distance_mm>300</min_emergency_distance_mm>
             <margin_detection_table_mm>50</margin_detection_table_mm>
         </detection>
-        <factor_close_enemy>-0.3</factor_close_enemy>
+        <factor_close_enemy>-2</factor_close_enemy>
     </enemy>
 
     <robot>
@@ -36,8 +36,8 @@
     <home>
         <!--<blue x="2650" y="1600" theta="-1.5708"/>
         <yellow x="350" y="1600" theta="-1.5708"/>-->
-        <blue x="2650" y="1750" theta="-1.5708"/>
-        <yellow x="350" y="1750" theta="-1.5708"/>
+        <blue x="2650" y="1700" theta="-1.5708"/>
+        <yellow x="350" y="1700" theta="-1.5708"/>
     </home>
     <spawn>
         <yellow>
@@ -49,27 +49,47 @@
         <width_mm>450</width_mm>
         <height_mm>450</height_mm>
     </spawn>
-    <usb>
-        <timeout_ms>300</timeout_ms>
-        <attempt>5</attempt>
-    </usb>
     <mission_score>
-        <banner>20</banner>
-        <go_home>10</go_home>
-        <concert>
-            <niv_1>4</niv_1>
-            <niv_2>8</niv_2>
-            <niv_3>16</niv_3>
-        </concert>
-        <pami>
-            <goupie>5</goupie>
-            <superstar>5</superstar>
-            <all_party>10</all_party>
-        </pami>
     </mission_score>
     <pami>
         <time_to_put_zone>77</time_to_put_zone>
         <time_to_remove_top_pot>65</time_to_remove_top_pot>
     </pami>
-    <static_strat>true</static_strat>
+    <static_strat>false</static_strat>
+    <factor>
+        <theta>20</theta>
+        <obs>0.6</obs>
+        <thermo>0.7</thermo>
+    </factor>
+    <timer_period_ms>20</timer_period_ms>
+    <thermo>
+        <yellow>
+            <start x="200" y="175" theta="0" />
+            <finish x="700" y="175" theta="0" />
+        </yellow>
+        <blue>
+            <start x="2800" y="175" theta="3.14" />
+            <finish x="2300" y="175" theta="3.14" />
+        </blue>
+    </thermo>
+    <cam>
+        <save_to_file>
+            <enabled>true</enabled>
+            <path>./cam/</path>
+        </save_to_file>
+        <enabled>true</enabled>
+        <!--<link>/dev/video0</link>-->
+        <link>http://192.168.1.21:8080/video</link>
+        <headless>true</headless>
+        <rois>
+            <roi x="300" y="700" w="50" h="50" />
+            <roi x="700" y="700" w="50" h="50" />
+            <roi x="1200" y="700" w="50" h="50" />
+            <roi x="1500" y="700" w="50" h="50" />
+        </rois>
+        <colors>
+            <color name="blue" hue_min="90" hue_max="130" />
+            <color name="yellow" hue_min="20" hue_max="40" />
+        </colors>
+    </cam>
 </config>

src/modelec_strat/data/obstacles.xml

@@ -1,8 +1,6 @@
 <Obstacles>
     <!-- TOP -->
-    <Obstacle id="1" x="1500" y="1800" theta="1.5708" width="1800" height="400" type="estrade"/>
-    <Obstacle id="2" x="1500" y="1775" theta="1.5708" width="900" height="450" type="estrade"/>
-    <Obstacle id="3" x="2150" y="1900" theta="1.5708" width="400" height="200" type="estrade"/>
+    <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" />

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

@@ -25,11 +25,14 @@ namespace Modelec
     class BaseAction
     {
     public:
-        enum Front
+        enum Side
         {
             FRONT = 1,
             BACK = 0,
             BOTH = -1,
+            LEFT = 2,
+            RIGHT = 3,
+            CENTER = 4,
         };
 
         using Ptr = std::shared_ptr<BaseAction>;
@@ -55,6 +58,8 @@ namespace Modelec
             const std::string& action_name,
             const std::shared_ptr<ActionExecutor>& action_executor);
 
+        virtual void End() = 0;
+
     protected:
         std::shared_ptr<ActionExecutor> action_executor_;
 

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

@@ -9,16 +9,21 @@ namespace Modelec
     {
     public:
         DownAction(const std::shared_ptr<ActionExecutor>& action_executor);
-        DownAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front);
+        DownAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, bool inverted = false);
 
         void Next() override;
         void Init(const std::vector<std::string>& params) override;
-        void SetFront(Front front);
+        void SetSide(Side side);
+        void SetInverted(bool inverted);
+
+        void End() override;
 
         inline static const std::string Name = ActionExec::DOWN;
 
     private:
-        Front front_;
+        Side side_;
+
+        bool inverted_;
 
         std::queue<int> steps_;
 

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

@@ -9,20 +9,22 @@ namespace Modelec
     {
     public:
         FreeAction(const std::shared_ptr<ActionExecutor>& action_executor);
-        FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front, int n);
-        FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Front> servo);
-        FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Front>> servos);
+        FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, int n);
+        FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Side> servo);
+        FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Side>> servos);
 
         void Next() override;
         void Init(const std::vector<std::string>& params) override;
-        void AddServo(int id, Front front);
-        void AddServo(std::pair<int, Front> servo);
-        void AddServos(const std::vector<std::pair<int, Front>>& servos);
+        void AddServo(int id, Side side);
+        void AddServo(std::pair<int, Side> servo);
+        void AddServos(const std::vector<std::pair<int, Side>>& servos);
+
+        void End() override;
 
         inline static const std::string Name = ActionExec::FREE;
 
     private:
-        std::vector<std::pair<int, Front>> servos_;
+        std::vector<std::pair<int, Side>> servos_;
 
         std::queue<int> steps_;
 

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

@@ -0,0 +1,38 @@
+#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

@@ -9,20 +9,22 @@ namespace Modelec
     {
     public:
         TakeAction(const std::shared_ptr<ActionExecutor>& action_executor);
-        TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front, int n);
-        TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Front> servo);
-        TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Front>> servos);
+        TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, int n);
+        TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Side> servo);
+        TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Side>> servos);
 
         void Next() override;
         void Init(const std::vector<std::string>& params) override;
-        void AddServo(int id, Front front);
-        void AddServo(std::pair<int, Front> servo);
-        void AddServos(const std::vector<std::pair<int, Front>>& servos);
+        void AddServo(int id, Side side);
+        void AddServo(std::pair<int, Side> servo);
+        void AddServos(const std::vector<std::pair<int, Side>>& servos);
+
+        void End() override;
 
         inline static const std::string Name = ActionExec::TAKE;
 
     private:
-        std::vector<std::pair<int, Front>> servos_;
+        std::vector<std::pair<int, Side>> servos_;
 
         std::queue<int> steps_;
 

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

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

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

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <queue>
+#include <modelec_strat/action/base_action.hpp>
+
+namespace Modelec
+{
+    class ToggleServoAction : public BaseAction
+    {
+    public:
+        ToggleServoAction(const std::shared_ptr<ActionExecutor>& action_executor);
+        ToggleServoAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, int n);
+        ToggleServoAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Side> servo);
+        ToggleServoAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Side>> servos);
+
+        void Next() override;
+        void Init(const std::vector<std::string>& params) override;
+        void AddServo(int id, Side side);
+        void AddServo(std::pair<int, Side> servo);
+        void AddServos(const std::vector<std::pair<int, Side>>& servos);
+
+        void End() override;
+
+        inline static const std::string Name = ActionExec::TOGGLE_SERVO;
+
+    private:
+        std::vector<std::pair<int, Side>> servos_;
+
+        std::queue<int> steps_;
+
+        inline static const bool registered_ =
+        []()
+        {
+            BaseAction::Registry()[Name] =
+                [](const std::shared_ptr<ActionExecutor>& exec)
+                {
+                    return std::make_shared<ToggleServoAction>(exec);
+                };
+            return true;
+        }();
+    };
+}

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

@@ -9,16 +9,18 @@ namespace Modelec
     {
     public:
         UPAction(const std::shared_ptr<ActionExecutor>& action_executor);
-        UPAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front);
+        UPAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side);
 
         void Next() override;
         void Init(const std::vector<std::string>& params) override;
-        void SetFront(Front front);
+        void SetSide(Side side);
+
+        void End() override;
 
         inline static const std::string Name = ActionExec::UP;
 
     private:
-        Front front_;
+        Side side_;
 
         std::queue<int> steps_;
 

src/modelec_strat/include/modelec_strat/action_executor.hpp

@@ -8,8 +8,12 @@
 #include <modelec_interfaces/msg/action_exec.hpp>
 
 #include <sensor_msgs/msg/joy.hpp>
+#include <std_msgs/msg/int64.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
@@ -31,25 +35,66 @@ namespace Modelec
 
         void ReInit();
 
-        void Down(BaseAction::Front front, bool force = false);
+        void Down(BaseAction::Side side, bool force = false, bool inverted = false);
 
-        void Up(BaseAction::Front front, bool force = false);
+        void Up(BaseAction::Side side, bool force = false);
 
-        void Take(const std::vector<std::pair<int, BaseAction::Front>>& servos, bool force = false);
+        void ToggleArm(BaseAction::Side side, bool force = false);
 
-        void Free(const std::vector<std::pair<int, BaseAction::Front>>& servos, bool force = false);
+        void RotateArm(BaseAction::Side side, bool force = false, bool rotated = false);
+
+        void Take(const std::vector<std::pair<int, BaseAction::Side>>& servos);
+
+        void Free(const std::vector<std::pair<int, BaseAction::Side>>& servos);
+
+        void ToggleServo(const std::vector<std::pair<int, BaseAction::Side>>& servos);
 
         void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg);
 
         void MoveServo(const modelec_interfaces::msg::ActionServoPosArray& msg);
 
+        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_;
 
+        std::array<bool, 8> servo_pos_;
+
+        struct ArmState
+        {
+            bool down;
+            bool rotated;
+        };
+        std::array<ArmState, 2> arm_pos_;
+
+        std::array<float, 16> servo_value_ = {
+            2.91,
+            0.95,
+            3.2,
+            0,
+            1,
+            1,
+            1,
+            1,
+        };
+
+        std::map<BaseAction::Side, bool> thermo_state_ = {
+            {BaseAction::LEFT, false},
+            {BaseAction::RIGHT, false},
+        };
+
+        BaseAction::Side cam_side_ = BaseAction::Side::CENTER;
+
+        bool looking_on_front_ = true;
+
         bool IsEmpty() const;
 
         bool IsFull() const;
 
-        bool HasBox(BaseAction::Front front) const;
+        bool HasBox(BaseAction::Side side) const;
 
         bool HasFrontBox() const;
 
@@ -57,6 +102,10 @@ namespace Modelec
 
         bool HasOneBox() const;
 
+        void SendPoint(int point) const;
+
+        void AskColor() const;
+
     protected:
         rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_pub_;
         rclcpp::Publisher<modelec_interfaces::msg::ActionServoPosArray>::SharedPtr servo_move_pub_;
@@ -68,10 +117,17 @@ namespace Modelec
         rclcpp::Subscription<modelec_interfaces::msg::ActionRelayStateArray>::SharedPtr relay_move_res_sub_;
         rclcpp::Subscription<modelec_interfaces::msg::ActionServoTimedArray>::SharedPtr servo_timed_move_res_sub_;
 
+        rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr ask_color_client_;
+
         rclcpp::Subscription<modelec_interfaces::msg::ActionExec>::SharedPtr action_exec_sub_;
         rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
 
-        std::shared_ptr<BaseAction> action_;
+        rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
+
+        rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr ask_pub_;
+        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr color_sub_;
+
+        std::queue<std::shared_ptr<BaseAction>> action_;
 
         bool action_done_ = true;
         int step_running_ = 0;
@@ -84,6 +140,9 @@ namespace Modelec
 
         int current_step_;
 
+        float last_left_trig = 1.0f;
+        float last_right_trig = 1.0f;
+
     private:
         rclcpp::Node::SharedPtr node_;
     };

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

@@ -9,7 +9,7 @@ namespace Modelec {
     public:
         FreeMission(const std::shared_ptr<NavigationHelper>& nav,
                       const std::shared_ptr<ActionExecutor>& action_executor,
-                      BaseAction::Front front = BaseAction::FRONT);
+                      BaseAction::Side side = BaseAction::FRONT);
 
         void Start(rclcpp::Node::SharedPtr node) override;
         void Update() override;
@@ -21,14 +21,17 @@ namespace Modelec {
         enum Step
         {
             GO_TO_FREE,
+            CHECK_BOX,
             DOWN,
-            FREE,
+            FREE_FIRST,
+            ROTATE_ARM,
+            FREE_OTHER,
             UP,
             GO_BACK,
             DONE,
         };
 
-        BaseAction::Front front_;
+        BaseAction::Side side_;
 
         MissionStatus status_;
         std::shared_ptr<NavigationHelper> nav_;

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

@@ -9,7 +9,7 @@ namespace Modelec {
     public:
         TakeMission(const std::shared_ptr<NavigationHelper>& nav,
                       const std::shared_ptr<ActionExecutor>& action_executor,
-                      BaseAction::Front front = BaseAction::FRONT);
+                      BaseAction::Side side = BaseAction::FRONT);
 
         void Start(rclcpp::Node::SharedPtr node) override;
         void Update() override;
@@ -28,7 +28,7 @@ namespace Modelec {
             DONE,
         };
 
-        BaseAction::Front front_;
+        BaseAction::Side side_;
 
         std::shared_ptr<BoxObstacle> closestBox;
         MissionStatus status_;

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

@@ -0,0 +1,47 @@
+#pragma once
+
+#include <modelec_strat/missions/mission_base.hpp>
+#include <modelec_strat/navigation_helper.hpp>
+#include <modelec_strat/action_executor.hpp>
+
+namespace Modelec {
+    class ThermoMission : public Mission {
+    public:
+        ThermoMission(const std::shared_ptr<NavigationHelper>& nav,
+                      const std::shared_ptr<ActionExecutor>& action_executor);
+
+        void Start(rclcpp::Node::SharedPtr node) override;
+        void Update() override;
+        void Clear() override;
+        MissionStatus GetStatus() const override;
+        std::string GetName() const override { return "Thermo"; }
+
+        static bool IsThermoDone;
+
+    private:
+        enum Step
+        {
+            GO_TO_THERMO,
+            GO_TO_THERMO_CLOSE,
+            ACTIVATE_THERMO,
+            GO_TO_10,
+            DEACTIVATE_THERMO,
+            DONE,
+        };
+
+        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

@@ -19,7 +19,7 @@ namespace Modelec
     class NavigationHelper
     {
     public:
-        enum
+        enum Team
         {
             YELLOW = 0,
             BLUE = 1,
@@ -35,7 +35,7 @@ namespace Modelec
 
         std::shared_ptr<Pathfinding> GetPathfinding() const;
 
-        int GetTeamId() const;
+        Team GetTeamId() const;
 
         void Update();
 
@@ -95,6 +95,7 @@ namespace Modelec
         std::shared_ptr<T> GetClosestObstacle(const PosMsg::SharedPtr& pos) const;
 
         PosMsg::SharedPtr GetHomePosition();
+        std::array<Point, 2> GetThermoPositions();
 
         void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
 
@@ -107,7 +108,7 @@ namespace Modelec
 
         bool Replan(bool force = false);
 
-        void SetTeamId(int id);
+        void SetTeamId(Team id);
 
         void SetSpawn(const std::string& name);
 
@@ -134,12 +135,13 @@ namespace Modelec
 
         std::shared_ptr<Pathfinding> pathfinding_;
 
-        int team_id_ = YELLOW;
+        Team team_id_ = YELLOW;
         std::map<std::string, Point> spawn_yellow_;
         std::map<std::string, Point> spawn_blue_;
         Point spawn_;
 
         float factor_close_enemy_ = 0;
+        float factor_theta_ = 0;
 
         int enemy_emergency_distance_ = 0;
 
@@ -184,7 +186,7 @@ namespace Modelec
         std::shared_ptr<T> closest_obstacle = nullptr;
         auto robotPos = Point(pos->x, pos->y, pos->theta);
         auto enemyPos = Point(last_enemy_pos_.x, last_enemy_pos_.y, last_enemy_pos_.theta);
-        float distance = std::numeric_limits<float>::max();
+        double score = std::numeric_limits<double>::max();
 
         for (const auto& obstacle : GetPathfinding()->GetObstacles())
         {
@@ -192,17 +194,15 @@ namespace Modelec
             {
                 if (!obs->IsAtObjective())
                 {
-                    auto dist = Point::distance(robotPos, obs->GetPosition());
+                    auto obsPoint = obs->GetPosition();
+                    double distance = Point::distance(robotPos, obsPoint);
+                    double enemy_distance = Point::distance(enemyPos, obsPoint);
+                    double theta = std::abs(Point::angleDiff(robotPos, obsPoint));
 
-                    if (has_enemy_)
+                    double s = distance + (enemy_distance * factor_close_enemy_ * has_enemy_) + theta * factor_theta_;
+                    if (s < score)
                     {
-                        auto enemyDist = Point::distance(enemyPos, obs->GetPosition());
-                        dist *= (1.0f + factor_close_enemy_ * std::exp(-enemyDist / enemy_emergency_distance_));
-                    }
-
-                    if (dist < distance)
-                    {
-                        distance = dist;
+                        score = s;
                         closest_obstacle = obs;
                     }
                 }

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

@@ -8,6 +8,12 @@ namespace Modelec
     class BoxObstacle : public Obstacle
     {
     public:
+        enum Team
+        {
+            YELLOW = 0,
+            BLUE = 1,
+        };
+
         BoxObstacle() = default;
         BoxObstacle(const BoxObstacle&) = default;
 
@@ -20,8 +26,24 @@ namespace Modelec
 
         std::vector<Point> GetAllPossiblePositions() const;
 
+        void SetColor(size_t index, Team team);
+        Team GetColor(size_t index) const;
+        std::array<Team, 4> GetColors() const;
+
+        std::vector<int> GetSide(Team team) const;
+
+        void ParseColor(const std::string& colorStr);
+        void ParseColor(const std::vector<std::string>& colorVec);
+
     protected:
 
         std::vector<double> possible_angles_;
+
+        std::array<Team, 4> colors_ = {
+            BLUE,
+            BLUE,
+            BLUE,
+            BLUE
+        };
     };
 }

src/modelec_strat/include/modelec_strat/pami_manager.hpp

@@ -2,7 +2,6 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <modelec_utils/config.hpp>
 #include <modelec_utils/point.hpp>
 
 #include <modelec_strat/obstacle/obstacle.hpp>
@@ -29,11 +28,6 @@ namespace Modelec
 
         int score_to_add_ = 0;
 
-        int score_goupie_ = 0;
-        int score_superstar_ = 0;
-        int score_all_party_ = 0;
-        int score_free_zone_ = 0;
-
         rclcpp::TimerBase::SharedPtr timer_add_;
         // rclcpp::TimerBase::SharedPtr timer_remove_;
 

src/modelec_strat/include/modelec_strat/strat_fms.hpp

@@ -28,6 +28,7 @@ namespace Modelec
 
         TAKE_MISSION = modelec_interfaces::msg::StratState::TAKE_MISSION,
         FREE_MISSION = modelec_interfaces::msg::StratState::FREE_MISSION,
+        THERMO_MISSION = modelec_interfaces::msg::StratState::THERMO_MISSION,
 
         DO_GO_HOME = modelec_interfaces::msg::StratState::DO_GO_HOME,
         STOP = modelec_interfaces::msg::StratState::STOP,
@@ -64,6 +65,10 @@ namespace Modelec
         std::queue<State> game_action_sequence_;
         bool static_strat_ = false;
 
+        float factor_obs_;
+        float factor_thermo_;
+        int timer_period_ms_ = 100;
+
         std::shared_ptr<NavigationHelper> nav_;
         std::shared_ptr<ActionExecutor> action_executor_;
 

src/modelec_strat/src/action/down_action.cpp

@@ -2,15 +2,16 @@
 
 #include "modelec_strat/action_executor.hpp"
 
-Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor) : BaseAction(action_executor)
+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);
 }
 
-Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front) : DownAction(action_executor)
+Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, bool inverted) : DownAction(action_executor)
 {
-    front_ = front;
+    side_ = side;
+    inverted_ = inverted;
 }
 
 void Modelec::DownAction::Next()
@@ -29,43 +30,43 @@ void Modelec::DownAction::Next()
     case ActionExec::DOWN_STEP:
         {
             ActionServoTimedArray msg;
-            msg.items.resize(front_ == BOTH ? 8 : 4);
+            msg.items.resize(side_ == BOTH ? 8 : 4);
 
-            if (front_ == FRONT || front_ == BOTH)
+            if (side_ == FRONT || side_ == BOTH)
             {
                 msg.items[0].id = 0;
-                msg.items[0].start_angle = 1.95;
-                msg.items[0].end_angle = 3;
-                msg.items[0].duration_s = 1;
+                msg.items[0].start_angle = 1.76;
+                msg.items[0].end_angle = 2.93;
+                msg.items[0].duration_s = 2;
 
                 msg.items[1].id = 1;
-                msg.items[1].start_angle = 1.9;
-                msg.items[1].end_angle = 0.85;
-                msg.items[1].duration_s = 1;
+                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.3;
-                msg.items[2].end_angle = 0;
-                msg.items[2].duration_s = 0.5;
+                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.7;
-                msg.items[3].end_angle = 2.9;
-                msg.items[3].duration_s = 0.5;
+                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 (front_ == BACK || front_ == BOTH)
+            if (side_ == BACK || side_ == BOTH)
             {
-                int i = front_ == BOTH ? 4 : 0;
+                int i = side_ == BOTH ? 4 : 0;
                 msg.items[i].id = 8;
                 msg.items[i].start_angle = 0;
                 msg.items[i].end_angle = 0;
-                msg.items[i].duration_s = 1;
+                msg.items[i].duration_s = 0.5;
 
                 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 = 1;
+                msg.items[i+1].duration_s = 0.5;
 
                 msg.items[i+2].id = 10;
                 msg.items[i+2].start_angle = 0;
@@ -95,11 +96,35 @@ void Modelec::DownAction::Init(const std::vector<std::string>& params)
 {
     if (!params.empty())
     {
-        SetFront(static_cast<Front>(std::stoi(params[1])));
+        SetSide(static_cast<Side>(std::stoi(params[1])));
+        SetInverted(params.size() >= 3 ? (params[2] == "1" || params[2] == "true") : false);
     }
 }
 
-void Modelec::DownAction::SetFront(Front front)
+void Modelec::DownAction::SetSide(Side side)
 {
-    front_ = front;
+    side_ = side;
+}
+
+void Modelec::DownAction::SetInverted(bool inverted)
+{
+    inverted_ = inverted;
+}
+
+void Modelec::DownAction::End()
+{
+    if (side_ == BOTH)
+    {
+        action_executor_->arm_pos_[FRONT].down = true;
+        action_executor_->arm_pos_[BACK].down = true;
+
+        action_executor_->arm_pos_[FRONT].rotated = inverted_;
+        action_executor_->arm_pos_[BACK].rotated = inverted_;
+    }
+    else
+    {
+        action_executor_->arm_pos_[side_].down = true;
+
+        action_executor_->arm_pos_[side_].rotated = inverted_;
+    }
 }

src/modelec_strat/src/action/free_action.cpp

@@ -8,17 +8,17 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_ex
     steps_.push(ActionExec::DONE_STEP);
 }
 
-Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front, int n) : FreeAction(action_executor)
+Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, int n) : FreeAction(action_executor)
 {
-    AddServo(n, front);
+    AddServo(n, side);
 }
 
-Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Front> servo) : FreeAction(action_executor)
+Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Side> servo) : FreeAction(action_executor)
 {
     AddServo(servo.first, servo.second);
 }
 
-Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Front>> servos) : FreeAction(action_executor)
+Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Side>> servos) : FreeAction(action_executor)
 {
     AddServos(servos);
 }
@@ -45,8 +45,8 @@ 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 = servos_[i].second ? 3 : 0;
-                msg.items[i].end_angle = servos_[i].second ? 1 : 0;
+                msg.items[i].start_angle = 3;
+                msg.items[i].end_angle = 1;
                 msg.items[i].duration_s = 0.5;
             }
 
@@ -70,23 +70,32 @@ void Modelec::FreeAction::Init(const std::vector<std::string>& params)
         for (size_t i = 1; i < params.size(); i += 2)
         {
             int id = std::stoi(params[i]);
-            bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true;
-            AddServo(id, front ? FRONT : BACK);
+            bool side = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "side") : true;
+            AddServo(id, side ? FRONT : BACK);
         }
     }
 }
 
-void Modelec::FreeAction::AddServo(int id, Front front)
+void Modelec::FreeAction::AddServo(int id, Side side)
 {
-    servos_.emplace_back(id, front);
+    servos_.emplace_back(id, side);
 }
 
-void Modelec::FreeAction::AddServo(std::pair<int, Front> servo)
+void Modelec::FreeAction::AddServo(std::pair<int, Side> servo)
 {
     servos_.emplace_back(servo);
 }
 
-void Modelec::FreeAction::AddServos(const std::vector<std::pair<int, Front>>& servos)
+void Modelec::FreeAction::AddServos(const std::vector<std::pair<int, Side>>& servos)
 {
     servos_.insert(servos_.end(), servos.begin(), servos.end());
 }
+
+void Modelec::FreeAction::End()
+{
+    for (auto servo : servos_)
+    {
+        auto index = servo.first + (servo.second == FRONT ? 0 : 4);
+        action_executor_->servo_pos_[index] = false;
+    }
+}

src/modelec_strat/src/action/look_on_action.cpp

@@ -0,0 +1,69 @@
+#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

@@ -8,17 +8,17 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_ex
     steps_.push(ActionExec::DONE_STEP);
 }
 
-Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front, int n) : TakeAction(action_executor)
+Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, int n) : TakeAction(action_executor)
 {
-    AddServo(n, front);
+    AddServo(n, side);
 }
 
-Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Front> servo) : TakeAction(action_executor)
+Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Side> servo) : TakeAction(action_executor)
 {
     AddServo(servo);
 }
 
-Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Front>> servos) : TakeAction(action_executor)
+Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Side>> servos) : TakeAction(action_executor)
 {
     AddServos(servos);
 }
@@ -45,8 +45,8 @@ 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].start_angle = servos_[i].second ? 1 : 0;
-                msg.items[i].end_angle = servos_[i].second ? 3 : 0;
+                msg.items[i].start_angle = 0;
+                msg.items[i].end_angle = 3;
                 msg.items[i].duration_s = 0.5;
             }
 
@@ -70,23 +70,32 @@ void Modelec::TakeAction::Init(const std::vector<std::string>& params)
         for (size_t i = 1; i < params.size(); i += 2)
         {
             int id = std::stoi(params[i]);
-            bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true;
-            AddServo(id, front ? FRONT : BACK);
+            bool side = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "side") : true;
+            AddServo(id, side ? FRONT : BACK);
         }
     }
 }
 
-void Modelec::TakeAction::AddServo(int id, Front front)
+void Modelec::TakeAction::AddServo(int id, Side side)
 {
-    servos_.emplace_back(id, front);
+    servos_.emplace_back(id, side);
 }
 
-void Modelec::TakeAction::AddServo(std::pair<int, Front> servo)
+void Modelec::TakeAction::AddServo(std::pair<int, Side> servo)
 {
     servos_.emplace_back(servo);
 }
 
-void Modelec::TakeAction::AddServos(const std::vector<std::pair<int, Front>>& servos)
+void Modelec::TakeAction::AddServos(const std::vector<std::pair<int, Side>>& servos)
 {
     servos_.insert(servos_.end(), servos.begin(), servos.end());
 }
+
+void Modelec::TakeAction::End()
+{
+    for (auto servo : servos_)
+    {
+        auto index = servo.first + (servo.second == FRONT ? 0 : 4);
+        action_executor_->servo_pos_[index] = true;
+    }
+}

src/modelec_strat/src/action/thermo_action.cpp

@@ -0,0 +1,87 @@
+#include <modelec_strat/action/thermo_action.hpp>
+
+#include "modelec_strat/action_executor.hpp"
+
+Modelec::ThermoAction::ThermoAction(const std::shared_ptr<ActionExecutor>& action_executor) : BaseAction(action_executor)
+{
+    steps_.push(ActionExec::THERMO_STEP);
+    steps_.push(ActionExec::DONE_STEP);
+}
+
+Modelec::ThermoAction::ThermoAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, bool deploy) : ThermoAction(action_executor)
+{
+    side_ = side;
+    deploy_ = deploy;
+}
+
+void Modelec::ThermoAction::Next()
+{
+    if (steps_.empty())
+    {
+        done_ = true;
+        return;
+    }
+
+    auto step = steps_.front();
+    steps_.pop();
+
+    switch (step)
+    {
+    case ActionExec::THERMO_STEP:
+        {
+            modelec_interfaces::msg::ActionServoTimedArray msg;
+
+            msg.items.resize(side_ == BOTH ? 2 : 1);
+
+            if (side_ == LEFT || side_ == BOTH)
+            {
+                msg.items[0].id = 16;
+                msg.items[0].start_angle = deploy_ ? 1 : 2;
+                msg.items[0].end_angle = deploy_ ? 2 : 1;
+                msg.items[0].duration_s = 0.5;
+            }
+
+            if (side_ == RIGHT || side_ == BOTH)
+            {
+                msg.items[side_ == BOTH ? 1 : 0].id = 17;
+                msg.items[side_ == BOTH ? 1 : 0].start_angle = deploy_ ? 1 : 2;
+                msg.items[side_ == BOTH ? 1 : 0].end_angle = deploy_ ? 2 : 1;
+                msg.items[side_ == BOTH ? 1 : 0].duration_s = 0.5;
+            }
+
+            action_executor_->MoveServoTimed(msg);
+        }
+        break;
+    case ActionExec::DONE_STEP:
+        {
+            done_ = true;
+        }
+        break;
+    default:
+        break;
+    }
+}
+
+void Modelec::ThermoAction::Init(const std::vector<std::string>& params)
+{
+    if (!params.empty())
+    {
+        SetSide(static_cast<Side>(std::stoi(params[0])));
+        SetDeploy(params[1] == "1" || params[1] == "true");
+    }
+}
+
+void Modelec::ThermoAction::SetSide(Side side)
+{
+    side_ = side;
+}
+
+void Modelec::ThermoAction::SetDeploy(bool deploy)
+{
+    deploy_ = deploy;
+}
+
+void Modelec::ThermoAction::End()
+{
+    action_executor_->thermo_state_[side_] = deploy_;
+}

src/modelec_strat/src/action/toggle_servo_action.cpp

@@ -0,0 +1,101 @@
+#include <modelec_strat/action/toggle_servo_action.hpp>
+
+#include "modelec_strat/action_executor.hpp"
+
+Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr<ActionExecutor>& action_executor) : BaseAction(action_executor)
+{
+    steps_.push(ActionExec::TOGGLE_SERVO_STEP);
+    steps_.push(ActionExec::DONE_STEP);
+}
+
+Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side, int n) : ToggleServoAction(action_executor)
+{
+    AddServo(n, side);
+}
+
+Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Side> servo) : ToggleServoAction(action_executor)
+{
+    AddServo(servo.first, servo.second);
+}
+
+Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Side>> servos) : ToggleServoAction(action_executor)
+{
+    AddServos(servos);
+}
+
+void Modelec::ToggleServoAction::Next()
+{
+    if (steps_.empty())
+    {
+        done_ = true;
+        return;
+    }
+
+    auto step = steps_.front();
+    steps_.pop();
+
+    switch (step)
+    {
+    case ActionExec::TOGGLE_SERVO_STEP:
+        {
+            modelec_interfaces::msg::ActionServoTimedArray msg;
+
+            msg.items.resize(servos_.size());
+
+            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 = action_executor_->servo_pos_[servos_[i].first + (servos_[i].second ? 0 : 4)] ? 1 : 3;
+                msg.items[i].end_angle = action_executor_->servo_pos_[servos_[i].first + (servos_[i].second ? 0 : 4)] ? 3 : 1;
+                msg.items[i].duration_s = 0.5;
+            }
+
+            action_executor_->MoveServoTimed(msg);
+        }
+        break;
+    case ActionExec::DONE_STEP:
+        {
+            done_ = true;
+        }
+        break;
+    default:
+        break;
+    }
+}
+
+void Modelec::ToggleServoAction::Init(const std::vector<std::string>& params)
+{
+    if (params.size() >= 2)
+    {
+        for (size_t i = 1; i < params.size(); i += 2)
+        {
+            int id = std::stoi(params[i]);
+            bool side = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "side") : true;
+            AddServo(id, side ? FRONT : BACK);
+        }
+    }
+}
+
+void Modelec::ToggleServoAction::AddServo(int id, Side side)
+{
+    servos_.emplace_back(id, side);
+}
+
+void Modelec::ToggleServoAction::AddServo(std::pair<int, Side> servo)
+{
+    servos_.emplace_back(servo);
+}
+
+void Modelec::ToggleServoAction::AddServos(const std::vector<std::pair<int, Side>>& servos)
+{
+    servos_.insert(servos_.end(), servos.begin(), servos.end());
+}
+
+void Modelec::ToggleServoAction::End()
+{
+    for (auto servo : servos_)
+    {
+        auto index = servo.first + (servo.second == FRONT ? 0 : 4);
+        action_executor_->servo_pos_[index] = !action_executor_->servo_pos_[index];
+    }
+}

src/modelec_strat/src/action/up_action.cpp

@@ -8,9 +8,9 @@ Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_execut
     steps_.push(ActionExec::DONE_STEP);
 }
 
-Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front) : UPAction(action_executor)
+Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_executor, Side side) : UPAction(action_executor)
 {
-    front_ = front;
+    side_ = side;
 }
 
 void Modelec::UPAction::Next()
@@ -29,53 +29,53 @@ void Modelec::UPAction::Next()
     case ActionExec::UP_STEP:
         {
             ActionServoTimedArray msg;
-            msg.items.resize(front_ == BOTH ? 8 : 4);
+            msg.items.resize(side_ == BOTH ? 8 : 4);
 
-            if (front_ == FRONT || front_ == BOTH)
+            if (side_ == FRONT || side_ == BOTH)
             {
                 msg.items[0].id = 0;
-                msg.items[0].start_angle = 3;
-                msg.items[0].end_angle = 1.95;
-                msg.items[0].duration_s = 1;
+                msg.items[0].start_angle = 2.93;
+                msg.items[0].end_angle = 1.76;
+                msg.items[0].duration_s = 2;
 
                 msg.items[1].id = 1;
-                msg.items[1].start_angle = 0.85;
-                msg.items[1].end_angle = 1.9;
-                msg.items[1].duration_s = 1;
+                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 = 0;
-                msg.items[2].end_angle = 0.3;
-                msg.items[2].duration_s = 1;
+                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 = 2.9;
-                msg.items[3].end_angle = 2.7;
-                msg.items[3].duration_s = 1;
+                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 (front_ == BACK || front_ == BOTH) {
-                int i = front_ == BOTH ? 4 : 0;
+            if (side_ == BACK || side_ == BOTH) {
+                int i = side_ == BOTH ? 4 : 0;
 
                 msg.items[i].id = 8;
                 msg.items[i].start_angle = 0;
                 msg.items[i].end_angle = 0;
-                msg.items[i].duration_s = 1;
+                msg.items[i].duration_s = 0.5;
 
                 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 = 1;
+                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 = 1;
+                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 = 1;
+                msg.items[i+3].duration_s = 0.5;
             }
 
             action_executor_->MoveServoTimed(msg);
@@ -95,11 +95,24 @@ void Modelec::UPAction::Init(const std::vector<std::string>& params)
 {
     if (!params.empty())
     {
-        SetFront(static_cast<Front>(std::stoi(params[1])));
+        SetSide(static_cast<Side>(std::stoi(params[1])));
     }
 }
 
-void Modelec::UPAction::SetFront(Front front)
+void Modelec::UPAction::SetSide(Side side)
 {
-    front_ = front;
+    side_ = side;
+}
+
+void Modelec::UPAction::End()
+{
+    if (side_ == BOTH)
+    {
+        action_executor_->arm_pos_[FRONT].down = false;
+        action_executor_->arm_pos_[BACK].down = false;
+    }
+    else
+    {
+        action_executor_->arm_pos_[side_].down = false;
+    }
 }

src/modelec_strat/src/action_executor.cpp

@@ -4,6 +4,9 @@
 #include "modelec_strat/action/up_action.hpp"
 #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"
 
 namespace Modelec
@@ -26,12 +29,12 @@ namespace Modelec
             });
 
         servo_move_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionServoPosArray>(
-            "/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPosArray::SharedPtr)
+            "/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPosArray::SharedPtr msg)
             {
-                // BUG
-                // if ServoTimed is called this one will trigger so step_running_ will be decremented at the beginning of the Timed one
-                // step_running_ -= msg->items.size();
-                // Update();
+                for (const auto& item : msg->items)
+                {
+                    servo_value_[item.id] = item.angle;
+                }
             });
 
         relay_move_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionRelayStateArray>(
@@ -46,7 +49,7 @@ namespace Modelec
             {
                 auto token = split(msg->action, ActionExec::DELIMITER[0]);
 
-                if (token.size() == 0)
+                if (token.empty())
                 {
                     RCLCPP_WARN(
                         node_->get_logger(),
@@ -54,10 +57,10 @@ namespace Modelec
                     return;
                 }
 
-                action_ = BaseAction::CreateAction(
+                auto action = BaseAction::CreateAction(
                     token[0],
                     shared_from_this());
-                if (action_ == nullptr)
+                if (action == nullptr)
                 {
                     RCLCPP_WARN(
                         node_->get_logger(),
@@ -65,7 +68,8 @@ namespace Modelec
                         msg->action.c_str());
                     return;
                 }
-                action_->Init(token);
+                action->Init(token);
+                action_.push(action);
                 action_done_ = false;
                 step_running_ = 0;
                 Update();
@@ -84,26 +88,150 @@ namespace Modelec
             "/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() >= 5)
+                if (msg->buttons.size() >= 15)
                 {
                     if (msg->buttons[0] == 1) // A button
                     {
-                        Down(BaseAction::BOTH);
+                        if (action_done_)
+                        {
+                            ToggleServo({{0, BaseAction::FRONT}});
+                        }
                     }
                     else if (msg->buttons[1] == 1) // B button
                     {
-                        Up(BaseAction::BOTH);
+                        if (action_done_)
+                        {
+                            ToggleServo({{1, BaseAction::FRONT}});
+                        }
                     }
                     else if (msg->buttons[3] == 1) // X button
                     {
-                        Take({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}});
+                        if (action_done_)
+                        {
+                            ToggleServo({{3, BaseAction::FRONT}});
+                        }
                     }
                     else if (msg->buttons[4] == 1) // Y button
                     {
-                        Free({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}});
+                        if (action_done_)
+                        {
+                            ToggleServo({{2, BaseAction::FRONT}});
+                        }
+                    }
+                    else if (msg->buttons[6] == 1) // L1 button
+                    {
+                        if (action_done_)
+                        {
+                            RotateArm(BaseAction::BACK, false, !arm_pos_[BaseAction::BACK].rotated);
+                        }
+                    }
+                    else if (msg->buttons[7] == 1) // R1 button
+                    {
+                        if (action_done_)
+                        {
+                            RotateArm(BaseAction::FRONT, false, !arm_pos_[BaseAction::FRONT].rotated);
+                        }
+                    }
+                    else if (msg->buttons[14] == 1) // LT button
+                    {
+                        ActivateThermo(BaseAction::Side::LEFT, !thermo_state_[BaseAction::Side::LEFT]);
+                    }
+                    else if (msg->buttons[15] == 1) // LR button
+                    {
+                        ActivateThermo(BaseAction::Side::RIGHT, !thermo_state_[BaseAction::Side::RIGHT]);
+                    }
+                }
+                if (msg->axes.size() == 8)
+                {
+                    auto left_trig = msg->axes[4];
+                    auto right_trig = msg->axes[5];
+
+                    if (left_trig == 1 && last_left_trig == -1) // left trigger pressed
+                    {
+                        if (action_done_)
+                        {
+                            ToggleArm(BaseAction::BACK);
+                        }
+                        last_left_trig = left_trig;
+                    } else if (left_trig == -1 && last_left_trig == 1) // left trigger released
+                    {
+                        last_left_trig = left_trig;
+                    }
+
+                    if (right_trig == 1 && last_left_trig == -1) // right trigger pressed
+                    {
+                        if (action_done_)
+                        {
+                            ToggleArm(BaseAction::FRONT);
+                        }
+                        last_right_trig = right_trig;
+                    } else if (right_trig == -1 && last_right_trig == 1) // right trigger released
+                    {
+                        last_right_trig = right_trig;
+                    }
+
+                    auto btn_horizontal = msg->axes[6];
+                    auto btn_vertical = msg->axes[7];
+
+                    if (btn_horizontal == 1.0) // left
+                    {
+                        if (action_done_)
+                        {
+                            ToggleServo({{3, BaseAction::BACK}});
+                        }
+                    }
+                    else if (btn_horizontal == -1.0) // right
+                    {
+                        if (action_done_)
+                        {
+                            ToggleServo({{1, BaseAction::BACK}});
+                        }
+                    }
+                    if (btn_vertical == 1.0) // up
+                    {
+                        if (action_done_)
+                        {
+                            ToggleServo({{0, BaseAction::BACK}});
+                        }
+                    }
+                    else if (btn_vertical == -1.0) // down
+                    {
+                        if (action_done_)
+                        {
+                            ToggleServo({{2, BaseAction::BACK}});
+                        }
                     }
                 }
             });
+
+        score_pub_ = node_->create_publisher<std_msgs::msg::Int64>("/strat/score", 10);
+
+        ask_color_client_ = node_->create_client<std_srvs::srv::Trigger>("action/detect_color");
+
+        rclcpp::QoS qos(rclcpp::KeepLast(10));
+        qos.reliable();
+
+        color_sub_ = node_->create_subscription<std_msgs::msg::String>(
+            "/action/detect_color/res", qos, [this](const std_msgs::msg::String::SharedPtr msg)
+            {
+                auto res = split(msg->data, '|');
+                if (res.size() < 2 || res[0] != "1")
+                {
+                    RCLCPP_WARN(node_->get_logger(), "Color detection failed: %s", msg->data.c_str());
+                    return;
+                }
+
+                RCLCPP_DEBUG(node_->get_logger(), "Color detection succeeded: %s", res[1].c_str());
+
+                if (box_obstacles_[cam_side_] != nullptr) {
+                    box_obstacles_[cam_side_]->ParseColor(res[1]);
+                } else {
+                    RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box in the cam side");
+                }
+            });
+
+        ask_pub_ = node_->create_publisher<std_msgs::msg::Empty>(
+            "/action/detect_color/ask", qos);
     }
 
     rclcpp::Node::SharedPtr ActionExecutor::GetNode() const
@@ -118,27 +246,42 @@ namespace Modelec
 
     void ActionExecutor::Update()
     {
-        if (action_ != nullptr && !action_->IsDone() && step_running_ <= 0)
+        auto action = action_.front();
+        if (action != nullptr && !action->IsDone() && step_running_ <= 0)
         {
             RCLCPP_DEBUG(
                 node_->get_logger(),
                 "ActionExecutor Update: Executing next step of action, step_running_=%d", step_running_);
 
-            action_->Next();
-            if (action_->IsDone())
+            action->Next();
+            if (action->IsDone())
             {
-                action_done_ = true;
-                action_ = nullptr;
+                ActionFinished(action);
+                action_.pop();
+                if (action_.empty())
+                {
+                    action_done_ = true;
+                } else
+                {
+                    Update();
+                }
             }
         }
-        else if (action_ != nullptr && action_->IsDone())
+        else if (action != nullptr && action->IsDone())
         {
             RCLCPP_DEBUG(
                 node_->get_logger(),
                 "ActionExecutor Update: Action is done, step_running_=%d", step_running_);
 
-            action_done_ = true;
-            action_ = nullptr;
+            ActionFinished(action);
+
+            action_.pop();
+            if (action_.empty())
+            {
+                action_done_ = true;
+            } else {
+                Update();
+            }
         }
     }
 
@@ -146,13 +289,22 @@ namespace Modelec
     {
         action_done_ = true;
         step_running_ = 0;
-        action_ = nullptr;
+
+        std::queue<std::shared_ptr<BaseAction>> empty;
+        std::swap(action_, empty);
     }
 
-    void ActionExecutor::Down(BaseAction::Front front, bool force) {
-        if (action_done_ || force)
+    void ActionExecutor::Down(BaseAction::Side front, bool force, bool inverted) {
+        if (!arm_pos_[front].down || force)
         {
-            action_ = std::make_shared<DownAction>(shared_from_this(), front);
+            RCLCPP_DEBUG(
+                node_->get_logger(),
+                "ActionExecutor Down called for front=%d, force=%d",
+                static_cast<int>(front),
+                static_cast<int>(force));
+
+            auto action = std::make_shared<DownAction>(shared_from_this(), front, inverted);
+            action_.push(action);
             if (action_done_)
             {
                 step_running_ = 0;
@@ -163,10 +315,17 @@ namespace Modelec
         }
     }
 
-    void ActionExecutor::Up(BaseAction::Front front, bool force) {
-        if (action_done_ || force)
+    void ActionExecutor::Up(BaseAction::Side side, bool force) {
+        if ((arm_pos_[side].down) || force)
         {
-            action_ = std::make_shared<UPAction>(shared_from_this(), front);
+            RCLCPP_DEBUG(
+                node_->get_logger(),
+                "ActionExecutor Up called for side=%d, force=%d",
+                static_cast<int>(side),
+                static_cast<int>(force));
+
+            auto action = std::make_shared<UPAction>(shared_from_this(), side);
+            action_.push(action);
             if (action_done_)
             {
                 step_running_ = 0;
@@ -177,10 +336,50 @@ namespace Modelec
         }
     }
 
-    void ActionExecutor::Take(const std::vector<std::pair<int, BaseAction::Front>>& servos, bool force) {
-        if (action_done_ || force)
+    void ActionExecutor::ToggleArm(BaseAction::Side side, bool force)
+    {
+        RCLCPP_DEBUG(
+            node_->get_logger(),
+            "ActionExecutor ToggleArm called for side=%d, force=%d",
+            static_cast<int>(side),
+            static_cast<int>(force));
+
+        if (side == BaseAction::BOTH)
         {
-            action_ = std::make_shared<TakeAction>(shared_from_this(), servos);
+            ToggleArm(BaseAction::FRONT, force);
+            ToggleArm(BaseAction::BACK, force);
+            return;
+        }
+
+        if (arm_pos_[side].down)
+        {
+            Up(side, force);
+        }
+        else
+        {
+            Down(side, force, arm_pos_[side].rotated);
+        }
+    }
+
+    void ActionExecutor::RotateArm(BaseAction::Side side, bool force, bool rotated)
+    {
+        if ((arm_pos_[side].rotated != rotated || !arm_pos_[side].down) || force)
+        {
+            RCLCPP_DEBUG(
+                node_->get_logger(),
+                "ActionExecutor RotateArm called for side=%d, force=%d, rotated=%d",
+                static_cast<int>(side),
+                static_cast<int>(force),
+                static_cast<int>(rotated));
+
+            if (arm_pos_[side].down)
+            {
+                auto action = std::make_shared<UPAction>(shared_from_this(), side);
+                action_.push(action);
+            }
+
+            auto action = std::make_shared<DownAction>(shared_from_this(), side, rotated);
+            action_.push(action);
             if (action_done_)
             {
                 step_running_ = 0;
@@ -191,18 +390,56 @@ namespace Modelec
         }
     }
 
-    void ActionExecutor::Free(const std::vector<std::pair<int, BaseAction::Front>>& servos, bool force) {
-        if (action_done_ || force)
-        {
-            action_ = std::make_shared<FreeAction>(shared_from_this(), servos);
-            if (action_done_)
-            {
-                step_running_ = 0;
-            }
-            action_done_ = false;
+    void ActionExecutor::Take(const std::vector<std::pair<int, BaseAction::Side>>& servos) {
+        RCLCPP_DEBUG(
+            node_->get_logger(),
+            "ActionExecutor Take called for %d servos",
+            static_cast<int>(servos.size()));
 
-            Update();
+        auto action = std::make_shared<TakeAction>(shared_from_this(), servos);
+        action_.push(action);
+        if (action_done_)
+        {
+            step_running_ = 0;
         }
+        action_done_ = false;
+
+        Update();
+    }
+
+    void ActionExecutor::Free(const std::vector<std::pair<int, BaseAction::Side>>& servos) {
+        RCLCPP_DEBUG(
+            node_->get_logger(),
+            "ActionExecutor Free called for %d servos",
+            static_cast<int>(servos.size()));
+
+        auto action = std::make_shared<FreeAction>(shared_from_this(), servos);
+        action_.push(action);
+        if (action_done_)
+        {
+            step_running_ = 0;
+        }
+        action_done_ = false;
+
+        Update();
+    }
+
+    void ActionExecutor::ToggleServo(const std::vector<std::pair<int, BaseAction::Side>>& servos)
+    {
+        RCLCPP_DEBUG(
+            node_->get_logger(),
+            "ActionExecutor ToggleServo called for %d servos",
+            static_cast<int>(servos.size()));
+
+        auto action  = std::make_shared<ToggleServoAction>(shared_from_this(), servos);
+        action_.push(action);
+        if (action_done_)
+        {
+            step_running_ = 0;
+        }
+        action_done_ = false;
+
+        Update();
     }
 
     void ActionExecutor::MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg)
@@ -223,6 +460,46 @@ namespace Modelec
         step_running_ += msg.items.size();
     }
 
+    void ActionExecutor::ActivateThermo(BaseAction::Side side, bool deploy, bool force)
+    {
+        if (thermo_state_[side] != deploy || force)
+        {
+            auto action = std::make_shared<ThermoAction>(shared_from_this(), side, deploy);
+            action_.push(action);
+            if (action_done_)
+            {
+                step_running_ = 0;
+            }
+            action_done_ = false;
+
+            Update();
+        }
+    }
+
+    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(
+            node_->get_logger(),
+            "ActionExecutor ActionFinished called for action");
+        action->End();
+    }
+
     bool ActionExecutor::IsEmpty() const
     {
         return box_obstacles_[0] == nullptr && box_obstacles_[1] == nullptr;
@@ -233,9 +510,9 @@ namespace Modelec
         return box_obstacles_[0] != nullptr && box_obstacles_[1] != nullptr;
     }
 
-    bool ActionExecutor::HasBox(BaseAction::Front front) const
+    bool ActionExecutor::HasBox(BaseAction::Side side) const
     {
-        return box_obstacles_[front] != nullptr;
+        return box_obstacles_[side] != nullptr;
     }
 
     bool ActionExecutor::HasFrontBox() const
@@ -252,4 +529,37 @@ namespace Modelec
     {
         return HasFrontBox() != HasBackBox();
     }
+
+    void ActionExecutor::SendPoint(const int point) const
+    {
+        std_msgs::msg::Int64 msg;
+        msg.data = point;
+        score_pub_->publish(msg);
+    }
+
+    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/enemy_manager.cpp

@@ -7,28 +7,29 @@ namespace Modelec
 {
     EnemyManager::EnemyManager() : Node("enemy_manager")
     {
-        std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
-        if (!Config::load(config_path))
-        {
-            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str());
-        }
+        this->declare_parameter("enemy.detection.min_move_threshold_mm", 50.0);
+        this->declare_parameter("enemy.detection.refresh_rate", 1.0);
+        this->declare_parameter("enemy.detection.max_stationary_time_s", 10.0);
+        this->declare_parameter("enemy.detection.margin_detection_table_mm", 50.0);
+        this->declare_parameter("enemy.detection.min_emergency_distance_mm", 500.0);
 
-        min_move_threshold_mm_ = Config::get<float>("config.enemy.detection.min_move_threshold_mm", 50);
+        this->declare_parameter("map.size.map_width_mm", 3000.0);
+        this->declare_parameter("map.size.map_height_mm", 2000.0);
 
-        refresh_rate_s_ = Config::get<float>("config.enemy.detection.refresh_rate", 1.0);
+        this->declare_parameter("robot.size.width_mm", 500.0);
+        this->declare_parameter("robot.size.length_mm", 500.0);
 
-        max_stationary_time_s_ = Config::get<float>("config.enemy.detection.max_stationary_time_s", 10.0);
+        min_move_threshold_mm_ = this->get_parameter("enemy.detection.min_move_threshold_mm").as_double();
+        refresh_rate_s_ = this->get_parameter("enemy.detection.refresh_rate").as_double();
+        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_ = Config::get<float>("config.map.size.map_width_mm", 3000.0);
-        map_height_ = Config::get<float>("config.map.size.map_height_mm", 2000.0);
+        map_width_ = this->get_parameter("map.size.map_width_mm").as_double();
+        map_height_ = this->get_parameter("map.size.map_height_mm").as_double();
 
-        margin_detection_table_ = Config::get<float>("config.enemy.detection.margin_detection_table_mm", 50.0);
-
-        robot_width_ = Config::get<float>("config.robot.size.width_mm", 500.0);
-        robot_length_ = Config::get<float>("config.robot.size.length_mm", 500.0);
-        robot_radius_ = std::max(robot_width_, robot_length_) * 0.4;
-
-        min_emergency_distance_ = Config::get<float>("config.enemy.detection.min_emergency_distance_mm", 500.0f);
+        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,
@@ -50,7 +51,7 @@ namespace Modelec
         close_enemy_pos_pub_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
             "enemy/position/emergency", 10);
 
-        state_sub_ = create_subscription<modelec_interfaces::msg::StratState>("/strat/state", 10,
+        state_sub_ = create_subscription<modelec_interfaces::msg::StratState>("strat/state", 10,
                                                                               [this](
                                                                               const
                                                                               modelec_interfaces::msg::StratState::SharedPtr
@@ -69,7 +70,7 @@ namespace Modelec
                                                                               });
 
         enemy_long_time_pub_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
-            "/enemy/long_time", 10);
+            "enemy/long_time", 10);
 
         start_sub_ = this->create_subscription<std_msgs::msg::Bool>(
             "lidar/start", 10,
@@ -80,7 +81,7 @@ namespace Modelec
             });
 
         timer_ = this->create_wall_timer(
-            std::chrono::nanoseconds(static_cast<int>(refresh_rate_s_ )),
+            std::chrono::milliseconds(static_cast<int>(refresh_rate_s_ * 1000.0)),
             [this]()
             {
                 TimerCallback();

src/modelec_strat/src/missions/free_mission.cpp

@@ -5,8 +5,8 @@
 namespace Modelec {
     FreeMission::FreeMission(const std::shared_ptr<NavigationHelper>& nav,
         const std::shared_ptr<ActionExecutor>& action_executor,
-        BaseAction::Front front)
-     : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
+        BaseAction::Side side)
+     : side_(side), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
     {
     }
 
@@ -24,8 +24,11 @@ namespace Modelec {
         std::swap(steps_, empty);
 
         steps_.push(GO_TO_FREE);
+        steps_.push(CHECK_BOX);
         steps_.push(DOWN);
-        steps_.push(FREE);
+        steps_.push(FREE_FIRST);
+        steps_.push(ROTATE_ARM);
+        steps_.push(FREE_OTHER);
         steps_.push(UP);
         steps_.push(GO_BACK);
         steps_.push(DONE);
@@ -82,64 +85,101 @@ namespace Modelec {
                     return;
                 }
 
-                auto dist = std::clamp(Point::distance(Point(currPos->x, currPos->y, currPos->theta),
-                    nav_->GetClosestDepositeZone(nav_->GetCurrentPos())->GetPosition()), 0.0, 200.0);
-
                 auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta));
 
-                auto pos = depoPoint.GetTakePosition(dist);
+                auto pos = depoPoint.GetTakePosition(200.0);
 
-                RCLCPP_INFO(
-                    node_->get_logger(),
-                    "FreeMission: position (%.2d, %.2d) with distance %.2f",
-                    pos.x, pos.y, dist);
+                pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI));
 
-                pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI;
-
-                if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, front_ == BaseAction::FRONT) != Pathfinding::FREE)
+                if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE)
                 {
-                    if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE)
+                    if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE)
                     {
                         status_ = MissionStatus::FAILED;
                         return;
                     }
                 }
 
+                action_executor_->LookOn(BaseAction::Side::CENTER);
+
                 go_timeout_ = node_->now();
             }
+            break;
+        case CHECK_BOX:
+            {
+                auto obs = action_executor_->box_obstacles_[side_];
+
+                auto vect = obs->GetSide(nav_->GetTeamId() == NavigationHelper::BLUE ? BoxObstacle::BLUE : BoxObstacle::YELLOW);
+
+                RCLCPP_DEBUG(node_->get_logger(), "Box on side %d has %d boxes of his color side", static_cast<int>(side_), static_cast<int>(vect.size()));
+
+                if (vect.size() == 4)
+                {
+                    std::queue<int> empty;
+                    std::swap(steps_, empty);
+
+                    steps_.push(DOWN);
+                    steps_.push(FREE_FIRST);
+                    steps_.push(UP);
+                    steps_.push(GO_BACK);
+                    steps_.push(DONE);
+                } else if (vect.empty())
+                {
+                    std::queue<int> empty;
+                    std::swap(steps_, empty);
+
+                    steps_.push(ROTATE_ARM);
+                    steps_.push(FREE_OTHER);
+                    steps_.push(UP);
+                    steps_.push(GO_BACK);
+                    steps_.push(DONE);
+                }
+            }
+
             break;
         case DOWN:
             {
-                action_executor_->Down(front_);
+                action_executor_->Down(side_);
                 deploy_time_ = node_->now();
             }
 
             break;
-        case FREE:
+        case FREE_FIRST:
             {
-                action_executor_->Free({{0, front_}, {1, front_}, {2, front_}, {3, front_}});
+                auto obs = action_executor_->box_obstacles_[side_];
+
+                auto vect = obs->GetSide(nav_->GetTeamId() == NavigationHelper::BLUE ? BoxObstacle::BLUE : BoxObstacle::YELLOW);
+
+                auto servo = std::vector<std::pair<int, BaseAction::Side>>();
+                for (auto s : vect)
+                {
+                    servo.push_back({s, side_});
+                }
+
+                action_executor_->Free(servo);
+
                 deploy_time_ = node_->now();
 
-                auto obs = action_executor_->box_obstacles_[front_];
-                action_executor_->box_obstacles_[front_] = nullptr;
+                min_time_ = node_->now() + rclcpp::Duration::from_seconds(1);
+            }
+            break;
+        case ROTATE_ARM:
+            {
+                action_executor_->RotateArm(side_, false, true);
+                deploy_time_ = node_->now();
+            }
+            break;
+        case FREE_OTHER:
+            {
+                action_executor_->Free({{0, side_}, {1, side_}, {2, side_}, {3, side_}});
+                deploy_time_ = node_->now();
 
-                auto pos = nav_->GetCurrentPos();
-
-                obs->SetPosition(
-                    pos->x + 200 * cos(pos->theta + (front_ == BaseAction::FRONT ? 0 : M_PI)),
-                    pos->y + 200 * sin(pos->theta + (front_ == BaseAction::FRONT ? 0 : M_PI)),
-                    pos->theta);
-
-                obs->SetAtObjective(true);
-
-                nav_->GetPathfinding()->AddObstacle(obs);
-
-                min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5);
+                min_time_ = node_->now() + rclcpp::Duration::from_seconds(1);
             }
             break;
         case UP:
             {
-                action_executor_->Up(front_);
+                action_executor_->Up(side_);
                 deploy_time_ = node_->now();
             }
             break;
@@ -150,7 +190,7 @@ namespace Modelec {
                 auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta));
 
                 auto pos = depoPoint.GetTakePosition(300);
-                pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI;
+                pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI));
 
                 if (nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE) != Pathfinding::FREE)
                 {
@@ -164,6 +204,20 @@ namespace Modelec {
             break;
         case DONE:
             {
+                auto obs = action_executor_->box_obstacles_[side_];
+                action_executor_->box_obstacles_[side_] = nullptr;
+
+                auto pos = nav_->GetCurrentPos();
+
+                obs->SetPosition(
+                    pos->x + 300 * cos(pos->theta + (side_ == BaseAction::FRONT ? 0 : M_PI)),
+                    pos->y + 300 * sin(pos->theta + (side_ == BaseAction::FRONT ? 0 : M_PI)),
+                    pos->theta);
+
+                obs->SetAtObjective(true);
+
+                nav_->GetPathfinding()->AddObstacle(obs);
+
                 target_deposite_zone_->Validate(true);
             }
 

src/modelec_strat/src/missions/go_home_mission.cpp

@@ -13,7 +13,9 @@ namespace Modelec
     {
         node_ = node;
 
-        mission_score_ = Config::get<int>("config.mission_score.go_home", 0);
+        node->declare_parameter("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);
 

src/modelec_strat/src/missions/take_mission.cpp

@@ -5,8 +5,8 @@
 namespace Modelec {
     TakeMission::TakeMission(const std::shared_ptr<NavigationHelper>& nav,
         const std::shared_ptr<ActionExecutor>& action_executor,
-        BaseAction::Front front)
-     : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
+        BaseAction::Side side)
+     : side_(side), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
     {
     }
 
@@ -80,16 +80,16 @@ namespace Modelec {
                     break;
                 }
 
-                action_executor_->box_obstacles_[front_] = closestBox;
+                action_executor_->box_obstacles_[side_] = closestBox;
 
                 auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
-                pos.theta += (front_ == BaseAction::FRONT ? 0 : M_PI);
+                pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI));
 
-                if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE)
+                if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE)
                 {
-                    if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE)
+                    if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE)
                     {
-                        if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE)
+                        if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE)
                         {
                             status_ = MissionStatus::FAILED;
                             break;
@@ -102,16 +102,16 @@ namespace Modelec {
             break;
         case GO_TO_TAKE_CLOSE:
             {
-                if (action_executor_->box_obstacles_[front_] == nullptr)
+                if (action_executor_->box_obstacles_[side_] == nullptr)
                 {
                     status_ = MissionStatus::FAILED;
                     break;
                 }
 
-                auto pos = action_executor_->box_obstacles_[front_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition();
-                pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI;
+                auto pos = action_executor_->box_obstacles_[side_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition();
+                pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI));
 
-                if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE)
+                if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE)
                 {
                     status_ = MissionStatus::FAILED;
                     break;
@@ -122,32 +122,36 @@ namespace Modelec {
             break;
         case DOWN:
             {
-                action_executor_->Down(front_);
+                action_executor_->RotateArm(side_, false, false);
+
                 deploy_time_ = node_->now();
             }
             break;
         case TAKE:
             {
-                action_executor_->Take({{0, front_}, {1, front_}, {2, front_}, {3, front_}});
+                action_executor_->Take({{0, side_}, {1, side_}, {2, side_}, {3, side_}});
                 deploy_time_ = node_->now();
                 min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5);
             }
             break;
         case UP:
             {
-                action_executor_->Up(front_);
+                action_executor_->Up(side_);
+                action_executor_->LookOn(side_);
                 deploy_time_ = node_->now();
             }
             break;
         case DONE:
             {
-                if (action_executor_->box_obstacles_[front_] == nullptr)
+                if (action_executor_->box_obstacles_[side_] == nullptr)
                 {
                     status_ = MissionStatus::FAILED;
                     break;
                 }
 
-                nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[front_]->GetId());
+                nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[side_]->GetId());
+
+                action_executor_->AskColor();
             }
 
             status_ = MissionStatus::DONE;

src/modelec_strat/src/missions/thermo_mission.cpp

@@ -0,0 +1,149 @@
+#include <modelec_strat/missions/thermo_mission.hpp>
+
+#include "modelec_strat/action/base_action.hpp"
+
+namespace Modelec {
+
+    bool ThermoMission::IsThermoDone = false;
+
+    ThermoMission::ThermoMission(const std::shared_ptr<NavigationHelper>& nav,
+        const std::shared_ptr<ActionExecutor>& action_executor)
+     : status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
+    {
+    }
+
+    void ThermoMission::Start(rclcpp::Node::SharedPtr node)
+    {
+        node_ = node;
+
+        go_timeout_ = node_->now();
+        deploy_time_ = node_->now();
+        last_ask_waypoint_time_ = node_->now();
+
+        status_ = MissionStatus::RUNNING;
+
+        thermo_positions_ = nav_->GetThermoPositions();
+
+        std::queue<int> empty;
+        std::swap(steps_, empty);
+
+        steps_.push(GO_TO_THERMO);
+        steps_.push(GO_TO_THERMO_CLOSE);
+        steps_.push(ACTIVATE_THERMO);
+        steps_.push(GO_TO_10);
+        steps_.push(DEACTIVATE_THERMO);
+        steps_.push(DONE);
+    }
+
+    void ThermoMission::Update()
+    {
+        if (!action_executor_->IsActionDone())
+        {
+            return;
+        }
+
+        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();
+
+        switch (step_)
+        {
+        case GO_TO_THERMO:
+            {
+                auto start = thermo_positions_[0].GetTakePosition(CLOSE_DISTANCE, M_PI_2);
+
+                if (nav_->GoToRotateFirst(start, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE)
+                {
+                    status_ = MissionStatus::FAILED;
+                    break;
+                }
+
+                go_timeout_ = node_->now();
+            }
+            break;
+        case GO_TO_THERMO_CLOSE:
+            {
+                auto start = thermo_positions_[0];
+
+                if (nav_->GoToRotateFirst(start, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE)
+                {
+                    status_ = MissionStatus::FAILED;
+                    break;
+                }
+
+                go_timeout_ = node_->now();
+            }
+            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();
+            }
+            break;
+        case GO_TO_10:
+            {
+                auto pos = thermo_positions_[1];
+
+                if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE)
+                {
+                    status_ = MissionStatus::FAILED;
+                    break;
+                }
+            }
+            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();
+            }
+            break;
+        case DONE:
+            {
+                action_executor_->SendPoint(10);
+                IsThermoDone = true;
+            }
+
+            status_ = MissionStatus::DONE;
+            break;
+        default:
+            break;
+        }
+    }
+
+    void ThermoMission::Clear()
+    {
+    }
+
+    MissionStatus ThermoMission::GetStatus() const
+    {
+        return status_;
+    }
+
+}

src/modelec_strat/src/navigation_helper.cpp

@@ -1,7 +1,6 @@
 #include <modelec_strat/navigation_helper.hpp>
 #include <utility>
 #include <ament_index_cpp/get_package_share_directory.hpp>
-#include <modelec_utils/config.hpp>
 
 namespace Modelec
 {
@@ -10,12 +9,44 @@ namespace Modelec
 
     NavigationHelper::NavigationHelper(const rclcpp::Node::SharedPtr& node) : node_(node)
     {
+        node_->declare_parameter("factor_close_enemy", -0.5);
+        node_->declare_parameter("factor.theta", 20.0);
+        node_->declare_parameter("enemy.detection.min_emergency_distance_mm", 390);
+
+        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);
 
-        factor_close_enemy_ = Config::get<float>("config.enemy.factor_close_enemy", -0.5f);
-
-        enemy_emergency_distance_ = Config::get<int>("config.enemy.detection.min_emergency_distance_mm", 390);
-
         SetupSpawn();
 
         waypoint_reach_sub_ = node_->create_subscription<WaypointMsg>(
@@ -116,7 +147,7 @@ namespace Modelec
         return pathfinding_;
     }
 
-    int NavigationHelper::GetTeamId() const
+    NavigationHelper::Team NavigationHelper::GetTeamId() const
     {
         return team_id_;
     }
@@ -302,7 +333,7 @@ namespace Modelec
     {
         double angle = std::atan2(pos.y - current_pos_->y, pos.x - current_pos_->x);
 
-        if (std::abs(angle - (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4)
+        if (Point::angleDiff(angle, (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4)
         {
             Rotate(angle);
             return true;
@@ -518,9 +549,9 @@ namespace Modelec
             {
                 auto zonePoint = zone->GetPosition();
                 double distance = Point::distance(posPoint, zonePoint);
-                double enemy_distance = Point::distance(enemyPos, zone->GetPosition());
+                double enemy_distance = Point::distance(enemyPos, zonePoint);
                 double theta = std::abs(Point::angleDiff(posPoint, zonePoint));
-                double s = distance + enemy_distance * factor_close_enemy_ + theta * 2;
+                double s = distance + (enemy_distance * factor_close_enemy_ * has_enemy_) + theta * factor_theta_;
                 if (s < score)
                 {
                     score = s;
@@ -535,21 +566,35 @@ namespace Modelec
     PosMsg::SharedPtr NavigationHelper::GetHomePosition()
     {
         PosMsg::SharedPtr home = std::make_shared<PosMsg>();
-        if (team_id_ == YELLOW)
-        {
-            home->x = Config::get<int>("config.home.yellow@x", 0);
-            home->y = Config::get<int>("config.home.yellow@y", 0);
-            home->theta = Config::get<double>("config.home.yellow@theta", 0);
-        }
-        else
-        {
-            home->x = Config::get<int>("config.home.blue@x", 0);
-            home->y = Config::get<int>("config.home.blue@y", 0);
-            home->theta = Config::get<double>("config.home.blue@theta", 0);
-        }
+
+        std::string prefix = (team_id_ == YELLOW) ? "home.yellow" : "home.blue";
+
+        home->x = node_->get_parameter(prefix + ".x").as_int();
+        home->y = node_->get_parameter(prefix + ".y").as_int();
+        home->theta = node_->get_parameter(prefix + ".theta").as_double();
+
         return home;
     }
 
+    std::array<Point, 2> NavigationHelper::GetThermoPositions()
+    {
+        std::string prefix = (team_id_ == YELLOW) ? "thermo.yellow" : "thermo.blue";
+
+        Point start(
+            node_->get_parameter(prefix + ".start.x").as_int(),
+            node_->get_parameter(prefix + ".start.y").as_int(),
+            node_->get_parameter(prefix + ".start.theta").as_double()
+        );
+
+        Point finish(
+            node_->get_parameter(prefix + ".finish.x").as_int(),
+            node_->get_parameter(prefix + ".finish.y").as_int(),
+            node_->get_parameter(prefix + ".finish.theta").as_double()
+        );
+
+        return {start, finish};
+    }
+
     void NavigationHelper::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
     {
         if (!has_enemy_) has_enemy_ = true;
@@ -691,7 +736,7 @@ namespace Modelec
         return true;
     }
 
-    void NavigationHelper::SetTeamId(int id)
+    void NavigationHelper::SetTeamId(Team id)
     {
         team_id_ = id;
     }
@@ -722,6 +767,7 @@ namespace Modelec
 
     void NavigationHelper::AskWaypoint()
     {
+        RCLCPP_DEBUG(node_->get_logger(), "Asking for active waypoint...");
         std_msgs::msg::Empty msg;
         odo_ask_waypoint_pub_->publish(msg);
     }
@@ -758,15 +804,15 @@ namespace Modelec
     void NavigationHelper::SetupSpawn()
     {
         spawn_yellow_["top"] = Point(
-            Config::get<int>("config.spawn.yellow.top@x"),
-            Config::get<int>("config.spawn.yellow.top@y"),
-            Config::get<double>("config.spawn.yellow.top@theta")
+            node_->get_parameter("spawn.yellow_top.x").as_int(),
+            node_->get_parameter("spawn.yellow_top.y").as_int(),
+            node_->get_parameter("spawn.yellow_top.theta").as_double()
         );
 
         spawn_blue_["top"] = Point(
-            Config::get<int>("config.spawn.blue.top@x"),
-            Config::get<int>("config.spawn.blue.top@y"),
-            Config::get<double>("config.spawn.blue.top@theta")
+            node_->get_parameter("spawn.blue_top.x").as_int(),
+            node_->get_parameter("spawn.blue_top.y").as_int(),
+            node_->get_parameter("spawn.blue_top.theta").as_double()
         );
     }
 }

src/modelec_strat/src/obstacle/box.cpp

@@ -1,5 +1,7 @@
 #include <modelec_strat/obstacle/box.hpp>
 
+#include "modelec_utils/utils.hpp"
+
 namespace Modelec
 {
     BoxObstacle::BoxObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type)
@@ -57,4 +59,70 @@ namespace Modelec
         }
         return positions;
     }
+
+    void BoxObstacle::SetColor(size_t index, Team team)
+    {
+        if (index < colors_.size())
+        {
+            colors_[index] = team;
+        }
+    }
+
+    BoxObstacle::Team BoxObstacle::GetColor(size_t index) const
+    {
+        if (index < colors_.size())
+        {
+            return colors_[index];
+        }
+        return YELLOW;
+    }
+
+    std::array<BoxObstacle::Team, 4> BoxObstacle::GetColors() const
+    {
+        return colors_;
+    }
+
+    std::vector<int> BoxObstacle::GetSide(Team team) const
+    {
+        std::vector<int> sideColors;
+        for (size_t i = 0; i < colors_.size(); ++i)
+        {
+            if (colors_[i] == team)
+            {
+                sideColors.push_back(i);
+            }
+        }
+        return sideColors;
+    }
+
+    void BoxObstacle::ParseColor(const std::string& colorStr)
+    {
+        std::vector<std::string> tokens = split(colorStr, ';');
+        for (size_t i = 0; i < tokens.size() && i < colors_.size(); ++i)
+        {
+            if (tokens[i] == "yellow")
+            {
+                colors_[i] = YELLOW;
+            }
+            else if (tokens[i] == "blue")
+            {
+                colors_[i] = BLUE;
+            }
+        }
+    }
+
+    void BoxObstacle::ParseColor(const std::vector<std::string>& colorVec)
+    {
+        for (size_t i = 0; i < colorVec.size() && i < colors_.size(); ++i)
+        {
+            if (colorVec[i] == "yellow")
+            {
+                colors_[i] = YELLOW;
+            }
+            else if (colorVec[i] == "blue")
+            {
+                colors_[i] = BLUE;
+            }
+        }
+    }
 }

src/modelec_strat/src/pami_manager.cpp

@@ -5,23 +5,13 @@ namespace Modelec
 {
     PamiManger::PamiManger() : Node("pami_manager")
     {
-        std::string config_file = ament_index_cpp::get_package_share_directory("modelec_strat") +
-            "/data/config.xml";
+        this->declare_parameter("time_to_put_zone", 80);
+        this->declare_parameter("time_to_remove_top_pot", 70);
 
-        if (!Config::load(config_file))
-        {
-            RCLCPP_ERROR(get_logger(), "Failed to load configuration");
-        }
+        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();
 
-        time_to_put_zone_ = Config::get<int>("config.pami.time_to_put_zone", 80);
-        time_to_remove_top_pot_ = Config::get<int>("config.pami.time_to_remove_top_pot", 70);
-
-        score_goupie_ = Config::get<int>("config.mission_score.pami.goupie");
-        score_superstar_ = Config::get<int>("config.mission_score.pami.superstar");
-        score_all_party_ = Config::get<int>("config.mission_score.pami.all_party");
-        score_free_zone_ = 0;
-
-        score_to_add_ = 0; //score_goupie_ + score_superstar_ + /*score_all_party_ +*/ score_free_zone_;
+        score_to_add_ = 0;
 
         std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") +
             "/data/pami_zone.xml";
@@ -31,9 +21,9 @@ namespace Modelec
         }
 
         start_time_sub_ = create_subscription<std_msgs::msg::Int64>(
-            "/strat/start_time", 10, [this](const std_msgs::msg::Int64::SharedPtr msg)
+            "/strat/start_time", 10, [this](const std_msgs::msg::Int64::SharedPtr)
             {
-                auto now = std::chrono::system_clock::now().time_since_epoch();
+                /*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_);
 
@@ -51,7 +41,7 @@ namespace Modelec
                         }
 
                         timer_add_->cancel();
-                    });
+                    });*/
             });
 
         add_obs_pub_ = create_publisher<modelec_interfaces::msg::Obstacle>(
@@ -65,10 +55,10 @@ namespace Modelec
         reset_strat_sub_ = create_subscription<std_msgs::msg::Empty>(
             "/strat/reset", 10, [this](const std_msgs::msg::Empty::SharedPtr)
             {
-                if (timer_add_)
+                /*if (timer_add_)
                 {
                     timer_add_->cancel();
-                }
+                }*/
                 /*if (timer_remove_)
                 {
                     timer_remove_->cancel();

src/modelec_strat/src/pathfinding.cpp

@@ -1,6 +1,5 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <modelec_strat/pathfinding.hpp>
-#include <modelec_utils/config.hpp>
 
 namespace Modelec {
     struct AStarNode {
@@ -23,22 +22,33 @@ namespace Modelec {
     }
 
     Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr &node) : node_(node) {
-        map_width_mm_ = Config::get<int>("config.map.size.map_width_mm", 3000);
-        map_height_mm_ = Config::get<int>("config.map.size.map_height_mm", 2000);
+        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);
 
-        robot_length_mm_ = Config::get<int>("config.robot.size.length_mm", 300);
-        robot_width_mm_ = Config::get<int>("config.robot.size.width_mm", 300);
-        margin_mm_ = Config::get<int>("config.robot.size.margin_mm", 100);
+        map_width_mm_ = node_->get_parameter("map.size.map_width_mm").as_int();
+        map_height_mm_ = node_->get_parameter("map.size.map_height_mm").as_int();
+        grid_width_ = node_->get_parameter("map.size.grid_width").as_int();
+        grid_height_ = node_->get_parameter("map.size.grid_height").as_int();
 
-        enemy_length_mm_ = Config::get<int>("config.enemy.size.length_mm", 300);
-        enemy_width_mm_ = Config::get<int>("config.enemy.size.width_mm", 300);
+        node_->declare_parameter("robot.size.length_mm", 300);
+        node_->declare_parameter("robot.size.width_mm", 300);
+        node_->declare_parameter("robot.size.margin_mm", 100);
 
-        enemy_margin_mm_ = Config::get<int>("config.enemy.size.margin_mm", 50);
+        robot_length_mm_ = node_->get_parameter("robot.size.length_mm").as_int();
+        robot_width_mm_ = node_->get_parameter("robot.size.width_mm").as_int();
+        margin_mm_ = node_->get_parameter("robot.size.margin_mm").as_int();
 
-        factor_close_enemy_ = Config::get<float>("config.enemy.factor_close_enemy", -0.5f);
+        node_->declare_parameter("enemy.size.length_mm", 300);
+        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);
 
-        grid_width_ = Config::get<int>("config.map.size.grid_width", 300);
-        grid_height_ = Config::get<int>("config.map.size.grid_height", 200);
+        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";
@@ -262,7 +272,7 @@ namespace Modelec {
         const int goal_y = (grid_height_ - 1) - (goal->y / cell_size_mm_y);
 
         // log the x and y in the real format
-        RCLCPP_INFO(node_->get_logger(), "Start: (%d, %d), Goal: (%d, %d)", start_x * (int) cell_size_mm_x,
+        RCLCPP_DEBUG(node_->get_logger(), "Start: (%d, %d), Goal: (%d, %d)", start_x * (int) cell_size_mm_x,
                     (grid_height_ - 1 - start_y) * (int) cell_size_mm_y,
                     goal_x * (int) cell_size_mm_x,
                     (grid_height_ - 1 - goal_y) * (int) cell_size_mm_y);

src/modelec_strat/src/strat_fms.cpp

@@ -6,6 +6,7 @@
 #include <modelec_strat/missions/go_home_mission.hpp>
 #include <modelec_strat/missions/take_mission.hpp>
 #include <modelec_strat/missions/free_mission.hpp>
+#include <modelec_strat/missions/thermo_mission.hpp>
 
 #include "modelec_strat/action/base_action.hpp"
 
@@ -14,6 +15,16 @@ namespace Modelec
 
     StratFMS::StratFMS() : Node("start_fms")
     {
+        this->declare_parameter<bool>("static_strat", false);
+        this->declare_parameter<double>("factor.obs", 1.0);
+        this->declare_parameter<double>("factor.thermo", 0.5);
+        this->declare_parameter<int>("timer_period_ms", 100);
+
+        static_strat_ = this->get_parameter("static_strat").as_bool();
+        factor_obs_ = this->get_parameter("factor.obs").as_double();
+        factor_thermo_ = this->get_parameter("factor.thermo").as_double();
+        timer_period_ms_ = this->get_parameter("timer_period_ms").as_int();
+
         tir_sub_ = create_subscription<std_msgs::msg::Empty>(
             "/action/tir/start", 10, [this](const std_msgs::msg::Empty::SharedPtr)
             {
@@ -30,8 +41,7 @@ namespace Modelec
         team_id_sub_ = create_subscription<std_msgs::msg::Int8>(
             "/strat/team", 10, [this](const std_msgs::msg::Int8::SharedPtr msg)
             {
-                team_id_ = static_cast<int>(msg->data);
-                nav_->SetTeamId(team_id_);
+                nav_->SetTeamId(static_cast<NavigationHelper::Team>(msg->data));
             });
 
         spawn_id_sub_ = create_subscription<modelec_interfaces::msg::Spawn>(
@@ -39,7 +49,7 @@ namespace Modelec
             {
                 team_selected_ = true;
                 team_id_ = msg->team_id;
-                nav_->SetTeamId(team_id_);
+                nav_->SetTeamId(static_cast<NavigationHelper::Team>(team_id_));
                 nav_->SetSpawn(msg->name);
             });
 
@@ -59,16 +69,6 @@ namespace Modelec
             "/action/tir/arm/set", 10);
 
         start_odo_pub_ = create_publisher<std_msgs::msg::Bool>("/odometry/start", 10);
-
-        std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
-        if (!Config::load(config_path))
-        {
-            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str());
-        }
-
-        game_action_sequence_.push(State::TAKE_MISSION);
-        game_action_sequence_.push(State::FREE_MISSION);
-		static_strat_ = Config::get<bool>("config.static_strat", false);
     }
 
     void StratFMS::Init()
@@ -106,7 +106,7 @@ namespace Modelec
         current_mission_.reset();
         match_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
 
-        timer_ = create_wall_timer(std::chrono::milliseconds(100), [this]
+        timer_ = create_wall_timer(std::chrono::milliseconds(timer_period_ms_), [this]
         {
             Update();
         });
@@ -142,11 +142,19 @@ namespace Modelec
                 arm_msg.data = true;
                 tir_arm_set_pub_->publish(arm_msg);
 
-                action_executor_->Up(BaseAction::Front::BOTH, true);
+                action_executor_->Up(BaseAction::Side::BOTH, true);
                 action_executor_->Free({
                     {0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT},
                     {0, BaseAction::BACK}, {1, BaseAction::BACK}, {2, BaseAction::BACK}, {3, BaseAction::BACK},
-                }, true);
+                });
+
+                auto empty_queue_ = std::queue<State>();
+                std::swap(game_action_sequence_, empty_queue_);
+                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");
             }
@@ -170,18 +178,18 @@ namespace Modelec
 
         case State::SELECT_MISSION:
             {
-                auto elapsed = now - match_start_time_;
+                auto duration = (now - match_start_time_).seconds();
 
-                if (elapsed.seconds() >= 100)
+                if (duration >= 100)
                 {
                     Transition(State::STOP, "All missions done");
                 }
 
-                else if (elapsed.seconds() > 60 && !action_executor_->IsEmpty())
+                else if (duration > 60 && !action_executor_->IsEmpty() && duration < 90)
                 {
                     Transition(State::FREE_MISSION, "No Time left, freeing boxes");
                 }
-                else if (elapsed.seconds() < 80)
+                else if (duration < 80)
                 {
                     Transition(State::SELECT_GAME_ACTION, "Selecting a game action");
                 }
@@ -208,59 +216,69 @@ namespace Modelec
 					return;
 				}
 
+                auto pos = nav_->GetCurrentPos();
+                auto closestBox = nav_->GetClosestObstacle<BoxObstacle>(pos);
+                auto closestDeposite = nav_->GetClosestDepositeZone(pos);
 
-                // TODO : If close to border, do the side mission (thermometre)
+                auto thermoPos = nav_->GetThermoPositions()[0];
 
-                if (action_executor_->IsFull())
+                double distToBox = Point::distance(Point(pos->x, pos->y, pos->theta),
+                                            closestBox->GetPosition()) * factor_obs_;
+                double distToDeposite = Point::distance(Point(pos->x, pos->y, pos->theta),
+                                            closestDeposite->GetPosition());
+                double distToThermo = Point::distance(Point(pos->x, pos->y, pos->theta),
+                                            thermoPos) * factor_thermo_;
+
+                if (distToThermo < distToBox && distToThermo < distToDeposite && !ThermoMission::IsThermoDone)
                 {
-                    RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission");
-                    Transition(State::FREE_MISSION, "Selecting FREE mission");
-                }
-                else if (action_executor_->IsEmpty())
+                    RCLCPP_INFO(get_logger(), "Choosing THERMO mission (dist to thermo: %.2f < dist to box: %.2f, dist to deposite: %.2f)",
+                                distToThermo, distToBox, distToDeposite);
+                    Transition(State::THERMO_MISSION, "Selecting THERMO mission");
+                } else
                 {
-                    RCLCPP_INFO(get_logger(), "No box present on robot, selecting TAKE mission");
-                    Transition(State::TAKE_MISSION, "Selecting TAKE mission");
-                }
-                else
-                {
-                    auto pos = nav_->GetCurrentPos();
-                    auto closestBox = nav_->GetClosestObstacle<BoxObstacle>(pos);
-                    auto closestDeposite = nav_->GetClosestDepositeZone(pos);
-
-                    if (closestBox && closestDeposite)
+                    if (action_executor_->IsFull())
                     {
-                        double distToBox = Point::distance(Point(pos->x, pos->y, pos->theta),
-                                                          closestBox->GetPosition());
-                        double distToDeposite = Point::distance(Point(pos->x, pos->y, pos->theta),
-                                                               closestDeposite->GetPosition());
-
-                        if (distToBox < distToDeposite)
-                        {
-                            RCLCPP_INFO(get_logger(), "Choosing TAKE mission (dist to box: %.2f < dist to deposite: %.2f)",
-                                        distToBox, distToDeposite);
-                            Transition(State::TAKE_MISSION, "Selecting TAKE mission");
-                        }
-                        else
-                        {
-                            RCLCPP_INFO(get_logger(), "Choosing FREE mission (dist to deposite: %.2f < dist to box: %.2f)",
-                                        distToDeposite, distToBox);
-                            Transition(State::FREE_MISSION, "Selecting FREE mission");
-                        }
-                    }
-                    else if (closestBox)
-                    {
-                        RCLCPP_INFO(get_logger(), "Only box available, selecting TAKE mission");
-                        Transition(State::TAKE_MISSION, "Selecting TAKE mission");
-                    }
-                    else if (closestDeposite)
-                    {
-                        RCLCPP_INFO(get_logger(), "Only deposite available, selecting FREE mission");
+                        RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission");
                         Transition(State::FREE_MISSION, "Selecting FREE mission");
                     }
+                    else if (action_executor_->IsEmpty())
+                    {
+                        RCLCPP_INFO(get_logger(), "No box present on robot, selecting TAKE mission");
+                        Transition(State::TAKE_MISSION, "Selecting TAKE mission");
+                    }
                     else
                     {
-                        RCLCPP_WARN(get_logger(), "No box or deposite available, cannot select mission!");
-                        Transition(State::STOP, "No missions available");
+
+                        if (closestBox && closestDeposite)
+                        {
+                            if (distToBox < distToDeposite)
+                            {
+                                RCLCPP_INFO(get_logger(), "Choosing TAKE mission (dist to box: %.2f < dist to deposite: %.2f)",
+                                            distToBox, distToDeposite);
+                                Transition(State::TAKE_MISSION, "Selecting TAKE mission");
+                            }
+                            else
+                            {
+                                RCLCPP_INFO(get_logger(), "Choosing FREE mission (dist to deposite: %.2f < dist to box: %.2f)",
+                                            distToDeposite, distToBox);
+                                Transition(State::FREE_MISSION, "Selecting FREE mission");
+                            }
+                        }
+                        else if (closestBox)
+                        {
+                            RCLCPP_INFO(get_logger(), "Only box available, selecting TAKE mission");
+                            Transition(State::TAKE_MISSION, "Selecting TAKE mission");
+                        }
+                        else if (closestDeposite)
+                        {
+                            RCLCPP_INFO(get_logger(), "Only deposite available, selecting FREE mission");
+                            Transition(State::FREE_MISSION, "Selecting FREE mission");
+                        }
+                        else
+                        {
+                            RCLCPP_WARN(get_logger(), "No box or deposite available, cannot select mission!");
+                            Transition(State::STOP, "No missions available");
+                        }
                     }
                 }
             }
@@ -339,6 +357,27 @@ namespace Modelec
             }
             break;
 
+        case State::THERMO_MISSION:
+            {
+                if (!current_mission_)
+                {
+                    current_mission_ = std::make_unique<ThermoMission>(nav_, action_executor_);
+                    current_mission_->Start(shared_from_this());
+                }
+                current_mission_->Update();
+                if (current_mission_->GetStatus() == MissionStatus::DONE)
+                {
+                    current_mission_.reset();
+                    Transition(State::SELECT_MISSION, "Thermo done");
+                }
+                else if (current_mission_->GetStatus() == MissionStatus::FAILED)
+                {
+                    current_mission_.reset();
+                    RCLCPP_ERROR(get_logger(), "Thermo mission failed!");
+                    Transition(State::SELECT_MISSION, "Thermo mission failed");
+                }
+            }
+            break;
         case State::DO_GO_HOME:
             if (!current_mission_)
             {

src/modelec_utils/include/modelec_utils/config.hpp

@@ -4,6 +4,8 @@
 #include <tinyxml2.h>
 #include <unordered_map>
 #include <sstream>
+#include <vector>
+#include <functional>
 
 namespace Modelec
 {
@@ -11,13 +13,25 @@ namespace Modelec
     {
 
     public:
+
+        template<typename T>
+        using BuilderFunc = std::function<T(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());
 
+        template<typename T>
+        static std::vector<T> getArray(const std::string& prefix,
+                                    BuilderFunc<T> builder = [](const std::string& base) { return get<T>(base); });
+
+        static size_t count(const std::string& prefix);
+
+        static void printAll();
+
     private:
-        static void parseNode(tinyxml2::XMLElement* element, const std::string& prefix);
+        static void parseNode(tinyxml2::XMLElement* element, const std::string& key);
 
         static inline std::unordered_map<std::string, std::string> values_;
     };
@@ -44,4 +58,22 @@ namespace Modelec
         auto str = get<std::string>(key, default_value ? "true" : "false");
         return str == "true" || str == "1";
     }
+
+    template<typename T>
+    std::vector<T> Config::getArray(const std::string& prefix,
+                                BuilderFunc<T> builder)
+    {
+        std::vector<T> result;
+
+        size_t n = Config::count(prefix);
+        result.reserve(n);
+
+        for (size_t i = 0; i < n; ++i)
+        {
+            std::string base = prefix + "[" + std::to_string(i) + "]";
+            result.emplace_back(builder(base));
+        }
+
+        return result;
+    }
 }

src/modelec_utils/include/modelec_utils/point.hpp

@@ -1,7 +1,7 @@
 #pragma once
 
-#define CLOSE_DISTANCE 165
-#define BASE_DISTANCE 290
+#define CLOSE_DISTANCE 155
+#define BASE_DISTANCE 310
 
 namespace Modelec
 {
@@ -15,9 +15,15 @@ namespace Modelec
 
         static double distance(const Point& p1, const Point& p2);
         static double angleDiff(const Point& p1, const Point& p2);
+        static double angleDiff(const double& p1, const double& p2);
 
         double distance(const Point& p2) const;
         double angleDiff(const Point& p2) const;
+        double angleDiff(const double& p2) const;
+
+        void normalizeAngle();
+
+        static double normalizeAngle(double angle);
 
         [[nodiscard]] Point GetTakePosition(int distance, double angle) const;
         [[nodiscard]] Point GetTakePosition(int distance) const;

src/modelec_utils/src/config.cpp

@@ -1,3 +1,4 @@
+#include <iostream>
 #include <modelec_utils/config.hpp>
 
 namespace Modelec
@@ -15,27 +16,77 @@ namespace Modelec
             return false;
         }
 
-        parseNode(root, "");
+        parseNode(root, root->Name());
         return true;
     }
 
-    void Config::parseNode(tinyxml2::XMLElement* element, const std::string& prefix) {
-        std::string key_prefix = prefix.empty() ? element->Name() : prefix + "." + element->Name();
+    size_t Config::count(const std::string& prefix)
+    {
+        size_t max_index = 0;
+        bool found = false;
 
-        const char* text = element->GetText();
-        if (text && std::string(text).find_first_not_of(" \n\t") != std::string::npos) {
-            values_[key_prefix] = text;
+        for (const auto& [key, _] : values_)
+        {
+            if (key.rfind(prefix + "[", 0) == 0)
+            {
+                auto start = key.find('[', prefix.size());
+                auto end = key.find(']', start);
+                if (start != std::string::npos && end != std::string::npos)
+                {
+                    size_t index = std::stoul(key.substr(start + 1, end - start - 1));
+                    max_index = std::max(max_index, index);
+                    found = true;
+                }
+            }
         }
 
-        const tinyxml2::XMLAttribute* attr = element->FirstAttribute();
-        while (attr) {
-            std::string attr_key = key_prefix + "@" + attr->Name();
-            values_[attr_key] = attr->Value();
-            attr = attr->Next();
+        return found ? max_index + 1 : 0;
+    }
+
+    void Config::printAll()
+    {
+        for (const auto& [key, value] : values_)
+        {
+            std::cout << key << " = " << value << std::endl;
+        }
+    }
+
+    void Config::parseNode(tinyxml2::XMLElement* element, const std::string& key) {
+        if (const char* text = element->GetText())
+        {
+            if (std::string(text).find_first_not_of(" \n\t") != std::string::npos)
+            {
+                values_[key] = text;
+            }
         }
 
-        for (tinyxml2::XMLElement* child = element->FirstChildElement(); child; child = child->NextSiblingElement()) {
-            parseNode(child, key_prefix);
+        // 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())
+        {
+            std::string child_key = key + "." + child->Name();
+
+            if (child_count[child->Name()] > 1)
+            {
+                int index = child_index[child->Name()]++;
+                child_key += "[" + std::to_string(index) + "]";
+            }
+
+            parseNode(child, child_key);
         }
     }
 }

src/modelec_utils/src/point.cpp

@@ -9,12 +9,27 @@ namespace Modelec
     }
 
     double Point::angleDiff(const Point &p1, const Point &p2) {
-        double diff = std::fmod(p1.theta - p2.theta + M_PI, 2 * M_PI);
+        return angleDiff(p1.theta, p2.theta);
+    }
+
+    double Point::angleDiff(const double& p1, const double& p2)
+    {
+        double diff = std::fmod(p1 - p2 + M_PI, 2 * M_PI);
         if (diff < 0)
             diff += 2 * M_PI;
         return diff - M_PI;
     }
 
+    double Point::normalizeAngle(double angle)
+    {
+        double newAngle = std::fmod(angle + M_PI, 2.0 * M_PI);
+        if (newAngle < 0) {
+            newAngle += 2.0 * M_PI;
+        }
+
+        return newAngle - M_PI;
+    }
+
     double Point::distance(const Point& p2) const
     {
         return distance(*this, p2);
@@ -25,6 +40,16 @@ namespace Modelec
         return angleDiff(*this, p2);
     }
 
+    double Point::angleDiff(const double& p2) const
+    {
+        return angleDiff(theta, p2);
+    }
+
+    void Point::normalizeAngle()
+    {
+        theta = normalizeAngle(theta);
+    }
+
     Point Point::GetTakePosition(int distance, double angle) const
     {
         Point pos;

src/modelec_utils/test/config.test.cpp

@@ -48,3 +48,69 @@ TEST(ConfigTest, DefaultValues)
     EXPECT_EQ(Modelec::Config::get<std::string>("missing.string", "default"), "default");
     EXPECT_FALSE(Modelec::Config::get<bool>("missing.bool", false));
 }
+
+TEST(ConfigTest, CountArray)
+{
+    // Create temporary XML file
+    const std::string filepath = "test_array_config.xml";
+
+    std::ofstream file(filepath);
+    file <<
+        "<root>"
+        "   <item>1</item>"
+        "   <item>2</item>"
+        "   <item>3</item>"
+        "</root>";
+    file.close();
+
+    EXPECT_TRUE(Modelec::Config::load(filepath));
+
+    EXPECT_EQ(Modelec::Config::count("root.item"), 3);
+}
+
+TEST(ConfigTest, GetArray)
+{
+    // 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>"
+        "</root>";
+    file.close();
+
+    struct Item
+    {
+        int value;
+        std::string attr;
+    };
+
+    EXPECT_TRUE(Modelec::Config::load(filepath));
+
+    auto array = Modelec::Config::getArray<int>("root.item");
+
+    EXPECT_EQ(array.size(), 3);
+    EXPECT_EQ(array[0], 1);
+    EXPECT_EQ(array[1], 2);
+    EXPECT_EQ(array[2], 3);
+
+    auto array2 = Modelec::Config::getArray<Item>("root.item",
+        [](const std::string& base)
+        {
+            return Item{
+                Modelec::Config::get<int>(base, 0),
+                Modelec::Config::get<std::string>(base + "@a", "")
+            };
+        });
+
+    EXPECT_EQ(array2.size(), 3);
+    EXPECT_EQ(array2[0].value, 1);
+    EXPECT_EQ(array2[0].attr, "b");
+    EXPECT_EQ(array2[1].value, 2);
+    EXPECT_EQ(array2[1].attr, "c");
+    EXPECT_EQ(array2[2].value, 3);
+    EXPECT_EQ(array2[2].attr, "d");
+}

src/modelec_utils/test/point.test.cpp

@@ -33,13 +33,13 @@ TEST(PointTest, GetTakePositionDefaultAngle) {
 TEST(PointTest, GetTakeBasePosition) {
     Modelec::Point p(0, 0, 0);
     Modelec::Point base = p.GetTakeBasePosition();
-    EXPECT_EQ(base.x, 290);
+    EXPECT_EQ(base.x, BASE_DISTANCE);
     EXPECT_EQ(base.y, 0);
 }
 
 TEST(PointTest, GetTakeClosePosition) {
     Modelec::Point p(0, 0, 0);
     Modelec::Point close = p.GetTakeClosePosition();
-    EXPECT_EQ(close.x, 165);
+    EXPECT_EQ(close.x, CLOSE_DISTANCE);
     EXPECT_EQ(close.y, 0);
 }

start_ros2.sh

@@ -5,8 +5,8 @@ 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 RMW_IMPLEMENTATION=rmw_fastrtps_cpp
 export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/Modelec-ROS2/fastdds_setup.xml
-export ROS_DOMAIN_ID=128
+#export ROS_DOMAIN_ID=128
 
-exec ros2 launch modelec_core modelec.launch.py "$@"
+exec ros2 launch modelec_core modelec.launch.py with_color_detector:=false "$@"