BackFront + Joy + change IHM + some change + some fix (#26)

Co-authored-by: Modelec <modelec.isen@gmail.com>

install.sh

@@ -27,9 +27,9 @@ git submodule init
 git submodule update
 
 echo "source /opt/ros/jazzy/setup.bash
-source ~/modelec-marcel-ROS/install/setup.bash
+source ~/Modelec-ROS/install/setup.bash
 export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
-export FASTRTPS_DEFAULT_PROFILES_FILE=~/modelec-marcel-ROS/fastdds_setup.xml
+export FASTRTPS_DEFAULT_PROFILES_FILE=~/Modelec-ROS/fastdds_setup.xml
 export ROS_DOMAIN_ID=128" >> ~/.bashrc
 
 source ~/.bashrc

no_lidar.ros2_launch_marcel.desktop

@@ -2,7 +2,7 @@
 Type=Application
 Name=No Lidar
 Comment=Lance le système ROS 2 avec GUI
-Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh with_joy:=false with_lidar:=false
+Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh with_joy:=false with_rplidar:=false
 Icon=utilities-terminal
 Terminal=true
 Categories=Utility;

simulated_pcb/action.py

@@ -210,7 +210,7 @@ if __name__ == "__main__":
             sim.stop()
         print("Stopped.")
 
-# socat -d -d pty,raw,echo=0,link=/tmp/ACTION_SIM pty,raw,echo=0,link=/tmp/ACTION_USB
-# python3 simulated_pcb/action.py --port /tmp/ACTION_SIM
-# socat -d -d pty,raw,echo=0,link=/tmp/ODO_SIM pty,raw,echo=0,link=/tmp/ODO_USB
-# python3 simulated_pcb/action.py --port /tmp/ODO_SIM
+# socat -d -d pty,raw,echo=0,link=/tmp/SIM_ACTION pty,raw,echo=0,link=/tmp/USB_ACTION
+# python3 simulated_pcb/action.py --port /tmp/SIM_ACTION
+# socat -d -d pty,raw,echo=0,link=/tmp/SIM_ODO pty,raw,echo=0,link=/tmp/USB_ODO
+# python3 simulated_pcb/action.py --port /tmp/SIM_ODO

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

src/modelec_com/include/modelec_com/pcb_odo_interface.hpp

@@ -56,6 +56,7 @@ namespace Modelec
         rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoints>::SharedPtr odo_add_waypoints_subscriber_;
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_set_pos_subscriber_;
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_set_pid_subscriber_;
+        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr odo_ask_active_waypoint_subscriber_;
         rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_subscriber_;
 
         void AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg);

src/modelec_com/src/pcb_action_interface.cpp

@@ -263,11 +263,14 @@ namespace Modelec
         };*/
 
         servo_value_ = {
-            {0, 0},
-            {1, 3},
-            {2, 1.45},
-            {3, 1.6},
-            {4, 0},
+            {0, 2.95},
+            {1, 0.93},
+            {2, 0},
+            {3, 3},
+            {4, 0.8},
+            {5, 0.8},
+            {6, 0.8},
+            {7, 0.8},
         };
 
         std::string data = "MOV;SERVO;" + std::to_string(servo_value_.size()) + ";";

src/modelec_com/src/pcb_odo_interface.cpp

@@ -69,6 +69,13 @@ namespace Modelec
                 SetPIDCallback(msg);
             });
 
+        odo_ask_active_waypoint_subscriber_ = this->create_subscription<std_msgs::msg::Empty>(
+            "odometry/ask_active_waypoint", 10,
+            [this](const std_msgs::msg::Empty::SharedPtr)
+            {
+                GetData("WAYPOINT", {"ACTIVE"});
+            });
+
         joy_subscriber_ = this->create_subscription<sensor_msgs::msg::Joy>(
             "joy", 30,
             [this](const sensor_msgs::msg::Joy::SharedPtr msg)
@@ -170,12 +177,38 @@ namespace Modelec
             }
             else if (tokens[1] == "WAYPOINT")
             {
-                int id = std::stoi(tokens[2]);
+                if (tokens[2] == "REACH")
+                {
+                    // RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[3].c_str());
 
-                auto message = modelec_interfaces::msg::OdometryWaypoint();
-                message.id = id;
+                    int id = std::stoi(tokens[3]);
 
-                odo_waypoint_reach_publisher_->publish(message);
+                    auto message = modelec_interfaces::msg::OdometryWaypoint();
+                    message.id = id;
+
+                    odo_waypoint_reach_publisher_->publish(message);
+                }
+                else if (tokens.size() >= 7)
+                {
+                    int id = std::stoi(tokens[3]);
+                    bool is_end = tokens[4] == "1";
+                    long x = std::stol(tokens[5]);
+                    long y = std::stol(tokens[6]);
+                    double theta = std::stod(tokens[7]);
+                    bool reach = tokens.size() > 7 ? tokens[8] == "1" : false;
+
+                    if (reach)
+                    {
+                        auto message = modelec_interfaces::msg::OdometryWaypoint();
+                        message.id = id;
+                        message.is_end = is_end;
+                        message.x = x;
+                        message.y = y;
+                        message.theta = theta;
+
+                        odo_waypoint_reach_publisher_->publish(message);
+                    }
+                }
             }
             else if (tokens[1] == "PID")
             {
@@ -201,6 +234,7 @@ namespace Modelec
             }
             else if (tokens[1] == "WAYPOINT")
             {
+                // RCLCPP_INFO(this->get_logger(), "Waypoint added successfully.");
             }
             else if (tokens[1] == "PID")
             {

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': "/tmp/USB_ODO",
+                        'serial_port': "/dev/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': "/tmp/USB_ACTION",
+                        'serial_port': "/dev/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 []
@@ -172,4 +168,4 @@ def generate_launch_description():
         OpaqueFunction(function=launch_joy),
     ])
 
