start the strat :

- nav

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/11"; // TODO : check the real serial port
+        request->serial_port = "/dev/pts/10"; // 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_com/src/pcb_odo_interface.cpp

@@ -10,7 +10,7 @@ namespace Modelec
         auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
         request->name = "pcb_odo";
         request->bauds = 115200;
-        request->serial_port = "/dev/pts/6"; // TODO : check the real serial port
+        request->serial_port = "/dev/pts/8"; // 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)))
         {
@@ -69,7 +69,7 @@ namespace Modelec
         }
 
         odo_pos_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
-            "odometry/get_position", 10);
+            "odometry/position", 10);
 
         odo_speed_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometrySpeed>(
             "odometry/speed", 10);

src/modelec_core/launch/test.modelec.launch.yml

@@ -24,3 +24,8 @@ launch:
       pkg: 'modelec_core'
       exec: 'speed_result'
       name: 'speed_result'
+
+- node:
+      pkg: 'modelec_strat'
+      exec: 'strat_fsm'
+      name: 'strat_fsm'

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

@@ -31,24 +31,25 @@ namespace ModelecGUI {
 
         void mousePressEvent(QMouseEvent* event) override;
 
-        void onStartButtonClicked();
-
         void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
 
         QSvgRenderer* renderer;
 
-        QPushButton* startButton;
-
         QVBoxLayout* v_layout;
         QHBoxLayout* h_layout;
 
         QPoint robotPosPoint = QPoint(0, 0);
         std::vector<QPoint> qpoints;
         std::vector<modelec_interfaces::msg::OdometryAddWaypoint> points;
+        modelec_interfaces::msg::OdometryPos go_to_point;
+
+        bool lastWapointWasEnd = false;
 
         rclcpp::Node::SharedPtr node_;
 
-        rclcpp::Publisher<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr add_waypoint_publisher_;
-        rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odometry_subscriber_;
+        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_;
     };
 }

src/modelec_gui/src/pages/map_page.cpp

@@ -8,34 +8,49 @@ 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))
     {
-        startButton = new QPushButton("Start", this);
-
         v_layout = new QVBoxLayout(this);
         v_layout->addStretch();
 
         h_layout = new QHBoxLayout(this);
         h_layout->addStretch();
-        h_layout->addWidget(startButton);
         h_layout->addStretch();
 
         v_layout->addLayout(h_layout);
 
         this->setLayout(v_layout);
 
-        connect(startButton, &QPushButton::clicked, this, &MapPage::onStartButtonClicked);
-
         qpoints = {};
         points = {};
 
-        add_waypoint_publisher_ = node_->create_publisher<modelec_interfaces::msg::OdometryAddWaypoint>("odometry/add_waypoint", 30);
+        add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>("odometry/add_waypoint", 10,
+            [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) {
+                if (lastWapointWasEnd)
+                {
+                    qpoints.clear();
+                    points.clear();
+                    lastWapointWasEnd = false;
+                }
+
+                if (msg->is_end)
+                {
+                    lastWapointWasEnd = true;
+                }
+
+                qpoints.push_back(QPoint(Modelec::mapValue(static_cast<int>(msg->x), 0, 3000, 0, width()),
+                                          height() - Modelec::mapValue(static_cast<int>(msg->y), 0, 2000, 0, height())));
+                points.push_back(*msg);
+                update();
+        });
 
         // lambda
-        odometry_subscriber_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>("odometry/get_position", 10,
+        odometry_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>("odometry/position", 10,
             [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
                 robotPosPoint.setX(Modelec::mapValue(static_cast<int>(msg->x), 0, 3000, 0, width()));
                 robotPosPoint.setY(height() - Modelec::mapValue(static_cast<int>(msg->y), 0, 2000, 0, height()));
                 update();
         });
+
+        go_to_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("nav/go_to", 10);
     }
 
     void MapPage::setPlaymatGrid()
@@ -57,6 +72,7 @@ namespace ModelecGUI
         QPainter painter(this);
         renderer->render(&painter, rect()); // Scales SVG to widget size
 
+
         painter.setRenderHint(QPainter::Antialiasing);
 
         // --- Draw lines ---
@@ -90,6 +106,13 @@ namespace ModelecGUI
     {
         QWidget::mousePressEvent(event);
 
+        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);
+
+        go_to_pub_->publish(msg);
+
+        /*
         qpoints.push_back(event->pos());
 
         modelec_interfaces::msg::OdometryAddWaypoint msg;
@@ -112,19 +135,6 @@ namespace ModelecGUI
 
         points.push_back(msg);
 
-        update();
-    }
-
-    void MapPage::onStartButtonClicked()
-    {
-        points.back().is_end = true;
-
-        for (const auto& point : points) {
-            add_waypoint_publisher_->publish(point);
-        }
-
-        qpoints.clear();
-        points.clear();
-        update();
+        update();*/
     }
 }

