Dynamic strat BackFront (#27)

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

fastdds_setup.xml

@@ -23,7 +23,7 @@
                     <!-- End repeat -->
                     <locator>
                         <udpv4>
-                            <address>100.113.219.4</address> <!-- ackimixs -->
+                            <address>100.84.205.108</address> <!-- ackimixs -->
                         </udpv4>
                     </locator>
                     <locator>

joy.ros2_launch_marcel.desktop

@@ -2,7 +2,7 @@
 Type=Application
 Name=Joy
 Comment=Lance le système ROS 2 avec GUI
-Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh
+Exec=/home/modelec/Modelec-ROS2/start_ros2.sh
 Icon=utilities-terminal
 Terminal=true
 Categories=Utility;

no_lidar.joy.ros2_launch_marcel.desktop

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

no_lidar.ros2_launch_marcel.desktop

@@ -2,7 +2,9 @@
 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_rplidar:=false
+
+Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_joy:=false with_rplidar:=false
+
 Icon=utilities-terminal
 Terminal=true
 Categories=Utility;

ros2_launch_marcel.desktop

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

src/modelec_com/src/pcb_action_interface.cpp

@@ -446,6 +446,10 @@ namespace Modelec
 
                 relay_move_res_pub_->publish(relay_msg);
             }
+            else if (tokens[1] == "TIR")
+            {
+                // Do nothing for now
+            }
             else
             {
                 RCLCPP_WARN(this->get_logger(), "Unknown message format for OK response: '%s'", trim(msg).c_str());

src/modelec_strat/data/obstacles.xml

@@ -18,7 +18,7 @@
         <possible-angle theta="-1.5708" />
     </Box>
     <Box id="22" x="2825" y="400" theta="3.1415" width="200" height="150" type="box"/>
-    <Box id="23" x="1900" y="175" theta="-1.5708" width="200" height="150" type="box"/>
+    <Box id="23" x="1900" y="175" theta="1.5708" width="200" height="150" type="box"/>
 
     <!-- PAMI Start -->
     <Obstacle id="30" x="75" y="1750" theta="1.5708" width="150" height="450" type="pami-start"/>

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

@@ -9,20 +9,20 @@ 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, Front 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::pair<int, Front> servo);
-        FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos);
+        FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Front>> servos);
 
         void Next() override;
         void Init(const std::vector<std::string>& params) override;
-        void AddServo(int id, bool front);
+        void AddServo(int id, Front front);
-        void AddServo(std::pair<int, bool> servo);
+        void AddServo(std::pair<int, Front> servo);
-        void AddServos(const std::vector<std::pair<int, bool>>& servos);
+        void AddServos(const std::vector<std::pair<int, Front>>& servos);
 
         inline static const std::string Name = ActionExec::FREE;
 
     private:
-        std::vector<std::pair<int, bool>> servos_;
+        std::vector<std::pair<int, Front>> servos_;
 
         std::queue<int> steps_;
 

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

@@ -9,20 +9,20 @@ 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, Front 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::pair<int, Front> servo);
-        TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos);
+        TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Front>> servos);
 
         void Next() override;
         void Init(const std::vector<std::string>& params) override;
-        void AddServo(int id, bool front);
+        void AddServo(int id, Front front);
-        void AddServo(std::pair<int, bool> servo);
+        void AddServo(std::pair<int, Front> servo);
-        void AddServos(const std::vector<std::pair<int, bool>>& servos);
+        void AddServos(const std::vector<std::pair<int, Front>>& servos);
 
         inline static const std::string Name = ActionExec::TAKE;
 
     private:
-        std::vector<std::pair<int, bool>> servos_;
+        std::vector<std::pair<int, Front>> servos_;
 
         std::queue<int> steps_;
 

src/modelec_strat/include/modelec_strat/action_executor.hpp

@@ -35,9 +35,9 @@ namespace Modelec
 
         void Up(BaseAction::Front front, bool force = false);
 
-        void Take(std::vector<std::pair<int, bool>> servos, bool force = false);
+        void Take(const std::vector<std::pair<int, BaseAction::Front>>& servos, bool force = false);
 
-        void Free(std::vector<std::pair<int, bool>> servos, bool force = false);
+        void Free(const std::vector<std::pair<int, BaseAction::Front>>& servos, bool force = false);
 
         void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg);
 
@@ -45,6 +45,18 @@ namespace Modelec
 
         std::array<std::shared_ptr<BoxObstacle>, 2> box_obstacles_;
 
+        bool IsEmpty() const;
+
+        bool IsFull() const;
+
+        bool HasBox(BaseAction::Front front) const;
+
+        bool HasFrontBox() const;
+
+        bool HasBackBox() const;
+
+        bool HasOneBox() const;
+
     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

@@ -8,7 +8,8 @@ namespace Modelec {
     class FreeMission : public Mission {
     public:
         FreeMission(const std::shared_ptr<NavigationHelper>& nav,
-                      const std::shared_ptr<ActionExecutor>& action_executor);
+                      const std::shared_ptr<ActionExecutor>& action_executor,
+                      BaseAction::Front front = BaseAction::FRONT);
 
         void Start(rclcpp::Node::SharedPtr node) override;
         void Update() override;
@@ -25,7 +26,9 @@ namespace Modelec {
             UP,
             GO_BACK,
             DONE,
-        } step_;
+        };
+
+        BaseAction::Front front_;
 
         MissionStatus status_;
         std::shared_ptr<NavigationHelper> nav_;
