up to last odo version

install.sh

@@ -21,24 +21,14 @@ sudo apt upgrade -y
 
 sudo apt install ros-dev-tools ros-jazzy-desktop -y
 
+sudo apt-get install qt6-base-dev qt6-svg-dev libxml2-dev socat -y
+
 git submodule init
 git submodule update
 
-cd WiringPi || exit 1
+echo "source /opt/ros/jazzy/setup.bash
-
+source ~/modelec-marcel-ROS/install/setup.bash
-./build debian
+export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
-mv ./debian-template/wiringpi_*.deb .
-
-# install it
-sudo apt install ./wiringpi_*.deb -y
-
-cd ..
-
-sudo apt-get install qt6-base-dev qt6-svg-dev libxml2-dev socat -y
-
-echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc
-echo "source ~/modelec-marcel-ROS/install/setup.bash" >> ~/.bashrc
-echo "export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
 export FASTRTPS_DEFAULT_PROFILES_FILE=~/modelec-marcel-ROS/fastdds_setup.xml
 export ROS_DOMAIN_ID=128" >> ~/.bashrc
 

simulated_pcb/action.py

@@ -122,7 +122,7 @@ class SimulatedPCB:
 
 if __name__ == '__main__':
     parser = argparse.ArgumentParser(description="Simulated PCB")
-    parser.add_argument('--port', type=str, default='/dev/pts/6', help='Serial port to use')
+    parser.add_argument('--port', type=str, default='/tmp/USB_ACTION_SIM', help='Serial port to use')
     args = parser.parse_args()
 
     sim = None
@@ -132,3 +132,8 @@ if __name__ == '__main__':
         if sim:
             sim.stop()
         print("Simulation stopped")
+
+# socat -d -d pty,raw,echo=0,link=/tmp/MARCEL_ACTION_SIM pty,raw,echo=0,link=/tmp/MARCEL_ACTION
+# python3 simulated_pcb/action.py --port /tmp/MARCEL_ACTION_SIM
+# socat -d -d pty,raw,echo=0,link=/tmp/ENEMY_ACTION_SIM pty,raw,echo=0,link=/tmp/ENEMY_ACTION
+# python3 simulated_pcb/action.py --port /tmp/ENEMY_ACTION_SIM

simulated_pcb/odo.py

@@ -124,20 +124,24 @@ class SimulatedPCB:
             print(f'[TX] OK;START;1')
             self.ser.write(f'OK;START;1\n'.encode())
 
-        elif msg.startswith("SET;WAYPOINT"):
+        elif msg.startswith("SET;WAYPOINTS;"):
             parts = msg.split(';')
-            if len(parts) >= 7:
+            if len(parts) / 7 >= 1:
-                wp = {
+                self.waypoints.clear()
-                    'id': int(parts[2]),
+                self.waypoint_order.clear()
-                    'type': int(parts[3]),
+                for i in range(2, len(parts), 5):
-                    'x': float(parts[4]),
+                    wp = {
-                    'y': float(parts[5]),
+                        'id': int(parts[i]),
-                    'theta': float(parts[6])
+                        'type': int(parts[i + 1]),
-                }
+                        'x': float(parts[i + 2]),
-                self.waypoints[wp['id']] = wp
+                        'y': float(parts[i + 3]),
-                if wp['id'] not in self.waypoint_order:
+                        'theta': float(parts[i + 4])
-                    self.waypoint_order.append(wp['id'])
+                    }
-                self.ser.write(f'OK;WAYPOINT;{wp["id"]}\n'.encode())
+                    self.waypoints[wp['id']] = wp
+                    if wp['id'] not in self.waypoint_order:
+                        self.waypoint_order.append(wp['id'])
+                        self.ser.write(f'OK;WAYPOINT;{wp["id"]}\n'.encode())
+                self.ser.write(b'OK;WAYPOINTS;1\n')
 
         elif msg.startswith("SET;POS"):
             parts = msg.split(';')

src/modelec_com/include/modelec_com/pcb_odo_interface.hpp

@@ -11,8 +11,8 @@
 #include <modelec_interfaces/msg/odometry_pos.hpp>
 #include <modelec_interfaces/msg/odometry_speed.hpp>
 #include <modelec_interfaces/msg/odometry_to_f.hpp>
-#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
+#include <modelec_interfaces/msg/odometry_waypoint.hpp>
-#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
+#include <modelec_interfaces/msg/odometry_waypoints.hpp>
 #include <modelec_interfaces/msg/odometry_start.hpp>
 #include <modelec_interfaces/msg/odometry_pid.hpp>
 
@@ -64,83 +64,22 @@ namespace Modelec
         rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr odo_get_pos_sub_;
         rclcpp::Publisher<modelec_interfaces::msg::OdometrySpeed>::SharedPtr odo_speed_publisher_;
         rclcpp::Publisher<modelec_interfaces::msg::OdometryToF>::SharedPtr odo_tof_publisher_;
-        rclcpp::Publisher<modelec_interfaces::msg::OdometryWaypointReach>::SharedPtr odo_waypoint_reach_publisher_;
+        rclcpp::Publisher<modelec_interfaces::msg::OdometryWaypoint>::SharedPtr odo_waypoint_reach_publisher_;
         rclcpp::Publisher<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_pid_publisher_;
 
-        rclcpp::Subscription<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr odo_add_waypoint_subscriber_;
+        rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoint>::SharedPtr odo_add_waypoint_subscriber_;
+        rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoints>::SharedPtr odo_add_waypoints_subscriber_;
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_set_pos_subscriber_;
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_set_pid_subscriber_;
 
-        void AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg);
+        void AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg);
+        void AddWaypointsCallback(const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg);
         void SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
         void SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg);
 
-        rclcpp::Service<modelec_interfaces::srv::OdometryToF>::SharedPtr get_tof_service_;
-        rclcpp::Service<modelec_interfaces::srv::OdometrySpeed>::SharedPtr get_speed_service_;
-        rclcpp::Service<modelec_interfaces::srv::OdometryPosition>::SharedPtr get_position_service_;
-        rclcpp::Service<modelec_interfaces::srv::OdometryStart>::SharedPtr set_start_service_;
-        rclcpp::Service<modelec_interfaces::srv::OdometryGetPid>::SharedPtr get_pid_service_;
-        rclcpp::Service<modelec_interfaces::srv::OdometrySetPid>::SharedPtr set_pid_service_;
-        rclcpp::Service<modelec_interfaces::srv::OdometryAddWaypoint>::SharedPtr add_waypoint_service_;
-
         rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr start_odo_sub_;
         bool start_odo_ = false;
 
-        // rclcpp::TimerBase::SharedPtr odo_get_pos_timer_;
-
-        // Promises and mutexes to synchronize service responses asynchronously
-        std::queue<std::promise<long>> tof_promises_;
-        std::mutex tof_mutex_;
-
-        std::queue<std::promise<OdometryData>> speed_promises_;
-        std::mutex speed_mutex_;
-
-        std::queue<std::promise<OdometryData>> pos_promises_;
-        std::mutex pos_mutex_;
-
-        std::queue<std::promise<bool>> start_promises_;
-        std::mutex start_mutex_;
-
-        std::queue<std::promise<PIDData>> get_pid_promises_;
-        std::mutex get_pid_mutex_;
-
-        std::queue<std::promise<bool>> set_pid_promises_;
-        std::mutex set_pid_mutex_;
-
-        std::queue<std::promise<bool>> add_waypoint_promises_;
-        std::mutex add_waypoint_mutex_;
-
-        // Service handlers using async wait with promises
-        void HandleGetTof(const std::shared_ptr<modelec_interfaces::srv::OdometryToF::Request> request,
-                          std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response);
-
-        void HandleGetSpeed(const std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Request> request,
-                            std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response);
-
-        void HandleGetPosition(const std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Request> request,
-                               std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response);
-
-        void HandleGetStart(const std::shared_ptr<modelec_interfaces::srv::OdometryStart::Request> request,
-                            std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response);
-
-        void HandleGetPID(const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request> request,
-                          std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response);
-
-        void HandleSetPID(const std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Request> request,
-                          std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response);
-
-        void HandleAddWaypoint(const std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Request> request,
-                               std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Response> response);
-
-        // Resolving methods called by subscriber callback
-        void ResolveToFRequest(long distance);
-        void ResolveSpeedRequest(const OdometryData& speed);
-        void ResolvePositionRequest(const OdometryData& position);
-        void ResolveStartRequest(bool start);
-        void ResolveGetPIDRequest(const PIDData& pid);
-        void ResolveSetPIDRequest(bool success);
-        void ResolveAddWaypointRequest(bool success);
-
         int timeout_ms = 1000;
         int attempt = 5;
 
@@ -161,7 +100,8 @@ namespace Modelec
         void SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
         void SetRobotPos(long x, long y, double theta);
 
-        void AddWaypoint(modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg);
+        void AddWaypoints(modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg);
+        void AddWaypoint(modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg);
         void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta);
 
         void SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg);
@@ -169,6 +109,6 @@ namespace Modelec
 
         void GetPID();
         void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg);
-        void SetPID(float p, float i, float d);
+        void SetPID(std::string name, float p, float i, float d);
     };
 } // namespace Modelec

src/modelec_com/src/pcb_alim_interface.cpp

@@ -8,16 +8,18 @@ namespace Modelec
     PCBAlimInterface::PCBAlimInterface() : Node("pcb_alim_interface")
     {
         // Service to create a new serial listener
-        std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
+        declare_parameter<std::string>("serial_port", "/dev/ttyUSB0");
-        if (!Config::load(config_path))
+        declare_parameter<int>("baudrate", 115200);
-        {
+        declare_parameter<std::string>("name", "pcb_action");
-            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str());
-        }
 
         auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
-        request->name = Config::get<std::string>("config.usb.pcb.pcb_alim.name", "pcb_odo");
+        request->name = get_parameter("name").as_string();
-        request->bauds = Config::get<int>("config.usb.pcb.pcb_alim.baudrate", 115200);
+        request->bauds = get_parameter("baudrate").as_int();
-        request->serial_port = Config::get<std::string>("config.usb.pcb.pcb_alim.port", "/dev/ttyUSB0");
+        request->serial_port = get_parameter("serial_port").as_string();
+
+        RCLCPP_INFO(this->get_logger(), "Requesting serial listener with parameters: "
+            "name='%s', bauds=%ld, serial_port='%s'",
+            request->name.c_str(), request->bauds, request->serial_port.c_str());
 
         auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
         while (!client->wait_for_service(std::chrono::seconds(1)))

src/modelec_com/src/pcb_odo_interface.cpp

@@ -8,17 +8,15 @@ namespace Modelec
 {
     PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface")
     {
-        std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
+        declare_parameter<std::string>("serial_port", "/dev/USB_ODO");
-        if (!Config::load(config_path))
+        declare_parameter<int>("baudrate", 115200);
-        {
+        declare_parameter<std::string>("name", "pcb_action");
-            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str());
-        }
 
         // Service to create a new serial listener
         auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
-        request->name = Config::get<std::string>("config.usb.pcb.pcb_odo.name", "pcb_odo");
+        request->name = get_parameter("name").as_string();
-        request->bauds = Config::get<int>("config.usb.pcb.pcb_odo.baudrate", 115200);
+        request->bauds = get_parameter("baudrate").as_int();
-        request->serial_port = Config::get<std::string>("config.usb.pcb.pcb_odo.port", "/dev/ttyUSB0");
+        request->serial_port = get_parameter("serial_port").as_string();
 
         auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
         while (!client->wait_for_service(std::chrono::seconds(1)))
@@ -96,21 +94,26 @@ namespace Modelec
         odo_tof_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryToF>(
             "odometry/tof", 10);
 
-        odo_waypoint_reach_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryWaypointReach>(
+        odo_waypoint_reach_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryWaypoint>(
             "odometry/waypoint_reach", 10);
 
         odo_pid_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryPid>(
             "odometry/get_pid", 10);
 
-        odo_add_waypoint_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>(
+        odo_add_waypoint_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryWaypoint>(
             "odometry/add_waypoint", 30,
-            [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg)
+            [this](const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg)
             {
-                RCLCPP_INFO(this->get_logger(), "Received add waypoint request: %d %s %d %d %f",
-                            msg->id, msg->is_end ? "end" : "not end", msg->x, msg->y, msg->theta);
                 AddWaypointCallback(msg);
             });
 
+        odo_add_waypoints_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryWaypoints>(
+            "odometry/add_waypoint", 30,
+            [this](const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg)
+            {
+                AddWaypointsCallback(msg);
+            });
+
         odo_set_pos_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryPos>(
             "odometry/set_position", 10,
             [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
@@ -125,63 +128,6 @@ namespace Modelec
                 SetPIDCallback(msg);
             });
 
-        // Services
-        get_tof_service_ = create_service<modelec_interfaces::srv::OdometryToF>(
-            "odometry/tof",
-            [this](const std::shared_ptr<modelec_interfaces::srv::OdometryToF::Request> request,
-                   std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response)
-            {
-                HandleGetTof(request, response);
-            });
-
-        get_speed_service_ = create_service<modelec_interfaces::srv::OdometrySpeed>(
-            "odometry/speed",
-            [this](const std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Request> request,
-                   std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response)
-            {
-                HandleGetSpeed(request, response);
-            });
-
-        get_position_service_ = create_service<modelec_interfaces::srv::OdometryPosition>(
-            "odometry/get_position",
-            [this](const std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Request> request,
-                   std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response)
-            {
-                HandleGetPosition(request, response);
-            });
-
-        set_start_service_ = create_service<modelec_interfaces::srv::OdometryStart>(
-            "odometry/start",
-            [this](const std::shared_ptr<modelec_interfaces::srv::OdometryStart::Request> request,
-                   std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response)
-            {
-                HandleGetStart(request, response);
-            });
-
-        get_pid_service_ = create_service<modelec_interfaces::srv::OdometryGetPid>(
-            "odometry/get_pid",
-            [this](const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request> request,
-                   std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response)
-            {
-                HandleGetPID(request, response);
-            });
-
-        set_pid_service_ = create_service<modelec_interfaces::srv::OdometrySetPid>(
-            "odometry/set_pid",
-            [this](const std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Request> request,
-                   std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response)
-            {
-                HandleSetPID(request, response);
-            });
-
-        add_waypoint_service_ = create_service<modelec_interfaces::srv::OdometryAddWaypoint>(
-            "odometry/add_waypoint",
-            [this](const std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Request> request,
-                   std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Response> response)
-            {
-                HandleAddWaypoint(request, response);
-            });
-
         start_odo_sub_ = this->create_subscription<std_msgs::msg::Bool>(
             "odometry/start", 10,
             [this](const std_msgs::msg::Bool::SharedPtr msg)
@@ -193,18 +139,6 @@ namespace Modelec
                     SendOrder("START", {std::to_string(msg->data)});
                 }
             });
-
-        /*odo_get_pos_timer_ = this->create_wall_timer(
-            std::chrono::milliseconds(500),
-            [this]()
-            {
-                if (isOk && start_odo_)
-                {
-                    RCLCPP_INFO(this->get_logger(), "Requesting position from PCB");
-
-                    GetPos();
-                }
-            });*/
     }
 
     PCBOdoInterface::~PCBOdoInterface()
