rewrite action hope this work

2026-01-18 16:47:43 +01:00 · 2025-12-20 22:01:40 +01:00
parent eea5746adb
commit 8b1e263758
19 changed files with 303 additions and 239 deletions
--- a/simulated_pcb/odo.py
+++ b/simulated_pcb/odo.py
@@ -14,16 +14,20 @@ class SimulatedPCB:
        self.x = 1225.0
        self.y = 300.0
        self.theta = math.pi / 2
+
        self.vx = 0.0
        self.vy = 0.0
        self.vtheta = 0.0
+
        self.last_update = time.time()
        self.last_send = time.time()

        self.pid = [0, 0, 0]
        self.tof = {0: 1000, 1: 1000, 2: 1000}
-        self.waypoints = {}  # waypoints by id
-        self.waypoint_order = []  # ordered list of ids
+
+        # --- waypoint system ---
+        self.waypoints = {}          # id -> waypoint
+        self.waypoint_queue = []     # FIFO execution
        self.current_wp_id = None

        self.thread = threading.Thread(target=self.run)
@@ -37,52 +41,53 @@ class SimulatedPCB:
        dt = now - self.last_update
        self.last_update = now

-        if self.start:
-            if self.waypoint_order:
-                wp = self.waypoints[self.waypoint_order[0]]
-                dx = wp['x'] - self.x
-                dy = wp['y'] - self.y
-                distance = math.hypot(dx, dy)
-                angle = math.atan2(dy, dx)
-                angle_diff = self.normalize_angle(wp['theta'] - self.theta)
+        if self.start and self.waypoint_queue:
+            wp_id = self.waypoint_queue[0]
+            wp = self.waypoints[wp_id]

-                if wp['type'] == 1:
-                    if distance < 5.0 and abs(angle_diff) < 0.15:
-                        self.vx = 0.0
-                        self.vy = 0.0
-                        self.vtheta = 0.0
-                        self.ser.write(f'SET;WAYPOINT;{wp["id"]}\n'.encode('utf-8'))
-                        self.current_wp_id = wp['id']
-                        self.waypoint_order.pop(0)
-                        del self.waypoints[wp['id']]
-                    else:
-                        speed = min(100.0, distance / 2)
-                        self.vx = speed * math.cos(angle)
-                        self.vy = speed * math.sin(angle)
-                        self.vtheta = max(-9.0, min(9.0, angle_diff * 2))
-                else:
-                    speed = min(150.0, distance / 2)
-                    self.vx = speed * math.cos(angle)
-                    self.vy = speed * math.sin(angle)
-                    self.vtheta = 0.0
-                    if distance < 100:
-                        self.ser.write(f'SET;WAYPOINT;{wp["id"]}\n'.encode('utf-8'))
-                        self.current_wp_id = wp['id']
-                        self.waypoint_order.pop(0)
-                        del self.waypoints[wp['id']]
-            else:
-                self.vx = 0.0
-                self.vy = 0.0
+            dx = wp['x'] - self.x
+            dy = wp['y'] - self.y
+            distance = math.hypot(dx, dy)
+
+            target_angle = math.atan2(dy, dx)
+            theta_error = self.normalize_angle(wp['theta'] - self.theta)
+
+            # --- motion ---
+            if wp['type'] == 1:  # precise waypoint
+                speed = min(120.0, distance * 1)
+                self.vtheta = max(-6.0, min(6.0, theta_error * 2))
+            else:  # transit waypoint
+                speed = min(180.0, distance * 2)
                self.vtheta = 0.0

-            self.x += self.vx * dt
-            self.y += self.vy * dt
-            self.theta += self.vtheta * dt
-            self.theta = self.normalize_angle(self.theta)
+            self.vx = speed * math.cos(target_angle)
+            self.vy = speed * math.sin(target_angle)