@@ -38,5 +41,9 @@ namespace Modelec {
 
         rclcpp::Time go_timeout_;
         rclcpp::Time deploy_time_;
+
+        std::optional<rclcpp::Time> min_time_;
+
+        rclcpp::Time last_ask_waypoint_time_;
     };
 }

src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp

@@ -20,14 +20,11 @@ namespace Modelec
     private:
         enum Step
         {
-            GO_FRONT,
-            AWAIT_95S,
-            GO_HOME,
-            DONE,
-
             ROTATE_TO_HOME,
+            GO_HOME,
             GO_CLOSE,
-        } step_;
+            DONE,
+        };
 
         MissionStatus status_;
         std::shared_ptr<NavigationHelper> nav_;
@@ -38,5 +35,9 @@ namespace Modelec
         rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
         int mission_score_ = 0;
         rclcpp::Time go_timeout_;
+
+        std::optional<rclcpp::Time> min_time_;
+
+        rclcpp::Time last_ask_waypoint_time_;
     };
 }

src/modelec_strat/include/modelec_strat/missions/mission_base.hpp

@@ -24,5 +24,8 @@ namespace Modelec
         virtual void Clear() = 0;
         virtual MissionStatus GetStatus() const = 0;
         virtual std::string GetName() const = 0;
+
+    protected:
+        std::queue<int> steps_;
     };
 }

src/modelec_strat/include/modelec_strat/missions/take_mission.hpp

@@ -8,7 +8,8 @@ namespace Modelec {
     class TakeMission : public Mission {
     public:
         TakeMission(const std::shared_ptr<NavigationHelper>& nav,
-                      const std::shared_ptr<ActionExecutor>& action_executor);
+                      const std::shared_ptr<ActionExecutor>& action_executor,
+                      BaseAction::Front front = BaseAction::FRONT);
 
         void Start(rclcpp::Node::SharedPtr node) override;
         void Update() override;
@@ -25,7 +26,9 @@ namespace Modelec {
             TAKE,
             UP,
             DONE,
-        } step_;
+        };
+
+        BaseAction::Front front_;
 
         std::shared_ptr<BoxObstacle> closestBox;
         MissionStatus status_;
@@ -35,5 +38,9 @@ namespace Modelec {
 
         rclcpp::Time go_timeout_;
         rclcpp::Time deploy_time_;
+
+        std::optional<rclcpp::Time> min_time_;
+
+        rclcpp::Time last_ask_waypoint_time_;
     };
 }

src/modelec_strat/include/modelec_strat/navigation_helper.hpp

@@ -60,17 +60,17 @@ namespace Modelec
 
         bool HasArrived() const;
 
-        bool RotateTo(const PosMsg::SharedPtr& pos);
+        bool RotateTo(const PosMsg::SharedPtr& pos, bool front = true);
-        bool RotateTo(const Point& pos);
+        bool RotateTo(const Point& pos, bool front = true);
         void Rotate(double angle);
 
         int GoTo(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
         int GoTo(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE);
         int GoTo(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
 
-        int GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
+        int GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE, bool front = true);
-        int GoToRotateFirst(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE);
+        int GoToRotateFirst(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE, bool front = true);
-        int GoToRotateFirst(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
+        int GoToRotateFirst(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE, bool front = true);
 
         int CanGoTo(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
         int CanGoTo(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE);

src/modelec_strat/include/modelec_strat/strat_fms.hpp

@@ -62,6 +62,7 @@ namespace Modelec
         int team_id_ = 0;
 
         std::queue<State> game_action_sequence_;
+        bool static_strat_ = false;
 
         std::shared_ptr<NavigationHelper> nav_;
         std::shared_ptr<ActionExecutor> action_executor_;

src/modelec_strat/src/action/down_action.cpp

@@ -35,12 +35,12 @@ void Modelec::DownAction::Next()
             {
                 msg.items[0].id = 0;
                 msg.items[0].start_angle = 1.95;
-                msg.items[0].end_angle = 2.95;
+                msg.items[0].end_angle = 3;
                 msg.items[0].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].end_angle = 0.85;
                 msg.items[1].duration_s = 1;
 
                 msg.items[2].id = 2;
@@ -50,7 +50,7 @@ void Modelec::DownAction::Next()
 
                 msg.items[3].id = 3;
                 msg.items[3].start_angle = 2.7;
-                msg.items[3].end_angle = 3;
+                msg.items[3].end_angle = 2.9;
                 msg.items[3].duration_s = 1;
             }
 

src/modelec_strat/src/action/free_action.cpp

@@ -8,17 +8,17 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_ex
     steps_.push(ActionExec::DONE_STEP);
 }
 
-Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n) : FreeAction(action_executor)
+Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front, int n) : FreeAction(action_executor)
 {
     AddServo(n, front);
 }
 
-Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo) : FreeAction(action_executor)
+Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Front> 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)
+Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Front>> servos) : FreeAction(action_executor)
 {
     AddServos(servos);
 }
