diff --git a/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp b/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp
index 2bb25c0..2611d6b 100644
--- a/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp
+++ b/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp
@@ -42,14 +42,10 @@ namespace ModelecGUI {
         QLineEdit *xSpeedBox_, *ySpeedBox_, *thetaSpeedBox_;
         QHBoxLayout *speedLayout_;
 
-        QPushButton *startTest_;
-
         rclcpp::Node::SharedPtr node_;
 
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr sub_;
 
-        rclcpp::Publisher<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr pub_add_waypoint_;
-
         // client
         rclcpp::Client<modelec_interfaces::srv::OdometrySpeed>::SharedPtr client_;
         rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;
diff --git a/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp
index ffb6ce3..de2aa0b 100644
--- a/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp
+++ b/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp
@@ -14,6 +14,7 @@
 
 #include <modelec_interfaces/msg/odometry_add_waypoint.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>
@@ -54,14 +55,6 @@ namespace ModelecGUI {
 
         QSvgRenderer* renderer;
 
-        QVBoxLayout* v_layout;
-        QHBoxLayout* h_layout;
-
-        QPushButton* tirette_button_;
-
-        QLabel* timer_label_;
-        QLabel* score_label_;
-
         modelec_interfaces::msg::OdometryPos robotPos;
         std::vector<QPoint> qpoints;
         //std::vector<modelec_interfaces::msg::OdometryAddWaypoint> points;
@@ -85,12 +78,10 @@ namespace ModelecGUI {
 
         rclcpp::Node::SharedPtr node_;
 
-        rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr tirette_pub_;
-
         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::Publisher<modelec_interfaces::msg::OdometryGoTo>::SharedPtr go_to_pub_;
         rclcpp::Client<modelec_interfaces::srv::MapSize>::SharedPtr map_client_;
         rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_added_sub_;
         rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_removed_sub_;
@@ -100,11 +91,5 @@ namespace ModelecGUI {
         bool hasEnemy = false;
         rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_pub_;
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
-        rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr strat_start_sub_;
-
-        bool isGameStarted_ = false;
-        long int start_time_ = 0;
-
-        rclcpp::Subscription<modelec_interfaces::msg::StratState>::SharedPtr strat_state_sub_;
     };
 }
diff --git a/src/modelec_gui/src/pages/odo_page.cpp b/src/modelec_gui/src/pages/odo_page.cpp
index bc73a4b..896ba43 100644
--- a/src/modelec_gui/src/pages/odo_page.cpp
+++ b/src/modelec_gui/src/pages/odo_page.cpp
@@ -138,27 +138,6 @@ namespace ModelecGUI
         speedLayout_->addWidget(ySpeedBox_);
         speedLayout_->addWidget(thetaSpeedBox_);
 
-        startTest_ = new QPushButton("Start Test");
-        connect(startTest_, &QPushButton::clicked, this, [this]() {
-            auto firstRequest = std::make_shared<modelec_interfaces::msg::OdometryAddWaypoint>();
-            firstRequest->id = 0;
-            firstRequest->is_end = false;
-            firstRequest->x = 100.0;
-            firstRequest->y = 0.0;
-            firstRequest->theta = 0.0;
-
-            pub_add_waypoint_->publish(*firstRequest);
-
-            auto secondRequest = std::make_shared<modelec_interfaces::msg::OdometryAddWaypoint>();
-            secondRequest->id = 1;
-            secondRequest->is_end = true;
-            secondRequest->x = 0.0;
-            secondRequest->y = 0.0;
-            secondRequest->theta = 0.0;
-
-            pub_add_waypoint_->publish(*secondRequest);
-        });
-
         mainLayout_ = new QVBoxLayout(this);
         mainLayout_->addWidget(startButton_);
         mainLayout_->addLayout(posLayout_);
@@ -167,7 +146,6 @@ namespace ModelecGUI
         mainLayout_->addWidget(setPID_);
         mainLayout_->addWidget(askSpeed_);
         mainLayout_->addLayout(speedLayout_);
-        mainLayout_->addWidget(startTest_);
         setLayout(mainLayout_);
 
         // Set up subscription
@@ -175,9 +153,6 @@ namespace ModelecGUI
             "/odometry/position", 10,
             std::bind(&OdoPage::PositionCallback, this, std::placeholders::_1));
 
-        pub_add_waypoint_ = node_->create_publisher<modelec_interfaces::msg::OdometryAddWaypoint>(
-            "/odometry/add_waypoint", 10);
-
         client_ = node_->create_client<modelec_interfaces::srv::OdometrySpeed>("odometry/speed");
         while (!client_->wait_for_service(std::chrono::seconds(1))) {
             if (!rclcpp::ok()) {
diff --git a/src/modelec_gui/src/pages/test_map_page.cpp b/src/modelec_gui/src/pages/test_map_page.cpp
index 008c307..d481b8f 100644
--- a/src/modelec_gui/src/pages/test_map_page.cpp
+++ b/src/modelec_gui/src/pages/test_map_page.cpp
@@ -13,31 +13,6 @@ namespace ModelecGUI
         ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
         ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
 
-        v_layout = new QVBoxLayout(this);
-
-        tirette_button_ = new QPushButton("Tirette", this);
-
-        timer_label_ = new QLabel("00", this);
-        timer_label_->setAlignment(Qt::AlignCenter);
-        timer_label_->setFont(QFont("Arial", 24));
-        timer_label_->setStyleSheet("QLabel { color: white; }");
-        score_label_ = new QLabel("Score: 0", this);
-        score_label_->setAlignment(Qt::AlignCenter);
-        score_label_->setFont(QFont("Arial", 24));
-        score_label_->setStyleSheet("QLabel { color: white; }");
-
-        h_layout = new QHBoxLayout(this);
-        h_layout->addStretch();
-        h_layout->addWidget(score_label_);
-        h_layout->addWidget(tirette_button_);
-        h_layout->addWidget(timer_label_);
-        h_layout->addStretch();
-
-        v_layout->addLayout(h_layout);
-        v_layout->addStretch();
-
-        this->setLayout(v_layout);
-
         qpoints = {};
 
         robot_length_ = Modelec::Config::get<int>("config.robot.size.length_mm", 200);
@@ -81,7 +56,7 @@ namespace ModelecGUI
                 obstacle_.erase(msg->id);
         });
 
-        go_to_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("nav/go_to", 10);
+        go_to_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryGoTo>("nav/go_to", 10);
 
         enemy_pos_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("enemy/position", 10);
 
@@ -94,30 +69,6 @@ namespace ModelecGUI
                 update();
             });
 