@@ -243,8 +177,6 @@ namespace Modelec
                 message.theta = theta;
 
                 odo_pos_publisher_->publish(message);
-
-                ResolvePositionRequest({x, y, theta});
             }
             else if (tokens[1] == "SPEED")
             {
@@ -258,7 +190,6 @@ namespace Modelec
                 message.theta = theta;
 
                 odo_speed_publisher_->publish(message);
-                ResolveSpeedRequest({x, y, theta});
             }
             else if (tokens[1] == "DIST")
             {
@@ -270,53 +201,46 @@ namespace Modelec
                 message.distance = dist;
 
                 odo_tof_publisher_->publish(message);
-                ResolveToFRequest(dist);
             }
             else if (tokens[1] == "WAYPOINT")
             {
                 int id = std::stoi(tokens[2]);
 
-                auto message = modelec_interfaces::msg::OdometryWaypointReach();
+                auto message = modelec_interfaces::msg::OdometryWaypoint();
                 message.id = id;
 
                 odo_waypoint_reach_publisher_->publish(message);
             }
             else if (tokens[1] == "PID")
             {
-                float p = std::stof(tokens[2]);
+                std::string name = tokens[2];
-                float i = std::stof(tokens[3]);
+                float p = std::stof(tokens[3]);
-                float d = std::stof(tokens[4]);
+                float i = std::stof(tokens[4]);
+                float d = std::stof(tokens[5]);
 
                 auto message = modelec_interfaces::msg::OdometryPid();
+                message.name = name;
                 message.p = p;
                 message.i = i;
                 message.d = d;
 
                 odo_pid_publisher_->publish(message);
-                ResolveGetPIDRequest({p, i, d});
             }
         }
         else if (tokens[0] == "OK")
         {
             if (tokens[1] == "START")
             {
-                // bool start = std::stoi(tokens[2]);
-                ResolveStartRequest(true);
             }
             else if (tokens[1] == "WAYPOINT")
             {
-                bool success = true;
-                ResolveAddWaypointRequest(success);
             }
 
             else if (tokens[1] == "PID")
             {
-                bool success = true;
-                ResolveSetPIDRequest(success);
             }
             else if (tokens[1] == "POS")
             {
-                // position set
             }
             else
             {
@@ -327,19 +251,13 @@ namespace Modelec
         {
             if (tokens[1] == "START")
             {
-                bool start = false;
-                ResolveStartRequest(start);
             }
             else if (tokens[1] == "WAYPOINT")
             {
-                bool success = false;
-                ResolveAddWaypointRequest(success);
             }
 
             else if (tokens[1] == "PID")
             {
-                bool success = false;
-                ResolveSetPIDRequest(success);
             }
             else
             {
@@ -348,7 +266,11 @@ namespace Modelec
         }
     }
 
-    void PCBOdoInterface::AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg)
+    void PCBOdoInterface::AddWaypointsCallback(const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg) {
+        AddWaypoints(msg);
+    }
+
+    void PCBOdoInterface::AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg)
     {
         AddWaypoint(msg);
     }
@@ -363,281 +285,6 @@ namespace Modelec
         SetPID(msg);
     }
 
-    void PCBOdoInterface::HandleGetTof(
-        const std::shared_ptr<modelec_interfaces::srv::OdometryToF::Request> request,
-        std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response)
-    {
-        if (!isOk)
-        {
-            RCLCPP_ERROR(get_logger(), "PCB not initialized");
-            return;
-        }
-
-        std::promise<long> promise;
-        auto future = promise.get_future();
-
-        {
-            std::lock_guard<std::mutex> lock(tof_mutex_);
-            tof_promises_.push(std::move(promise));
-        }
-
-        GetToF(request->n);
-
-        response->distance = future.get();
-    }
-
-    void PCBOdoInterface::HandleGetSpeed(
-        const std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Request>,
-        std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response)
-    {
-        if (!isOk)
-        {
-            RCLCPP_ERROR(get_logger(), "PCB not initialized");
-            return;
-        }
-
-        std::promise<OdometryData> promise;
-        auto future = promise.get_future();
-
-        {
-            std::lock_guard<std::mutex> lock(speed_mutex_);
-            speed_promises_.push(std::move(promise));
-        }
-
-        GetSpeed();
-
-        OdometryData result = future.get();
-        response->x = result.x;
-        response->y = result.y;
-        response->theta = result.theta;
-    }
-
-    void PCBOdoInterface::HandleGetPosition(
-        const std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Request>,
-        std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response)
-    {
-        if (!isOk)
-        {
-            RCLCPP_ERROR(get_logger(), "PCB not initialized");
-            return;
-        }
-
-        std::promise<OdometryData> promise;
-        auto future = promise.get_future();
-
-        {
-            std::lock_guard<std::mutex> lock(pos_mutex_);
-            pos_promises_.push(std::move(promise));
-        }
-
-        GetPos();
-
-        OdometryData result = future.get();
-        response->x = result.x;
-        response->y = result.y;
-        response->theta = result.theta;
-    }
-
-    void PCBOdoInterface::HandleGetStart(const std::shared_ptr<modelec_interfaces::srv::OdometryStart::Request> request,
-                                         std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response)
-    {
-        if (!isOk)
-        {
-            RCLCPP_ERROR(get_logger(), "PCB not initialized");
-            response->success = false;
-            return;
-        }
-
-        std::promise<bool> promise;
-        auto future = promise.get_future();
-
-        {
-            std::lock_guard<std::mutex> lock(start_mutex_);
-            start_promises_.push(std::move(promise));
-        }
-
-        SetStart(request->start);
-        response->success = future.get();
-    }
-
-    void PCBOdoInterface::HandleGetPID(const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request>,
-                                       std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response)
-    {
-        if (!isOk)
-        {
-            RCLCPP_ERROR(get_logger(), "PCB not initialized");
-            return;
-        }
-
-        std::promise<PIDData> promise;
-        auto future = promise.get_future();
-
-        {
-            std::lock_guard<std::mutex> lock(get_pid_mutex_);
-            get_pid_promises_.push(std::move(promise));
-        }
-
-        GetPID();
-
-        PIDData result = future.get();
-
-        response->p = result.p;
-        response->i = result.i;
-        response->d = result.d;
-    }
-
-    void PCBOdoInterface::HandleSetPID(const std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Request> request,
-                                       std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response)
-    {
-        if (!isOk)
-        {
-            RCLCPP_ERROR(get_logger(), "PCB not initialized");
-            response->success = false;
-            return;
-        }
-
-        std::promise<bool> promise;
-        auto future = promise.get_future();
-
-        {
-            std::lock_guard<std::mutex> lock(set_pid_mutex_);
-            set_pid_promises_.push(std::move(promise));
-        }
-
-        SetPID(request->p, request->i, request->d);
-
-        bool result = future.get();
-        response->success = result;
-    }
-
-    void PCBOdoInterface::HandleAddWaypoint(
-        const std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Request> request,
-        std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Response> response)
-    {
-        if (!isOk)
-        {
-            RCLCPP_ERROR(get_logger(), "PCB not initialized");
-            response->success = false;
-            return;
-        }
-
-        std::promise<bool> promise;
-        auto future = promise.get_future();
-
-        {
-            std::lock_guard<std::mutex> lock(add_waypoint_mutex_);
-            add_waypoint_promises_.push(std::move(promise));
-        }
-        AddWaypoint(request->id, request->is_end, request->x, request->y, request->theta);
-        bool result = future.get();
-        response->success = result;
-    }
-
-    void PCBOdoInterface::ResolveToFRequest(const long distance)
-    {
-        std::lock_guard<std::mutex> lock(tof_mutex_);
-        if (!tof_promises_.empty())
-        {
-            auto promise = std::move(tof_promises_.front());
-            tof_promises_.pop();
-            promise.set_value(distance);
-        }
-        else
-        {
-            RCLCPP_DEBUG(get_logger(), "No pending ToF request to resolve.");
-        }
-    }
-
-    void PCBOdoInterface::ResolveSpeedRequest(const OdometryData& speed)
-    {
-        std::lock_guard<std::mutex> lock(speed_mutex_);
-        if (!speed_promises_.empty())
-        {
-            auto promise = std::move(speed_promises_.front());
-            speed_promises_.pop();
-            promise.set_value(speed);
-        }
-        else
-        {
-            RCLCPP_DEBUG(get_logger(), "No pending Speed request to resolve.");
-        }
-    }
-
-    void PCBOdoInterface::ResolvePositionRequest(const OdometryData& position)
-    {
-        std::lock_guard<std::mutex> lock(pos_mutex_);
-        if (!pos_promises_.empty())
-        {
-            auto promise = std::move(pos_promises_.front());
-            pos_promises_.pop();
-            promise.set_value(position);
-        }
-        else
-        {
-            RCLCPP_DEBUG(get_logger(), "No pending Position request to resolve.");
-        }
-    }
-
-    void PCBOdoInterface::ResolveStartRequest(bool start)
-    {
-        std::lock_guard<std::mutex> lock(start_mutex_);
-        if (!start_promises_.empty())
-        {
-            auto promise = std::move(start_promises_.front());
-            start_promises_.pop();
-            promise.set_value(start);
-        }
-        else
-        {
-            RCLCPP_DEBUG(get_logger(), "No pending Start request to resolve.");
-        }
-    }
-
-    void PCBOdoInterface::ResolveGetPIDRequest(const PIDData& pid)
-    {
-        std::lock_guard<std::mutex> lock(get_pid_mutex_);
-        if (!get_pid_promises_.empty())
-        {
-            auto promise = std::move(get_pid_promises_.front());
-            get_pid_promises_.pop();
-            promise.set_value(pid);
-        }
-        else
-        {
-            RCLCPP_DEBUG(get_logger(), "No pending PID request to resolve.");
-        }
-    }
-
-    void PCBOdoInterface::ResolveSetPIDRequest(bool success)
-    {
-        std::lock_guard<std::mutex> lock(set_pid_mutex_);
-        if (!set_pid_promises_.empty())
-        {
-            auto promise = std::move(set_pid_promises_.front());
-            set_pid_promises_.pop();
-            promise.set_value(success);
-        }
-        else
-        {
-            RCLCPP_DEBUG(get_logger(), "No pending Set PID request to resolve.");
-        }
-    }
-
-    void PCBOdoInterface::ResolveAddWaypointRequest(bool success)
-    {
-        std::lock_guard<std::mutex> lock(add_waypoint_mutex_);
-        if (!add_waypoint_promises_.empty())
-        {
-            auto promise = std::move(add_waypoint_promises_.front());
-            add_waypoint_promises_.pop();
-            promise.set_value(success);
-        }
-        else
-        {
-            RCLCPP_DEBUG(get_logger(), "No pending Add Waypoint request to resolve.");
-        }
-    }
-
     void PCBOdoInterface::SendToPCB(const std::string& data)
     {
         if (pcb_publisher_)
@@ -703,8 +350,30 @@ namespace Modelec
         SendOrder("POS", data);
     }
 
+    void PCBOdoInterface::AddWaypoints(modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg)
+    {
+        if (!start_odo_)
+        {
+            SendOrder("START", {std::to_string(true)});
+            start_odo_ = true;
+        }
+
+        std::vector<std::string> data(msg->waypoints.size() * 5);
+
+        for (const auto& wp : msg->waypoints)
+        {
+            data.push_back(std::to_string(wp.id));
+            data.push_back(std::to_string(wp.is_end));
+            data.push_back(std::to_string(wp.x));
+            data.push_back(std::to_string(wp.y));
+            data.push_back(std::to_string(wp.theta));
+        }
+
+        SendOrder("WAYPOINT", data);
+    }
+
     void PCBOdoInterface::AddWaypoint(
-        const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg)
+        const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg)
     {
         AddWaypoint(msg->id, msg->is_end, msg->x, msg->y, msg->theta);
     }