-        if now - self.last_send > 0.1 and self.start:
-            print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}')
-            self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n'.encode())
+            reached_pos = distance < 5.0
+            reached_theta = abs(theta_error) < 0.15 or wp['type'] == 0
+
+            if reached_pos and reached_theta:
+                self.vx = self.vy = self.vtheta = 0.0
+                self.current_wp_id = wp_id
+
+                self.ser.write(f"SET;WAYPOINT;REACH;{wp_id}\n".encode())
+
+                self.waypoint_queue.pop(0)
+                del self.waypoints[wp_id]
+
+        else:
+            self.vx = self.vy = self.vtheta = 0.0
+
+        # --- integrate ---
+        self.x += self.vx * dt
+        self.y += self.vy * dt
+        self.theta = self.normalize_angle(self.theta + self.vtheta * dt)
+
+        # --- periodic position ---
+        if self.start and now - self.last_send > 0.1:
+            self.ser.write(
+                f"SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n".encode()
+            )
            self.last_send = now

    def run(self):
@@ -98,73 +103,62 @@ class SimulatedPCB:

    def handle_message(self, msg):
        if msg == "GET;POS":
-            print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}')
-            self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}\n'.encode())
+            self.ser.write(
+                f"SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}\n".encode()
+            )

        elif msg == "GET;SPEED":
-            print(f'[TX] SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}')
-            self.ser.write(f'SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}\n'.encode())
+            self.ser.write(
+                f"SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}\n".encode()
+            )

        elif msg.startswith("GET;DIST;"):
            n = int(msg.split(';')[2])
            dist = self.tof.get(n, 0)
-            print(f'[TX] SET;DIST;{n};{dist}')
-            self.ser.write(f'SET;DIST;{n};{dist}\n'.encode())
+            self.ser.write(f"SET;DIST;{n};{dist}\n".encode())

        elif msg == "GET;PID":
-            print(f'[TX] SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}')
-            self.ser.write(f'SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}\n'.encode())
+            self.ser.write(f"SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}\n".encode())

        elif msg.startswith("SET;PID"):
-            print(f'[TX] OK;PID;1')
-            self.ser.write(f'OK;PID;1\n'.encode())
+            self.ser.write(b"OK;PID;1\n")

        elif msg.startswith("SET;START;"):
            self.start = msg.split(';')[2] == '1'
-            print(f'[TX] OK;START;1')
-            self.ser.write(f'OK;START;1\n'.encode())
+            self.ser.write(b"OK;START;1\n")

+        # --- MULTI WAYPOINT (clears previous) ---
        elif msg.startswith("SET;WAYPOINT;"):
-            parts = msg.split(';')
-            if len(parts) >= 7:
-                wp = {
-                    'id': int(parts[2]),
-                    'type': int(parts[3]),
-                    'x': float(parts[4]),
-                    'y': float(parts[5]),
-                    'theta': float(parts[6])
-                }
-                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())
+            parts = msg.split(';')[2:]

-        elif msg.startswith("SET;WAYPOINTS;"):
-            parts = msg.split(';')
-            if len(parts) / 7 >= 1:
-                self.waypoints.clear()
-                self.waypoint_order.clear()
-                for i in range(2, len(parts), 5):
-                    wp = {
-                        'id': int(parts[i]),
-                        'type': int(parts[i + 1]),
-                        'x': float(parts[i + 2]),
-                        'y': float(parts[i + 3]),
-                        'theta': float(parts[i + 4])
-                    }
-                    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')
+            if len(parts) % 5 != 0:
+                self.ser.write(b"ERR;WAYPOINT;0\n")
+                return
+
+            # clear existing waypoints
+            self.waypoints.clear()
+            self.waypoint_queue.clear()
+
+            for i in range(0, len(parts), 5):
+                wp = {
+                    'id': int(parts[i]),
+                    'type': int(parts[i + 1]),
+                    'x': float(parts[i + 2]),
+                    'y': float(parts[i + 3]),
+                    'theta': float(parts[i + 4]),
+                }
+
+                self.waypoints[wp['id']] = wp
+                self.waypoint_queue.append(wp['id'])
+
+                self.ser.write(f"OK;WAYPOINT;{wp['id']}\n".encode())

        elif msg.startswith("SET;POS"):
            parts = msg.split(';')
            self.x = float(parts[2])
            self.y = float(parts[3])
            self.theta = float(parts[4])
