STRAT v1 :

- pathfinding ultra optiomiz
- évitement STATIQUE d'objet (évitement dynamique dans quelque jours)
- ui de test

src/modelec_com/src/pcb_alim_interface.cpp

@@ -10,7 +10,7 @@ namespace Modelec
         auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
         request->name = "pcb_alim";
         request->bauds = 115200;
-        request->serial_port = "/dev/pts/10"; // TODO : check the real serial port
+        request->serial_port = "/dev/pts/11"; // TODO : check the real serial port
         auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
         while (!client->wait_for_service(std::chrono::seconds(1)))
         {

src/modelec_gui/CMakeLists.txt

@@ -14,6 +14,7 @@ set(CMAKE_AUTOUIC ON)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(modelec_interfaces REQUIRED)
 find_package(modelec_utils COMPONENTS
         modelec_utils
@@ -45,6 +46,7 @@ add_executable(modelec_gui
 ament_target_dependencies(modelec_gui
         rclcpp
         std_msgs
+        std_srvs
         modelec_interfaces
 )
 target_link_libraries(modelec_gui

src/modelec_gui/include/modelec_gui/modelec_gui.hpp

@@ -33,5 +33,7 @@ namespace ModelecGUI
         QAction* playmat_map_;
         QAction* playmat_grid_;
 
+        QAction* toggle_show_obstacle_action_;
+
     };
 }

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

@@ -10,8 +10,13 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <std_srvs/srv/empty.hpp>
+
 #include <modelec_interfaces/msg/odometry_add_waypoint.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>
 
 namespace ModelecGUI {
     class MapPage : public QWidget
@@ -26,6 +31,8 @@ namespace ModelecGUI {
 
         void setPlaymatMap();
 
+        void toggleShowObstacle();
+
     protected:
         void paintEvent(QPaintEvent*) override;
 
@@ -33,6 +40,8 @@ namespace ModelecGUI {
 
         void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
 
+        void OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg);
+
         QSvgRenderer* renderer;
 
         QVBoxLayout* v_layout;
@@ -45,11 +54,20 @@ namespace ModelecGUI {
 
         bool lastWapointWasEnd = false;
 
+        std::map<int, modelec_interfaces::msg::Obstacle> obstacle_;
+        bool show_obstacle_ = true;
+
+        int map_width_ = 0;
+        int map_height_ = 0;
+
         rclcpp::Node::SharedPtr node_;
 
         rclcpp::Subscription<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr add_waypoint_sub_;
 
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odometry_sub_;
         rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr go_to_pub_;
+        rclcpp::Client<modelec_interfaces::srv::MapSize>::SharedPtr map_client_;
+        rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_sub_;
+        rclcpp::Client<std_srvs::srv::Empty>::SharedPtr ask_map_obstacle_client_;
     };
 }

src/modelec_gui/package.xml

@@ -12,6 +12,7 @@
   <depend>modelec_interfaces</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/modelec_gui/src/modelec_gui.cpp

@@ -35,6 +35,8 @@ namespace ModelecGUI {
         playmat_map_ = new QAction("Map", this);
         playmat_grid_ = new QAction("Grid", this);
 
+        toggle_show_obstacle_action_ = new QAction("Toggle Show Obstacle", this);
+
         exit_action_ = new QAction("Exit", this);
 
         optionsMenu->addAction(home_action_);
@@ -44,6 +46,7 @@ namespace ModelecGUI {
         optionsMenu->addMenu(playmat_map_menu_);
         playmat_map_menu_->addAction(playmat_map_);
         playmat_map_menu_->addAction(playmat_grid_);
+        optionsMenu->addAction(toggle_show_obstacle_action_);
         optionsMenu->addSeparator();
         optionsMenu->addAction(exit_action_);
 
@@ -73,6 +76,14 @@ namespace ModelecGUI {
             }
         });
 
+        connect(toggle_show_obstacle_action_, &QAction::triggered, this, [this]() {
+            auto map_page = dynamic_cast<MapPage *>(stackedWidget->currentWidget());
+            if (map_page)
+            {
+                map_page->toggleShowObstacle();
+            }
+        });
+
         connect(exit_action_, &QAction::triggered, this, [this]() {
             close();
         });

src/modelec_gui/src/pages/map_page.cpp

@@ -6,7 +6,7 @@
 
 namespace ModelecGUI
 {
-    MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), renderer(new QSvgRenderer(QString(":/img/playmat/grid_v1.svg"), this)), node_(std::move(node))
+    MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), renderer(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this)), node_(node)
     {
         v_layout = new QVBoxLayout(this);
         v_layout->addStretch();
@@ -22,7 +22,7 @@ namespace ModelecGUI
         qpoints = {};
         points = {};
 
-        add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>("odometry/add_waypoint", 10,
+        add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>("odometry/add_waypoint", 100,
             [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) {
                 if (lastWapointWasEnd)
                 {
@@ -50,7 +50,53 @@ namespace ModelecGUI
                 update();
         });
 
+        obstacle_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>("nav/obstacle", 40,
+            [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
+                OnObstacleReceived(msg);
+        });
+
         go_to_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("nav/go_to", 10);
+
+        // client to nav/map
+        map_client_ = node_->create_client<modelec_interfaces::srv::MapSize>("nav/map_size");
+        while (!map_client_->wait_for_service(std::chrono::seconds(1))) {
+            if (!rclcpp::ok()) {
+                RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
+                return;
+            }
+            RCLCPP_INFO(node_->get_logger(), "Waiting for the service...");
+        }
+
+        auto result = map_client_->async_send_request(std::make_shared<modelec_interfaces::srv::MapSize::Request>());
+        if (rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result) ==
+            rclcpp::FutureReturnCode::SUCCESS)
+        {
+            if (auto res = result.get())
+            {
+                RCLCPP_INFO(node_->get_logger(), "Map received: %d x %d", res->width, res->height);
+                map_width_ = res->width;
+                map_height_ = res->height;
+                /* TODO - obstacle
+                 * idea
+                 * send only the size of the map
+                 * and then send the obstacle with id throw the topic obstacle
+                 * problem : if not initialized, idk what to do
+                 * maybe solution ask to send back the obstacle throw an other service
+                 */
+            }
+        }
+
+        ask_map_obstacle_client_ = node_->create_client<std_srvs::srv::Empty>("nav/ask_map_obstacle");
+        while (!ask_map_obstacle_client_->wait_for_service(std::chrono::seconds(1))) {
+            if (!rclcpp::ok()) {
+                RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
+                return;
+            }
+            RCLCPP_INFO(node_->get_logger(), "Waiting for the service...");
+        }
+
+        auto result2 = ask_map_obstacle_client_->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
+        rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result2);
     }
 
     void MapPage::setPlaymatGrid()
@@ -65,6 +111,11 @@ namespace ModelecGUI
         update();
     }
 