@@ -746,12 +415,13 @@ namespace Modelec
 
     void PCBOdoInterface::SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg)
     {
-        SetPID(msg->p, msg->i, msg->d);
+        SetPID(msg->name, msg->p, msg->i, msg->d);
     }
 
-    void PCBOdoInterface::SetPID(float p, float i, float d)
+    void PCBOdoInterface::SetPID(std::string name, float p, float i, float d)
     {
         std::vector<std::string> data = {
+            name,
             std::to_string(p),
             std::to_string(i),
             std::to_string(d)

src/modelec_core/CMakeLists.txt

@@ -17,14 +17,6 @@ find_package(Boost REQUIRED COMPONENTS system)
 find_package(modelec_interfaces REQUIRED)
 find_package(modelec_utils REQUIRED)
 
-add_executable(gamecontroller_listener src/gamecontroller_listener.cpp)
-ament_target_dependencies(gamecontroller_listener rclcpp std_msgs sensor_msgs modelec_interfaces)
-target_link_libraries(gamecontroller_listener modelec_utils::utils)
-target_include_directories(gamecontroller_listener PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>
-)
-
 add_executable(speed_result src/speed_result.cpp)
 ament_target_dependencies(speed_result rclcpp std_msgs sensor_msgs modelec_interfaces)
 target_link_libraries(speed_result modelec_utils::utils)
@@ -43,7 +35,6 @@ endif()
 
 # Install targets
 install(TARGETS
-  gamecontroller_listener
   speed_result
   DESTINATION lib/${PROJECT_NAME}
 )

src/modelec_core/include/modelec/gamecontroller_listener.hpp

@@ -1,65 +0,0 @@
-#pragma once
-
-#include "modelec_interfaces/msg/pca9685_servo.hpp"
-#include <array>
-#include <rclcpp/publisher.hpp>
-#include <std_msgs/msg/string.hpp>
-#include <std_msgs/msg/empty.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <sensor_msgs/msg/joy.hpp>
-#include <modelec_interfaces/msg/servo_mode.hpp>
-#include <modelec_interfaces/msg/pca9685_servo.hpp>
-#include <modelec_interfaces/srv/add_servo_motor.hpp>
-
-namespace Modelec {
-    class ControllerListener : public rclcpp::Node
-    {
-        using ServoMode = modelec_interfaces::msg::ServoMode;
-
-        std::array<int, 3> pinces = {
-            ServoMode::PINCE_CLOSED,
-            ServoMode::PINCE_CLOSED,
-            ServoMode::PINCE_CLOSED,
-        };
-
-        int arm = ServoMode::ARM_BOTTOM;
-
-        struct SolarPannelServo {
-            int pin;
-            float startAngle;
-            float endAngle;
-        };
-        
-        std::array<SolarPannelServo, 2> solarPannelServos = {
-            { { 6, 16.0f, 40.0f }, { 7, 25.0f, 3.0f } }
-        };
-
-    public:
-        ControllerListener();
-
-    private:
-        void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg);
-
-        rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr subscription_;
-        rclcpp::Publisher<ServoMode>::SharedPtr servo_publisher_;
-        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr odometry_publisher_;
-        rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr clear_pca_publisher_;
-        rclcpp::Publisher<modelec_interfaces::msg::PCA9685Servo>::SharedPtr pca9685_publisher_;
-        rclcpp::Client<modelec_interfaces::srv::AddServoMotor>::SharedPtr client_;
-
-        void CheckButton(const sensor_msgs::msg::Joy::SharedPtr msg);
-        void CheckAxis(const sensor_msgs::msg::Joy::SharedPtr msg);
-
-        bool button_2_was_pressed = false;
-        bool button_3_was_pressed = false;
-        bool button_1_was_pressed = false;
-        bool button_0_was_pressed = false;
-        bool button_4_was_pressed = false;
-        bool button_5_was_pressed = false;
-
-        int last_speed = 0;
-        int last_rotation = 0;
-        float last_solar_1_angle = 0;
-        float last_solar_2_angle = 0;
-    };
-}

src/modelec_core/src/gamecontroller_listener.cpp

@@ -1,310 +0,0 @@
-#include "modelec/gamecontroller_listener.hpp"
-#include "modelec_utils/utils.hpp"
-#include <modelec_interfaces/srv/add_serial_listener.hpp>
-
-namespace Modelec
-{
-    ControllerListener::ControllerListener() : Node("controller_listener")
-    {
-        // Subscribe to the 'joy' topic
-        subscription_ = this->create_subscription<sensor_msgs::msg::Joy>(
-            "joy", 10,
-            std::bind(&ControllerListener::joy_callback, this, std::placeholders::_1));
-
-        servo_publisher_ = this->create_publisher<ServoMode>("arm_control", 10);
-
-        // Service to create a new serial listener
-        auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
-        request->name = "odometry";
-        request->bauds = 115200;
-        request->serial_port = "/dev/ttyACM0";
-        auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
-        while (!client->wait_for_service(std::chrono::seconds(1)))
-        {
-            if (!rclcpp::ok())
-            {
-                RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
-                return;
-            }
-            RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
-        }
-        auto result = client->async_send_request(request);
-        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
-            rclcpp::FutureReturnCode::SUCCESS)
-        {
-            if (auto res = result.get())
-            {
-                if (res->success)
-                {
-                    RCLCPP_INFO(this->get_logger(), "Serial listener created");
-
-                    odometry_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
-
-                    clear_pca_publisher_ = this->create_publisher<std_msgs::msg::Empty>("clear_pca9685", 10);
-
-                    pca9685_publisher_ = this->create_publisher<modelec_interfaces::msg::PCA9685Servo>(
-                        "servo_control", 10);
-                }
-                else
-                {
-                    RCLCPP_ERROR(this->get_logger(), "Failed to create serial listener");
-                }
-            }
-            else
-            {
-                RCLCPP_ERROR(this->get_logger(), "Failed to get result from service");
-            }
-        }
-        else
-        {
-            RCLCPP_ERROR(this->get_logger(), "Service call failed");
-        }
-
-        client_ = this->create_client<modelec_interfaces::srv::AddServoMotor>("add_servo");
-
-        // ensure the server is up
-        while (!client_->wait_for_service(std::chrono::seconds(1)))
-        {
-            if (!rclcpp::ok())
-            {
-                RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
-                return;
-            }
-            RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
-        }
-
-        for (auto servo : solarPannelServos)
-        {
-            auto request = std::make_shared<modelec_interfaces::srv::AddServoMotor::Request>();
-            request->pin = servo.pin;
-            auto res = client_->async_send_request(request);
-            if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), res) ==
-                rclcpp::FutureReturnCode::SUCCESS)
-            {
-                if (!res.get()->success)
-                {
-                    RCLCPP_ERROR(this->get_logger(), "Failed to add servo on pin %d", servo.pin);
-                }
-                else
-                {
-                    RCLCPP_INFO(this->get_logger(), "Added servo on pin %d", servo.pin);
-                }
-            }
-            else
-            {
-                RCLCPP_ERROR(this->get_logger(), "Service call failed");
-            }
-        }
-    }
-
-    void ControllerListener::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg)
-    {
-        // to check mapping : https://index.ros.org//p/joy/
-        // bon en fait la doc dit de la merde pour voir les bon checker directement en loggant le topic joy
-        CheckButton(msg);
-        CheckAxis(msg);
-    }
-
-    void ControllerListener::CheckButton(const sensor_msgs::msg::Joy::SharedPtr msg)
-    {
-        if (msg->buttons[2] == 1)
-        {
-            if (button_2_was_pressed)
-            {
-                return;
-            }
-            auto message = ServoMode();
-            message.pin = 0;
-            if (pinces[0] == ServoMode::PINCE_CLOSED)
-            {
-                pinces[0] = ServoMode::PINCE_OPEN;
-            }
-            else
-            {
-                pinces[0] = ServoMode::PINCE_CLOSED;
-            }
-            message.mode = pinces[0];
-            servo_publisher_->publish(message);
-            button_2_was_pressed = true;
-        }
-        else
-        {
-            button_2_was_pressed = false;
-        }
-        if (msg->buttons[3] == 1)
-        {
-            if (button_3_was_pressed)
-            {
-                return;
-            }
-            auto message = ServoMode();
-            message.pin = 1;
-            if (pinces[1] == ServoMode::PINCE_CLOSED)
-            {
-                pinces[1] = ServoMode::PINCE_OPEN;
-            }
-            else
-            {
-                pinces[1] = ServoMode::PINCE_CLOSED;
-            }
-            message.mode = pinces[1];
-            servo_publisher_->publish(message);
-            button_3_was_pressed = true;
-        }
-        else
-        {
-            button_3_was_pressed = false;
-        }
-        if (msg->buttons[1] == 1)
-        {
-            if (button_1_was_pressed)
-            {
-                return;
-            }
-            auto message = ServoMode();
-            message.pin = 2;
-            if (pinces[2] == ServoMode::PINCE_CLOSED)
-            {
-                pinces[2] = ServoMode::PINCE_OPEN;
-            }
-            else
-            {
-                pinces[2] = ServoMode::PINCE_CLOSED;
-            }
-            message.mode = pinces[2];
-            servo_publisher_->publish(message);
-            button_1_was_pressed = true;
-        }
-        else
-        {
-            button_1_was_pressed = false;
-        }
-
-        // arm
-        if (msg->buttons[0] == 1)
-        {
-            if (button_0_was_pressed)
-            {
-                return;
-            }
-            auto message = ServoMode();
-            if (arm == ServoMode::ARM_BOTTOM)
-            {
-                arm = ServoMode::ARM_TOP;
-            }
-            else
-            {
-                arm = ServoMode::ARM_BOTTOM;
-            }
-            message.mode = arm;
-            message.is_arm = true;
-            servo_publisher_->publish(message);
-            button_0_was_pressed = true;
-        }
-        else
-        {
-            button_0_was_pressed = false;
-        }
-
-        if (msg->buttons[4])
-        {
-            if (button_4_was_pressed)
-            {
-                return;
-            }
-            auto message = std_msgs::msg::String();
-            message.data = "W";
-            odometry_publisher_->publish(message);
-            button_4_was_pressed = true;
-        }
-        else
-        {
-            button_4_was_pressed = false;
-        }
-
-        if (msg->buttons[5])
-        {
-            if (button_5_was_pressed)
-            {
-                return;
-            }
-            clear_pca_publisher_->publish(std_msgs::msg::Empty());
-            button_5_was_pressed = true;
-        }
-        else
-        {
-            button_5_was_pressed = false;
-        }
-    }
-
-    void ControllerListener::CheckAxis(const sensor_msgs::msg::Joy::SharedPtr msg)
-    {
-        auto message = std_msgs::msg::String();
-        int speed = 0;
-        if (msg->axes[1] < 0.1 && msg->axes[1] > -0.1)
-        {
-            speed = 0;
-        }
-        else
-        {
-            speed = static_cast<int>(Modelec::mapValue(static_cast<float>(msg->axes[1]), -1.0f, 1.0f, -310.0f, 310.0f));
-        }
-
-        if (speed != last_speed)
-        {
-            message.data = "V " + std::to_string(speed);
-            odometry_publisher_->publish(message);
-            last_speed = speed;
-        }
-
-        int rotation = 0;
-        if (msg->axes[3] < 0.1 && msg->axes[3] > -0.1)
-        {
-            rotation = 0;
-        }
-        else
-        {
-            rotation = static_cast<int>(Modelec::mapValue(static_cast<float>(-msg->axes[3]), -1.0f, 1.0f, -310.0f,
-                                                          310.0f));
-        }
-
-        if (rotation != last_rotation)
-        {
-            message.data = "R " + std::to_string(rotation);
-            odometry_publisher_->publish(message);
-            last_rotation = rotation;
-        }
-
-        if (msg->axes[2] != last_solar_1_angle)
-        {
-            int solarPannelAngle = static_cast<int>(Modelec::mapValue(static_cast<float>(msg->axes[2]), -1.0f, 1.0f,
-                                                                      solarPannelServos[0].startAngle,
-                                                                      solarPannelServos[0].endAngle));
-            auto solarPannelAngleMessage = modelec_interfaces::msg::PCA9685Servo();
-            solarPannelAngleMessage.pin = solarPannelServos[0].pin;
-            solarPannelAngleMessage.angle = solarPannelAngle;
-            pca9685_publisher_->publish(solarPannelAngleMessage);
-            last_solar_1_angle = msg->axes[2];
-        }
-
-        if (msg->axes[5] != last_solar_2_angle)
-        {
-            int solarPannelAngle = static_cast<int>(Modelec::mapValue(static_cast<float>(msg->axes[5]), -1.0f, 1.0f,
-                                                                      solarPannelServos[1].startAngle,
-                                                                      solarPannelServos[1].endAngle));
-            auto solarPannelAngleMessage = modelec_interfaces::msg::PCA9685Servo();
-            solarPannelAngleMessage.pin = solarPannelServos[1].pin;
-            solarPannelAngleMessage.angle = solarPannelAngle;
-            pca9685_publisher_->publish(solarPannelAngleMessage);
-            last_solar_2_angle = msg->axes[5];
-        }
-    }
-}
-
-int main(int argc, char** argv)
-{
-    rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<Modelec::ControllerListener>());
-    rclcpp::shutdown();
-    return 0;
-}

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