-            print(f'[TX] OK;POS;1')
-            self.ser.write(b'OK;POS;1\n')
+            self.ser.write(b"OK;POS;1\n")

    def stop(self):
        self.running = False
@@ -174,13 +168,15 @@ 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='/dev/pts/6')
    args = parser.parse_args()

    sim = None
    try:
        sim = SimulatedPCB(port=args.port)
+        while True:
+            time.sleep(1)
    except KeyboardInterrupt:
        if sim:
            sim.stop()
-        print("Simulation stopped")
+        print("Simulation stopped")
--- a/src/modelec_core/launch/modelec.launch.py
+++ b/src/modelec_core/launch/modelec.launch.py
@@ -99,7 +99,7 @@ def generate_launch_description():
                    executable='pcb_odo_interface',
                    name='pcb_odo_interface',
                    parameters=[{
-                        'serial_port': "/dev/USB_ODO",
+                        'serial_port': "/tmp/USB_ODO",
                        'baudrate': 115200,
                        'name': "pcb_odo",
                    }]
@@ -109,7 +109,7 @@ def generate_launch_description():
                    executable='pcb_action_interface',
                    name='pcb_action_interface',
                    parameters=[{
-                        'serial_port': "/dev/USB_ACTION",
+                        'serial_port': "/tmp/USB_ACTION",
                        'baudrate': 115200,
                        'name': "pcb_action",
                    }]
@@ -138,11 +138,7 @@ def generate_launch_description():
                package='joy',
                executable='joy_node',
                name='joy_node',
-                output='screen',
-                parameters=[{
-                    'dev': '/dev/input/js0',  # optional: specify your joystick device
-                    'deadzone': 0.05
-                }]
+                output='screen'
            )
            return [joy]
        return []
--- a/src/modelec_gui/include/modelec_gui/pages/home_page.hpp
+++ b/src/modelec_gui/include/modelec_gui/pages/home_page.hpp
@@ -40,6 +40,7 @@ namespace ModelecGUI

        QSvgRenderer* renderer_;

-        std::vector<QPushButton*> spawn_buttons_;
+        QPushButton* yellow_spawn_buttons_;
+        QPushButton* blue_spawn_buttons_;
    };
 }
--- a/src/modelec_gui/src/pages/home_page.cpp
+++ b/src/modelec_gui/src/pages/home_page.cpp
@@ -12,52 +12,64 @@ namespace ModelecGUI
    {
        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");
+        auto* layout = new QHBoxLayout(this);
+        layout->setContentsMargins(0, 0, 0, 0);
+        layout->setSpacing(0);

-        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;
-                auto ratioY = 800 / 2000.0f;
+        yellow_spawn_buttons_ = new QPushButton("Yellow", this);
+        blue_spawn_buttons_   = new QPushButton("Blue", this);

-                auto* button = new QPushButton(this);
-                spawn_buttons_.push_back(button);
+        yellow_spawn_buttons_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
+        blue_spawn_buttons_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);

-                button->setText(msg->team_id == 0 ? "Yellow" : "Blue");
-                button->setStyleSheet(
-                    msg->team_id == 0
-                        ? "background-color: rgba(255, 255, 0, 128); border: none; color: black; font-size: 24px;"
-                        : "background-color: rgba(0, 0, 255, 128); border: none; color: white; font-size: 24px;"
-                );
+        yellow_spawn_buttons_->setStyleSheet(
+            "QPushButton {"
+            "  background-color: rgba(255, 215, 0, 140);"
+            "  border: none;"
+            "  font-size: 32px;"
+            "  font-weight: bold;"
+            "  color: black;"
+            "}"
+            "QPushButton:hover {"
+            "  background-color: rgba(255, 215, 0, 180);"
+            "}"
+        );

-                button->move(
-                    static_cast<int>(msg->x * ratioX - (w * ratioX) / 2),
-                    static_cast<int>(800 - msg->y * ratioY - (h * ratioY) / 2)
-                );
+        blue_spawn_buttons_->setStyleSheet(
+            "QPushButton {"
+            "  background-color: rgba(0, 90, 200, 140);"
+            "  border: none;"
+            "  font-size: 32px;"
+            "  font-weight: bold;"
+            "  color: white;"
+            "}"
+            "QPushButton:hover {"
+            "  background-color: rgba(0, 90, 200, 180);"
+            "}"
+        );

