rewrite action hope this work

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.start and self.waypoint_queue:
-            if self.waypoint_order:
+            wp_id = self.waypoint_queue[0]
-                wp = self.waypoints[self.waypoint_order[0]]
+            wp = self.waypoints[wp_id]
-                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 wp['type'] == 1:
+            dx = wp['x'] - self.x
-                    if distance < 5.0 and abs(angle_diff) < 0.15:
+            dy = wp['y'] - self.y
-                        self.vx = 0.0
+            distance = math.hypot(dx, dy)
-                        self.vy = 0.0
+
-                        self.vtheta = 0.0
+            target_angle = math.atan2(dy, dx)
-                        self.ser.write(f'SET;WAYPOINT;{wp["id"]}\n'.encode('utf-8'))
+            theta_error = self.normalize_angle(wp['theta'] - self.theta)
-                        self.current_wp_id = wp['id']
+
-                        self.waypoint_order.pop(0)
+            # --- motion ---
-                        del self.waypoints[wp['id']]
+            if wp['type'] == 1:  # precise waypoint
-                    else:
+                speed = min(120.0, distance * 1)
-                        speed = min(100.0, distance / 2)
+                self.vtheta = max(-6.0, min(6.0, theta_error * 2))
-                        self.vx = speed * math.cos(angle)
+            else:  # transit waypoint
-                        self.vy = speed * math.sin(angle)
+                speed = min(180.0, distance * 2)
-                        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
                 self.vtheta = 0.0
 
-            self.x += self.vx * dt
+            self.vx = speed * math.cos(target_angle)
-            self.y += self.vy * dt
+            self.vy = speed * math.sin(target_angle)
-            self.theta += self.vtheta * dt
-            self.theta = self.normalize_angle(self.theta)
 
-        if now - self.last_send > 0.1 and self.start:
+            reached_pos = distance < 5.0
-            print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}')
+            reached_theta = abs(theta_error) < 0.15 or wp['type'] == 0
-            self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n'.encode())
+
+            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(
-            self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}\n'.encode())
+                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(
-            self.ser.write(f'SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}\n'.encode())
+                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(b"OK;PID;1\n")
-            self.ser.write(f'OK;PID;1\n'.encode())
 
         elif msg.startswith("SET;START;"):
             self.start = msg.split(';')[2] == '1'
-            print(f'[TX] OK;START;1')
+            self.ser.write(b"OK;START;1\n")
-            self.ser.write(f'OK;START;1\n'.encode())
 
+        # --- MULTI WAYPOINT (clears previous) ---
         elif msg.startswith("SET;WAYPOINT;"):
-            parts = msg.split(';')
+            parts = msg.split(';')[2:]
-            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())
 
-        elif msg.startswith("SET;WAYPOINTS;"):
+            if len(parts) % 5 != 0:
-            parts = msg.split(';')
+                self.ser.write(b"ERR;WAYPOINT;0\n")
-            if len(parts) / 7 >= 1:
+                return
-                self.waypoints.clear()
+
-                self.waypoint_order.clear()
+            # clear existing waypoints
-                for i in range(2, len(parts), 5):
+            self.waypoints.clear()
-                    wp = {
+            self.waypoint_queue.clear()
-                        'id': int(parts[i]),
+
-                        'type': int(parts[i + 1]),
+            for i in range(0, len(parts), 5):
-                        'x': float(parts[i + 2]),
+                wp = {
-                        'y': float(parts[i + 3]),
+                    'id': int(parts[i]),
-                        'theta': float(parts[i + 4])
+                    'type': int(parts[i + 1]),
-                    }
+                    'x': float(parts[i + 2]),
-                    self.waypoints[wp['id']] = wp
+                    'y': float(parts[i + 3]),
-                    if wp['id'] not in self.waypoint_order:
+                    'theta': float(parts[i + 4]),
-                        self.waypoint_order.append(wp['id'])
+                }
-                        self.ser.write(f'OK;WAYPOINT;{wp["id"]}\n'.encode())
+
-                self.ser.write(b'OK;WAYPOINTS;1\n')
+                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")

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',
+                output='screen'
-                parameters=[{
-                    'dev': '/dev/input/js0',  # optional: specify your joystick device
-                    'deadzone': 0.05
-                }]
             )
             return [joy]
         return []

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_;
     };
 }

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* layout = new QHBoxLayout(this);
-        auto h = Modelec::Config::get<int>("config.spawn.height_mm");
+        layout->setContentsMargins(0, 0, 0, 0);
+        layout->setSpacing(0);
 