-# to run in debug : , prefix=['xterm -e gdb -ex run --args']
+# to run in debug : , prefix=['xterm -e gdb -ex run --args']

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

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/CMakeLists.txt

@@ -37,7 +37,7 @@ set(strat_fsm_sources
 )
 
 add_executable(strat_fsm ${strat_fsm_sources})
-ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces ament_index_cpp)
+ament_target_dependencies(strat_fsm rclcpp std_msgs sensor_msgs std_srvs modelec_interfaces ament_index_cpp)
 target_link_libraries(strat_fsm modelec_utils::utils modelec_utils::config)
 target_include_directories(strat_fsm PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

src/modelec_strat/data/deposite_zone.xml

@@ -1,32 +1,71 @@
 <Map>
     <DepositeZone id="0">
         <Pos x="1250" y="1450" theta="0" w="200" h="200" />
+        <TakeAngle>
+            <Angle>-1.5708</Angle>
+        </TakeAngle>
     </DepositeZone>
     <DepositeZone id="1">
         <Pos x="1750" y="1450" theta="0" w="200" h="200" />
+        <TakeAngle>
+            <Angle>-1.5708</Angle>
+        </TakeAngle>
     </DepositeZone>
     <DepositeZone id="2">
         <Pos x="100" y="800" theta="0" w="200" h="200" />
+        <TakeAngle>
+            <Angle>0</Angle>
+        </TakeAngle>
     </DepositeZone>
     <DepositeZone id="3">
         <Pos x="800" y="800" theta="0" w="200" h="200" />
+        <TakeAngle>
+            <Angle>0</Angle>
+            <Angle>1.5708</Angle>
+            <Angle>-1.5708</Angle>
+            <Angle>3.1415</Angle>
+        </TakeAngle>
     </DepositeZone>
     <DepositeZone id="4">
         <Pos x="1500" y="800" theta="0" w="200" h="200" />
+        <TakeAngle>
+            <Angle>0</Angle>
+            <Angle>1.5708</Angle>
+            <Angle>-1.5708</Angle>
+            <Angle>3.1415</Angle>
+        </TakeAngle>
     </DepositeZone>
     <DepositeZone id="5">
         <Pos x="2200" y="800" theta="0" w="200" h="200" />
+        <TakeAngle>
+            <Angle>0</Angle>
+            <Angle>1.5708</Angle>
+            <Angle>-1.5708</Angle>
+            <Angle>3.1415</Angle>
+        </TakeAngle>
     </DepositeZone>
     <DepositeZone id="6">
         <Pos x="2900" y="800" theta="0" w="200" h="200" />
+        <TakeAngle>
+            <Angle>3.1415</Angle>
+        </TakeAngle>
     </DepositeZone>
     <DepositeZone id="7">
         <Pos x="700" y="100" theta="0" w="200" h="200" />
+        <TakeAngle>
+            <Angle>1.5708</Angle>
+        </TakeAngle>
     </DepositeZone>
     <DepositeZone id="8">
         <Pos x="1500" y="100" theta="0" w="200" h="200" />
+        <TakeAngle>
+            <Angle>1.5708</Angle>
+        </TakeAngle>
     </DepositeZone>
     <DepositeZone id="9">
         <Pos x="2300" y="100" theta="0" w="200" h="200" />
+        <TakeAngle>
+            <Angle>1.5708</Angle>
+        </TakeAngle>
     </DepositeZone>
 </Map>

src/modelec_strat/include/modelec_strat/action/base_action.hpp

@@ -25,6 +25,13 @@ namespace Modelec
     class BaseAction
     {
     public:
+        enum Front
+        {
+            FRONT = 1,
+            BACK = 0,
+            BOTH = -1,
+        };
+
         using Ptr = std::shared_ptr<BaseAction>;
         using FactoryFn =
             std::function<Ptr(const std::shared_ptr<ActionExecutor>&)>;
@@ -41,13 +48,9 @@ namespace Modelec
         {
         }
         virtual ~BaseAction() = default;
-        virtual void Execute() = 0;
         virtual void Next() = 0;
         virtual bool IsDone() const { return done_; }
         virtual void Init(const std::vector<std::string>& params) = 0;
-
-        // static constexpr std::string_view Name = "BaseAction";
-
         static Ptr CreateAction(
             const std::string& action_name,
             const std::shared_ptr<ActionExecutor>& action_executor);

src/modelec_strat/include/modelec_strat/action/down_action.hpp

@@ -9,17 +9,16 @@ namespace Modelec
     {
     public:
         DownAction(const std::shared_ptr<ActionExecutor>& action_executor);
-        DownAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front);
+        DownAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front);
 
-        void Execute() override;
         void Next() override;
         void Init(const std::vector<std::string>& params) override;
-        void SetFront(bool front);
+        void SetFront(Front front);
 
         inline static const std::string Name = ActionExec::DOWN;
 
     private:
-        bool front_;
+        Front front_;
 
         std::queue<int> steps_;
 

src/modelec_strat/include/modelec_strat/action/free_action.hpp

@@ -10,18 +10,19 @@ namespace Modelec
     public:
         FreeAction(const std::shared_ptr<ActionExecutor>& action_executor);
         FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n);