@@ -44,9 +44,9 @@ void Modelec::FreeAction::Next()
 
             for (size_t i = 0; i < servos_.size(); i++)
             {
-                msg.items[i].id = servos_[i].first + (servos_[i].second ? 3 : 11);
+                msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12);
-                msg.items[i].start_angle = servos_[i].second ? 0.8 : 0;
+                msg.items[i].start_angle = servos_[i].second ? 3 : 0;
-                msg.items[i].end_angle = servos_[i].second ? 2.5 : 0;
+                msg.items[i].end_angle = servos_[i].second ? 1 : 0;
                 msg.items[i].duration_s = 0.5;
             }
 
@@ -71,22 +71,22 @@ void Modelec::FreeAction::Init(const std::vector<std::string>& params)
         {
             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);
+            AddServo(id, front ? FRONT : BACK);
         }
     }
 }
 
-void Modelec::FreeAction::AddServo(int id, bool front)
+void Modelec::FreeAction::AddServo(int id, Front front)
 {
     servos_.emplace_back(id, front);
 }
 
-void Modelec::FreeAction::AddServo(std::pair<int, bool> servo)
+void Modelec::FreeAction::AddServo(std::pair<int, Front> servo)
 {
     servos_.emplace_back(servo);
 }
 
-void Modelec::FreeAction::AddServos(const std::vector<std::pair<int, bool>>& servos)
+void Modelec::FreeAction::AddServos(const std::vector<std::pair<int, Front>>& servos)
 {
     servos_.insert(servos_.end(), servos.begin(), servos.end());
 }

src/modelec_strat/src/action/take_action.cpp

@@ -6,17 +6,17 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_ex
 {
 }
 
-Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n) : TakeAction(action_executor)
+Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front, int n) : TakeAction(action_executor)
 {
     AddServo(n, front);
 }
 
-Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo) : TakeAction(action_executor)
+Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Front> 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)
+Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Front>> servos) : TakeAction(action_executor)
 {
     AddServos(servos);
 }
@@ -42,9 +42,9 @@ void Modelec::TakeAction::Next()
 
             for (size_t i = 0; i < servos_.size(); i++)
             {
-                msg.items[i].id = servos_[i].first + (servos_[i].second ? 3 : 11);
+                msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12);
-                msg.items[i].start_angle = servos_[i].second ? 0.8 : 0;
+                msg.items[i].start_angle = servos_[i].second ? 1 : 0;
-                msg.items[i].end_angle = servos_[i].second ? 2.7 : 0;
+                msg.items[i].end_angle = servos_[i].second ? 3 : 0;
                 msg.items[i].duration_s = 0.5;
             }
 
@@ -69,22 +69,22 @@ void Modelec::TakeAction::Init(const std::vector<std::string>& params)
         {
             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);
+            AddServo(id, front ? FRONT : BACK);
         }
     }
 }
 
-void Modelec::TakeAction::AddServo(int id, bool front)
+void Modelec::TakeAction::AddServo(int id, Front front)
 {
     servos_.emplace_back(id, front);
 }
 
-void Modelec::TakeAction::AddServo(std::pair<int, bool> servo)
+void Modelec::TakeAction::AddServo(std::pair<int, Front> servo)
 {
     servos_.emplace_back(servo);
 }
 
-void Modelec::TakeAction::AddServos(const std::vector<std::pair<int, bool>>& servos)
+void Modelec::TakeAction::AddServos(const std::vector<std::pair<int, Front>>& servos)
 {
     servos_.insert(servos_.end(), servos.begin(), servos.end());
 }

src/modelec_strat/src/action/up_action.cpp

@@ -34,12 +34,12 @@ void Modelec::UPAction::Next()
             if (front_ == FRONT || front_ == BOTH)
             {
                 msg.items[0].id = 0;
-                msg.items[0].start_angle = 2.95;
+                msg.items[0].start_angle = 3;
                 msg.items[0].end_angle = 1.95;
                 msg.items[0].duration_s = 1;
 
                 msg.items[1].id = 1;
-                msg.items[1].start_angle = 0.9;
+                msg.items[1].start_angle = 0.85;
                 msg.items[1].end_angle = 1.9;
                 msg.items[1].duration_s = 1;
 
@@ -49,8 +49,8 @@ void Modelec::UPAction::Next()
                 msg.items[2].duration_s = 1;
 
                 msg.items[3].id = 3;
-                msg.items[3].start_angle = 3;
+                msg.items[3].start_angle = 2.9;
-                msg.items[3].end_angle = 2.5;
+                msg.items[3].end_angle = 2.7;
                 msg.items[3].duration_s = 1;
             }
 

src/modelec_strat/src/action_executor.cpp

@@ -96,11 +96,11 @@ namespace Modelec
                     }
                     else if (msg->buttons[3] == 1) // X button
                     {
-                        Take({{0, true}, {1, true}, {2, true}, {3, true}});
+                        Take({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}});
                     }
                     else if (msg->buttons[4] == 1) // Y button
                     {
-                        Free({{0, true}, {1, true}, {2, true}, {3, true}});
+                        Free({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}});
                     }
                 }
             });
@@ -177,7 +177,7 @@ namespace Modelec
         }
     }
 