-        spawn_sub_ = node_->create_subscription<modelec_interfaces::msg::Spawn>("nav/spawn", 10,
+        yellow_spawn_buttons_ = new QPushButton("Yellow", this);
-            [this, w, h](const modelec_interfaces::msg::Spawn::SharedPtr msg)
+        blue_spawn_buttons_   = new QPushButton("Blue", this);
-            {
-                auto ratioX = 1200 / 3000.0f;
-                auto ratioY = 800 / 2000.0f;
 
-                auto* button = new QPushButton(this);
+        yellow_spawn_buttons_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
-                spawn_buttons_.push_back(button);
+        blue_spawn_buttons_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
 
-                button->setText(msg->team_id == 0 ? "Yellow" : "Blue");
+        yellow_spawn_buttons_->setStyleSheet(
-                button->setStyleSheet(
+            "QPushButton {"
-                    msg->team_id == 0
+            "  background-color: rgba(255, 215, 0, 140);"
-                        ? "background-color: rgba(255, 255, 0, 128); border: none; color: black; font-size: 24px;"
+            "  border: none;"
-                        : "background-color: rgba(0, 0, 255, 128); border: none; color: white; font-size: 24px;"
+            "  font-size: 32px;"
-                );
+            "  font-weight: bold;"
+            "  color: black;"
+            "}"
+            "QPushButton:hover {"
+            "  background-color: rgba(255, 215, 0, 180);"
+            "}"
+        );
 
-                button->move(
+        blue_spawn_buttons_->setStyleSheet(
-                    static_cast<int>(msg->x * ratioX - (w * ratioX) / 2),
+            "QPushButton {"
-                    static_cast<int>(800 - msg->y * ratioY - (h * ratioY) / 2)
+            "  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]()
+        connect(blue_spawn_buttons_, &QPushButton::clicked, this, [this]()
-                {
+        {
-                    modelec_interfaces::msg::Spawn team_msg;
+            modelec_interfaces::msg::Spawn msg;
-                    team_msg.team_id = msg->team_id;
+            msg.team_id = 1;
-                    team_msg.name = modelec_interfaces::msg::Spawn::TOP;
+            msg.name = modelec_interfaces::msg::Spawn::TOP;
-                    spawn_pub_->publish(team_msg);
+            spawn_pub_->publish(msg);
-
+            emit TeamChoose();
-                    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()

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;
             }

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;
             }

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_;

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_;
     };

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_;

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 (!obs->IsAtObjective())
-                if (dist < distance)
                 {
-                    distance = dist;
+                    auto dist = Point::distance(robotPos, obs->GetPosition());
-                    closest_obstacle = obs;
+                    if (dist < distance)
+                    {
+                        distance = dist;
+                        closest_obstacle = obs;
+                    }
                 }
             }
         }

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_;
     };

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 GetId() const;
-        int GetX() const { return x_; }
+        int GetX() const;
-        int GetY() const { return y_; }
+        int GetY() const;
-        double GetTheta() const { return theta_; }
+        double GetTheta() const;
-        int GetWidth() const { return w_; }
+        int GetWidth() const;
-        int GetHeight() const { return h_; }
+        int GetHeight() const;
-        const std::string& Type() const { return type_; }
+        const std::string& Type() const;
-        Point GetPosition() const { return Point(x_, y_, theta_); }
+        Point GetPosition() const;
 
-        void SetId(int id) { id_ = id; }
+        void SetId(int id);
-        void SetX(int x) { x_ = x; }
+        void SetX(int x);
-        void SetY(int y) { y_ = y; }
+        void SetY(int y);
-        void SetTheta(double theta) { theta_ = theta; }
+        void SetTheta(double theta);
-        void SetWidth(int w) { w_ = w; }
+        void SetWidth(int w);
-        void SetHeight(int h) { h_ = h; }
+        void SetHeight(int h);
-        void SetType(const std::string& type) { type_ = type; }
+        void SetType(const std::string& type);
 
-        void SetPosition(int x, int y, double theta)
+        void SetPosition(int x, int y, double theta);
-        {
-            x_ = x;
-            y_ = y;
-            theta_ = theta;
-        }
 
-        void SetPosition(const Point& p)
+        void SetPosition(const Point& p);
-        {
-            x_ = p.x;
-            y_ = p.y;
-            theta_ = p.theta;
-        }
 
-        void SetSize(int w, int h)
+        void SetSize(int w, int h);
-        {
+
-            w_ = w;
+        bool IsAtObjective() const;
-            h_ = h;
+        void SetAtObjective(bool atObjective);
-        }
 
     protected:
         int id_, x_, y_, w_, h_;
         double theta_;
         std::string type_;
+
+        bool isAtObjective = false;
     };
 }

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_;

src/modelec_strat/src/missions/free_mission.cpp

@@ -28,8 +28,8 @@ namespace Modelec {
         {
             if ((node_->now() - go_timeout_).seconds() < 5)
             {
-                nav_->AskWaypoint();
+                // nav_->AskWaypoint();
-                return;
+                // 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:

src/modelec_strat/src/missions/take_mission.cpp

@@ -28,8 +28,8 @@ namespace Modelec {
         {
             if ((node_->now() - go_timeout_).seconds() < 5)
             {
-                nav_->AskWaypoint();
+                // nav_->AskWaypoint();
-                return;
+                // 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:

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")
-        );
     }
 }

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);

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;
+    }
 }

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();
-                }
+                }*/
             });
     }