-                button->setFixedSize(w * ratioX, h * ratioY);
+        layout->addWidget(yellow_spawn_buttons_, 1);
+        layout->addWidget(blue_spawn_buttons_, 1);

-                button->show();
+        connect(yellow_spawn_buttons_, &QPushButton::clicked, this, [this]()
+        {
+            modelec_interfaces::msg::Spawn msg;
+            msg.team_id = 0;
+            msg.name = modelec_interfaces::msg::Spawn::TOP;
+            spawn_pub_->publish(msg);
+            emit TeamChoose();
+        });

-                connect(button, &QPushButton::clicked, this, [this, msg]()
-                {
-                    modelec_interfaces::msg::Spawn team_msg;
-                    team_msg.team_id = msg->team_id;
-                    team_msg.name = modelec_interfaces::msg::Spawn::TOP;
-                    spawn_pub_->publish(team_msg);
-
-                    emit TeamChoose();
-                });
-            });
+        connect(blue_spawn_buttons_, &QPushButton::clicked, this, [this]()
+        {
+            modelec_interfaces::msg::Spawn msg;
+            msg.team_id = 1;
+            msg.name = modelec_interfaces::msg::Spawn::TOP;
+            spawn_pub_->publish(msg);
+            emit TeamChoose();
+        });

        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_->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_);
-        rclcpp::spin_until_future_complete(node_->get_node_base_interface(), res);
    }

    void HomePage::Init()
--- a/src/modelec_gui/src/pages/map_page.cpp
+++ b/src/modelec_gui/src/pages/map_page.cpp
@@ -167,7 +167,6 @@ namespace ModelecGUI
        {
            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;
            }
--- a/src/modelec_gui/src/pages/test_map_page.cpp
+++ b/src/modelec_gui/src/pages/test_map_page.cpp
@@ -118,7 +118,6 @@ namespace ModelecGUI
        {
            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;
            }
--- a/src/modelec_strat/include/modelec_strat/action_executor.hpp
+++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp
@@ -10,6 +10,7 @@
 #include <sensor_msgs/msg/joy.hpp>

 #include "action/base_action.hpp"
+#include "obstacle/box.hpp"

 namespace Modelec
 {
@@ -41,6 +42,9 @@ namespace Modelec
        void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg);

        void MoveServo(const modelec_interfaces::msg::ActionServoPosArray& msg);
+
+        std::array<std::shared_ptr<BoxObstacle>, 2> box_obstacles_;
+
    protected:
        rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_pub_;
        rclcpp::Publisher<modelec_interfaces::msg::ActionServoPosArray>::SharedPtr servo_move_pub_;
--- a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp
+++ b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp
@@ -23,6 +23,7 @@ namespace Modelec {
            DOWN,
            FREE,
            UP,
+            GO_BACK,
            DONE,
        } step_;

@@ -33,6 +34,8 @@ namespace Modelec {

        std::shared_ptr<DepositeZone> target_deposite_zone_;

+        double angle_;
+
        rclcpp::Time go_timeout_;
        rclcpp::Time deploy_time_;
    };
--- a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp
+++ b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp
@@ -20,12 +20,14 @@ namespace Modelec {
        enum Step
        {
            GO_TO_TAKE,
+            GO_TO_TAKE_CLOSE,
            DOWN,
            TAKE,
            UP,
            DONE,
        } step_;

+        std::shared_ptr<BoxObstacle> closestBox;
        MissionStatus status_;
        std::shared_ptr<NavigationHelper> nav_;
        std::shared_ptr<ActionExecutor> action_executor_;
--- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
+++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
@@ -188,11 +188,14 @@ namespace Modelec
        {
            if (auto obs = std::dynamic_pointer_cast<T>(obstacle.second))
            {
-                auto dist = Point::distance(robotPos, obs->GetPosition());
-                if (dist < distance)
+                if (!obs->IsAtObjective())
                {
-                    distance = dist;
-                    closest_obstacle = obs;
+                    auto dist = Point::distance(robotPos, obs->GetPosition());
+                    if (dist < distance)
+                    {
+                        distance = dist;
+                        closest_obstacle = obs;
+                    }
                }
            }
        }
--- a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp
+++ b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp
@@ -15,16 +15,12 @@ namespace Modelec
        BoxObstacle(tinyxml2::XMLElement* obstacleElem);
        BoxObstacle(const modelec_interfaces::msg::Obstacle& msg);

-        bool IsAtObjective() const;
-        void SetAtObjective(bool atObjective);
-
        Point GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const;
        Point GetOptimizedGetPos(const Point& currentPos) const;

        std::vector<Point> GetAllPossiblePositions() const;

    protected:
-        bool isAtObjective = false;

        std::vector<double> possible_angles_;
    };