-    void ActionExecutor::Take(std::vector<std::pair<int, bool>> servos, bool force) {
+    void ActionExecutor::Take(const std::vector<std::pair<int, BaseAction::Front>>& servos, bool force) {
         if (action_done_ || force)
         {
             action_ = std::make_shared<TakeAction>(shared_from_this(), servos);
@@ -191,7 +191,7 @@ namespace Modelec
         }
     }
 
-    void ActionExecutor::Free(std::vector<std::pair<int, bool>> servos, bool force) {
+    void ActionExecutor::Free(const std::vector<std::pair<int, BaseAction::Front>>& servos, bool force) {
         if (action_done_ || force)
         {
             action_ = std::make_shared<FreeAction>(shared_from_this(), servos);
@@ -222,4 +222,34 @@ namespace Modelec
         servo_move_pub_->publish(msg);
         step_running_ += msg.items.size();
     }
+
+    bool ActionExecutor::IsEmpty() const
+    {
+        return box_obstacles_[0] == nullptr && box_obstacles_[1] == nullptr;
+    }
+
+    bool ActionExecutor::IsFull() const
+    {
+        return box_obstacles_[0] != nullptr && box_obstacles_[1] != nullptr;
+    }
+
+    bool ActionExecutor::HasBox(BaseAction::Front front) const
+    {
+        return box_obstacles_[front] != nullptr;
+    }
+
+    bool ActionExecutor::HasFrontBox() const
+    {
+        return HasBox(BaseAction::FRONT);
+    }
+
+    bool ActionExecutor::HasBackBox() const
+    {
+        return HasBox(BaseAction::BACK);
+    }
+
+    bool ActionExecutor::HasOneBox() const
+    {
+        return HasFrontBox() != HasBackBox();
+    }
 }

src/modelec_strat/src/deposite_zone.cpp

@@ -21,8 +21,7 @@ namespace Modelec
             {
                 for (auto takePos = TakeAngleElem->FirstChildElement("Angle"); takePos; takePos = takePos->NextSiblingElement("Angle"))
                 {
-                    double angle;
+                    double angle = takePos->DoubleText(0);
-                    takePos->QueryDoubleAttribute("value", &angle);
                     take_angle_.push_back(angle);
                 }
             }
@@ -48,8 +47,8 @@ namespace Modelec
         for (const auto& angle : take_angle_)
         {
             Point p = Point(
-                static_cast<int>(position_.x + dist * std::cos(angle)),
+                position_.x + dist * std::cos(angle),
-                static_cast<int>(position_.y + dist * std::sin(angle)),
+                position_.y + dist * std::sin(angle),
                 angle);
 
             double distance = p.distance(currentPos);

src/modelec_strat/src/missions/free_mission.cpp

@@ -3,8 +3,11 @@
 #include "modelec_strat/action/base_action.hpp"
 
 namespace Modelec {
-    FreeMission::FreeMission(const std::shared_ptr<NavigationHelper>& nav, const std::shared_ptr<ActionExecutor>& action_executor)
+    FreeMission::FreeMission(const std::shared_ptr<NavigationHelper>& nav,
-     : step_(GO_TO_FREE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)  {
+        const std::shared_ptr<ActionExecutor>& action_executor,
+        BaseAction::Front front)
+     : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
+    {
     }
 
     void FreeMission::Start(rclcpp::Node::SharedPtr node)
@@ -12,9 +15,20 @@ namespace Modelec {
         node_ = node;
 
         go_timeout_ = node_->now();
+        deploy_time_ = node_->now();
+        last_ask_waypoint_time_ = node_->now();
 
         status_ = MissionStatus::RUNNING;
-        step_ = GO_TO_FREE;
+
+        std::queue<int> empty;
+        std::swap(steps_, empty);
+
+        steps_.push(GO_TO_FREE);
+        steps_.push(DOWN);
+        steps_.push(FREE);
+        steps_.push(UP);
+        steps_.push(GO_BACK);
+        steps_.push(DONE);
     }
 
     void FreeMission::Update()
@@ -26,10 +40,11 @@ namespace Modelec {
 
         if (!nav_->HasArrived())
         {
-            if ((node_->now() - go_timeout_).seconds() < 5)
+            if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1)
             {
-                // nav_->AskWaypoint();
+                nav_->AskWaypoint();
-                // return;
+                last_ask_waypoint_time_ = node_->now();
+                return;
             }
             if ((node_->now() - go_timeout_).seconds() < 10)
             {
@@ -37,6 +52,22 @@ namespace Modelec {
             }
         }
 
+        if (min_time_.has_value())
+        {
+            if ((node_->now() - min_time_.value()).seconds() < 0.1)
+            {
+                return;
+            }
+            else
+            {
+                min_time_.reset();
+            }
+        }
+
+
+		auto step_ = steps_.front();
+		steps_.pop();
+
         switch (step_)
         {
         case GO_TO_FREE:
@@ -50,19 +81,30 @@ namespace Modelec {
 
                 auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta));
 
-                if (nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist), true, Pathfinding::FREE))
+                auto pos = depoPoint.GetTakePosition(dist);
+
+                RCLCPP_INFO(
+                    node_->get_logger(),
+                    "FreeMission: position (%.2d, %.2d) with distance %.2f",
+                    pos.x, pos.y, dist);
+
+                pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI;
+
+                if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, front_ == BaseAction::FRONT) != Pathfinding::FREE)
                 {
-                    nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist), true, Pathfinding::FREE | Pathfinding::OBSTACLE);
+                    if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE)
+                    {
+                        status_ = MissionStatus::FAILED;
+                        return;
+                    }
                 }
 
                 go_timeout_ = node_->now();
             }
-
-            step_ = FREE;
             break;
         case DOWN:
             {
-                action_executor_->Down(BaseAction::FRONT);
+                action_executor_->Down(front_);
                 deploy_time_ = node_->now();
             }
 