+    void MapPage::toggleShowObstacle()
+    {
+        show_obstacle_ = !show_obstacle_;
+    }
+
     void MapPage::paintEvent(QPaintEvent* paint_event)
     {
         QWidget::paintEvent(paint_event);
@@ -100,6 +151,16 @@ namespace ModelecGUI
             painter.setPen(Qt::NoPen);
             painter.drawEllipse(qpoints[i], radius, radius);
         }
+
+        if (show_obstacle_)
+        {
+            float ratioBetweenMapAndWidget = height() / 2000.0f;
+            for (auto & [index, obs] : obstacle_)
+            {
+                painter.setBrush(Qt::black);
+                painter.drawRect(obs.x * ratioBetweenMapAndWidget, height() - (obs.y + obs.height) * ratioBetweenMapAndWidget, obs.width * ratioBetweenMapAndWidget, obs.height * ratioBetweenMapAndWidget);
+            }
+        }
     }
 
     void MapPage::mousePressEvent(QMouseEvent* event)
@@ -109,6 +170,7 @@ namespace ModelecGUI
         modelec_interfaces::msg::OdometryPos msg;
         msg.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000);
         msg.y = 2000 - Modelec::mapValue(event->pos().y(), 0, height(), 0, 2000);
+        msg.theta = 0;
 
         go_to_pub_->publish(msg);
 