+        FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo);
+        FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos);
 
-        void Execute() override;
         void Next() override;
         void Init(const std::vector<std::string>& params) override;
-        void SetFront(bool front);
-        void SetN(int n);
+        void AddServo(int id, bool front);
+        void AddServo(std::pair<int, bool> servo);
+        void AddServos(const std::vector<std::pair<int, bool>>& servos);
 
         inline static const std::string Name = ActionExec::FREE;
 
     private:
-        bool front_;
-        int n_;
+        std::vector<std::pair<int, bool>> servos_;
 
         std::queue<int> steps_;
 

src/modelec_strat/include/modelec_strat/action/take_action.hpp

@@ -10,18 +10,19 @@ namespace Modelec
     public:
         TakeAction(const std::shared_ptr<ActionExecutor>& action_executor);
         TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n);
+        TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo);
+        TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos);
 
-        void Execute() override;
         void Next() override;
         void Init(const std::vector<std::string>& params) override;
-        void SetFront(bool front);
-        void SetN(int n);
+        void AddServo(int id, bool front);
+        void AddServo(std::pair<int, bool> servo);
+        void AddServos(const std::vector<std::pair<int, bool>>& servos);
 
         inline static const std::string Name = ActionExec::TAKE;
 
     private:
-        bool front_;
-        int n_;
+        std::vector<std::pair<int, bool>> servos_;
 
         std::queue<int> steps_;
 

src/modelec_strat/include/modelec_strat/action/up_action.hpp

@@ -9,17 +9,16 @@ namespace Modelec
     {
     public:
         UPAction(const std::shared_ptr<ActionExecutor>& action_executor);
-        UPAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front);
+        UPAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front);
 
-        void Execute() override;
         void Next() override;
         void Init(const std::vector<std::string>& params) override;
-        void SetFront(bool front);
+        void SetFront(Front front);
 
         inline static const std::string Name = ActionExec::UP;
 
     private:
-        bool front_;
+        Front front_;
 
         std::queue<int> steps_;
 

src/modelec_strat/include/modelec_strat/action_executor.hpp

@@ -7,6 +7,11 @@
 #include <modelec_interfaces/msg/action_servo_timed_array.hpp>
 #include <modelec_interfaces/msg/action_exec.hpp>
 
+#include <sensor_msgs/msg/joy.hpp>
+
+#include "action/base_action.hpp"
+#include "obstacle/box.hpp"
+
 namespace Modelec
 {
     class BaseAction;
@@ -14,16 +19,6 @@ namespace Modelec
     class ActionExecutor : public std::enable_shared_from_this<ActionExecutor>
     {
     public:
-        enum Action
-        {
-            NONE,
-            UP,
-            DOWN,
-
-            TAKE,
-            FREE
-        };
-
         ActionExecutor();
 
         ActionExecutor(const rclcpp::Node::SharedPtr& node);
@@ -36,18 +31,20 @@ namespace Modelec
 
         void ReInit();
 
-        void Down(bool front = true);
+        void Down(BaseAction::Front front, bool force = false);
 
-        void Up(bool front = true);
+        void Up(BaseAction::Front front, bool force = false);
 
-        void Take(bool front = true, int n = 0);
-
-        void Free(bool front = true, int n = 0);
+        void Take(std::vector<std::pair<int, bool>> servos, bool force = false);
 
+        void Free(std::vector<std::pair<int, bool>> servos, bool force = false);
 
         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_;
@@ -60,6 +57,7 @@ namespace Modelec
         rclcpp::Subscription<modelec_interfaces::msg::ActionServoTimedArray>::SharedPtr servo_timed_move_res_sub_;
 
         rclcpp::Subscription<modelec_interfaces::msg::ActionExec>::SharedPtr action_exec_sub_;
+        rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
 
         std::shared_ptr<BaseAction> action_;
 

src/modelec_strat/include/modelec_strat/deposite_zone.hpp

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <limits>
 #include <queue>
 #include <tinyxml2.h>
 #include <modelec_utils/point.hpp>
@@ -14,6 +13,8 @@ namespace Modelec
 
         Point GetPosition() const;
 
+        Point GetBestTakePosition(const Point& currentPos, int dist = CLOSE_DISTANCE) const;
+
         void Validate(bool valid);
 
         bool Validate() const;
@@ -29,6 +30,8 @@ namespace Modelec
         int w_, h_;
         Point position_;
 
+        std::vector<double> take_angle_;
+
         bool has_box_ = false;
     };
 }

src/modelec_strat/include/modelec_strat/missions/free_mission.hpp

@@ -20,7 +20,10 @@ namespace Modelec {
         enum Step
         {
             GO_TO_FREE,
+            DOWN,
             FREE,
+            UP,
+            GO_BACK,
             DONE,
         } step_;
 
@@ -31,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,10 +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

@@ -113,6 +113,8 @@ namespace Modelec
 
         Point GetSpawn() const;
 
+        void AskWaypoint();
+
     protected:
         void OnWaypointReach(const WaypointMsg::SharedPtr msg);
 
@@ -171,8 +173,7 @@ namespace Modelec
         rclcpp::Publisher<modelec_interfaces::msg::Spawn>::SharedPtr spawn_pub_;
         rclcpp::Service<std_srvs::srv::Empty>::SharedPtr ask_spawn_srv_;
 
-        rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr odo_get_pos_pub_;
-        rclcpp::Time last_odo_get_pos_time_;
+        rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr odo_ask_waypoint_pub_;
 
     };
 