@@ -70,42 +112,50 @@ namespace Modelec {
             break;
         case FREE:
             {
-                action_executor_->Free({{0, true}, {1, true}, {2, true}, {3, true}});
+                action_executor_->Free({{0, front_}, {1, front_}, {2, front_}, {3, front_}});
                 deploy_time_ = node_->now();
 
-                auto obs = action_executor_->box_obstacles_[0];
+                auto obs = action_executor_->box_obstacles_[front_];
-                action_executor_->box_obstacles_[0] = nullptr;
+                action_executor_->box_obstacles_[front_] = nullptr;
 
                 auto pos = nav_->GetCurrentPos();
 
                 obs->SetPosition(
-                    pos->x + 250 * cos(pos->theta),
+                    pos->x + 200 * cos(pos->theta + (front_ == BaseAction::FRONT ? 0 : M_PI)),
-                    pos->y + 250 * sin(pos->theta),
+                    pos->y + 200 * sin(pos->theta + (front_ == BaseAction::FRONT ? 0 : M_PI)),
                     pos->theta);
 
                 obs->SetAtObjective(true);
 
                 nav_->GetPathfinding()->AddObstacle(obs);
-            }
 
-            step_ = UP;
+                min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5);
+            }
             break;
         case UP:
             {
-                action_executor_->Up(BaseAction::FRONT);
+                action_executor_->Up(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);
+                auto currPos = nav_->GetCurrentPos();
+
+                auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta));
+
+                auto pos = depoPoint.GetTakePosition(300);
+                pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI;
+
+                if (nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE) != Pathfinding::FREE)
+                {
+                    status_ = MissionStatus::FAILED;
+                    return;
+                }
 
                 go_timeout_ = node_->now();
             }
 
-            step_ = DONE;
             break;
         case DONE:
             {

src/modelec_strat/src/missions/go_home_mission.cpp

@@ -5,7 +5,7 @@
 namespace Modelec
 {
     GoHomeMission::GoHomeMission(const std::shared_ptr<NavigationHelper>& nav, const rclcpp::Time& start_time) :
-        step_(ROTATE_TO_HOME), status_(MissionStatus::READY), nav_(nav), start_time_(start_time)
+        status_(MissionStatus::READY), nav_(nav), start_time_(start_time)
     {
     }
 
@@ -18,18 +18,27 @@ namespace Modelec
         score_pub_ = node_->create_publisher<std_msgs::msg::Int64>("/strat/score", 10);
 
         go_timeout_ = node_->now();
+        last_ask_waypoint_time_ = node_->now();
 
         status_ = MissionStatus::RUNNING;
-        step_ = ROTATE_TO_HOME;
+
+        std::queue<int> empty;
+        std::swap(steps_, empty);
+
+        steps_.push(ROTATE_TO_HOME);
+        steps_.push(GO_HOME);
+        steps_.push(GO_CLOSE);
+        steps_.push(DONE);
     }
 
     void GoHomeMission::Update()
     {
         if (!nav_->HasArrived())
         {
-            if ((node_->now() - go_timeout_).seconds() < 2)
+            if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1)
             {
-                // nav_->AskWaypoint();
+                nav_->AskWaypoint();
+                last_ask_waypoint_time_ = node_->now();
                 return;
             }
             if ((node_->now() - go_timeout_).seconds() < 10)
@@ -38,6 +47,21 @@ namespace Modelec
             }
         }
 
+        if (min_time_.has_value())
+        {
+            if ((node_->now() - min_time_.value()).seconds() < 0.1)
+            {
+                return;
+            }
+            else
+            {
+                min_time_.reset();
+            }
+        }
+
+        auto step_ = steps_.front();
+        steps_.pop();
+
         switch (step_)
         {
         case ROTATE_TO_HOME:
@@ -55,8 +79,6 @@ namespace Modelec
                 nav_->RotateTo(home_point_);
 
                 go_timeout_ = node_->now();
-
-                step_ = GO_HOME;
             }
             break;
         case GO_HOME:
@@ -71,23 +93,18 @@ namespace Modelec
                 }
 
                 go_timeout_ = node_->now();
-
-                step_ = GO_CLOSE;
             }
             break;
         case GO_CLOSE:
             {
-                if ((node_->now() - start_time_).seconds() < 94)
+                /*if ((node_->now() - start_time_).seconds() < 94)
                 {
                     break;
-                }
+                }*/
 
                 nav_->GoTo(home_point_, true);
 
                 go_timeout_ = node_->now();
-
-                step_ = DONE;
-
             }
             break;
         case DONE:

src/modelec_strat/src/missions/take_mission.cpp