@@ -12,12 +12,12 @@
 
 #include <std_srvs/srv/empty.hpp>
 
-#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
+#include <modelec_interfaces/msg/odometry_waypoint.hpp>
+#include <modelec_interfaces/msg/odometry_waypoints.hpp>
 #include <modelec_interfaces/msg/odometry_pos.hpp>
 #include <modelec_interfaces/srv/map.hpp>
 #include <modelec_interfaces/srv/map_size.hpp>
 #include <modelec_interfaces/msg/obstacle.hpp>
-#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
 #include <modelec_interfaces/msg/strat_state.hpp>
 
 #include <std_msgs/msg/bool.hpp>
@@ -78,7 +78,8 @@ namespace ModelecGUI {
 
         rclcpp::Node::SharedPtr node_;
 
-        rclcpp::Subscription<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr add_waypoint_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoint>::SharedPtr add_waypoint_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoints>::SharedPtr add_waypoints_sub_;
 
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odometry_sub_;
         rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr score_sub_;

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

@@ -9,7 +9,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <modelec_interfaces/msg/odometry_pos.hpp>
-#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
+#include <modelec_interfaces/msg/odometry_waypoint.hpp>
 #include <modelec_interfaces/srv/odometry_speed.hpp>
 #include <modelec_interfaces/srv/odometry_start.hpp>
 #include <modelec_interfaces/srv/odometry_get_pid.hpp>
@@ -45,13 +45,13 @@ namespace ModelecGUI
 
         rclcpp::Node::SharedPtr node_;
 
-        rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr sub_;
+        // rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr sub_;
 
         // client
-        rclcpp::Client<modelec_interfaces::srv::OdometrySpeed>::SharedPtr client_;
+        /* rclcpp::Client<modelec_interfaces::srv::OdometrySpeed>::SharedPtr client_;
-        rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;
+         rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;
-        rclcpp::Client<modelec_interfaces::srv::OdometryGetPid>::SharedPtr client_get_pid_;
+         rclcpp::Client<modelec_interfaces::srv::OdometryGetPid>::SharedPtr client_get_pid_;
-        rclcpp::Client<modelec_interfaces::srv::OdometrySetPid>::SharedPtr client_set_pid_;
+         rclcpp::Client<modelec_interfaces::srv::OdometrySetPid>::SharedPtr client_set_pid_;*/
 
         void PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
     };

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

@@ -12,13 +12,13 @@
 
 #include <std_srvs/srv/empty.hpp>
 
-#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
+#include <modelec_interfaces/msg/odometry_waypoint.hpp>
+#include <modelec_interfaces/msg/odometry_waypoints.hpp>
 #include <modelec_interfaces/msg/odometry_pos.hpp>
 #include <modelec_interfaces/msg/odometry_go_to.hpp>
 #include <modelec_interfaces/srv/map.hpp>
 #include <modelec_interfaces/srv/map_size.hpp>
 #include <modelec_interfaces/msg/obstacle.hpp>
-#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
 #include <modelec_interfaces/msg/strat_state.hpp>
 
 #include <std_msgs/msg/bool.hpp>
@@ -48,8 +48,6 @@ namespace ModelecGUI
 
         void mousePressEvent(QMouseEvent* event) override;
 
-        void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
-
         void OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg);
 
         void resizeEvent(QResizeEvent* event) override;
@@ -79,7 +77,8 @@ namespace ModelecGUI
 
         rclcpp::Node::SharedPtr node_;
 
-        rclcpp::Subscription<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr add_waypoint_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoint>::SharedPtr add_waypoint_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoints>::SharedPtr add_waypoints_sub_;
 
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odometry_sub_;
         rclcpp::Publisher<modelec_interfaces::msg::OdometryGoTo>::SharedPtr go_to_pub_;

src/modelec_gui/src/modelec_gui.cpp

@@ -13,25 +13,25 @@ namespace ModelecGUI {
         resize(1200, 800);
 
         home_page_ = new HomePage(get_node(), this);
-        odo_page_ = new OdoPage(get_node(), this);
+        //odo_page_ = new OdoPage(get_node(), this);
         test_map_page_ = new TestMapPage(get_node(), this);
         map_page_ = new MapPage(get_node(), this);
-        action_page_ = new ActionPage(get_node(), this);
+        // action_page_ = new ActionPage(get_node(), this);
-        alim_page_ = new AlimPage(get_node(), this);
+        // alim_page_ = new AlimPage(get_node(), this);
 
         stackedWidget->addWidget(home_page_);
-        stackedWidget->addWidget(odo_page_);
+        // stackedWidget->addWidget(odo_page_);
         stackedWidget->addWidget(test_map_page_);
         stackedWidget->addWidget(map_page_);
-        stackedWidget->addWidget(action_page_);
+        // stackedWidget->addWidget(action_page_);
-        stackedWidget->addWidget(alim_page_);
+        // stackedWidget->addWidget(alim_page_);
         setCentralWidget(stackedWidget);
 
         setupMenu();
 
         connect(home_page_, &HomePage::TeamChoose, this, [this]()
         {
-            stackedWidget->setCurrentIndex(3);
+            stackedWidget->setCurrentWidget(map_page_);
         });
     }
 
@@ -68,25 +68,25 @@ namespace ModelecGUI {
         optionsMenu->addAction(exit_action_);
 
         connect(home_action_, &QAction::triggered, this, [this]() {
-            stackedWidget->setCurrentIndex(0);
+            stackedWidget->setCurrentWidget(home_page_);
             home_page_->Init();
             map_page_->Reset();
         });
 
         connect(odo_action_, &QAction::triggered, this, [this]() {
-            stackedWidget->setCurrentIndex(1);
+            stackedWidget->setCurrentWidget(odo_page_);
         });
 
         connect(action_action_, &QAction::triggered, this, [this]() {
-            stackedWidget->setCurrentIndex(4);
+            stackedWidget->setCurrentWidget(action_page_);
         });
 
         connect(alim_action_, &QAction::triggered, this, [this]() {
-            stackedWidget->setCurrentIndex(5);
+            stackedWidget->setCurrentWidget(alim_page_);
         });
 
         connect(map_action_, &QAction::triggered, this, [this]() {
-            stackedWidget->setCurrentIndex(2);
+            stackedWidget->setCurrentWidget(map_page_);
         });
 
         connect(playmat_map_, &QAction::triggered, this, [this]() {

src/modelec_gui/src/pages/home_page.cpp

@@ -2,9 +2,7 @@
 #include <modelec_gui/pages/home_page.hpp>
 
 #include <QVBoxLayout>
-#include <modelec_interfaces/msg/detail/servo_mode__builder.hpp>
+#include <modelec_utils/config.hpp>
-
-#include "../../../modelec_utils/include/modelec_utils/config.hpp"
 
 namespace ModelecGUI
 {
@@ -12,12 +10,12 @@ namespace ModelecGUI
         : QWidget(parent), node_(node),
           renderer_(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this))
     {
-        spawn_pub_ = node_->create_publisher<modelec_interfaces::msg::Spawn>("/strat/spawn", 10);
+        spawn_pub_ = node_->create_publisher<modelec_interfaces::msg::Spawn>("strat/spawn", 10);
 
         auto w = Modelec::Config::get<int>("config.spawn.width_mm");
         auto h = Modelec::Config::get<int>("config.spawn.height_mm");
 
-        spawn_sub_ = node_->create_subscription<modelec_interfaces::msg::Spawn>("/nav/spawn", 10,
+        spawn_sub_ = node_->create_subscription<modelec_interfaces::msg::Spawn>("nav/spawn", 10,
             [this, w, h](const modelec_interfaces::msg::Spawn::SharedPtr msg)
             {
                 auto ratioX = 1200 / 3000.0f;
@@ -53,9 +51,9 @@ namespace ModelecGUI
                 });
             });
 
-        reset_strat_pub_ = node_->create_publisher<std_msgs::msg::Empty>("/strat/reset", 10);
+        reset_strat_pub_ = node_->create_publisher<std_msgs::msg::Empty>("strat/reset", 10);
 
-        ask_spawn_client_ = node_->create_client<std_srvs::srv::Empty>("/nav/ask_spawn");
+        ask_spawn_client_ = node_->create_client<std_srvs::srv::Empty>("nav/ask_spawn");
         ask_spawn_client_->wait_for_service();
         auto ask_spawn_request_ = std::make_shared<std_srvs::srv::Empty::Request>();
         auto res = ask_spawn_client_->async_send_request(ask_spawn_request_);

src/modelec_gui/src/pages/map_page.cpp

@@ -51,9 +51,9 @@ namespace ModelecGUI
         enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
         enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
 
-        add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>(
+        add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryWaypoint>(
             "odometry/add_waypoint", 100,
-            [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg)
+            [this](const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg)
             {
                 if (lastWapointWasEnd)
                 {
@@ -74,6 +74,26 @@ namespace ModelecGUI
                 update();
             });
 
+        add_waypoints_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryWaypoints>(
+        "odometry/add_waypoints", 10,
+        [this](const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg)
+        {
+            qpoints.clear();
+            lastWapointWasEnd = false;
+
+            qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_,
+                                     height() - robotPos.y * ratioBetweenMapAndWidgetY_));
+
+            for (const auto& point : msg->waypoints)
+            {
+                qpoints.push_back(QPoint(point.x * ratioBetweenMapAndWidgetX_,
+                                         height() - point.y * ratioBetweenMapAndWidgetY_));
+            }
+
+            update();
+        });
+
+
         odometry_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>("odometry/position", 10,
             [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
             {
@@ -111,7 +131,7 @@ namespace ModelecGUI
                 update();
             });
 
-        strat_start_sub_ = node_->create_subscription<std_msgs::msg::Int64>("/strat/start_time", 10,
+        strat_start_sub_ = node_->create_subscription<std_msgs::msg::Int64>("strat/start_time", 10,
                                                                             [this](
                                                                             const std_msgs::msg::Int64::SharedPtr msg)
                                                                             {
@@ -119,7 +139,7 @@ namespace ModelecGUI
                                                                                 start_time_ = msg->data;
                                                                             });
 
-        strat_state_sub_ = node_->create_subscription<modelec_interfaces::msg::StratState>("/strat/state", 10,
+        strat_state_sub_ = node_->create_subscription<modelec_interfaces::msg::StratState>("strat/state", 10,
             [this](const modelec_interfaces::msg::StratState::SharedPtr msg)
             {
                 if (msg->state == modelec_interfaces::msg::StratState::STOP)

src/modelec_gui/src/pages/odo_page.cpp

@@ -13,7 +13,7 @@ namespace ModelecGUI
             RCLCPP_INFO(node_->get_logger(), "Start button clicked.");
             auto request = std::make_shared<modelec_interfaces::srv::OdometryStart::Request>();
             request->start = true;
-            client_start_->async_send_request(
+            /*client_start_->async_send_request(
                 request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedFuture response)
                 {
                     if (response.get()->success)
@@ -24,7 +24,7 @@ namespace ModelecGUI
                     {
                         RCLCPP_ERROR(node_->get_logger(), "Failed to send start command.");
                     }
-                });
+                });*/
         });
 
         // Initialize the UI components
@@ -49,7 +49,7 @@ namespace ModelecGUI
             auto request = std::make_shared<modelec_interfaces::srv::OdometryGetPid::Request>();
 
             // Make the asynchronous service call
-            client_get_pid_->async_send_request(
+            /*client_get_pid_->async_send_request(
                 request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryGetPid>::SharedFuture response)
                 {
                     RCLCPP_INFO(node_->get_logger(), "Received PID response.");
@@ -68,7 +68,7 @@ namespace ModelecGUI
                     {
                         RCLCPP_ERROR(node_->get_logger(), "Failed to get response for PID request.");
                     }
-                });
+                });*/
         });
         pPIDBox_ = new QDoubleSpinBox(this);
         pPIDBox_->setMinimum(0);
@@ -106,7 +106,7 @@ namespace ModelecGUI
             request->d = dPIDBox_->text().toFloat();
 
             // Make the asynchronous service call
-            client_set_pid_->async_send_request(
+            /*client_set_pid_->async_send_request(
                 request, [this](rclcpp::Client<modelec_interfaces::srv::OdometrySetPid>::SharedFuture response)
                 {
                     if (response.get()->success)
@@ -117,7 +117,7 @@ namespace ModelecGUI
                     {
                         RCLCPP_ERROR(node_->get_logger(), "Failed to send set PID command.");
                     }
-                });
+                });*/
         });
 
         askSpeed_ = new QPushButton("Ask speed");
@@ -127,7 +127,7 @@ namespace ModelecGUI
             auto request = std::make_shared<modelec_interfaces::srv::OdometrySpeed::Request>();
 
             // Make the asynchronous service call
-            client_->async_send_request(
+            /*client_->async_send_request(
                 request, [this](rclcpp::Client<modelec_interfaces::srv::OdometrySpeed>::SharedFuture response)
                 {
                     if (auto res = response.get())
@@ -143,7 +143,7 @@ namespace ModelecGUI
                     {
                         RCLCPP_ERROR(node_->get_logger(), "Failed to get response for speed request.");
                     }
-                });
+                });*/
         });
 
         xSpeedBox_ = new QLineEdit("x speed: ?");
@@ -169,11 +169,11 @@ namespace ModelecGUI
         setLayout(mainLayout_);
 
         // Set up subscription
-        sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
+        /*sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
-            "/odometry/position", 10,
+            "/odom/position", 10,
-            std::bind(&OdoPage::PositionCallback, this, std::placeholders::_1));
+            std::bind(&OdoPage::PositionCallback, this, std::placeholders::_1));*/
 
-        client_ = node_->create_client<modelec_interfaces::srv::OdometrySpeed>("odometry/speed");
+        /*client_ = node_->create_client<modelec_interfaces::srv::OdometrySpeed>("odometry/speed");
         while (!client_->wait_for_service(std::chrono::seconds(1)))
         {
             if (!rclcpp::ok())
@@ -216,7 +216,7 @@ namespace ModelecGUI
                 return;
             }
             RCLCPP_INFO(node_->get_logger(), "Service not available, waiting again...");
-        }
+        }*/
     }
 
     OdoPage::~OdoPage() = default;

src/modelec_gui/src/pages/test_map_page.cpp

@@ -25,17 +25,17 @@ namespace ModelecGUI
         enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
         enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
 
-        add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>(
+        add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryWaypoint>(
             "odometry/add_waypoint", 100,
-            [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg)
+            [this](const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg)
             {
                 if (lastWapointWasEnd)
                 {
                     qpoints.clear();
                     lastWapointWasEnd = false;
 
-                    qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_,
+                    qpoints.emplace_back(robotPos.x * ratioBetweenMapAndWidgetX_,
-                                             height() - robotPos.y * ratioBetweenMapAndWidgetY_));
+                                             height() - robotPos.y * ratioBetweenMapAndWidgetY_);
                 }
 
                 if (msg->is_end)
@@ -43,8 +43,27 @@ namespace ModelecGUI
                     lastWapointWasEnd = true;
                 }
 
-                qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_,
+                qpoints.emplace_back(msg->x * ratioBetweenMapAndWidgetX_,
-                                         height() - msg->y * ratioBetweenMapAndWidgetY_));
+                                         height() - msg->y * ratioBetweenMapAndWidgetY_);
+                update();
+            });
+
+        add_waypoints_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryWaypoints>(
+            "odometry/add_waypoints", 10,
+            [this](const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg)
+            {
+                qpoints.clear();
+                lastWapointWasEnd = false;
+
+                qpoints.emplace_back(robotPos.x * ratioBetweenMapAndWidgetX_,
+                                         height() - robotPos.y * ratioBetweenMapAndWidgetY_);
+
+                for (const auto& point : msg->waypoints)
+                {
+                    qpoints.emplace_back(point.x * ratioBetweenMapAndWidgetX_,
+                                             height() - point.y * ratioBetweenMapAndWidgetY_);
+                }
+
                 update();
             });
 