@@ -187,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;
+                    }
                 }
             }
         }

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

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/action/down_action.cpp

@@ -8,15 +8,11 @@ Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_ex
     steps_.push(ActionExec::DONE_STEP);
 }
 
-Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front) : DownAction(action_executor)
+Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front) : DownAction(action_executor)
 {
     front_ = front;
 }
 
-void Modelec::DownAction::Execute()
-{
-}
-
 void Modelec::DownAction::Next()
 {
     if (steps_.empty())
@@ -33,27 +29,54 @@ void Modelec::DownAction::Next()
     case ActionExec::DOWN_STEP:
         {
             ActionServoTimedArray msg;
-            msg.items.resize(4);
+            msg.items.resize(front_ == BOTH ? 8 : 4);
 
-            msg.items[0].id = front_ ? 0 : 8;
-            msg.items[0].start_angle = front_ ? 1.49 : 0;
-            msg.items[0].end_angle = front_ ? 0 : 0;
-            msg.items[0].duration_s = 1;
+            if (front_ == FRONT || front_ == BOTH)
+            {
+                msg.items[0].id = 0;
+                msg.items[0].start_angle = 1.95;
+                msg.items[0].end_angle = 2.95;
+                msg.items[0].duration_s = 1;
 
-            msg.items[1].id = front_ ? 1 : 9;
-            msg.items[1].start_angle = front_ ? 1.5 : 0;
-            msg.items[1].end_angle = front_ ? 3 : 0;
-            msg.items[1].duration_s = 1;
+                msg.items[1].id = 1;
+                msg.items[1].start_angle = 1.9;
+                msg.items[1].end_angle = 0.9;
+                msg.items[1].duration_s = 1;
 
-            msg.items[2].id = front_ ? 2 : 10;
-            msg.items[2].start_angle = front_ ? 3 : 0;
-            msg.items[2].end_angle = front_ ? 1.45 : 0;
-            msg.items[2].duration_s = 1;
+                msg.items[2].id = 2;
+                msg.items[2].start_angle = 0.3;
+                msg.items[2].end_angle = 0;
+                msg.items[2].duration_s = 1;
 
-            msg.items[3].id = front_ ? 3 : 11;
-            msg.items[3].start_angle = front_ ? 0 : 0;
-            msg.items[3].end_angle = front_ ? 1.6 : 0;
-            msg.items[3].duration_s = 1;
+                msg.items[3].id = 3;
+                msg.items[3].start_angle = 2.7;
+                msg.items[3].end_angle = 3;
+                msg.items[3].duration_s = 1;
+            }
+
+            if (front_ == BACK || front_ == BOTH)
+            {
+                int i = front_ == BOTH ? 4 : 0;
+                msg.items[i].id = 8;
+                msg.items[i].start_angle = 0;
+                msg.items[i].end_angle = 0;
+                msg.items[i].duration_s = 1;
+
+                msg.items[i+1].id = 9;
+                msg.items[i+1].start_angle = 0;
+                msg.items[i+1].end_angle = 0;
+                msg.items[i+1].duration_s = 1;
+
+                msg.items[i+2].id = 10;
+                msg.items[i+2].start_angle = 0;
+                msg.items[i+2].end_angle = 0;
+                msg.items[i+2].duration_s = 1;
+
+                msg.items[i+3].id = 11;
+                msg.items[i+3].start_angle = 0;
+                msg.items[i+3].end_angle = 0;
+                msg.items[i+3].duration_s = 1;
+            }
 
             action_executor_->MoveServoTimed(msg);
         }
@@ -72,11 +95,11 @@ void Modelec::DownAction::Init(const std::vector<std::string>& params)
 {
     if (!params.empty())
     {
-        SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
+        SetFront(static_cast<Front>(std::stoi(params[1])));
     }
 }
 
-void Modelec::DownAction::SetFront(bool front)
+void Modelec::DownAction::SetFront(Front front)
 {
     front_ = front;
 }

src/modelec_strat/src/action/free_action.cpp

@@ -10,12 +10,17 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_ex
 
 Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n) : FreeAction(action_executor)
 {
-    front_ = front;
-    n_ = n;
+    AddServo(n, front);
 }
 
-void Modelec::FreeAction::Execute()
+Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo) : FreeAction(action_executor)
 {
+    AddServo(servo.first, servo.second);
+}
+
+Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos) : FreeAction(action_executor)
+{
+    AddServos(servos);
 }
 
 void Modelec::FreeAction::Next()
@@ -35,12 +40,16 @@ void Modelec::FreeAction::Next()
         {
             modelec_interfaces::msg::ActionServoTimedArray msg;
 
-            msg.items.resize(1);
+            msg.items.resize(servos_.size());
+
+            for (size_t i = 0; i < servos_.size(); i++)
+            {
+                msg.items[i].id = servos_[i].first + (servos_[i].second ? 3 : 11);
+                msg.items[i].start_angle = servos_[i].second ? 0.8 : 0;
+                msg.items[i].end_angle = servos_[i].second ? 2.5 : 0;
+                msg.items[i].duration_s = 0.5;
+            }
 
-            msg.items[0].id = n_ + (front_ ? 3 : 11);
-            msg.items[0].start_angle = front_ ? 3 : 0;
-            msg.items[0].end_angle = front_ ? 0 : 0;
-            msg.items[0].duration_s = 0.5;
             action_executor_->MoveServoTimed(msg);
         }
         break;