@@ -3,8 +3,11 @@
 #include "modelec_strat/action/base_action.hpp"
 
 namespace Modelec {
-    TakeMission::TakeMission(const std::shared_ptr<NavigationHelper>& nav, const std::shared_ptr<ActionExecutor>& action_executor)
+    TakeMission::TakeMission(const std::shared_ptr<NavigationHelper>& nav,
-     : step_(GO_TO_TAKE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)  {
+        const std::shared_ptr<ActionExecutor>& action_executor,
+        BaseAction::Front front)
+     : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
+    {
     }
 
     void TakeMission::Start(rclcpp::Node::SharedPtr node)
@@ -12,9 +15,20 @@ namespace Modelec {
         node_ = node;
 
         go_timeout_ = node_->now();
+        deploy_time_ = node_->now();
+        last_ask_waypoint_time_ = node_->now();
 
         status_ = MissionStatus::RUNNING;
-        step_ = GO_TO_TAKE;
+
+        std::queue<int> empty;
+        std::swap(steps_, empty);
+
+        steps_.push(GO_TO_TAKE);
+        steps_.push(GO_TO_TAKE_CLOSE);
+        steps_.push(DOWN);
+        steps_.push(TAKE);
+        steps_.push(UP);
+        steps_.push(DONE);
     }
 
     void TakeMission::Update()
@@ -26,10 +40,11 @@ namespace Modelec {
 
         if (!nav_->HasArrived())
         {
-            if ((node_->now() - go_timeout_).seconds() < 5)
+            if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1)
             {
-                // nav_->AskWaypoint();
+                nav_->AskWaypoint();
-                // return;
+                last_ask_waypoint_time_ = node_->now();
+                return;
             }
             if ((node_->now() - go_timeout_).seconds() < 10)
             {
@@ -37,6 +52,21 @@ namespace Modelec {
             }
         }
 
+        if (min_time_.has_value())
+        {
+            if ((node_->now() - min_time_.value()).seconds() < 0.1)
+            {
+                return;
+            }
+            else
+            {
+                min_time_.reset();
+            }
+        }
+
+        auto step_ = steps_.front();
+        steps_.pop();
+
         switch (step_)
         {
         case GO_TO_TAKE:
@@ -46,58 +76,76 @@ namespace Modelec {
 
                 action_executor_->box_obstacles_[0] = closestBox;
 
-                auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
+                action_executor_->box_obstacles_[front_] = closestBox;
 
-                if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL))
+                auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
+                pos.theta += (front_ == BaseAction::FRONT ? 0 : M_PI);
+
+                if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE)
                 {
-                    if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL))
+                    if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE)
                     {
-                        nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
+                        if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE)
+                        {
+                            status_ = MissionStatus::FAILED;
+                            break;
+                        }
                     }
                 }
 
                 go_timeout_ = node_->now();
             }
-
-            step_ = GO_TO_TAKE_CLOSE;
             break;
         case GO_TO_TAKE_CLOSE:
             {
-                auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition();
+                if (action_executor_->box_obstacles_[front_] == nullptr)
+                {
+                    status_ = MissionStatus::FAILED;
+                    break;
+                }
 
-                nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
+                auto pos = action_executor_->box_obstacles_[front_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition();
+                pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI;
+
+                if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE)
+                {
+                    status_ = MissionStatus::FAILED;
+                    break;
+                }
 
                 go_timeout_ = node_->now();
             }
-
-            step_ = DOWN;
             break;
         case DOWN:
             {
-                action_executor_->Down(BaseAction::FRONT);
+                action_executor_->Down(front_);
                 deploy_time_ = node_->now();
             }
-
-            step_ = TAKE;
             break;
         case TAKE:
             {
-                action_executor_->Take({{0, true}, {1, true}, {2, true}, {3, true}});
+                action_executor_->Take({{0, front_}, {1, front_}, {2, front_}, {3, front_}});
                 deploy_time_ = node_->now();
+                min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5);
             }
-
-            step_ = UP;
             break;
         case UP:
             {
-                action_executor_->Up(BaseAction::FRONT);
+                action_executor_->Up(front_);
                 deploy_time_ = node_->now();
             }
-
-            step_ = DONE;
             break;
         case DONE:
-            nav_->GetPathfinding()->RemoveObstacle(closestBox->GetId());
+            {
+                if (action_executor_->box_obstacles_[front_] == nullptr)
+                {
+                    status_ = MissionStatus::FAILED;
+                    break;
+                }
+
+                nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[front_]->GetId());
+            }
+
             status_ = MissionStatus::DONE;
             break;
         default:

src/modelec_strat/src/navigation_helper.cpp

@@ -286,11 +286,11 @@ namespace Modelec
         return waypoint_queue_.empty() && waypoints_.back().reached;
     }
 