-
-        tirette_pub_ = node_->create_publisher<std_msgs::msg::Bool>("tirette", 10);
-
-        strat_start_sub_ = node_->create_subscription<std_msgs::msg::Int64>("/strat/start_time", 10,
-            [this](const std_msgs::msg::Int64::SharedPtr msg){
-                isGameStarted_ = true;
-                start_time_ = msg->data;
-        });
-
-        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)
-                {
-                    RCLCPP_INFO(node_->get_logger(), "Game stop");
-                    isGameStarted_ = false;
-                }
-        });
-
-        connect(tirette_button_, &QPushButton::clicked, this, [this]() {
-            std_msgs::msg::Bool msg;
-            msg.data = true;
-            tirette_pub_->publish(msg);
-        });
-
         // 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))) {
@@ -179,15 +130,6 @@ namespace ModelecGUI
     {
         QWidget::paintEvent(paint_event);
 
-        if (isGameStarted_)
-        {
-            auto now = std::chrono::system_clock::now().time_since_epoch();
-            auto start = std::chrono::nanoseconds(start_time_);
-            auto elapsed = now - start;
-            auto elapsed_s = std::chrono::duration_cast<std::chrono::seconds>(elapsed).count();
-            timer_label_->setText(QString::number(elapsed_s));
-        }
-
         QPainter painter(this);
         renderer->render(&painter, rect()); // Scales SVG to widget size
         painter.save();
@@ -259,10 +201,19 @@ namespace ModelecGUI
 
         if (event->button() == Qt::LeftButton)
         {
-            modelec_interfaces::msg::OdometryPos msg;
+            modelec_interfaces::msg::OdometryGoTo 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;
+            msg.close = true;
+            if (show_obstacle_)
+            {
+                msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL;
+            }
+            else
+            {
+                msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL | modelec_interfaces::msg::OdometryGoTo::OBSTACLE | modelec_interfaces::msg::OdometryGoTo::ENEMY;
+            }
 
             go_to_pub_->publish(msg);
         }
diff --git a/src/modelec_interfaces/CMakeLists.txt b/src/modelec_interfaces/CMakeLists.txt
index fd0a699..0dfc066 100644
--- a/src/modelec_interfaces/CMakeLists.txt
+++ b/src/modelec_interfaces/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(rosidl_default_generators REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Alim/AlimEmg.msg"
   "msg/Odometry/OdometryPos.msg"
+  "msg/Odometry/OdometryGoTo.msg"
   "msg/Odometry/OdometrySpeed.msg"
   "msg/Odometry/OdometryToF.msg"
   "msg/Odometry/OdometryWaypointReach.msg"
diff --git a/src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg b/src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg
index e939edd..cd5c859 100644
--- a/src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg
+++ b/src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg
@@ -1,5 +1,5 @@
 uint8 id
 bool is_end
-int64 x
-int64 y
+int32 x
+int32 y
 float64 theta
\ No newline at end of file
diff --git a/src/modelec_interfaces/msg/Odometry/OdometryGoTo.msg b/src/modelec_interfaces/msg/Odometry/OdometryGoTo.msg
new file mode 100644
index 0000000..f5df1a0
--- /dev/null
+++ b/src/modelec_interfaces/msg/Odometry/OdometryGoTo.msg
@@ -0,0 +1,10 @@
+int8 FREE=1
+int8 WALL=2
+int8 OBSTACLE=4
+int8 ENEMY=8
+
+int32 x
+int32 y
+float64 theta
+bool close
+int8 mask 1
diff --git a/src/modelec_interfaces/msg/Odometry/OdometryPos.msg b/src/modelec_interfaces/msg/Odometry/OdometryPos.msg
index 2659cab..b324b47 100644
--- a/src/modelec_interfaces/msg/Odometry/OdometryPos.msg
+++ b/src/modelec_interfaces/msg/Odometry/OdometryPos.msg
@@ -1,3 +1,3 @@
-int64 x
-int64 y
+int32 x
+int32 y
 float64 theta
diff --git a/src/modelec_interfaces/msg/Odometry/OdometrySpeed.msg b/src/modelec_interfaces/msg/Odometry/OdometrySpeed.msg
index ecf3ac5..eb43caa 100644
--- a/src/modelec_interfaces/msg/Odometry/OdometrySpeed.msg
+++ b/src/modelec_interfaces/msg/Odometry/OdometrySpeed.msg
@@ -1,3 +1,3 @@
-int64 x
-int64 y
+int32 x
+int32 y
 int64 theta
diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
index 6f6df25..01d61fe 100644
--- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
+++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
@@ -4,6 +4,7 @@
 #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/odometry_go_to.hpp>
 
 #include "deposite_zone.hpp"
 #include "pathfinding.hpp"
@@ -123,7 +124,7 @@ namespace Modelec {
         rclcpp::Subscription<WaypointReachMsg>::SharedPtr waypoint_reach_sub_;
         rclcpp::Publisher<WaypointMsg>::SharedPtr waypoint_pub_;
 
-        rclcpp::Subscription<PosMsg>::SharedPtr go_to_sub_;
+        rclcpp::Subscription<modelec_interfaces::msg::OdometryGoTo>::SharedPtr go_to_sub_;
         rclcpp::Subscription<PosMsg>::SharedPtr pos_sub_;
         rclcpp::Publisher<PosMsg>::SharedPtr pos_pub_;
 
diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp
index 8d9a790..002a678 100644
--- a/src/modelec_strat/src/enemy_manager.cpp
+++ b/src/modelec_strat/src/enemy_manager.cpp
@@ -144,7 +144,7 @@ namespace Modelec
                 last_publish_time_ = this->now();
                 last_movement_time_ = this->now();  // Mise à jour du dernier vrai mouvement
                 enemy_pos_pub_->publish(enemy_pos);
-                RCLCPP_INFO(this->get_logger(), "Enemy moved: x=%ld, y=%ld", enemy_pos.x, enemy_pos.y);
+                RCLCPP_INFO(this->get_logger(), "Enemy moved: x=%d, y=%d", enemy_pos.x, enemy_pos.y);
             }
             else
             {
@@ -152,7 +152,7 @@ namespace Modelec
                 if ((now - last_movement_time_).seconds() > max_stationary_time_s_)
                 {
                     enemy_long_time_pub_->publish(last_enemy_pos_);
-                    RCLCPP_WARN(this->get_logger(), "Enemy has been stationary for too long at x=%ld y=%ld", last_enemy_pos_.x, last_enemy_pos_.y);
+                    RCLCPP_WARN(this->get_logger(), "Enemy has been stationary for too long at x=%d y=%d", last_enemy_pos_.x, last_enemy_pos_.y);
 
                     last_movement_time_ = now;
                 }
diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp
index ca5b05f..9a8039c 100644
--- a/src/modelec_strat/src/navigation_helper.cpp
+++ b/src/modelec_strat/src/navigation_helper.cpp
@@ -39,9 +39,9 @@ namespace Modelec {
 
         pos_pub_ = node_->create_publisher<PosMsg>("odometry/set_position", 10);
 
-        go_to_sub_ = node_->create_subscription<PosMsg>(
-            "nav/go_to", 10, [this](const PosMsg::SharedPtr msg) {
-                GoTo(msg, false, Pathfinding::FREE | Pathfinding::WALL);
+        go_to_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryGoTo>(
+            "nav/go_to", 10, [this](const modelec_interfaces::msg::OdometryGoTo::SharedPtr msg) {
+                GoTo(msg->x, msg->y, msg->theta, msg->close, msg->mask);
             });
 
         enemy_pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
@@ -478,14 +478,28 @@ namespace Modelec {
 
     bool NavigationHelper::EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg)
     {
-        for (size_t i = -1; i + 1 < waypoints_.size(); ++i)
+        auto curr = Waypoint(*current_pos_, -1, false);
+        std::vector<Waypoint> waypointsList;
+        waypointsList.push_back(curr);
+
+        for (auto& waypoint : waypoints_)
         {
-            auto wp = i == -1 ? Waypoint(*current_pos_, -1, false) : waypoints_[i];
-            auto next_wp = waypoints_[i + 1];
+            if (waypoint.reached)
+            {
+                continue;
+            }
+            waypointsList.push_back(waypoint);
+        }
+
+        for (size_t i = 0; i + 1 < waypointsList.size(); ++i)
+        {
+            auto wp = waypointsList[i];
+            auto next_wp = waypointsList[i + 1];
             if (DoesLineIntersectCircle(
                     Point(wp.x, wp.y, wp.theta),
                     Point(next_wp.x, next_wp.y, next_wp.theta),
-                    Point(msg.x, msg.y, msg.theta), (pathfinding_->enemy_length_mm_ + pathfinding_->robot_length_mm_ + pathfinding_->enemy_margin_mm_) / 2.0f))
+                    Point(msg.x, msg.y, msg.theta),
+                    (pathfinding_->enemy_length_mm_ + pathfinding_->robot_length_mm_ + pathfinding_->enemy_margin_mm_) / 2.0f))
             {
                 return true;
             }
diff --git a/src/modelec_utils/src/point.cpp b/src/modelec_utils/src/point.cpp
index bc9a2e4..7dc41ba 100644
--- a/src/modelec_utils/src/point.cpp
+++ b/src/modelec_utils/src/point.cpp
@@ -24,7 +24,7 @@ namespace Modelec
 
     Point Point::GetTakeBasePosition() const
     {
-        return GetTakePosition(350, theta);
+        return GetTakePosition(320, theta);
     }
 
     Point Point::GetTakeClosePosition() const