@@ -58,17 +67,26 @@ void Modelec::FreeAction::Init(const std::vector<std::string>& params)
 {
     if (params.size() >= 2)
     {
-        SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
-        SetN(std::stoi(params[2]));
+        for (size_t i = 1; i < params.size(); i += 2)
+        {
+            int id = std::stoi(params[i]);
+            bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true;
+            AddServo(id, front);
+        }
     }
 }
 
-void Modelec::FreeAction::SetFront(bool front)
+void Modelec::FreeAction::AddServo(int id, bool front)
 {
-    front_ = front;
+    servos_.emplace_back(id, front);
 }
 
-void Modelec::FreeAction::SetN(int n)
+void Modelec::FreeAction::AddServo(std::pair<int, bool> servo)
 {
-    n_ = n;
+    servos_.emplace_back(servo);
+}
+
+void Modelec::FreeAction::AddServos(const std::vector<std::pair<int, bool>>& servos)
+{
+    servos_.insert(servos_.end(), servos.begin(), servos.end());
 }

src/modelec_strat/src/action/take_action.cpp

@@ -4,18 +4,21 @@
 
 Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor) : BaseAction(action_executor)
 {
-    steps_.push(ActionExec::TAKE_STEP);
-    steps_.push(ActionExec::DONE_STEP);
 }
 
 Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n) : TakeAction(action_executor)
 {
-    front_ = front;
-    n_ = n;
+    AddServo(n, front);
 }
 
-void Modelec::TakeAction::Execute()
+Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo) : TakeAction(action_executor)
 {
+    AddServo(servo);
+}
+
+Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos) : TakeAction(action_executor)
+{
+    AddServos(servos);
 }
 
 void Modelec::TakeAction::Next()
@@ -35,12 +38,16 @@ void Modelec::TakeAction::Next()
         {
             modelec_interfaces::msg::ActionServoTimedArray msg;
 
-            msg.items.resize(1);
+            msg.items.resize(servos_.size());
+
+            for (size_t i = 0; i < servos_.size(); i++)
+            {
+                msg.items[i].id = servos_[i].first + (servos_[i].second ? 3 : 11);
+                msg.items[i].start_angle = servos_[i].second ? 0.8 : 0;
+                msg.items[i].end_angle = servos_[i].second ? 2.7 : 0;
+                msg.items[i].duration_s = 0.5;
+            }
 
-            msg.items[0].id = n_ + (front_ ? 3 : 11);
-            msg.items[0].start_angle = front_ ? 0 : 0;
-            msg.items[0].end_angle = front_ ? 3 : 0;
-            msg.items[0].duration_s = 0.5;
             action_executor_->MoveServoTimed(msg);
         }
         break;
@@ -58,17 +65,26 @@ void Modelec::TakeAction::Init(const std::vector<std::string>& params)
 {
     if (params.size() >= 2)
     {
-        SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
-        SetN(std::stoi(params[2]));
+        for (size_t i = 1; i < params.size(); i += 2)
+        {
+            int id = std::stoi(params[i]);
+            bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true;
+            AddServo(id, front);
+        }
     }
 }
 
-void Modelec::TakeAction::SetFront(bool front)
+void Modelec::TakeAction::AddServo(int id, bool front)
 {
-    front_ = front;
+    servos_.emplace_back(id, front);
 }
 
-void Modelec::TakeAction::SetN(int n)
+void Modelec::TakeAction::AddServo(std::pair<int, bool> servo)
 {
-    n_ = n;
+    servos_.emplace_back(servo);
+}
+
+void Modelec::TakeAction::AddServos(const std::vector<std::pair<int, bool>>& servos)
+{
+    servos_.insert(servos_.end(), servos.begin(), servos.end());
 }

src/modelec_strat/src/action/up_action.cpp

@@ -8,15 +8,11 @@ Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_execut
     steps_.push(ActionExec::DONE_STEP);
 }
 
-Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front) : UPAction(action_executor)
+Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front) : UPAction(action_executor)
 {
     front_ = front;
 }
 