-    bool NavigationHelper::RotateTo(const PosMsg::SharedPtr& pos)
+    bool NavigationHelper::RotateTo(const PosMsg::SharedPtr& pos, bool front)
     {
         double angle = std::atan2(pos->y - current_pos_->y, pos->x - current_pos_->x);
 
-        if (std::abs(angle - current_pos_->theta) > M_PI / 3)
+        if (std::abs(angle - (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4)
         {
             Rotate(angle);
             return true;
@@ -298,11 +298,11 @@ namespace Modelec
         return false;
     }
 
-    bool NavigationHelper::RotateTo(const Point& pos)
+    bool NavigationHelper::RotateTo(const Point& pos, bool front)
     {
         double angle = std::atan2(pos.y - current_pos_->y, pos.x - current_pos_->x);
 
-        if (std::abs(angle - current_pos_->theta) > M_PI / 4)
+        if (std::abs(angle - (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4)
         {
             Rotate(angle);
             return true;
@@ -364,7 +364,7 @@ namespace Modelec
         return GoTo(goal.x, goal.y, goal.theta, isClose, collisionMask);
     }
 
-    int NavigationHelper::GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask)
+    int NavigationHelper::GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask, bool front)
     {
         last_go_to_ = {goal, isClose, collisionMask};
 
@@ -376,7 +376,7 @@ namespace Modelec
         }
 
         auto p = Point(wl[0].x, wl[0].y, wl[0].theta);
-        if (RotateTo(p))
+        if (RotateTo(p, front))
         {
             await_rotate_ = true;
 
@@ -403,18 +403,18 @@ namespace Modelec
         return res;
     }
 
-    int NavigationHelper::GoToRotateFirst(int x, int y, double theta, bool isClose, int collisionMask)
+    int NavigationHelper::GoToRotateFirst(int x, int y, double theta, bool isClose, int collisionMask, bool front)
     {
         PosMsg::SharedPtr goal = std::make_shared<PosMsg>();
         goal->x = x;
         goal->y = y;
         goal->theta = theta;
-        return GoToRotateFirst(goal, isClose, collisionMask);
+        return GoToRotateFirst(goal, isClose, collisionMask, front);
     }
 
-    int NavigationHelper::GoToRotateFirst(const Point& goal, bool isClose, int collisionMask)
+    int NavigationHelper::GoToRotateFirst(const Point& goal, bool isClose, int collisionMask, bool front)
     {
-        return GoToRotateFirst(goal.x, goal.y, goal.theta, isClose, collisionMask);
+        return GoToRotateFirst(goal.x, goal.y, goal.theta, isClose, collisionMask, front);
     }
 
     int NavigationHelper::CanGoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask)

src/modelec_strat/src/obstacle/box.cpp

@@ -45,7 +45,7 @@ namespace Modelec
             }
         }
 
-        return Point(x_, y_, optimizedAngle);
+        return {x_, y_, optimizedAngle};
     }
 
     std::vector<Point> BoxObstacle::GetAllPossiblePositions() const

src/modelec_strat/src/strat_fms.cpp

@@ -67,9 +67,10 @@ namespace Modelec
         }
 
         game_action_sequence_.push(State::TAKE_MISSION);
-        game_action_sequence_.push(State::FREE_MISSION);
         game_action_sequence_.push(State::TAKE_MISSION);
         game_action_sequence_.push(State::FREE_MISSION);
+        game_action_sequence_.push(State::FREE_MISSION);
+		static_strat_ = true;
     }
 
     void StratFMS::Init()
@@ -145,8 +146,8 @@ namespace Modelec
 
                 action_executor_->Up(BaseAction::Front::BOTH, true);
                 action_executor_->Free({
-                    {0, true}, {1, true}, {2, true}, {3, true},
+                    {0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT},
-                    {0, false}, {1, false}, {2, false}, {3, false},
+                    {0, BaseAction::BACK}, {1, BaseAction::BACK}, {2, BaseAction::BACK}, {3, BaseAction::BACK},
                 }, true);
 
                 Transition(State::WAIT_START, "System ready");
@@ -178,7 +179,11 @@ namespace Modelec
                     Transition(State::STOP, "All missions done");
                 }
 
-                else if (elapsed.seconds() < 70)
+                else if (elapsed.seconds() > 60 && !action_executor_->IsEmpty())
+                {
+                    Transition(State::FREE_MISSION, "No Time left, freeing boxes");
+                }
+                else if (elapsed.seconds() < 80)
                 {
                     Transition(State::SELECT_GAME_ACTION, "Selecting a game action");
                 }
@@ -191,15 +196,74 @@ namespace Modelec
             }
         case State::SELECT_GAME_ACTION:
             {
-                if (game_action_sequence_.empty())
+
+				if (static_strat_) {
+					if (game_action_sequence_.empty()) {
+						Transition(State::DO_GO_HOME, "No more game actions in sequence");
+						return;
+					}
+
+					auto next_action = game_action_sequence_.front();
+					game_action_sequence_.pop();
+
+					Transition(next_action, "Selecting next game action from sequence");
+					return;
+				}
+
+
+                // TODO : If close to border, do the side mission (thermometre)
+
+                if (action_executor_->IsFull())
                 {
-                    Transition(State::DO_GO_HOME, "No more game actions");
+                    RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission");
+                    Transition(State::FREE_MISSION, "Selecting FREE mission");
+                }
+                else if (action_executor_->IsEmpty())
+                {
+                    RCLCPP_INFO(get_logger(), "No box present on robot, selecting TAKE mission");
+                    Transition(State::TAKE_MISSION, "Selecting TAKE mission");
                 }
                 else
                 {
-                    auto action = game_action_sequence_.front();
+                    auto pos = nav_->GetCurrentPos();
-                    game_action_sequence_.pop();
+                    auto closestBox = nav_->GetClosestObstacle<BoxObstacle>(pos);
-                    Transition(action, "Selected game action");
+                    auto closestDeposite = nav_->GetClosestDepositeZone(pos);
+
+                    if (closestBox && closestDeposite)
+                    {
+                        double distToBox = Point::distance(Point(pos->x, pos->y, pos->theta),
+                                                          closestBox->GetPosition());
+                        double distToDeposite = Point::distance(Point(pos->x, pos->y, pos->theta),
+                                                               closestDeposite->GetPosition());
+
+                        if (distToBox < distToDeposite)
+                        {
+                            RCLCPP_INFO(get_logger(), "Choosing TAKE mission (dist to box: %.2f < dist to deposite: %.2f)",
+                                        distToBox, distToDeposite);
+                            Transition(State::TAKE_MISSION, "Selecting TAKE mission");
+                        }
+                        else
+                        {
+                            RCLCPP_INFO(get_logger(), "Choosing FREE mission (dist to deposite: %.2f < dist to box: %.2f)",
+                                        distToDeposite, distToBox);
+                            Transition(State::FREE_MISSION, "Selecting FREE mission");
+                        }
+                    }
+                    else if (closestBox)
+                    {
+                        RCLCPP_INFO(get_logger(), "Only box available, selecting TAKE mission");
+                        Transition(State::TAKE_MISSION, "Selecting TAKE mission");
+                    }
+                    else if (closestDeposite)
+                    {
+                        RCLCPP_INFO(get_logger(), "Only deposite available, selecting FREE mission");
+                        Transition(State::FREE_MISSION, "Selecting FREE mission");
+                    }
+                    else
+                    {
+                        RCLCPP_WARN(get_logger(), "No box or deposite available, cannot select mission!");
+                        Transition(State::STOP, "No missions available");
+                    }
                 }
             }
 