@@ -229,7 +248,8 @@ namespace ModelecGUI
             msg.close = true;
             if (show_obstacle_)
             {
-                msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL;
+                msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL |
+                                    modelec_interfaces::msg::OdometryGoTo::OBSTACLE | modelec_interfaces::msg::OdometryGoTo::ENEMY;
             }
             else
             {

src/modelec_interfaces/CMakeLists.txt

@@ -12,47 +12,44 @@ find_package(geometry_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-  "msg/Alim/AlimEmg.msg"
+        "msg/Alim/AlimEmg.msg"
-  "msg/Odometry/OdometryPos.msg"
+        "msg/Odometry/OdometryPos.msg"
-  "msg/Odometry/OdometryGoTo.msg"
+        "msg/Odometry/OdometryGoTo.msg"
-  "msg/Odometry/OdometrySpeed.msg"
+        "msg/Odometry/OdometrySpeed.msg"
-  "msg/Odometry/OdometryToF.msg"
+        "msg/Odometry/OdometryToF.msg"
-  "msg/Odometry/OdometryWaypointReach.msg"
+        "msg/Odometry/OdometryRealign.msg"
-  "msg/Odometry/OdometryAddWaypoint.msg"
+        "msg/Odometry/OdometryWaypoint.msg"
-  "msg/Odometry/OdometryStart.msg"
+        "msg/Odometry/OdometryWaypoints.msg"
-  "msg/Odometry/PID/OdometryPid.msg"
+        "msg/Odometry/OdometryStart.msg"
-  "msg/Strat/StratState.msg"
+        "msg/Odometry/PID/OdometryPid.msg"
-  "msg/Map/Map.msg"
+        "msg/Strat/StratState.msg"
-  "msg/Map/Obstacle.msg"
+        "msg/Map/Map.msg"
-  "msg/Map/Spawn.msg"
+        "msg/Map/Obstacle.msg"
-  "msg/PCA9685Servo.msg"
+        "msg/Map/Spawn.msg"
-  "msg/ServoMode.msg"
+        "msg/Action/ActionAscPos.msg"
-  "msg/Solenoid.msg"
+        "msg/Action/ActionRelayState.msg"
-  "msg/Button.msg"
+        "msg/Action/ActionServoPos.msg"
-  "msg/Action/ActionAscPos.msg"
+        "msg/Action/ActionExec.msg"
-  "msg/Action/ActionRelayState.msg"
+        "srv/Alim/AlimOut.srv"
-  "msg/Action/ActionServoPos.msg"
+        "srv/Alim/AlimIn.srv"
-  "msg/Action/ActionExec.msg"
+        "srv/Alim/AlimTemp.srv"
-  "srv/Alim/AlimOut.srv"
+        "srv/Alim/AlimBau.srv"
-  "srv/Alim/AlimIn.srv"
+        "srv/Alim/AlimEmg.srv"
-  "srv/Alim/AlimTemp.srv"
+        "srv/Odometry/OdometryPosition.srv"
-  "srv/Alim/AlimBau.srv"
+        "srv/Odometry/OdometrySpeed.srv"
-  "srv/Alim/AlimEmg.srv"
+        "srv/Odometry/OdometryToF.srv"
-  "srv/Odometry/OdometryPosition.srv"
+        "srv/Odometry/OdometryStart.srv"
-  "srv/Odometry/OdometrySpeed.srv"
+        "srv/Odometry/OdometryAddWaypoint.srv"
-  "srv/Odometry/OdometryToF.srv"
+        "srv/Odometry/PID/OdometryGetPid.srv"
-  "srv/Odometry/OdometryStart.srv"
+        "srv/Odometry/PID/OdometrySetPid.srv"
-  "srv/Odometry/OdometryAddWaypoint.srv"
+        "srv/Map/Map.srv"
-  "srv/Odometry/PID/OdometryGetPid.srv"
+        "srv/Map/MapSize.srv"
-  "srv/Odometry/PID/OdometrySetPid.srv"
+        "srv/AddServoMotor.srv"
-  "srv/Map/Map.srv"
+        "srv/AddSolenoid.srv"
-  "srv/Map/MapSize.srv"
+        "srv/Tirette.srv"
-  "srv/AddServoMotor.srv"
+        "srv/AddButton.srv"
-  "srv/AddSolenoid.srv"
+        "srv/Button.srv"
-  "srv/Tirette.srv"
+        "srv/AddSerialListener.srv"
-  "srv/AddButton.srv"
-  "srv/Button.srv"
-  "srv/AddSerialListener.srv"
   DEPENDENCIES std_msgs geometry_msgs
 )
 

src/modelec_interfaces/msg/Alim/AlimEmg.msg

@@ -1 +1 @@
-bool activate
+bool activate

src/modelec_interfaces/msg/Odometry/OdometryRealign.msg

@@ -0,0 +1,13 @@
+int8 BACKWARD=1
+int8 FORWARD=2
+
+int8 X=3
+int8 Y=4
+int8 XY=5
+
+int8 direction
+int8 axis
+float32 x
+float32 y
+float32 theta
+bool force 0

src/modelec_interfaces/msg/Odometry/OdometryWaypointReach.msg

@@ -1 +0,0 @@
-uint8 id

src/modelec_interfaces/msg/Odometry/OdometryWaypoints.msg

@@ -0,0 +1 @@
+OdometryWaypoint[] waypoints

src/modelec_interfaces/msg/Odometry/PID/OdometryPid.msg

@@ -1,3 +1,9 @@
+string POS="POS"
+string THETA="THETA"
+string LEFT="LEFT"
+string RIGHT="RIGHT"
+
+string name
 float32 p
 float32 i
 float32 d

src/modelec_interfaces/msg/PCA9685Servo.msg

@@ -1,2 +0,0 @@
-uint8 pin
-int64 angle

src/modelec_interfaces/msg/ServoMode.msg

@@ -1,11 +0,0 @@
-uint8 PINCE_CLOSED=0
-uint8 PINCE_MIDDLE=1
-uint8 PINCE_OPEN=2
-uint8 PINCE_FULLY_OPEN=3
-uint8 ARM_BOTTOM=4
-uint8 ARM_MIDDLE=5
-uint8 ARM_TOP=6
-
-uint8 pin
-uint8 mode
-bool is_arm

src/modelec_interfaces/msg/Solenoid.msg

@@ -1,2 +0,0 @@
-uint8 pin
-bool state

src/modelec_strat/include/modelec_strat/navigation_helper.hpp

@@ -1,8 +1,8 @@
 #pragma once
 
 #include <rclcpp/rclcpp.hpp>
-#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
+#include <modelec_interfaces/msg/odometry_waypoint.hpp>
-#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
+#include <modelec_interfaces/msg/odometry_waypoints.hpp>
 #include <modelec_interfaces/msg/odometry_pos.hpp>
 #include <modelec_interfaces/msg/odometry_go_to.hpp>
 #include <modelec_interfaces/msg/spawn.hpp>
@@ -41,6 +41,13 @@ namespace Modelec
 
         // void SendWaypoint() const;
         // void SendWaypoint(const std::vector<WaypointMsg>& waypoints) const;
+        void SendWaypoint() const;
+        void SendWaypoint(const std::vector<WaypointMsg>& waypoints) const;
+
+        void SendWaypoints() const;
+        void SendWaypoints(const std::vector<WaypointMsg>& waypoints) const;
+        void SendWaypoints(const WaypointsMsg& waypoints) const;
+
         void SendGoTo();
 
         void AddWaypoint(const PosMsg& pos, int index);
@@ -86,7 +93,6 @@ namespace Modelec
         PosMsg::SharedPtr GetHomePosition();
 
         void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
-        void OnEnemyPositionClose(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
 
         void OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
 
@@ -104,7 +110,7 @@ namespace Modelec
         Point GetSpawn() const;
 
     protected:
-        void OnWaypointReach(const WaypointReachMsg::SharedPtr msg);
+        void OnWaypointReach(const WaypointMsg::SharedPtr msg);
 
         void OnPos(const PosMsg::SharedPtr msg);
 
@@ -140,15 +146,15 @@ namespace Modelec
 
         std::map<int, std::shared_ptr<DepositeZone>> deposite_zones_;
 
-        rclcpp::Subscription<WaypointReachMsg>::SharedPtr waypoint_reach_sub_;
+        rclcpp::Subscription<WaypointMsg>::SharedPtr waypoint_reach_sub_;
         rclcpp::Publisher<WaypointMsg>::SharedPtr waypoint_pub_;
+        rclcpp::Publisher<WaypointsMsg>::SharedPtr waypoints_pub_;
 
         rclcpp::Subscription<modelec_interfaces::msg::OdometryGoTo>::SharedPtr go_to_sub_;
         rclcpp::Subscription<PosMsg>::SharedPtr pos_sub_;
         rclcpp::Publisher<PosMsg>::SharedPtr pos_pub_;
 
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
-        rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr close_enemy_pos_sub_;
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_long_time_sub_;
 
         rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr start_odo_pub_;
@@ -165,4 +171,4 @@ namespace Modelec
         rclcpp::Time last_odo_get_pos_time_;
 
     };
-}
+}

src/modelec_strat/include/modelec_strat/pathfinding.hpp

@@ -6,8 +6,8 @@
 
 #include <tinyxml2.h>
 
-#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
+#include <modelec_interfaces/msg/odometry_waypoint.hpp>
-#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
+#include <modelec_interfaces/msg/odometry_waypoints.hpp>
 #include <modelec_interfaces/msg/odometry_pos.hpp>
 #include <modelec_interfaces/msg/map.hpp>
 #include <modelec_interfaces/srv/map.hpp>
@@ -21,8 +21,8 @@
 
 namespace Modelec
 {
-    using WaypointMsg = modelec_interfaces::msg::OdometryAddWaypoint;
+    using WaypointMsg = modelec_interfaces::msg::OdometryWaypoint;
-    using WaypointReachMsg = modelec_interfaces::msg::OdometryWaypointReach;
+    using WaypointsMsg = modelec_interfaces::msg::OdometryWaypoints;
     using PosMsg = modelec_interfaces::msg::OdometryPos;
     using WaypointListMsg = std::vector<WaypointMsg>;
 
@@ -65,7 +65,7 @@ namespace Modelec
 
         Pathfinding(const rclcpp::Node::SharedPtr& node);
 
-        rclcpp::Node::SharedPtr GetNode() const;
+        [[nodiscard]] rclcpp::Node::SharedPtr GetNode() const;
 
         std::pair<int, WaypointListMsg> FindPath(const PosMsg::SharedPtr& start,
                                                  const PosMsg::SharedPtr& goal, bool isClose = false,
@@ -77,7 +77,7 @@ namespace Modelec
 
         void SetCurrentPos(const PosMsg::SharedPtr& pos);
 
-        std::shared_ptr<Obstacle> GetObstacle(int id) const;
+        [[nodiscard]] std::shared_ptr<Obstacle> GetObstacle(int id) const;
 
         void RemoveObstacle(int id);
 
@@ -162,4 +162,4 @@ namespace Modelec
 
         return closest_obstacle;
     }
-}
+}