-void Modelec::UPAction::Execute()
-{
-}
-
 void Modelec::UPAction::Next()
 {
     if (steps_.empty())
@@ -33,27 +29,54 @@ void Modelec::UPAction::Next()
     case ActionExec::UP_STEP:
         {
             ActionServoTimedArray msg;
-            msg.items.resize(4);
+            msg.items.resize(front_ == BOTH ? 8 : 4);
 
-            msg.items[0].id = front_ ? 0 : 8;
-            msg.items[0].start_angle = front_ ? 0 : 0;
-            msg.items[0].end_angle = front_ ? 1.49 : 0;
-            msg.items[0].duration_s = 1;
+            if (front_ == FRONT || front_ == BOTH)
+            {
+                msg.items[0].id = 0;
+                msg.items[0].start_angle = 2.95;
+                msg.items[0].end_angle = 1.95;
+                msg.items[0].duration_s = 1;
 
-            msg.items[1].id = front_ ? 1 : 9;
-            msg.items[1].start_angle = front_ ? 3 : 0;
-            msg.items[1].end_angle = front_ ? 1.5 : 0;
-            msg.items[1].duration_s = 1;
+                msg.items[1].id = 1;
+                msg.items[1].start_angle = 0.9;
+                msg.items[1].end_angle = 1.9;
+                msg.items[1].duration_s = 1;
 
-            msg.items[2].id = front_ ? 2 : 10;
-            msg.items[2].start_angle = front_ ? 1.45 : 0;
-            msg.items[2].end_angle = front_ ? 3 : 0;
-            msg.items[2].duration_s = 1;
+                msg.items[2].id = 2;
+                msg.items[2].start_angle = 0;
+                msg.items[2].end_angle = 0.3;
+                msg.items[2].duration_s = 1;
 
-            msg.items[3].id = front_ ? 3 : 11;
-            msg.items[3].start_angle = front_ ? 1.6 : 0;
-            msg.items[3].end_angle = front_ ? 0 : 0;
-            msg.items[3].duration_s = 1;
+                msg.items[3].id = 3;
+                msg.items[3].start_angle = 3;
+                msg.items[3].end_angle = 2.5;
+                msg.items[3].duration_s = 1;
+            }
+
+            if (front_ == BACK || front_ == BOTH) {
+                int i = front_ == BOTH ? 4 : 0;
+
+                msg.items[i].id = 8;
+                msg.items[i].start_angle = 0;
+                msg.items[i].end_angle = 0;
+                msg.items[i].duration_s = 1;
+
+                msg.items[i+1].id = 9;
+                msg.items[i+1].start_angle = 0;
+                msg.items[i+1].end_angle = 0;
+                msg.items[i+1].duration_s = 1;
+
+                msg.items[i+2].id = 10;
+                msg.items[i+2].start_angle = 0;
+                msg.items[i+2].end_angle = 0;
+                msg.items[i+2].duration_s = 1;
+
+                msg.items[i+3].id = 11;
+                msg.items[i+3].start_angle = 0;
+                msg.items[i+3].end_angle = 0;
+                msg.items[i+3].duration_s = 1;
+            }
 
             action_executor_->MoveServoTimed(msg);
         }
@@ -72,11 +95,11 @@ void Modelec::UPAction::Init(const std::vector<std::string>& params)
 {
     if (!params.empty())
     {
-        SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
+        SetFront(static_cast<Front>(std::stoi(params[1])));
     }
 }
 
-void Modelec::UPAction::SetFront(bool front)
+void Modelec::UPAction::SetFront(Front front)
 {
     front_ = front;
 }

src/modelec_strat/src/action_executor.cpp

@@ -79,6 +79,31 @@ namespace Modelec
                 step_running_ -= msg->items.size();
                 Update();
             });
+
+        joy_sub_ = node_->create_subscription<sensor_msgs::msg::Joy>(
+            "/joy", 10, [this](const sensor_msgs::msg::Joy::SharedPtr msg)
+            {
+                // use game controller to manually control all the action. make it carefully
+                if (msg->buttons.size() >= 5)
+                {
+                    if (msg->buttons[0] == 1) // A button
+                    {
+                        Down(BaseAction::BOTH);
+                    }
+                    else if (msg->buttons[1] == 1) // B button
+                    {
+                        Up(BaseAction::BOTH);
+                    }
+                    else if (msg->buttons[3] == 1) // X button
+                    {
+                        Take({{0, true}, {1, true}, {2, true}, {3, true}});
+                    }
+                    else if (msg->buttons[4] == 1) // Y button
+                    {
+                        Free({{0, true}, {1, true}, {2, true}, {3, true}});
+                    }
+                }
+            });
     }
 
     rclcpp::Node::SharedPtr ActionExecutor::GetNode() const
@@ -124,45 +149,57 @@ namespace Modelec
         action_ = nullptr;
     }
 