src/modelec_gui/src/pages/test_page.cpp

@@ -152,7 +152,7 @@ namespace ModelecGUI
 
         // Set up subscription
         sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
-            "/odometry/get_position", 10,
+            "/odometry/position", 10,
             std::bind(&TestPage::PositionCallback, this, std::placeholders::_1));
 
         pub_add_waypoint_ = node_->create_publisher<modelec_interfaces::msg::OdometryAddWaypoint>(

src/modelec_strat/CMakeLists.txt

@@ -6,6 +6,19 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(std_msgs 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)
+target_include_directories(strat_fsm PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -14,4 +27,9 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+install(TARGETS
+  strat_fsm
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 ament_package()

src/modelec_strat/include/modelec_strat/action_executor.hpp

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+namespace Modelec
+{
+
+    class ActionExecutor
+    {
+    public:
+        ActionExecutor();
+
+        ActionExecutor(rclcpp::Node::SharedPtr node);
+
+        rclcpp::Node::SharedPtr getNode() const;
+
+    private:
+        rclcpp::Node::SharedPtr node_;
+    };
+
+}

src/modelec_strat/include/modelec_strat/mission_manager.hpp

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+namespace Modelec
+{
+
+    class MissionManager
+    {
+    public:
+        MissionManager();
+
+        MissionManager(rclcpp::Node::SharedPtr node);
+
+        rclcpp::Node::SharedPtr getNode() const;
+
+    private:
+        rclcpp::Node::SharedPtr node_;
+    };
+
+}

src/modelec_strat/include/modelec_strat/navigation_helper.hpp

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
+#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
+#include <modelec_interfaces/msg/odometry_pos.hpp>
+
+#include "pathfinding.hpp"
+
+namespace Modelec {
+
+    class NavigationHelper {
+    public:
+        NavigationHelper();
+
+        NavigationHelper(rclcpp::Node::SharedPtr node);
+
+        rclcpp::Node::SharedPtr getNode() const;
+
+        void SendWaypoint() const;
+
+        void AddWaypoint(const PosMsg &pos, int index);
+        void AddWaypoint(const WaypointMsg &waypoint);
+
+        void AddWaypoints(const std::initializer_list<PosMsg> &pos_list, int index);
+        void AddWaypoints(const std::initializer_list<WaypointMsg> &waypoint_list);
+
+        void SetWaypoints(const std::list<Waypoint> &waypoints);
+
+        bool HasArrived() const;
+
+        void GoTo(const PosMsg::SharedPtr &goal);
+
+        WaypointListMsg FindPath(const PosMsg::SharedPtr &goal);
+
+    protected:
+        void OnWaypointReach(const WaypointReachMsg::SharedPtr msg);
+
+        void OnPos(const PosMsg::SharedPtr msg);
+
+    private:
+        rclcpp::Node::SharedPtr node_;
+
+        std::unique_ptr<Pathfinding> pathfinding_;
+
+        std::list<Waypoint> waypoints_;
+
+        PosMsg::SharedPtr current_pos_;
+
+        rclcpp::Subscription<WaypointReachMsg>::SharedPtr waypoint_reach_sub_;
+        rclcpp::Publisher<WaypointMsg>::SharedPtr waypoint_pub_;
+
+        rclcpp::Subscription<PosMsg>::SharedPtr go_to_sub_;
+        rclcpp::Subscription<PosMsg>::SharedPtr pos_sub_;
+    };
+}

src/modelec_strat/include/modelec_strat/pathfinding.hpp

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
+#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
+#include <modelec_interfaces/msg/odometry_pos.hpp>
+
+namespace Modelec {
+
+    using WaypointMsg = modelec_interfaces::msg::OdometryAddWaypoint;
+    using WaypointReachMsg = modelec_interfaces::msg::OdometryWaypointReach;
+    using PosMsg = modelec_interfaces::msg::OdometryPos;
+    using WaypointListMsg = std::vector<WaypointMsg>;
+
+    class Waypoint : public WaypointMsg
+    {
+    public:
+        bool reached = false;
+
+        Waypoint(const PosMsg& pos, int index, bool isLast = false);
+
+        explicit Waypoint(const WaypointMsg& waypoint);
+
+        WaypointMsg toMsg() const;
+    };
+
+    /*
+     * - Pathfinding DStar Lite
+     *
+     *
+     *
+     */
+
+    class Pathfinding {
+    public:
+        Pathfinding();
+
+        Pathfinding(rclcpp::Node::SharedPtr node);
+
+        rclcpp::Node::SharedPtr getNode() const;
+
+        WaypointListMsg FindPath(const PosMsg::SharedPtr& start,
+                      const PosMsg::SharedPtr& goal);
+
+    private:
+        rclcpp::Node::SharedPtr node_;
+    };
+
+}

src/modelec_strat/include/modelec_strat/strat_fms.hpp

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "action_executor.hpp"
+#include "mission_manager.hpp"
+#include "navigation_helper.hpp"
+
+namespace Modelec
+{
+
+    /*
+     * TODO
+     * - yaml strat
+     * - setup des missions
+     * - gestion des interruptions (robot adverses / obstacles)
+     * - scoring des missions
+     *
+     *
+     *
+     * - Banderole
+     *
+     *
+     *
+     *
+     * - Gradin
+     */
+
+    class StratFMS : public rclcpp::Node
+    {
+    public:
+        StratFMS();
+
+        void Init();
+
+    private:
+        // rclcpp::TimerBase::SharedPtr timer_;
+
+        std::unique_ptr<NavigationHelper> nav_;
+        std::unique_ptr<MissionManager> mission_manager_;
+        std::unique_ptr<ActionExecutor> action_executor_;
+
+    };
+}

src/modelec_strat/src/action_executor.cpp

@@ -0,0 +1,17 @@
+#include <modelec_strat/action_executor.hpp>
+
+namespace Modelec
+{
+    ActionExecutor::ActionExecutor()
+    {
+    }
+
+    ActionExecutor::ActionExecutor(rclcpp::Node::SharedPtr node) : node_(std::move(node))
+    {
+    }
+
+    rclcpp::Node::SharedPtr ActionExecutor::getNode() const
+    {
+        return node_;
+    }
+}

src/modelec_strat/src/mission_manager.cpp

@@ -0,0 +1,17 @@
+#include <modelec_strat/mission_manager.hpp>
+
+namespace Modelec
+{
+    MissionManager::MissionManager()
+    {
+    }
+
+    MissionManager::MissionManager(rclcpp::Node::SharedPtr node) : node_(std::move(node))
+    {
+    }
+
+    rclcpp::Node::SharedPtr MissionManager::getNode() const
+    {
+        return node_;
+    }
+}

src/modelec_strat/src/navigation_helper.cpp

@@ -0,0 +1,182 @@
+#include <modelec_strat/navigation_helper.hpp>
+#include <utility>
+
+namespace Modelec {
+    NavigationHelper::NavigationHelper()
+    {
+    }
+
+    NavigationHelper::NavigationHelper(rclcpp::Node::SharedPtr node) : node_(std::move(node))
+    {
+        pathfinding_ = std::make_unique<Pathfinding>(std::move(node));
+
+        waypoint_reach_sub_ = node_->create_subscription<WaypointReachMsg>(
+            "odometry/waypoint_reach", 10,
+            [this](const WaypointReachMsg::SharedPtr msg) {
+                OnWaypointReach(msg);
+            });
+
+        waypoint_pub_ = node_->create_publisher<WaypointMsg>("odometry/add_waypoint", 30);
+
+        pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
+            "odometry/position", 20,
+            [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
+                OnPos(msg);
+            });
+
+        go_to_sub_ = node_->create_subscription<PosMsg>(
+            "nav/go_to", 10, [this](const PosMsg::SharedPtr msg) {
+                GoTo(msg);
+            });
+    }
+
+    rclcpp::Node::SharedPtr NavigationHelper::getNode() const
+    {
+        return node_;
+    }
+
+    void NavigationHelper::SendWaypoint() const
+    {
+        for (auto & w : waypoints_)
+        {
+            waypoint_pub_->publish(w.toMsg());
+        }
+    }
+
+    void NavigationHelper::AddWaypoint(const PosMsg& pos, int index)
+    {
+        for (auto w = waypoints_.begin(); w != waypoints_.end(); ++w)
+        {
+            if (w->id == index)
+            {
+                waypoints_.insert(w, Waypoint(pos, index));
+
+                while (w != waypoints_.end())
+                {
+                    w->id++;
+                    ++w;
+                }
+
+                return;
+            }
+        }
+        waypoints_.back().is_end = false;
+        auto newWaypoint = Waypoint(pos, index);
+        newWaypoint.is_end = true;
+        waypoints_.emplace_back(newWaypoint);
+    }
+
+    void NavigationHelper::AddWaypoint(const WaypointMsg& waypoint)
+    {
+        for (auto w = waypoints_.begin(); w != waypoints_.end(); ++w)
+        {
+            if (w->id == waypoint.id)
+            {
+                waypoints_.insert(w, Waypoint(waypoint));
+
+                while (w != waypoints_.end())
+                {
+                    w->id++;
+                    ++w;
+                }
+
+                return;
+            }
+        }
+
+        waypoints_.back().is_end = false;
+        auto newWaypoint = Waypoint(waypoint);
+        newWaypoint.is_end = true;
+        waypoints_.emplace_back(newWaypoint);
+    }
+
+    void NavigationHelper::AddWaypoints(const std::initializer_list<PosMsg>& pos_list, int index)
+    {
+        for (auto w = waypoints_.begin(); w != waypoints_.end(); ++w)
+        {
+            if (w->id == index)
+            {
+                waypoints_.erase(w, waypoints_.end());
+
+                for (const auto& pos : pos_list)
+                {
+                    waypoints_.emplace_back(pos, index);
+                    index++;
+                }
+            }
+        }
+    }
+
+    void NavigationHelper::AddWaypoints(const std::initializer_list<WaypointMsg>& waypoint_list)
+    {
+        int index = waypoint_list.begin()->id;
+        for (auto w = waypoints_.begin(); w != waypoints_.end(); ++w)
+        {
+            if (w->id == index)
+            {
+                waypoints_.erase(w, waypoints_.end());
+
+                for (const auto& pos : waypoint_list)
+                {
+                    waypoints_.emplace_back(pos);
+                    index++;
+                }
+            }
+        }
+        waypoints_.back().is_end = false;
+
+        for (auto & w : waypoint_list)
+        {
+            waypoints_.emplace_back(w);
+        }
+        waypoints_.back().is_end = true;
+    }
+
+    void NavigationHelper::SetWaypoints(const std::list<Waypoint>& waypoints)
+    {
+        waypoints_.clear();
+        for (const auto& waypoint : waypoints)
+        {
+            waypoints_.emplace_back(waypoint);
+        }
+    }
+
+    bool NavigationHelper::HasArrived() const
+    {
+        return waypoints_.back().reached;
+    }
+
+    void NavigationHelper::GoTo(const PosMsg::SharedPtr& goal)
+    {
+        auto waypointList = FindPath(goal);
+        waypoints_.clear();
+        for (const auto& waypoint : waypointList)
+        {
+            waypoints_.emplace_back(waypoint);
+        }
+
+        SendWaypoint();
+    }
+
+    WaypointListMsg NavigationHelper::FindPath(const PosMsg::SharedPtr& goal)
+    {
+        return pathfinding_->FindPath(current_pos_, goal);
+    }
+
+    void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr msg)
+    {
+        for (auto & waypoint : waypoints_)
+        {
+            if (waypoint.id == msg->id)
+            {
+                waypoint.reached = true;
+            }
+        }
+    }
+
+    void NavigationHelper::OnPos(const PosMsg::SharedPtr msg)
+    {
+        current_pos_ = msg;
+    }
+
+}

src/modelec_strat/src/pathfinding.cpp

@@ -0,0 +1,61 @@
+#include <modelec_strat/pathfinding.hpp>
+
+namespace Modelec {
+
+    Pathfinding::Pathfinding()
+    {
+    }
+
+    Pathfinding::Pathfinding(rclcpp::Node::SharedPtr node) : node_(std::move(node))
+    {
+    }
+
+    rclcpp::Node::SharedPtr Pathfinding::getNode() const
+    {
+        return node_;
+    }
+
+    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));
+        return waypoints;
+    }
+
+    Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos& pos, int index, bool isLast)
+    {
+        id = index;
+        x = pos.x;
+        y = pos.y;
+        theta = pos.theta;
+        is_end = isLast;
+        reached = false;
+    }
+
+    Waypoint::Waypoint(const WaypointMsg& waypoint)
+    {
+        id = waypoint.id;
+        x = waypoint.x;
+        y = waypoint.y;
+        theta = waypoint.theta;
+        is_end = waypoint.is_end;
+        reached = false;
+    }
+
+    WaypointMsg Waypoint::toMsg() const
+    {
+        return static_cast<OdometryAddWaypoint_<std::allocator<void>>>(*this);
+    }
+}

src/modelec_strat/src/strat_fms.cpp

@@ -0,0 +1,26 @@
+#include <modelec_strat/strat_fms.hpp>
+
+namespace Modelec
+{
+    StratFMS::StratFMS() : Node("start_fms")
+    {
+    }
+
+    void StratFMS::Init()
+    {
+        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");
+    }
+}
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<Modelec::StratFMS>();
+    node->Init();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}