@@ -137,4 +199,9 @@ namespace ModelecGUI
 
         update();*/
     }
+
+    void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg)
+    {
+        obstacle_.emplace(msg->id, *msg);
+    }
 }

src/modelec_gui/src/pages/test_page.cpp

@@ -5,7 +5,7 @@
 namespace ModelecGUI
 {
 
-    TestPage::TestPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), node_(std::move(node))
+    TestPage::TestPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), node_(node)
     {
         startButton_ = new QPushButton("Start");
         connect(startButton_, &QPushButton::clicked, this, [this]() {
@@ -41,12 +41,15 @@ namespace ModelecGUI
 
         askPID_ = new QPushButton("Ask PID");
         connect(askPID_, &QPushButton::clicked, this, [this]() {
+            RCLCPP_INFO(node_->get_logger(), "Ask PID button clicked.");
             // Create a request for the PID service
             auto request = std::make_shared<modelec_interfaces::srv::OdometryGetPid::Request>();
 
             // Make the asynchronous service call
             client_get_pid_->async_send_request(request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryGetPid>::SharedFuture response) {
+                RCLCPP_INFO(node_->get_logger(), "Received PID response.");
                 if (auto res = response.get()) {
+                    RCLCPP_INFO(node_->get_logger(), "PID values received: p=%f, i=%f, d=%f", res->p, res->i, res->d);
                     QMetaObject::invokeMethod(this, [this, res]() {
                         pPIDBox_->setText(QString("%1").arg(res->p));
                         iPIDBox_->setText(QString("%1").arg(res->i));
@@ -158,10 +161,7 @@ namespace ModelecGUI
         pub_add_waypoint_ = node_->create_publisher<modelec_interfaces::msg::OdometryAddWaypoint>(
             "/odometry/add_waypoint", 10);
 
-        // Set up service client
         client_ = node_->create_client<modelec_interfaces::srv::OdometrySpeed>("odometry/speed");
-
-        // Wait for the service to be available
         while (!client_->wait_for_service(std::chrono::seconds(1))) {
             if (!rclcpp::ok()) {
                 RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
@@ -171,7 +171,6 @@ namespace ModelecGUI
         }
 
         client_start_ = node_->create_client<modelec_interfaces::srv::OdometryStart>("odometry/start");
-
         while (!client_start_->wait_for_service(std::chrono::seconds(1))) {
             if (!rclcpp::ok()) {
                 RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");

src/modelec_interfaces/CMakeLists.txt

@@ -19,6 +19,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Odometry/OdometryAddWaypoint.msg"
   "msg/Odometry/OdometryStart.msg"
   "msg/Odometry/PID/OdometryPid.msg"
+  "msg/Map/Map.msg"
+  "msg/Map/Obstacle.msg"
   "msg/PCA9685Servo.msg"
   "msg/ServoMode.msg"
   "msg/Solenoid.msg"
@@ -35,6 +37,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/Odometry/OdometryAddWaypoint.srv"
   "srv/Odometry/PID/OdometryGetPid.srv"
   "srv/Odometry/PID/OdometrySetPid.srv"
+  "srv/Map/Map.srv"
+  "srv/Map/MapSize.srv"
   "srv/AddServoMotor.srv"
   "srv/AddSolenoid.srv"
   "srv/Tirette.srv"

src/modelec_interfaces/msg/Map/Map.msg

@@ -0,0 +1,3 @@
+int32 width
+int32 height
+int32[] map

src/modelec_interfaces/msg/Map/Obstacle.msg

@@ -0,0 +1,5 @@
+int32 id
+int32 x
+int32 y
+int32 width
+int32 height

src/modelec_interfaces/srv/Map/Map.srv

@@ -0,0 +1,4 @@
+---
+int32 width
+int32 height
+int32[] map

src/modelec_interfaces/srv/Map/MapSize.srv

@@ -0,0 +1,3 @@
+---
+int32 width
+int32 height

src/modelec_strat/CMakeLists.txt

@@ -7,14 +7,16 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(tinyxml2 REQUIRED)
 find_package(rclcpp REQUIRED)
 
 find_package(modelec_interfaces REQUIRED)
 find_package(modelec_utils REQUIRED)
 
 add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/mission_manager.cpp)
-ament_target_dependencies(strat_fsm rclcpp std_msgs modelec_interfaces)
-target_link_libraries(strat_fsm modelec_utils::modelec_utils)
+ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces)
+target_link_libraries(strat_fsm modelec_utils::modelec_utils tinyxml2::tinyxml2)
 target_include_directories(strat_fsm PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>

src/modelec_strat/include/modelec_strat/action_executor.hpp

@@ -10,7 +10,7 @@ namespace Modelec
     public:
         ActionExecutor();
 
-        ActionExecutor(rclcpp::Node::SharedPtr node);
+        ActionExecutor(const rclcpp::Node::SharedPtr& node);
 
         rclcpp::Node::SharedPtr getNode() const;
 

src/modelec_strat/include/modelec_strat/mission_manager.hpp

@@ -10,7 +10,7 @@ namespace Modelec
     public:
         MissionManager();
 
-        MissionManager(rclcpp::Node::SharedPtr node);
+        MissionManager(const rclcpp::Node::SharedPtr& node);
 
         rclcpp::Node::SharedPtr getNode() const;
 

src/modelec_strat/include/modelec_strat/navigation_helper.hpp

@@ -13,11 +13,12 @@ namespace Modelec {
     public:
         NavigationHelper();
 
-        NavigationHelper(rclcpp::Node::SharedPtr node);
+        NavigationHelper(const rclcpp::Node::SharedPtr& node);
 
         rclcpp::Node::SharedPtr getNode() const;
 
         void SendWaypoint() const;
+        void SendWaypoint(const std::vector<WaypointMsg> &waypoints) const;
 
         void AddWaypoint(const PosMsg &pos, int index);
         void AddWaypoint(const WaypointMsg &waypoint);

src/modelec_strat/include/modelec_strat/pathfinding.hpp

@@ -2,9 +2,18 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <std_srvs/srv/empty.hpp>
+
+#include <tinyxml2.h>
+
 #include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
 #include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
 #include <modelec_interfaces/msg/odometry_pos.hpp>
+#include <modelec_interfaces/msg/map.hpp>
+#include <modelec_interfaces/srv/map.hpp>
+#include <modelec_interfaces/srv/map_size.hpp>
+#include <modelec_interfaces/msg/obstacle.hpp>
+
 
 namespace Modelec {
 
@@ -36,15 +45,47 @@ namespace Modelec {
     public:
         Pathfinding();
 
-        Pathfinding(rclcpp::Node::SharedPtr node);
+        Pathfinding(const rclcpp::Node::SharedPtr& node);
 
         rclcpp::Node::SharedPtr getNode() const;
 
         WaypointListMsg FindPath(const PosMsg::SharedPtr& start,
                       const PosMsg::SharedPtr& goal);
 
+        bool LoadObstaclesFromXML(const std::string& filename);
+
+    protected:
+        void HandleMapRequest(
+            const std::shared_ptr<modelec_interfaces::srv::Map::Request> request,
+            const std::shared_ptr<modelec_interfaces::srv::Map::Response> response);
+
+        void HandleMapSizeRequest(
+            const std::shared_ptr<modelec_interfaces::srv::MapSize::Request> request,
+            const std::shared_ptr<modelec_interfaces::srv::MapSize::Response> response);
+
+        void HandleAskObstacleRequest(
+            const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+            const std::shared_ptr<std_srvs::srv::Empty::Response> response);
+
     private:
+        int robot_width_mm_;
+        int robot_length_mm_;
+
+        int grid_width_;
+        int grid_height_;
+
+        std::vector<std::vector<int>> grid_;
+
+        std::map<int, modelec_interfaces::msg::Obstacle> obstacle_map_;
+
         rclcpp::Node::SharedPtr node_;
+
+        rclcpp::Service<modelec_interfaces::srv::Map>::SharedPtr map_srv_;
+        rclcpp::Service<modelec_interfaces::srv::MapSize>::SharedPtr map_size_srv_;
+        rclcpp::Service<std_srvs::srv::Empty>::SharedPtr ask_obstacle_srv_;
+
+        rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr map_pub_;
+
     };
 
-}
+}

src/modelec_strat/map/obstacles.xml

@@ -0,0 +1,25 @@
+<Obstacles>
+    <!-- TOP -->
+    <Obstacle id="1" x="600" y="1800" width="450" height="200" />
+    <Obstacle id="2" x="1050" y="1550" width="900" height="450" />
+    <Obstacle id="3" x="1950" y="1800" width="450" height="200" />
+
+    <!-- Gradin TOP TO BOTTOM    LEFT -->
+    <Obstacle id="10" x="600" y="1650" width="450" height="150" />
+    <Obstacle id="11" x="0" y="1100" width="150" height="450" />
+    <Obstacle id="12" x="900" y="900" width="400" height="100" />
+    <Obstacle id="13" x="0" y="200" width="130" height="400" />
+    <Obstacle id="14" x="570" y="200" width="400" height="100" />
+
+    <!-- Gradin TOP TO BOTTOM    RIGHT -->
+    <Obstacle id="20" x="1950" y="1650" width="450" height="150" />
+    <Obstacle id="21" x="2850" y="1100" width="150" height="450" />
+    <Obstacle id="22" x="1700" y="900" width="400" height="100" />
+    <Obstacle id="23" x="2850" y="200" width="130" height="400" />
+    <Obstacle id="24" x="2000" y="200" width="400" height="100" />
+
+    <!-- PAMI Start -->
+    <Obstacle id="30" x="0" y="1550" width="150" height="450" />
+    <Obstacle id="31" x="2850" y="1550" width="150" height="450" />
+
+</Obstacles>

src/modelec_strat/package.xml

@@ -13,6 +13,8 @@
   <depend>modelec_utils</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>tinyxml2</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
 

src/modelec_strat/src/action_executor.cpp

@@ -6,7 +6,7 @@ namespace Modelec
     {
     }
 
-    ActionExecutor::ActionExecutor(rclcpp::Node::SharedPtr node) : node_(std::move(node))
+    ActionExecutor::ActionExecutor(const rclcpp::Node::SharedPtr& node) : node_(node)
     {
     }
 

src/modelec_strat/src/mission_manager.cpp

@@ -6,7 +6,7 @@ namespace Modelec
     {
     }
 
-    MissionManager::MissionManager(rclcpp::Node::SharedPtr node) : node_(std::move(node))
+    MissionManager::MissionManager(const rclcpp::Node::SharedPtr& node) : node_(node)
     {
     }
 

src/modelec_strat/src/navigation_helper.cpp

@@ -6,9 +6,9 @@ namespace Modelec {
     {
     }
 
-    NavigationHelper::NavigationHelper(rclcpp::Node::SharedPtr node) : node_(std::move(node))
+    NavigationHelper::NavigationHelper(const rclcpp::Node::SharedPtr& node) : node_(node)
     {
-        pathfinding_ = std::make_unique<Pathfinding>(std::move(node));
+        pathfinding_ = std::make_unique<Pathfinding>(node);
 
         waypoint_reach_sub_ = node_->create_subscription<WaypointReachMsg>(
             "odometry/waypoint_reach", 10,
@@ -16,7 +16,7 @@ namespace Modelec {
                 OnWaypointReach(msg);
             });
 
-        waypoint_pub_ = node_->create_publisher<WaypointMsg>("odometry/add_waypoint", 30);
+        waypoint_pub_ = node_->create_publisher<WaypointMsg>("odometry/add_waypoint", 100);
 
         pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
             "odometry/position", 20,
@@ -43,6 +43,14 @@ namespace Modelec {
         }
     }
 
+    void NavigationHelper::SendWaypoint(const std::vector<WaypointMsg>& waypoints) const
+    {
+        for (auto & w : waypoints)
+        {
+            waypoint_pub_->publish(w);
+        }
+    }
+
     void NavigationHelper::AddWaypoint(const PosMsg& pos, int index)
     {
         for (auto w = waypoints_.begin(); w != waypoints_.end(); ++w)
@@ -148,14 +156,7 @@ namespace Modelec {
 
     void NavigationHelper::GoTo(const PosMsg::SharedPtr& goal)
     {
-        auto waypointList = FindPath(goal);
-        waypoints_.clear();
-        for (const auto& waypoint : waypointList)
-        {
-            waypoints_.emplace_back(waypoint);
-        }
-
-        SendWaypoint();
+        SendWaypoint(FindPath(goal));
     }
 
     WaypointListMsg NavigationHelper::FindPath(const PosMsg::SharedPtr& goal)

src/modelec_strat/src/pathfinding.cpp

@@ -2,12 +2,69 @@
 
 namespace Modelec {
 
+    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
+        {
+            return f > other.f;
+        }
+    };
+
+    double heuristic(int x1, int y1, int x2, int y2)
+    {
+        return std::hypot(x1 - x2, y1 - y2);  // Euclidean distance
+    }
+
     Pathfinding::Pathfinding()
     {
     }
 
-    Pathfinding::Pathfinding(rclcpp::Node::SharedPtr node) : node_(std::move(node))
+    Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr& node) : node_(node)
     {
+        robot_length_mm_ = 300.0f;
+        robot_width_mm_ = 300.0f;
+
+        grid_width_ = 300;
+        grid_height_ = 200;
+
+        if (!LoadObstaclesFromXML("/home/acki/ros2_ws/src/modelec_strat/map/obstacles.xml"))
+        {
+            RCLCPP_WARN(node_->get_logger(), "Failed to load obstacles from XML");
+        }
+
+        map_pub_ = node_->create_publisher<modelec_interfaces::msg::Obstacle>(
+            "nav/obstacle", 40);
+
+        ask_obstacle_srv_ = node_->create_service<std_srvs::srv::Empty>(
+            "nav/ask_map_obstacle",
+            [this](const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+                const std::shared_ptr<std_srvs::srv::Empty::Response> response) {
+                RCLCPP_INFO(node_->get_logger(), "Ask obstacle request received");
+                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) {
+                RCLCPP_INFO(node_->get_logger(), "Map request received");
+                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) {
+                RCLCPP_INFO(node_->get_logger(), "Map size request received");
+                HandleMapSizeRequest(request, response);
+            });
+
     }
 
     rclcpp::Node::SharedPtr Pathfinding::getNode() const
@@ -17,23 +74,316 @@ namespace Modelec {
 
     WaypointListMsg Pathfinding::FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal)
     {
-        /* TODO - pathfinding
-         *   This is a stub for the pathfinding algorithm. The pathfinding algorithm should
-         */
-
         WaypointListMsg waypoints;
-        waypoints.push_back(Waypoint(*start, 0));
-        WaypointMsg middleW;
-        middleW.x = 500;
-        middleW.y = 1500;
-        middleW.theta = 0;
-        middleW.id = 1;
-        middleW.is_end = false;
-        waypoints.push_back(middleW);
-        waypoints.push_back(Waypoint(*goal, 2, true));
+
+        // Constantes de ta map
+        constexpr float map_width_mm = 3000.0f;  // mm
+        constexpr float map_height_mm = 2000.0f; // mm
+        const float cell_size_mm_x = map_width_mm / grid_width_;
+        const float cell_size_mm_y = map_height_mm / grid_height_;
+
+        // Robot dimensions
+        const int inflate_x = std::ceil((robot_width_mm_ / 2.0f) / cell_size_mm_x);
+        const int inflate_y = std::ceil((robot_length_mm_ / 2.0f) / cell_size_mm_y);
+
+        // 1. Build fresh empty grid
+        grid_.clear();
+        grid_.resize(grid_height_);
+        for (auto& row : grid_)
+        {
+            row.assign(grid_width_, 0); // 0 = free
+        }
+
+        // 2. Fill obstacles with inflation
+        for (const auto& [id, obstacle] : obstacle_map_)
+        {
+            int x_start = std::max(0, (int)(obstacle.x / cell_size_mm_x) - inflate_x);
+            int y_start = std::max(0, (int)(obstacle.y / cell_size_mm_y) - inflate_y);
+            int x_end = std::min(grid_width_ - 1, (int)((obstacle.x + obstacle.width) / cell_size_mm_x) + inflate_x);
+            int y_end = std::min(grid_height_ - 1, (int)((obstacle.y + obstacle.height) / cell_size_mm_y) + inflate_y);
+
+            // Inverser l'axe Y
+            y_start = (grid_height_ - 1) - y_start;
+            y_end = (grid_height_ - 1) - y_end;
+            if (y_start > y_end) std::swap(y_start, y_end);
+
+            for (int y = y_start; y <= y_end; ++y)
+            {
+                for (int x = x_start; x <= x_end; ++x)
+                {
+                    grid_[y][x] = 1; // mark as obstacle
+                }
+            }
+        }
+
+        // 3. Convert start and goal (avec inversion Y)
+        const int start_x = start->x / cell_size_mm_x;
+        const int start_y = (grid_height_ - 1) - (start->y / cell_size_mm_y);
+        const int goal_x = goal->x / cell_size_mm_x;
+        const int goal_y = (grid_height_ - 1) - (goal->y / cell_size_mm_y);
+
+        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_)
+        {
+            RCLCPP_WARN(node_->get_logger(), "Start or Goal out of bounds");
+            return waypoints;
+        }
+
+        if (grid_[start_y][start_x] == 1 || grid_[goal_y][goal_x] == 1)
+        {
+            RCLCPP_WARN(node_->get_logger(), "Start or Goal inside an obstacle");
+            return waypoints;
+        }
+
+        // 4. A* algorithm
+        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) {
+            return (int64_t)x << 32 | (uint32_t)y;
+        };
+
+        auto neighbors = [](int x, int y) {
+            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
+            };
+        };
+
+        AStarNode start_node{start_x, start_y};
+        start_node.g = 0;
+        start_node.f = heuristic(start_x, start_y, goal_x, goal_y);
+
+        open_list.push(start_node);
+        all_nodes[hash(start_x, start_y)] = start_node;
+
+        bool path_found = false;
+
+        while (!open_list.empty())
+        {
+            AStarNode current = open_list.top();
+            open_list.pop();
+
+            if (current.x == goal_x && current.y == goal_y)
+            {
+                path_found = true;
+                break;
+            }
+
+            for (const auto& [nx, ny] : neighbors(current.x, current.y))
+            {
+                if (nx < 0 || ny < 0 || ny >= (int)grid_.size() || nx >= (int)grid_[0].size())
+                    continue;
+
+                if (grid_[ny][nx] == 1)
+                    continue;
+
+                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)
+                {
+                    AStarNode neighbor;
+                    neighbor.x = nx;
+                    neighbor.y = ny;
+                    neighbor.g = tentative_g;
+                    neighbor.f = tentative_g + heuristic(nx, ny, goal_x, goal_y);
+                    neighbor.parent_x = current.x;
+                    neighbor.parent_y = current.y;
+                    all_nodes[neighbor_hash] = neighbor;
+                    open_list.push(neighbor);
+                }
+            }
+        }
+
+        if (!path_found)
+        {
+            RCLCPP_WARN(node_->get_logger(), "No path found");
+            return waypoints;
+        }
+
+        // 5. Reconstruct path
+        std::vector<std::pair<int, int>> path;
+        int cx = goal_x;
+        int cy = goal_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())
+                break;
+            cx = it->second.parent_x;
+            cy = it->second.parent_y;
+        }
+        path.emplace_back(start_x, start_y);
+        std::reverse(path.begin(), path.end());
+
+        // 6. Path smoothing
+        std::vector<std::pair<int, int>> smooth_path;
+        size_t current = 0;
+        while (current < path.size())
+        {
+            size_t next = current + 1;
+            while (next < path.size())
+            {
+                bool clear = true;
+                int x0 = path[current].first;
+                int y0 = path[current].second;
+                int x1 = path[next].first;
+                int y1 = path[next].second;
+
+                int dx = std::abs(x1 - x0);
+                int dy = -std::abs(y1 - y0);
+                int sx = x0 < x1 ? 1 : -1;
+                int sy = y0 < y1 ? 1 : -1;
+                int err = dx + dy;
+                int x = x0;
+                int y = y0;
+
+                while (true)
+                {
+                    if (grid_[y][x] == 1)
+                    {
+                        clear = false;
+                        break;
+                    }
+                    if (x == x1 && y == y1)
+                        break;
+                    int e2 = 2 * err;
+                    if (e2 >= dy)
+                    {
+                        err += dy;
+                        x += sx;
+                    }
+                    if (e2 <= dx)
+                    {
+                        err += dx;
+                        y += sy;
+                    }
+                }
+
+                if (!clear)
+                    break;
+                next++;
+            }
+
+            smooth_path.push_back(path[current]);
+            if (next == path.size())
+            {
+                smooth_path.push_back(path.back());
+                break;
+            }
+            current = next - 1;
+        }
+
+        // 7. Fill Waypoints (reconvertir grille -> millimètres)
+        int id = 0;
+        for (size_t i = 0; i < smooth_path.size(); ++i)
+        {
+            const auto& [x, y] = smooth_path[i];
+
+            // Skip first point if it's too close to robot position
+            if (i == 0)
+            {
+                float world_x = x * cell_size_mm_x;
+                float world_y = (grid_height_ - 1 - y) * cell_size_mm_y;
+
+                float dx = std::abs(world_x - start->x);
+                float dy = std::abs(world_y - start->y);
+
+                if (dx < 50 && dy < 50) // <== seuil de 50mm
+                    continue;
+            }
+
+            WaypointMsg wp;
+            wp.x = x * cell_size_mm_x;
+            wp.y = (grid_height_ - 1 - y) * cell_size_mm_y;
+            wp.theta = 0;
+            wp.id = id++;
+            wp.is_end = false;
+            waypoints.push_back(wp);
+        }
+        if (!waypoints.empty())
+        {
+            waypoints.back().is_end = true;
+        }
+
         return waypoints;
     }
 
+
+    bool Pathfinding::LoadObstaclesFromXML(const std::string& filename)
+    {
+        tinyxml2::XMLDocument doc;
+        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");
+        if (!root)
+        {
+            RCLCPP_ERROR(node_->get_logger(), "No <Obstacles> root element in file");
+            return false;
+        }
+
+        for (tinyxml2::XMLElement* obstacleElem = root->FirstChildElement("Obstacle");
+             obstacleElem != nullptr;
+             obstacleElem = obstacleElem->NextSiblingElement("Obstacle"))
+        {
+            modelec_interfaces::msg::Obstacle obs;
+
+            if (obstacleElem->QueryIntAttribute("id", &obs.id) != tinyxml2::XML_SUCCESS ||
+                obstacleElem->QueryIntAttribute("x", &obs.x) != tinyxml2::XML_SUCCESS ||
+                obstacleElem->QueryIntAttribute("y", &obs.y) != tinyxml2::XML_SUCCESS ||
+                obstacleElem->QueryIntAttribute("width", &obs.width) != tinyxml2::XML_SUCCESS ||
+                obstacleElem->QueryIntAttribute("height", &obs.height) != tinyxml2::XML_SUCCESS)
+            {
+                RCLCPP_WARN(node_->get_logger(), "Incomplete obstacle definition in XML, skipping one obstacle");
+                continue;
+            }
+
+            obstacle_map_[obs.id] = obs;
+        }
+
+        RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", obstacle_map_.size());
+        return true;
+    }
+
+    void Pathfinding::HandleMapRequest(const std::shared_ptr<modelec_interfaces::srv::Map::Request>,
+                                       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 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)
+    {
+        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>)
+    {
+        for (auto & [index, obs] : obstacle_map_)
+        {
+            map_pub_->publish(obs);
+        }
+    }
+
     Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos& pos, int index, bool isLast)
     {
         id = index;

src/modelec_strat/src/strat_fms.cpp

@@ -11,7 +11,7 @@ namespace Modelec
         nav_ = std::make_unique<NavigationHelper>(shared_from_this());
         mission_manager_ = std::make_unique<MissionManager>(shared_from_this());
         action_executor_ = std::make_unique<ActionExecutor>(shared_from_this());
-        RCLCPP_INFO(this->get_logger(), "Strat fully initialized");
+        RCLCPP_INFO(this->get_logger(), "StratFMS fully initialized");
     }
 }