-    void ActionExecutor::Down(bool front) {
-        if (action_done_)
+    void ActionExecutor::Down(BaseAction::Front front, bool force) {
+        if (action_done_ || force)
         {
             action_ = std::make_shared<DownAction>(shared_from_this(), front);
+            if (action_done_)
+            {
+                step_running_ = 0;
+            }
             action_done_ = false;
-            step_running_ = 0;
 
             Update();
         }
     }
 
-    void ActionExecutor::Up(bool front) {
-        if (action_done_)
+    void ActionExecutor::Up(BaseAction::Front front, bool force) {
+        if (action_done_ || force)
         {
             action_ = std::make_shared<UPAction>(shared_from_this(), front);
+            if (action_done_)
+            {
+                step_running_ = 0;
+            }
             action_done_ = false;
-            step_running_ = 0;
 
             Update();
         }
     }
 
-    void ActionExecutor::Take(bool front, int n) {
-        if (action_done_)
+    void ActionExecutor::Take(std::vector<std::pair<int, bool>> servos, bool force) {
+        if (action_done_ || force)
         {
-            action_ = std::make_shared<TakeAction>(shared_from_this(), front, n);
+            action_ = std::make_shared<TakeAction>(shared_from_this(), servos);
+            if (action_done_)
+            {
+                step_running_ = 0;
+            }
             action_done_ = false;
-            step_running_ = 0;
 
             Update();
         }
     }
 
-    void ActionExecutor::Free(bool front, int n) {
-        if (action_done_)
+    void ActionExecutor::Free(std::vector<std::pair<int, bool>> servos, bool force) {
+        if (action_done_ || force)
         {
-            action_ = std::make_shared<FreeAction>(shared_from_this(), front, n);
+            action_ = std::make_shared<FreeAction>(shared_from_this(), servos);
+            if (action_done_)
+            {
+                step_running_ = 0;
+            }
             action_done_ = false;
-            step_running_ = 0;
 
             Update();
         }

src/modelec_strat/src/deposite_zone.cpp

@@ -1,3 +1,4 @@
+#include <cmath>
 #include <modelec_strat/deposite_zone.hpp>
 #include <rclcpp/logging.hpp>
 
@@ -15,6 +16,16 @@ namespace Modelec
             posElem->QueryDoubleAttribute("theta", &position_.theta);
             posElem->QueryIntAttribute("w", &w_);
             posElem->QueryIntAttribute("h", &h_);
+
+            if (auto TakeAngleElem = obstacleElem->FirstChildElement("TakeAngle"))
+            {
+                for (auto takePos = TakeAngleElem->FirstChildElement("Angle"); takePos; takePos = takePos->NextSiblingElement("Angle"))
+                {
+                    double angle;
+                    takePos->QueryDoubleAttribute("value", &angle);
+                    take_angle_.push_back(angle);
+                }
+            }
         }
     }
 
@@ -23,6 +34,38 @@ namespace Modelec
         return position_;
     }
 
+    Point DepositeZone::GetBestTakePosition(const Point& currentPos, int dist) const
+    {
+        if (take_angle_.empty())
+        {
+            RCLCPP_WARN(rclcpp::get_logger("DepositeZone"), "No take angles defined for DepositeZone id=%d", id_);
+            return position_;
+        }
+
+        double min_distance = std::numeric_limits<double>::max();
+        double best_angle = take_angle_.front();
+
+        for (const auto& angle : take_angle_)
+        {
+            Point p = Point(
+                static_cast<int>(position_.x + dist * std::cos(angle)),
+                static_cast<int>(position_.y + dist * std::sin(angle)),
+                angle);
+
+            double distance = p.distance(currentPos);
+
+            if (distance < min_distance)
+            {
+                min_distance = distance;
+                best_angle = angle;
+            }
+        }
+
+        Point best_position = position_;
+        best_position.theta = best_angle;
+        return best_position;
+    }
+
     void DepositeZone::Validate(bool valid)
     {
         has_box_ = valid;

src/modelec_strat/src/missions/free_mission.cpp

@@ -1,5 +1,7 @@
 #include <modelec_strat/missions/free_mission.hpp>
 
+#include "modelec_strat/action/base_action.hpp"
+
 namespace Modelec {
     FreeMission::FreeMission(const std::shared_ptr<NavigationHelper>& nav, const std::shared_ptr<ActionExecutor>& action_executor)
      : step_(GO_TO_FREE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)  {
@@ -24,6 +26,11 @@ namespace Modelec {
 
         if (!nav_->HasArrived())
         {
+            if ((node_->now() - go_timeout_).seconds() < 5)
+            {
+                // nav_->AskWaypoint();
+                // return;
+            }
             if ((node_->now() - go_timeout_).seconds() < 10)
             {
                 return;
@@ -37,30 +44,68 @@ namespace Modelec {
                 auto currPos = nav_->GetCurrentPos();
 
                 auto dist = std::clamp(Point::distance(Point(currPos->x, currPos->y, currPos->theta),
-                    nav_->GetClosestDepositeZone(nav_->GetCurrentPos())->GetPosition()), 0.0, 200.0);
+                    nav_->GetClosestDepositeZone(nav_->GetCurrentPos())->GetPosition()), 0.0, 300.0);
 
                 target_deposite_zone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), {}, true);
 
-                auto depoPoint = target_deposite_zone_->GetPosition();
+                auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta));
 
-                auto angle = atan2(depoPoint.y - currPos->y,
-                    depoPoint.x - currPos->x);
-
-                nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist,
-                    angle + M_PI), true, Pathfinding::FREE);
+                if (nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist), true, Pathfinding::FREE))
+                {
+                    nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist), true, Pathfinding::FREE | Pathfinding::OBSTACLE);
+                }
 
                 go_timeout_ = node_->now();
-
-                step_ = FREE;
             }
+
+            step_ = FREE;
+            break;
+        case DOWN:
+            {
+                action_executor_->Down(BaseAction::FRONT);
+                deploy_time_ = node_->now();
+            }
+
+            step_ = FREE;
             break;
         case FREE:
             {
-                action_executor_->Down();
+                action_executor_->Free({{0, true}, {1, true}, {2, true}, {3, true}});
                 deploy_time_ = node_->now();
 
-                step_ = DONE;
+                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;
+            break;
+        case UP:
+            {
+                action_executor_->Up(BaseAction::FRONT);
+                deploy_time_ = node_->now();
+            }
+
+            step_ = GO_BACK;
+            break;
+        case GO_BACK:
+            {
+                nav_->GoTo(target_deposite_zone_->GetPosition().GetTakePosition(500), true, Pathfinding::FREE | Pathfinding::OBSTACLE);
+
+                go_timeout_ = node_->now();
+            }
+
+            step_ = DONE;
             break;
         case DONE:
             {
@@ -83,4 +128,4 @@ namespace Modelec {
         return status_;
     }
 
-}
+}

src/modelec_strat/src/missions/go_home_mission.cpp