--- a/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp
+++ b/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp
@@ -26,46 +26,37 @@ namespace Modelec

        virtual modelec_interfaces::msg::Obstacle toMsg() const;

-        int GetId() const { return id_; }
-        int GetX() const { return x_; }
-        int GetY() const { return y_; }
-        double GetTheta() const { return theta_; }
-        int GetWidth() const { return w_; }
-        int GetHeight() const { return h_; }
-        const std::string& Type() const { return type_; }
-        Point GetPosition() const { return Point(x_, y_, theta_); }
+        int GetId() const;
+        int GetX() const;
+        int GetY() const;
+        double GetTheta() const;
+        int GetWidth() const;
+        int GetHeight() const;
+        const std::string& Type() const;
+        Point GetPosition() const;

-        void SetId(int id) { id_ = id; }
-        void SetX(int x) { x_ = x; }
-        void SetY(int y) { y_ = y; }
-        void SetTheta(double theta) { theta_ = theta; }
-        void SetWidth(int w) { w_ = w; }
-        void SetHeight(int h) { h_ = h; }
-        void SetType(const std::string& type) { type_ = type; }
+        void SetId(int id);
+        void SetX(int x);
+        void SetY(int y);
+        void SetTheta(double theta);
+        void SetWidth(int w);
+        void SetHeight(int h);
+        void SetType(const std::string& type);

-        void SetPosition(int x, int y, double theta)
-        {
-            x_ = x;
-            y_ = y;
-            theta_ = theta;
-        }
+        void SetPosition(int x, int y, double theta);

-        void SetPosition(const Point& p)
-        {
-            x_ = p.x;
-            y_ = p.y;
-            theta_ = p.theta;
-        }
+        void SetPosition(const Point& p);

-        void SetSize(int w, int h)
-        {
-            w_ = w;
-            h_ = h;
-        }
+        void SetSize(int w, int h);
+
+        bool IsAtObjective() const;
+        void SetAtObjective(bool atObjective);

    protected:
        int id_, x_, y_, w_, h_;
        double theta_;
        std::string type_;
+
+        bool isAtObjective = false;
    };
 }
--- a/src/modelec_strat/include/modelec_strat/pami_manager.hpp
+++ b/src/modelec_strat/include/modelec_strat/pami_manager.hpp
@@ -35,7 +35,7 @@ namespace Modelec
        int score_free_zone_ = 0;

        rclcpp::TimerBase::SharedPtr timer_add_;
-        rclcpp::TimerBase::SharedPtr timer_remove_;
+        // rclcpp::TimerBase::SharedPtr timer_remove_;

        rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr start_time_sub_;
        rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr add_obs_pub_;
--- a/src/modelec_strat/src/missions/free_mission.cpp
+++ b/src/modelec_strat/src/missions/free_mission.cpp
@@ -28,8 +28,8 @@ namespace Modelec {
        {
            if ((node_->now() - go_timeout_).seconds() < 5)
            {
-                nav_->AskWaypoint();
-                return;
+                // nav_->AskWaypoint();
+                // return;
            }
            if ((node_->now() - go_timeout_).seconds() < 10)
            {
@@ -50,11 +50,11 @@ namespace Modelec {

                auto depoPoint = target_deposite_zone_->GetPosition();

-                auto angle = atan2(depoPoint.y - currPos->y,
+                angle_ = atan2(depoPoint.y - currPos->y,
                    depoPoint.x - currPos->x);

                nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist,
-                    angle + M_PI), true, Pathfinding::FREE);
+                    angle_ + M_PI), true, Pathfinding::FREE);

                go_timeout_ = node_->now();
            }