@@ -207,28 +271,73 @@ namespace Modelec
         case State::TAKE_MISSION:
             if (!current_mission_)
             {
-                current_mission_ = std::make_unique<TakeMission>(nav_, action_executor_);
+                if (action_executor_->HasFrontBox())
-                current_mission_->Start(shared_from_this());
+                {
+                    if (action_executor_->HasBackBox())
+                    {
+                        RCLCPP_WARN(get_logger(), "Both front and back box obstacles are occupied!");
+                        current_mission_.reset();
+                        Transition(State::SELECT_MISSION, "Cannot take box, both sides occupied");
+                        break;
+                    }
+
+                    RCLCPP_INFO(get_logger(), "Front box obstacle is occupied, taking from back");
+                    current_mission_ = std::make_unique<TakeMission>(nav_, action_executor_, BaseAction::BACK);
+                    current_mission_->Start(shared_from_this());
+                }
+                else
+                {
+                    current_mission_ = std::make_unique<TakeMission>(nav_, action_executor_, BaseAction::FRONT);
+                    current_mission_->Start(shared_from_this());
+                }
             }
             current_mission_->Update();
             if (current_mission_->GetStatus() == MissionStatus::DONE)
             {
                 current_mission_.reset();
                 Transition(State::SELECT_MISSION, "Take done");
+            }
+			else if (current_mission_->GetStatus() == MissionStatus::FAILED)
+            {
+                current_mission_.reset();
+                RCLCPP_ERROR(get_logger(), "Take mission failed!");
+                Transition(State::SELECT_MISSION, "Take mission failed");
             }
             break;
 
         case State::FREE_MISSION:
             if (!current_mission_)
             {
-                current_mission_ = std::make_unique<FreeMission>(nav_, action_executor_);
+                if (!action_executor_->HasFrontBox())
-                current_mission_->Start(shared_from_this());
+                {
+                    if (!action_executor_->HasBackBox())
+                    {
+                        RCLCPP_WARN(get_logger(), "Both front and back box obstacles are free!");
+                        Transition(State::SELECT_MISSION, "Cannot free box, both sides empty");
+                        break;
+                    }
+
+                    RCLCPP_INFO(get_logger(), "Front box obstacle is occupied, taking from back");
+                    current_mission_ = std::make_unique<FreeMission>(nav_, action_executor_, BaseAction::BACK);
+                    current_mission_->Start(shared_from_this());
+                }
+                else
+                {
+                    current_mission_ = std::make_unique<FreeMission>(nav_, action_executor_, BaseAction::FRONT);
+                    current_mission_->Start(shared_from_this());
+                }
             }
             current_mission_->Update();
             if (current_mission_->GetStatus() == MissionStatus::DONE)
             {
                 current_mission_.reset();
                 Transition(State::SELECT_MISSION, "Free done");
+            }
+			else if (current_mission_->GetStatus() == MissionStatus::FAILED)
+            {
+                current_mission_.reset();
+                RCLCPP_ERROR(get_logger(), "Free mission failed!");
+                Transition(State::SELECT_MISSION, "Free mission failed");
             }
             break;
 
@@ -243,6 +352,12 @@ namespace Modelec
             {
                 current_mission_.reset();
                 Transition(State::STOP, "Cleanup done");
+            }
+			else if (current_mission_->GetStatus() == MissionStatus::FAILED)
+            {
+                current_mission_.reset();
+                RCLCPP_ERROR(get_logger(), "Go Home mission failed!");
+                Transition(State::STOP, "Go Home mission failed");
             }
             break;
 

src/modelec_utils/include/modelec_utils/point.hpp

@@ -1,6 +1,6 @@
 #pragma once
 
-#define CLOSE_DISTANCE 150
+#define CLOSE_DISTANCE 165
 #define BASE_DISTANCE 290
 
 namespace Modelec

start_ros2.sh

@@ -2,11 +2,11 @@
 set -e
 
 source /opt/ros/jazzy/setup.bash
-source /home/modelec/modelec-marcel-ROS/install/setup.bash
+source /home/modelec/Modelec-ROS2/install/setup.bash
 
 export RCL_LOG_LEVEL=info
 export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
-export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/modelec-marcel-ROS/fastdds_setup.xml
+export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/Modelec-ROS2/fastdds_setup.xml
 export ROS_DOMAIN_ID=128
 
 exec ros2 launch modelec_core modelec.launch.py "$@"