@@ -27,6 +27,11 @@ namespace Modelec
     {
         if (!nav_->HasArrived())
         {
+            if ((node_->now() - go_timeout_).seconds() < 2)
+            {
+                // nav_->AskWaypoint();
+                return;
+            }
             if ((node_->now() - go_timeout_).seconds() < 10)
             {
                 return;

src/modelec_strat/src/missions/take_mission.cpp

@@ -1,5 +1,7 @@
 #include <modelec_strat/missions/take_mission.hpp>
 
+#include "modelec_strat/action/base_action.hpp"
+
 namespace Modelec {
     TakeMission::TakeMission(const std::shared_ptr<NavigationHelper>& nav, const std::shared_ptr<ActionExecutor>& action_executor)
      : step_(GO_TO_TAKE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)  {
@@ -24,7 +26,12 @@ namespace Modelec {
 
         if (!nav_->HasArrived())
         {
-            if ((node_->now() - go_timeout_).seconds() < 20)
+            if ((node_->now() - go_timeout_).seconds() < 5)
+            {
+                // nav_->AskWaypoint();
+                // return;
+            }
+            if ((node_->now() - go_timeout_).seconds() < 10)
             {
                 return;
             }
@@ -35,28 +42,62 @@ 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);
+                if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL))
+                {
+                    if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL))
+                    {
+                        nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
+                    }
+                }
 
                 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:
+            {
+                action_executor_->Down(BaseAction::FRONT);
+                deploy_time_ = node_->now();
+            }
+
             step_ = TAKE;
             break;
         case TAKE:
             {
-                action_executor_->Up();
+                action_executor_->Take({{0, true}, {1, true}, {2, true}, {3, true}});
+                deploy_time_ = node_->now();
+            }
+
+            step_ = UP;
+            break;
+        case UP:
+            {
+                action_executor_->Up(BaseAction::FRONT);
                 deploy_time_ = node_->now();
             }
 
             step_ = DONE;
             break;
         case DONE:
+            nav_->GetPathfinding()->RemoveObstacle(closestBox->GetId());
             status_ = MissionStatus::DONE;
             break;
         default:
@@ -73,4 +114,4 @@ namespace Modelec {
         return status_;
     }
 
-}
+}

src/modelec_strat/src/navigation_helper.cpp

@@ -97,10 +97,8 @@ namespace Modelec
                 }
             });
 
-        odo_get_pos_pub_ = node_->create_publisher<std_msgs::msg::Empty>(
-            "odometry/get/pos", 30);
-
-        last_odo_get_pos_time_ = node_->now();
+        odo_ask_waypoint_pub_ = node_->create_publisher<std_msgs::msg::Empty>(
+            "odometry/ask_active_waypoint", 30);
     }
 
     void NavigationHelper::ReInit()
@@ -720,6 +718,12 @@ namespace Modelec
         return spawn_;
     }
 
+    void NavigationHelper::AskWaypoint()
+    {
+        std_msgs::msg::Empty msg;
+        odo_ask_waypoint_pub_->publish(msg);
+    }
+
     void NavigationHelper::OnWaypointReach(const WaypointMsg::SharedPtr msg)
     {
         for (auto& waypoint : waypoints_)
@@ -757,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();
-                }
+                }*/
             });
     }
 

src/modelec_strat/src/strat_fms.cpp

@@ -7,6 +7,8 @@
 #include <modelec_strat/missions/take_mission.hpp>
 #include <modelec_strat/missions/free_mission.hpp>
 
+#include "modelec_strat/action/base_action.hpp"
+
 namespace Modelec
 {
 
@@ -141,6 +143,12 @@ namespace Modelec
                 arm_msg.data = true;
                 tir_arm_set_pub_->publish(arm_msg);
 
+                action_executor_->Up(BaseAction::Front::BOTH, true);
+                action_executor_->Free({
+                    {0, true}, {1, true}, {2, true}, {3, true},
+                    {0, false}, {1, false}, {2, false}, {3, false},
+                }, true);
+
                 Transition(State::WAIT_START, "System ready");
             }
             break;

src/modelec_utils/include/modelec_utils/point.hpp

@@ -1,5 +1,8 @@
 #pragma once
 
+#define CLOSE_DISTANCE 150
+#define BASE_DISTANCE 290
+
 namespace Modelec
 {
     struct Point {
@@ -13,6 +16,9 @@ namespace Modelec
         static double distance(const Point& p1, const Point& p2);
         static double angleDiff(const Point& p1, const Point& p2);
 
+        double distance(const Point& p2) const;
+        double angleDiff(const Point& p2) const;
+
         [[nodiscard]] Point GetTakePosition(int distance, double angle) const;
         [[nodiscard]] Point GetTakePosition(int distance) const;
 

src/modelec_utils/src/point.cpp

@@ -15,6 +15,16 @@ namespace Modelec
         return diff - M_PI;
     }
 
+    double Point::distance(const Point& p2) const
+    {
+        return distance(*this, p2);
+    }
+
+    double Point::angleDiff(const Point& p2) const
+    {
+        return angleDiff(*this, p2);
+    }
+
     Point Point::GetTakePosition(int distance, double angle) const
     {
         Point pos;
@@ -31,11 +41,11 @@ namespace Modelec
 
     Point Point::GetTakeBasePosition() const
     {
-        return GetTakePosition(290, theta);
+        return GetTakePosition(BASE_DISTANCE, theta);
     }
 
     Point Point::GetTakeClosePosition() const
     {
-        return GetTakePosition(150, theta);
+        return GetTakePosition(CLOSE_DISTANCE, theta);
     }
 }