@@ -73,6 +73,20 @@ namespace Modelec {
            {
                action_executor_->Free({{0, true}, {1, true}, {2, true}, {3, true}});
                deploy_time_ = node_->now();
+
+                auto obs = action_executor_->box_obstacles_[0];
+                action_executor_->box_obstacles_[0] = nullptr;
+
+                auto pos = nav_->GetCurrentPos();
+
+                obs->SetPosition(
+                    pos->x + 250 * cos(pos->theta),
+                    pos->y + 250 * sin(pos->theta),
+                    pos->theta);
+
+                obs->SetAtObjective(true);
+
+                nav_->GetPathfinding()->AddObstacle(obs);
            }

            step_ = UP;
@@ -83,6 +97,16 @@ namespace Modelec {
                deploy_time_ = node_->now();
            }

+            step_ = GO_BACK;
+            break;
+        case GO_BACK:
+            {
+                nav_->GoToRotateFirst(target_deposite_zone_->GetPosition().GetTakePosition(350,
+                    angle_ + M_PI), true, Pathfinding::FREE | Pathfinding::OBSTACLE);
+
+                go_timeout_ = node_->now();
+            }
+
            step_ = DONE;
            break;
        case DONE:
--- a/src/modelec_strat/src/missions/take_mission.cpp
+++ b/src/modelec_strat/src/missions/take_mission.cpp
@@ -28,8 +28,8 @@ namespace Modelec {
        {
            if ((node_->now() - go_timeout_).seconds() < 5)
            {
-                nav_->AskWaypoint();
-                return;
+                // nav_->AskWaypoint();
+                // return;
            }
            if ((node_->now() - go_timeout_).seconds() < 10)
            {
@@ -42,17 +42,28 @@ namespace Modelec {
        case GO_TO_TAKE:
            {

-                auto closestBox = nav_->GetClosestObstacle<BoxObstacle>(nav_->GetCurrentPos());
+                closestBox = nav_->GetClosestObstacle<BoxObstacle>(nav_->GetCurrentPos());
+
+                action_executor_->box_obstacles_[0] = closestBox;

                auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();

-                nav_->GetPathfinding()->RemoveObstacle(closestBox->GetId());
-
                nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL);

                go_timeout_ = node_->now();
            }

+            step_ = GO_TO_TAKE_CLOSE;
+            break;
+        case GO_TO_TAKE_CLOSE:
+            {
+                auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition();
+
+                nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
+
+                go_timeout_ = node_->now();
+            }
+
            step_ = DOWN;
            break;
        case DOWN:
@@ -80,6 +91,7 @@ namespace Modelec {
            step_ = DONE;
            break;
        case DONE:
+            nav_->GetPathfinding()->RemoveObstacle(closestBox->GetId());
            status_ = MissionStatus::DONE;
            break;
        default:
--- a/src/modelec_strat/src/navigation_helper.cpp
+++ b/src/modelec_strat/src/navigation_helper.cpp
@@ -761,34 +761,10 @@ namespace Modelec
            Config::get<double>("config.spawn.yellow.top@theta")
        );

-        spawn_yellow_["side"] = Point(
-            Config::get<int>("config.spawn.yellow.side@x"),
-            Config::get<int>("config.spawn.yellow.side@y"),
-            Config::get<double>("config.spawn.yellow.side@theta")
-        );
-
-        spawn_yellow_["bottom"] = Point(
-            Config::get<int>("config.spawn.yellow.bottom@x"),
-            Config::get<int>("config.spawn.yellow.bottom@y"),
-            Config::get<double>("config.spawn.yellow.bottom@theta")
-        );
-
        spawn_blue_["top"] = Point(
            Config::get<int>("config.spawn.blue.top@x"),
            Config::get<int>("config.spawn.blue.top@y"),
            Config::get<double>("config.spawn.blue.top@theta")
        );
-
-        spawn_blue_["side"] = Point(
-            Config::get<int>("config.spawn.blue.side@x"),
-            Config::get<int>("config.spawn.blue.side@y"),
-            Config::get<double>("config.spawn.blue.side@theta")
-        );
-
-        spawn_blue_["bottom"] = Point(
-            Config::get<int>("config.spawn.blue.bottom@x"),
-            Config::get<int>("config.spawn.blue.bottom@y"),
-            Config::get<double>("config.spawn.blue.bottom@theta")
-        );
    }
 }
--- a/src/modelec_strat/src/obstacle/box.cpp
+++ b/src/modelec_strat/src/obstacle/box.cpp
@@ -24,16 +24,6 @@ namespace Modelec
    {
    }

-    bool BoxObstacle::IsAtObjective() const
-    {
-        return isAtObjective;
-    }
-
-    void BoxObstacle::SetAtObjective(bool atObjective)
-    {
-        isAtObjective = atObjective;
-    }
-
    Point BoxObstacle::GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const
    {
        Point p = Point(msg->x, msg->y, msg->theta);
--- a/src/modelec_strat/src/obstacle/obstacle.cpp
+++ b/src/modelec_strat/src/obstacle/obstacle.cpp
@@ -43,4 +43,79 @@ namespace Modelec

        return msg;
    }
+
+    int Obstacle::GetId() const
+    { return id_; }
+
+    int Obstacle::GetX() const
+    { return x_; }
+
+    int Obstacle::GetY() const
+    { return y_; }
+
+    double Obstacle::GetTheta() const
+    { return theta_; }
+
+    int Obstacle::GetWidth() const
+    { return w_; }
+
+    int Obstacle::GetHeight() const
+    { return h_; }
+
+    const std::string& Obstacle::Type() const
+    { return type_; }
+
+    Point Obstacle::GetPosition() const
+    { return Point(x_, y_, theta_); }
+
+    void Obstacle::SetId(int id)
+    { id_ = id; }
+
+    void Obstacle::SetX(int x)
+    { x_ = x; }
+
+    void Obstacle::SetY(int y)
+    { y_ = y; }
+
+    void Obstacle::SetTheta(double theta)
+    { theta_ = theta; }
+
+    void Obstacle::SetWidth(int w)
+    { w_ = w; }
+
+    void Obstacle::SetHeight(int h)
+    { h_ = h; }
+
+    void Obstacle::SetType(const std::string& type)
+    { type_ = type; }
+
+    void Obstacle::SetPosition(int x, int y, double theta)
+    {
+        x_ = x;
+        y_ = y;
+        theta_ = theta;
+    }
+
+    void Obstacle::SetPosition(const Point& p)
+    {
+        x_ = p.x;
+        y_ = p.y;
+        theta_ = p.theta;
+    }
+
+    void Obstacle::SetSize(int w, int h)
+    {
+        w_ = w;
+        h_ = h;
+    }
+
+    bool Obstacle::IsAtObjective() const
+    {
+        return isAtObjective;
+    }
+
+    void Obstacle::SetAtObjective(bool atObjective)
+    {
+        isAtObjective = atObjective;
+    }
 }
--- a/src/modelec_strat/src/pami_manager.cpp
+++ b/src/modelec_strat/src/pami_manager.cpp
@@ -37,21 +37,6 @@ namespace Modelec
                auto goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_remove_top_pot_);
                auto second_goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_put_zone_);

-                timer_remove_ = create_wall_timer(
-                    goal - now,
-                    [this]()
-                    {
-                        modelec_interfaces::msg::Obstacle topLeft;
-                        topLeft.id = 10;
-                        remove_obs_pub_->publish(topLeft);
-
-                        modelec_interfaces::msg::Obstacle topRight;
-                        topRight.id = 20;
-                        remove_obs_pub_->publish(topRight);
-
-                        timer_remove_->cancel();
-                    });
-
                timer_add_ = create_wall_timer(
                    second_goal - now,
                    [this]()
@@ -84,10 +69,10 @@ namespace Modelec
                {
                    timer_add_->cancel();
                }
-                if (timer_remove_)
+                /*if (timer_remove_)
                {
                    timer_remove_->cancel();
-                }
+                }*/
            });
    }