src/modelec_strat/src/navigation_helper.cpp

@@ -1,14 +1,12 @@
 #include <modelec_strat/navigation_helper.hpp>
 #include <utility>
 #include <ament_index_cpp/get_package_share_directory.hpp>
-
+#include <modelec_utils/config.hpp>
-#include "../../modelec_utils/include/modelec_utils/config.hpp"
 
 namespace Modelec
 {
     NavigationHelper::NavigationHelper()
-    {
+    = default;
-    }
 
     NavigationHelper::NavigationHelper(const rclcpp::Node::SharedPtr& node) : node_(node)
     {
@@ -20,14 +18,15 @@ namespace Modelec
 
         SetupSpawn();
 
-        waypoint_reach_sub_ = node_->create_subscription<WaypointReachMsg>(
+        waypoint_reach_sub_ = node_->create_subscription<WaypointMsg>(
             "odometry/waypoint_reach", 10,
-            [this](const WaypointReachMsg::SharedPtr msg)
+            [this](const WaypointMsg::SharedPtr msg)
             {
                 OnWaypointReach(msg);
             });
 
-        waypoint_pub_ = node_->create_publisher<WaypointMsg>("odometry/add_waypoint", 100);
+        waypoint_pub_ = node_->create_publisher<WaypointMsg>("odometry/add_waypoint", 30);
+        waypoints_pub_ = node_->create_publisher<WaypointsMsg>("odometry/add_waypoints",  30);
 
         pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
             "odometry/position", 20,
@@ -51,21 +50,14 @@ namespace Modelec
                 OnEnemyPosition(msg);
             });
 
-        close_enemy_pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
-            "enemy/position/emergency", 10,
-            [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
-            {
-                OnEnemyPositionClose(msg);
-            });
-
         enemy_pos_long_time_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
-            "/enemy/long_time", 10,
+            "enemy/long_time", 10,
             [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
             {
                 OnEnemyPositionLongTime(msg);
             });
 
-        start_odo_pub_ = node_->create_publisher<std_msgs::msg::Bool>("/odometry/start", 10);
+        start_odo_pub_ = node_->create_publisher<std_msgs::msg::Bool>("odometry/start", 10);
 
         std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") +
             "/data/deposite_zone.xml";
@@ -77,7 +69,7 @@ namespace Modelec
         spawn_pub_ = node_->create_publisher<modelec_interfaces::msg::Spawn>("nav/spawn", 10);
 
         ask_spawn_srv_ = node->create_service<std_srvs::srv::Empty>(
-            "/nav/ask_spawn", [this](const std_srvs::srv::Empty::Request::SharedPtr,
+            "nav/ask_spawn", [this](const std_srvs::srv::Empty::Request::SharedPtr,
                                      const std_srvs::srv::Empty::Response::SharedPtr)
             {
                 for (auto& ys : spawn_yellow_)
@@ -137,13 +129,13 @@ namespace Modelec
         {
             last_odo_get_pos_time_ = node_->now();
             std_msgs::msg::Empty empty_msg;
-            odo_get_pos_pub_->publish(empty_msg);
+            // odo_get_pos_pub_->publish(empty_msg);
         }
     }
 
     void NavigationHelper::SendGoTo()
     {
-        while (!waypoint_queue_.empty())
+        /*while (!waypoint_queue_.empty())
         {
             waypoint_queue_.pop();
         }
@@ -156,28 +148,44 @@ namespace Modelec
         }
 
         auto w = waypoint_queue_.front().ToMsg();
-        /*RCLCPP_INFO(node_->get_logger(), "Sending waypoint: x: %d, y: %d, theta: %f, id: %d",
-                     w.x, w.y, w.theta, w.id);*/
         waypoint_pub_->publish(w);
-        waypoint_queue_.pop();
+        waypoint_queue_.pop();*/
+
+        SendWaypoints();
     }
 
-    /*    void NavigationHelper::SendWaypoint() const
+    void NavigationHelper::SendWaypoint() const
+    {
+        for (auto& w : waypoints_)
         {
-            for (auto& w : waypoints_)
+            waypoint_pub_->publish(w.ToMsg());
-            {
+        }
-                waypoint_pub_->publish(w.ToMsg());
+    }
-            }
+
+    void NavigationHelper::SendWaypoint(const std::vector<WaypointMsg>& waypoints) const
+    {
+        for (auto& w : waypoints)
+        {
+            waypoint_pub_->publish(w);
+        }
+    }
+
+    void NavigationHelper::SendWaypoints() const
+    {
+        WaypointsMsg waypoints_msg;
+
+        for (auto& w : waypoints_)
+        {
+            waypoints_msg.waypoints.push_back(w.ToMsg());
         }
 
-        void NavigationHelper::SendWaypoint(const std::vector<WaypointMsg>& waypoints) const
+        SendWaypoints(waypoints_msg);
-        {
+    }
-            for (auto& w : waypoints)
+
-            {
+    void NavigationHelper::SendWaypoints(const WaypointsMsg& waypoints) const
-                waypoint_pub_->publish(w);
+    {
-            }
+        waypoints_pub_->publish(waypoints);
-        }
+    }
-    */
 
     void NavigationHelper::AddWaypoint(const PosMsg& pos, int index)
     {
@@ -302,7 +310,7 @@ namespace Modelec
     {
         double angle = std::atan2(pos.y - current_pos_->y, pos.x - current_pos_->x);
 
-        if (std::abs(angle - current_pos_->theta) > M_PI / 3)
+        if (std::abs(angle - current_pos_->theta) > M_PI / 4)
         {
             Rotate(angle);
             return true;
@@ -518,7 +526,8 @@ namespace Modelec
                 auto zonePoint = zone->GetNextPotPos().GetTakeBasePosition();
                 double distance = Point::distance(posPoint, zonePoint);
                 double enemy_distance = Point::distance(enemyPos, zone->GetPosition());
-                double s = distance + enemy_distance * factor_close_enemy_;
+                double theta = std::abs(Point::angleDiff(posPoint, zonePoint));
+                double s = distance + enemy_distance * factor_close_enemy_ + theta * 2;
                 if (s < score)
                 {
                     score = s;
@@ -599,31 +608,6 @@ namespace Modelec
         }
     }
 
-    void NavigationHelper::OnEnemyPositionClose(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
-    {
-        if (!last_was_close_enemy_)
-        {
-            RCLCPP_INFO(node_->get_logger(), "Enemy is close, replanning...");
-
-            last_was_close_enemy_ = true;
-
-            pathfinding_->OnEnemyPosition(msg);
-            last_enemy_pos_ = *msg;
-
-            std_msgs::msg::Bool start_odo_msg;
-            start_odo_msg.data = false;
-            start_odo_pub_->publish(start_odo_msg);
-
-            /*waypoints_.clear();
-
-            Waypoint w(*msg, -1, false);
-
-            waypoints_.emplace_back(w);
-
-            SendGoTo();*/
-        }
-    }
-
     void NavigationHelper::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
     {
         pathfinding_->OnEnemyPositionLongTime(msg);
@@ -632,8 +616,7 @@ namespace Modelec
         for (auto& [id, zone] : deposite_zones_)
         {
             auto zonePos = zone->GetPosition();
-            if (Point::distance(enemy_pos, zonePos) < pathfinding_->robot_width_mm_ + (zone->GetWidth() / 2) +
+            if (Point::distance(enemy_pos, zonePos) < (zone->GetWidth() / 2) + pathfinding_->enemy_margin_mm_)
-                pathfinding_->enemy_margin_mm_)
             {
                 std::shared_ptr<Obstacle> obs = std::make_shared<Obstacle>(
                     id, zonePos.x, zonePos.y, zonePos.theta, zone->GetWidth(), zone->GetHeight(), "enemy-zone");
@@ -694,13 +677,13 @@ namespace Modelec
     {
         if (last_go_to_.goal)
         {
-            if (GoTo(last_go_to_.goal, last_go_to_.isClose, last_go_to_.collisionMask) != Pathfinding::FREE)
+            if (GoToRotateFirst(last_go_to_.goal, last_go_to_.isClose, last_go_to_.collisionMask) != Pathfinding::FREE)
             {
-                if (GoTo(last_go_to_.goal, true, last_go_to_.collisionMask) != Pathfinding::FREE)
+                if (GoToRotateFirst(last_go_to_.goal, true, last_go_to_.collisionMask) != Pathfinding::FREE)
                 {
                     if (!force) return false;
 
-                    if (GoTo(current_pos_, true,
+                    if (GoToRotateFirst(current_pos_, true,
                              Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE | Pathfinding::ENEMY) !=
                         Pathfinding::FREE)
                     {
@@ -742,9 +725,9 @@ namespace Modelec
         return spawn_;
     }
 
-    void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr)
+    void NavigationHelper::OnWaypointReach(const WaypointMsg::SharedPtr msg)
     {
-        /*for (auto& waypoint : waypoints_)
+        for (auto& waypoint : waypoints_)
         {
             if (waypoint.id == msg->id)
             {
@@ -759,12 +742,12 @@ namespace Modelec
                     {
                         waypoints_.emplace_back(w);
                     }
-                    SendWaypoint();
+                    SendGoTo();
                 }
             }
-        }*/
+        }
 
-        if (await_rotate_)
+        /*if (await_rotate_)
         {
             await_rotate_ = false;
 
@@ -789,7 +772,7 @@ namespace Modelec
             {
                 waypoints_.back().reached = true;
             }
-        }
+        }*/
     }
 
     void NavigationHelper::OnPos(const PosMsg::SharedPtr msg)
@@ -836,4 +819,4 @@ namespace Modelec
             Config::get<double>("config.spawn.blue.bottom@theta")
         );
     }
-}
+}

src/modelec_strat/src/pathfinding.cpp

@@ -3,33 +3,27 @@
 #include <modelec_strat/obstacle/column.hpp>
 #include <modelec_utils/config.hpp>
 
-namespace Modelec
+namespace Modelec {
-{
+    struct AStarNode {
-    struct AStarNode
-    {
         int x, y;
         double g = std::numeric_limits<double>::infinity(); // Cost from start
         double f = std::numeric_limits<double>::infinity(); // g + heuristic
         int parent_x = -1;
         int parent_y = -1;
 
-        bool operator>(const AStarNode& other) const
+        bool operator>(const AStarNode &other) const {
-        {
             return f > other.f;
         }
     };
 
-    double Heuristic(int x1, int y1, int x2, int y2)
+    double Heuristic(int x1, int y1, int x2, int y2) {
-    {
         return std::hypot(x1 - x2, y1 - y2); // Euclidean distance
     }
 
-    Pathfinding::Pathfinding()
+    Pathfinding::Pathfinding() {
-    {
     }
 
-    Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr& node) : node_(node)
+    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);
 
@@ -48,16 +42,14 @@ namespace Modelec
         grid_height_ = Config::get<int>("config.map.size.grid_height", 200);
 
         std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") +
-            "/data/obstacles.xml";
+                                     "/data/obstacles.xml";
-        if (!LoadObstaclesFromXML(obstacles_path))
+        if (!LoadObstaclesFromXML(obstacles_path)) {
-        {
             RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML");
         }
 
         obstacle_add_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>(
             "obstacle/add", 10,
-            [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg)
+            [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
-            {
                 RCLCPP_INFO(node_->get_logger(), "Obstacle add request received");
                 AddObstacle(std::make_shared<Obstacle>(*msg));
             });
@@ -67,8 +59,7 @@ namespace Modelec
 
         obstacle_remove_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>(
             "obstacle/remove", 10,
-            [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg)
+            [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
-            {
                 RCLCPP_INFO(node_->get_logger(), "Obstacle remove request received");
                 RemoveObstacle(msg->id);
             });
@@ -79,24 +70,21 @@ namespace Modelec
         ask_obstacle_srv_ = node_->create_service<std_srvs::srv::Empty>(
             "nav/ask_map_obstacle",
             [this](const std::shared_ptr<std_srvs::srv::Empty::Request> request,
-                   const std::shared_ptr<std_srvs::srv::Empty::Response> response)
+                   const std::shared_ptr<std_srvs::srv::Empty::Response> response) {
-            {
                 HandleAskObstacleRequest(request, response);
             });
 
         map_srv_ = node_->create_service<modelec_interfaces::srv::Map>(
             "nav/map",
             [this](const std::shared_ptr<modelec_interfaces::srv::Map::Request> request,
-                   const std::shared_ptr<modelec_interfaces::srv::Map::Response> response)
+                   const std::shared_ptr<modelec_interfaces::srv::Map::Response> response) {
-            {
                 HandleMapRequest(request, response);
             });
 
         map_size_srv_ = node_->create_service<modelec_interfaces::srv::MapSize>(
             "nav/map_size",
             [this](const std::shared_ptr<modelec_interfaces::srv::MapSize::Request> request,
-                   const std::shared_ptr<modelec_interfaces::srv::MapSize::Response> response)
+                   const std::shared_ptr<modelec_interfaces::srv::MapSize::Response> response) {
-            {
                 HandleMapSizeRequest(request, response);
             });
 
@@ -104,8 +92,7 @@ namespace Modelec
             "odometry/add_waypoint", 100);
     }
 
-    rclcpp::Node::SharedPtr Pathfinding::GetNode() const
+    rclcpp::Node::SharedPtr Pathfinding::GetNode() const {
-    {
         return node_;
     }
 
@@ -138,11 +125,9 @@ namespace Modelec
         }
     }*/
 
-    std::pair<int, WaypointListMsg> Pathfinding::FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal,
+    std::pair<int, WaypointListMsg> Pathfinding::FindPath(const PosMsg::SharedPtr &start, const PosMsg::SharedPtr &goal,
-                                                          bool isClose, int collisionMask)
+                                                          bool isClose, int collisionMask) {
-    {
+        if (!start || !goal) {
-        if (!start || !goal)
-        {
             RCLCPP_WARN(node_->get_logger(), "Start or Goal position is null");
             return {-3, WaypointListMsg()};
         }
@@ -156,13 +141,10 @@ namespace Modelec
         int inflate_x;
         int inflate_y;
 
-        if (isClose)
+        if (isClose) {
-        {
             inflate_x = std::ceil(((robot_width_mm_) / 2.0f) / cell_size_mm_x);
             inflate_y = std::ceil(((robot_length_mm_) / 2.0f) / cell_size_mm_y);
-        }
+        } else {
-        else
-        {
             inflate_x = std::ceil(((robot_width_mm_ + margin_mm_) / 2.0f) / cell_size_mm_x);
             inflate_y = std::ceil(((robot_length_mm_ + margin_mm_) / 2.0f) / cell_size_mm_y);
         }
@@ -170,27 +152,22 @@ namespace Modelec
         // 1. Build fresh empty grid
         grid_.clear();
         grid_.resize(grid_height_);
-        for (auto& row : grid_)
+        for (auto &row: grid_) {
-        {
             row.assign(grid_width_, FREE);
         }
 
-        if (has_enemy_pos_)
+        if (has_enemy_pos_) {
-        {
             int ex = (last_enemy_pos_.x / cell_size_mm_x);
             int ey = ((map_height_mm_ - last_enemy_pos_.y) / cell_size_mm_y);
 
             const int inflate_enemy_x = std::ceil((enemy_margin_mm_ + (enemy_width_mm_ / 2)) / cell_size_mm_x) +
-                inflate_x;
+                                        inflate_x;
             const int inflate_enemy_y = std::ceil((enemy_margin_mm_ + (enemy_length_mm_ / 2)) / cell_size_mm_y) +
-                inflate_y;
+                                        inflate_y;
 
-            for (int y = ey - inflate_enemy_y; y <= ey + inflate_enemy_y; ++y)
+            for (int y = ey - inflate_enemy_y; y <= ey + inflate_enemy_y; ++y) {
-            {
+                for (int x = ex - inflate_enemy_x; x <= ex + inflate_enemy_x; ++x) {
-                for (int x = ex - inflate_enemy_x; x <= ex + inflate_enemy_x; ++x)
+                    if (x >= 0 && y >= 0 && x < grid_width_ && y < grid_height_) {
-                {
-                    if (x >= 0 && y >= 0 && x < grid_width_ && y < grid_height_)
-                    {
                         grid_[y][x] |= ENEMY;
                     }
                 }
@@ -198,20 +175,16 @@ namespace Modelec
         }
 
         // Bord gauche et droit
-        for (int y = 0; y < grid_height_; ++y)
+        for (int y = 0; y < grid_height_; ++y) {
-        {
+            for (int x = 0; x < inflate_x; ++x) {
-            for (int x = 0; x < inflate_x; ++x)
-            {
                 grid_[y][x] |= WALL;
                 grid_[y][grid_width_ - 1 - x] |= WALL;
             }
         }
 
         // Bord haut et bas
-        for (int x = 0; x < grid_width_; ++x)
+        for (int x = 0; x < grid_width_; ++x) {
-        {
+            for (int y = 0; y < inflate_y; ++y) {
-            for (int y = 0; y < inflate_y; ++y)
-            {
                 grid_[y][x] |= WALL;
                 grid_[grid_height_ - 1 - y][x] |= WALL;
             }
@@ -219,8 +192,7 @@ namespace Modelec
 
         // 2. Fill obstacles with inflation
         // TODO some bug exist with the inflate
-        for (const auto& [id, obstacle] : obstacle_map_)
+        for (const auto &[id, obstacle]: obstacle_map_) {
-        {
             float cx = obstacle->GetX();
             float cy = obstacle->GetY();
             float width = obstacle->GetWidth() + inflate_x * 2 * cell_size_mm_x;
@@ -231,12 +203,11 @@ namespace Modelec
             float dy = height / 2.0f;
 
             // Compute corners in local space and rotate+translate to world
-            std::vector<std::pair<float, float>> corners = {
+            std::vector<std::pair<float, float> > corners = {
                 {-dx, -dy}, {dx, -dy}, {dx, dy}, {-dx, dy}
             };
 
-            for (auto& [x, y] : corners)
+            for (auto &[x, y]: corners) {
-            {
                 float rx = x * std::cos(theta) - y * std::sin(theta);
                 float ry = x * std::sin(theta) + y * std::cos(theta);
                 x = rx + cx;
@@ -249,24 +220,21 @@ namespace Modelec
             float min_y = corners[0].second;
             float max_y = corners[0].second;
 
-            for (const auto& [x, y] : corners)
+            for (const auto &[x, y]: corners) {
-            {
                 min_x = std::min(min_x, x);
                 max_x = std::max(max_x, x);
                 min_y = std::min(min_y, y);
                 max_y = std::max(max_y, y);
             }
 
-            int x_start = std::max(0, (int)(min_x / cell_size_mm_x));
+            int x_start = std::max(0, (int) (min_x / cell_size_mm_x));
-            int x_end = std::min(grid_width_ - 1, (int)(max_x / cell_size_mm_x));
+            int x_end = std::min(grid_width_ - 1, (int) (max_x / cell_size_mm_x));
-            int y_start = std::max(0, (int)(min_y / cell_size_mm_y));
+            int y_start = std::max(0, (int) (min_y / cell_size_mm_y));
-            int y_end = std::min(grid_height_ - 1, (int)(max_y / cell_size_mm_y));
+            int y_end = std::min(grid_height_ - 1, (int) (max_y / cell_size_mm_y));
 
             // Mark cells that fall inside rotated rectangle
-            for (int y = y_start; y <= y_end; ++y)
+            for (int y = y_start; y <= y_end; ++y) {
-            {
+                for (int x = x_start; x <= x_end; ++x) {
-                for (int x = x_start; x <= x_end; ++x)
-                {
                     // Convert cell center to world space
                     float wx = (x + 0.5f) * cell_size_mm_x;
                     float wy = (y + 0.5f) * cell_size_mm_y;
@@ -278,11 +246,9 @@ namespace Modelec
                     float lx = dx_ * std::cos(-theta) - dy_ * std::sin(-theta);
                     float ly = dx_ * std::sin(-theta) + dy_ * std::cos(-theta);
 
-                    if (std::abs(lx) <= dx + 1 && std::abs(ly) <= dy + 1)
+                    if (std::abs(lx) <= dx + 1 && std::abs(ly) <= dy + 1) {
-                    {
                         int gy = (grid_height_ - 1) - y;
-                        if (x >= 0 && gy >= 0 && x < grid_width_ && gy < grid_height_)
+                        if (x >= 0 && gy >= 0 && x < grid_width_ && gy < grid_height_) {
-                        {
                             grid_[gy][x] |= OBSTACLE;
                         }
                     }
@@ -300,38 +266,31 @@ namespace Modelec
 
         if (start_x < 0 || start_y < 0 || goal_x < 0 || goal_y < 0 ||
             start_x >= grid_width_ || start_y >= grid_height_ ||
-            goal_x >= grid_width_ || goal_y >= grid_height_)
+            goal_x >= grid_width_ || goal_y >= grid_height_) {
-        {
             RCLCPP_WARN(node_->get_logger(), "Start or Goal out of bounds");
             return {-2, waypoints};
         }
 
-        if (!TestCollision(start_x, start_y, collisionMask) || !TestCollision(goal_x, goal_y, collisionMask))
+        if (!TestCollision(start_x, start_y, collisionMask) || !TestCollision(goal_x, goal_y, collisionMask)) {
-        {
+            if (!TestCollision(start_x, start_y, collisionMask)) {
-            if (!TestCollision(start_x, start_y, collisionMask))
-            {
                 RCLCPP_WARN(node_->get_logger(), "Start inside an obstacle");
                 return {grid_[start_y][start_x], waypoints};
-            }
+            } else {
-            else
-            {
                 RCLCPP_WARN(node_->get_logger(), "Goal inside an obstacle");
                 return {grid_[goal_y][goal_x], waypoints};
             }
         }
 
         // 4. A* algorithm
-        std::priority_queue<AStarNode, std::vector<AStarNode>, std::greater<AStarNode>> open_list;
+        std::priority_queue<AStarNode, std::vector<AStarNode>, std::greater<AStarNode> > open_list;
         std::unordered_map<int64_t, AStarNode> all_nodes;
 
-        auto hash = [](int x, int y)
+        auto hash = [](int x, int y) {
-        {
+            return (int64_t) x << 32 | (uint32_t) y;
-            return (int64_t)x << 32 | (uint32_t)y;
         };
 
-        auto neighbors = [](int x, int y)
+        auto neighbors = [](int x, int y) {
-        {
+            return std::vector<std::pair<int, int> >{
-            return std::vector<std::pair<int, int>>{
                 {x + 1, y}, {x - 1, y}, {x, y + 1}, {x, y - 1},
                 {x + 1, y + 1}, {x - 1, y + 1}, {x + 1, y - 1}, {x - 1, y - 1} // diagonals
             };
@@ -346,20 +305,17 @@ namespace Modelec
 
         bool path_found = false;
 
-        while (!open_list.empty())
+        while (!open_list.empty()) {
-        {
             AStarNode current = open_list.top();
             open_list.pop();
 
-            if (current.x == goal_x && current.y == goal_y)
+            if (current.x == goal_x && current.y == goal_y) {
-            {
                 path_found = true;
                 break;
             }
 
-            for (const auto& [nx, ny] : neighbors(current.x, current.y))
+            for (const auto &[nx, ny]: neighbors(current.x, current.y)) {
-            {
+                if (nx < 0 || ny < 0 || ny >= (int) grid_.size() || nx >= (int) grid_[0].size())
-                if (nx < 0 || ny < 0 || ny >= (int)grid_.size() || nx >= (int)grid_[0].size())
                     continue;
 
                 if (!TestCollision(nx, ny, collisionMask))
@@ -368,8 +324,7 @@ namespace Modelec
                 double tentative_g = current.g + Heuristic(current.x, current.y, nx, ny);
                 int64_t neighbor_hash = hash(nx, ny);
 
-                if (all_nodes.find(neighbor_hash) == all_nodes.end() || tentative_g < all_nodes[neighbor_hash].g)
+                if (all_nodes.find(neighbor_hash) == all_nodes.end() || tentative_g < all_nodes[neighbor_hash].g) {
-                {
                     AStarNode neighbor;
                     neighbor.x = nx;
                     neighbor.y = ny;
@@ -383,19 +338,17 @@ namespace Modelec
             }
         }
 
-        if (!path_found)
+        if (!path_found) {
-        {
             RCLCPP_WARN(node_->get_logger(), "No path found");
             return {-1, waypoints};
         }
 
         // 5. Reconstruct path
-        std::vector<std::pair<int, int>> path;
+        std::vector<std::pair<int, int> > path;
         int cx = goal_x;
         int cy = goal_y;
 
-        while (!(cx == start_x && cy == start_y))
+        while (!(cx == start_x && cy == start_y)) {
-        {
             path.emplace_back(cx, cy);
             auto it = all_nodes.find(hash(cx, cy));
             if (it == all_nodes.end())
@@ -407,13 +360,11 @@ namespace Modelec
         std::reverse(path.begin(), path.end());
 
         // 6. Path smoothing
-        std::vector<std::pair<int, int>> smooth_path;
+        std::vector<std::pair<int, int> > smooth_path;
         size_t current = 0;
-        while (current < path.size())
+        while (current < path.size()) {
-        {
             size_t next = current + 1;
-            while (next < path.size())
+            while (next < path.size()) {
-            {
                 bool clear = true;
                 int x0 = path[current].first;
                 int y0 = path[current].second;
@@ -428,23 +379,19 @@ namespace Modelec
                 int x = x0;
                 int y = y0;
 
-                while (true)
+                while (true) {
-                {
+                    if (!TestCollision(x, y, collisionMask)) {
-                    if (!TestCollision(x, y, collisionMask))
-                    {
                         clear = false;
                         break;
                     }
                     if (x == x1 && y == y1)
                         break;
                     int e2 = 2 * err;
-                    if (e2 >= dy)
+                    if (e2 >= dy) {
-                    {
                         err += dy;
                         x += sx;
                     }
-                    if (e2 <= dx)
+                    if (e2 <= dx) {
-                    {
                         err += dx;
                         y += sy;
                     }
@@ -456,8 +403,7 @@ namespace Modelec
             }
 
             smooth_path.push_back(path[current]);
-            if (next == path.size())
+            if (next == path.size()) {
-            {
                 smooth_path.push_back(path.back());
                 break;
             }
@@ -498,13 +444,11 @@ namespace Modelec
 
         // 7. Fill Waypoints (reconvertir grille -> millimètres)
         int id = 0;
-        for (size_t i = 0; i < smooth_path.size(); ++i)
+        for (size_t i = 0; i < smooth_path.size(); ++i) {
-        {
+            const auto &[x, y] = smooth_path[i];
-            const auto& [x, y] = smooth_path[i];
 
             // Skip first point if it's too close to robot position
-            if (i == 0)
+            if (i == 0) {
-            {
                 float world_x = x * cell_size_mm_x;
                 float world_y = (grid_height_ - 1 - y) * cell_size_mm_y;
 
@@ -520,16 +464,15 @@ namespace Modelec
             wp.y = (grid_height_ - 1 - y) * cell_size_mm_y;
 
             // Calculer l'angle entre le point actuel et le prochain point
-            if (i < smooth_path.size() - 1) // Si ce n'est pas le dernier point
+            if (i < smooth_path.size() - 1) {
-            {
+                const auto &[next_x, next_y] = smooth_path[i + 1];
-                const auto& [next_x, next_y] = smooth_path[i + 1];
                 // Calcul de l'angle entre (x, y) et (next_x, next_y)
                 float delta_x = next_x * cell_size_mm_x - wp.x;
                 float delta_y = (grid_height_ - 1 - next_y) * cell_size_mm_y - wp.y;
                 wp.theta = std::atan2(delta_y, delta_x); // Calcul de l'angle en radians
-            }
+            } else {
-            else
+                wp.x = goal->x;
-            {
+                wp.y = goal->y;
                 wp.theta = goal->theta;
             }
 
@@ -537,26 +480,22 @@ namespace Modelec
             wp.is_end = false;
             waypoints.push_back(wp);
         }
-        if (!waypoints.empty())
+        if (!waypoints.empty()) {
-        {
             waypoints.back().is_end = true;
         }
 
         return {FREE, waypoints};
     }
 
-    void Pathfinding::SetCurrentPos(const PosMsg::SharedPtr& pos)
+    void Pathfinding::SetCurrentPos(const PosMsg::SharedPtr &pos) {
-    {
         current_start_ = pos;
     }
 
-    std::shared_ptr<Obstacle> Pathfinding::GetObstacle(int id) const
+    std::shared_ptr<Obstacle> Pathfinding::GetObstacle(int id) const {
-    {
         return obstacle_map_.at(id);
     }
 
-    void Pathfinding::RemoveObstacle(int id)
+    void Pathfinding::RemoveObstacle(int id) {
-    {
         obstacle_map_.erase(id);
 
         modelec_interfaces::msg::Obstacle msg;
@@ -564,38 +503,42 @@ namespace Modelec
         obstacle_remove_pub_->publish(msg);
     }
 
-    void Pathfinding::AddObstacle(const std::shared_ptr<Obstacle>& obstacle)
+    void Pathfinding::AddObstacle(const std::shared_ptr<Obstacle> &obstacle) {
-    {
         obstacle_map_[obstacle->GetId()] = obstacle;
         modelec_interfaces::msg::Obstacle msg = obstacle->toMsg();
         obstacle_add_pub_->publish(msg);
     }
 
-    std::shared_ptr<ColumnObstacle> Pathfinding::GetClosestColumn(const PosMsg::SharedPtr& pos,
+    std::shared_ptr<ColumnObstacle> Pathfinding::GetClosestColumn(const PosMsg::SharedPtr &pos,
-                                                                  const std::vector<int>& blacklistedId)
+                                                                  const std::vector<int> &blacklistedId) {
-    {
-        // TODO score (count dist and dist with enemy)
         std::shared_ptr<ColumnObstacle> 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 score = std::numeric_limits<float>::max();
 
-        for (const auto& [id, obstacle] : obstacle_map_)
+        for (const auto &[id, obstacle]: obstacle_map_) {
-        {
+            if (auto obs = std::dynamic_pointer_cast<ColumnObstacle>(obstacle)) {
-            if (auto obs = std::dynamic_pointer_cast<ColumnObstacle>(obstacle))
-            {
                 if (!obs->IsAtObjective() && std::find(blacklistedId.begin(), blacklistedId.end(), obs->GetId()) ==
-                    blacklistedId.end())
+                    blacklistedId.end()) {
-                {
+                    for (auto possiblePos: obs->GetAllPossiblePositions()) {
-                    for (auto possiblePos : obs->GetAllPossiblePositions())
-                    {
                         auto dist = Point::distance(robotPos, possiblePos);
                         auto distEnemy = Point::distance(enemyPos, possiblePos);
+                        auto takePoint = possiblePos;
+                        takePoint.theta += M_PI;
+                        double theta = std::abs(Point::angleDiff(robotPos, takePoint));
 
-                        auto s = dist + distEnemy * factor_close_enemy_;
+                        auto dist_score = dist;
+                        auto distEnemy_score = distEnemy * factor_close_enemy_ * has_enemy_pos_;
+                        auto theta_score = (theta / M_PI * 250);
+
+                        auto s = dist_score + distEnemy_score + theta_score;
+
+                        if (s < score) {
+                            /*RCLCPP_INFO(node_->get_logger(),
+                                        "Column %d at (%d, %d) with dist_s=%f, distEnemy_s=%f, theta=%f, theta_s=%f, score=%f",
+                                        obs->GetId(), possiblePos.x, possiblePos.y, dist_score, distEnemy_score, theta,
+                                        theta_score, s);*/
 
-                        if (s < score)
-                        {
                             score = s;
                             closest_obstacle = obs;
                         }
@@ -608,63 +551,50 @@ namespace Modelec
     }
 
     void Pathfinding::HandleMapRequest(const std::shared_ptr<modelec_interfaces::srv::Map::Request>,
-                                       const std::shared_ptr<modelec_interfaces::srv::Map::Response> response)
+                                       const std::shared_ptr<modelec_interfaces::srv::Map::Response> response) {
-    {
         response->width = grid_[0].size();
         response->height = grid_.size();
         response->map = std::vector<int>(grid_.size() * grid_[0].size());
-        for (size_t i = 0; i < grid_.size(); i++)
+        for (size_t i = 0; i < grid_.size(); i++) {
-        {
+            for (size_t j = 0; j < grid_[i].size(); j++) {
-            for (size_t j = 0; j < grid_[i].size(); j++)
-            {
                 response->map[i * grid_[i].size() + j] = grid_[i][j];
             }
         }
     }
 
     void Pathfinding::HandleMapSizeRequest(const std::shared_ptr<modelec_interfaces::srv::MapSize::Request>,
-                                           const std::shared_ptr<modelec_interfaces::srv::MapSize::Response> response)
+                                           const std::shared_ptr<modelec_interfaces::srv::MapSize::Response> response) {
-    {
         response->width = grid_width_;
         response->height = grid_height_;
     }
 
     void Pathfinding::HandleAskObstacleRequest(const std::shared_ptr<std_srvs::srv::Empty::Request>,
-                                               const std::shared_ptr<std_srvs::srv::Empty::Response>)
+                                               const std::shared_ptr<std_srvs::srv::Empty::Response>) {
-    {
+        for (auto &[index, obs]: obstacle_map_) {
-        for (auto& [index, obs] : obstacle_map_)
-        {
             obstacle_add_pub_->publish(obs->toMsg());
         }
     }
 
-    void Pathfinding::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
+    void Pathfinding::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
-    {
         last_enemy_pos_ = *msg;
         has_enemy_pos_ = true;
     }
 
-    void Pathfinding::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
+    void Pathfinding::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
-    {
         Point enemyPos(msg->x, msg->y, msg->theta);
-        for (auto& [index, obs] : obstacle_map_)
+        for (auto &[index, obs]: obstacle_map_) {
-        {
+            if (auto column = std::dynamic_pointer_cast<ColumnObstacle>(obs)) {
-            if (auto column = std::dynamic_pointer_cast<ColumnObstacle>(obs))
-            {
                 if (Point::distance(enemyPos, column->GetPosition()) < enemy_width_mm_ + (column->GetWidth() / 2) +
-                    enemy_margin_mm_)
+                    enemy_margin_mm_) {
-                {
                     RemoveObstacle(column->GetId());
                 }
             }
         }
     }
 
-    bool Pathfinding::TestCollision(int x, int y, int collisionMask)
+    bool Pathfinding::TestCollision(int x, int y, int collisionMask) {
-    {
         if (y < 0 || y >= static_cast<int>(grid_.size()) ||
-            x < 0 || x >= static_cast<int>(grid_[y].size()))
+            x < 0 || x >= static_cast<int>(grid_[y].size())) {
-        {
             RCLCPP_WARN(node_->get_logger(), "TestCollision: access out of bounds x=%d y=%d", x, y);
             return false; // ou true, selon ce que tu veux (false = pas de collision)
         }
@@ -672,34 +602,29 @@ namespace Modelec
         return (grid_[y][x] & collisionMask) && !(grid_[y][x] & ~collisionMask);
     }
 
-    bool Pathfinding::LoadObstaclesFromXML(const std::string& filename)
+    bool Pathfinding::LoadObstaclesFromXML(const std::string &filename) {
-    {
         tinyxml2::XMLDocument doc;
-        if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS)
+        if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS) {
-        {
             RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacle XML file: %s", filename.c_str());
             return false;
         }
 
-        tinyxml2::XMLElement* root = doc.FirstChildElement("Obstacles");
+        tinyxml2::XMLElement *root = doc.FirstChildElement("Obstacles");
-        if (!root)
+        if (!root) {
-        {
             RCLCPP_ERROR(node_->get_logger(), "No <Obstacles> root element in file");
             return false;
         }
 
-        for (tinyxml2::XMLElement* obstacleElem = root->FirstChildElement("Obstacle");
+        for (tinyxml2::XMLElement *obstacleElem = root->FirstChildElement("Obstacle");
              obstacleElem != nullptr;
-             obstacleElem = obstacleElem->NextSiblingElement("Obstacle"))
+             obstacleElem = obstacleElem->NextSiblingElement("Obstacle")) {
-        {
             std::shared_ptr<Obstacle> obs = std::make_shared<Obstacle>(obstacleElem);
             obstacle_map_[obs->GetId()] = obs;
         }
 
-        for (tinyxml2::XMLElement* obstacleElem = root->FirstChildElement("Gradin");
+        for (tinyxml2::XMLElement *obstacleElem = root->FirstChildElement("Gradin");
              obstacleElem != nullptr;
-             obstacleElem = obstacleElem->NextSiblingElement("Gradin"))
+             obstacleElem = obstacleElem->NextSiblingElement("Gradin")) {
-        {
             std::shared_ptr<ColumnObstacle> obs = std::make_shared<ColumnObstacle>(obstacleElem);
             obstacle_map_[obs->GetId()] = obs;
         }
@@ -708,8 +633,7 @@ namespace Modelec
         return true;
     }
 
-    Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos& pos, int index, bool isLast)
+    Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos &pos, int index, bool isLast) {
-    {
         id = index;
         x = pos.x;
         y = pos.y;
@@ -718,8 +642,7 @@ namespace Modelec
         reached = false;
     }
 
-    Waypoint::Waypoint(const WaypointMsg& waypoint)
+    Waypoint::Waypoint(const WaypointMsg &waypoint) {
-    {
         id = waypoint.id;
         x = waypoint.x;
         y = waypoint.y;
@@ -728,8 +651,7 @@ namespace Modelec
         reached = false;
     }
 
-    WaypointMsg Waypoint::ToMsg() const
+    WaypointMsg Waypoint::ToMsg() const {
-    {
+        return static_cast<OdometryWaypoint_<std::allocator<void> >>(*this);
-        return static_cast<OdometryAddWaypoint_<std::allocator<void>>>(*this);
     }
-}
+}

src/modelec_utils/include/modelec_utils/point.hpp

@@ -11,11 +11,12 @@ namespace Modelec
         Point(int x, int y, double theta) : x(x), y(y), theta(theta) {}
 
         static double distance(const Point& p1, const Point& p2);
+        static double angleDiff(const Point& p1, const Point& p2);
 
-        Point GetTakePosition(int distance, double angle) const;
+        [[nodiscard]] Point GetTakePosition(int distance, double angle) const;
-        Point GetTakePosition(int distance) const;
+        [[nodiscard]] Point GetTakePosition(int distance) const;
 
-        Point GetTakeBasePosition() const;
+        [[nodiscard]] Point GetTakeBasePosition() const;
-        Point GetTakeClosePosition() const;
+        [[nodiscard]] Point GetTakeClosePosition() const;
     };
 }

src/modelec_utils/src/point.cpp

@@ -8,6 +8,13 @@ namespace Modelec
         return sqrt(std::pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
     }
 
+    double Point::angleDiff(const Point &p1, const Point &p2) {
+        double diff = std::fmod(p1.theta - p2.theta + M_PI, 2 * M_PI);
+        if (diff < 0)
+            diff += 2 * M_PI;
+        return diff - M_PI;
+    }
+
     Point Point::GetTakePosition(int distance, double angle) const
     {
         Point pos;