new Front Back system

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

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

@@ -9,16 +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 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/up_action.hpp

@@ -9,16 +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 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

@@ -16,16 +16,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);
@@ -38,9 +28,9 @@ namespace Modelec
 
         void ReInit();
 
-        void Down(bool front = true);
+        void Down(BaseAction::Front front);
 
-        void Up(bool front = true);
+        void Up(BaseAction::Front front);
 
         void Take(bool front = true, int n = 0);
 

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

@@ -20,7 +20,9 @@ namespace Modelec {
         enum Step
         {
             GO_TO_FREE,
+            DOWN,
             FREE,
+            UP,
             DONE,
         } step_;
 

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

@@ -20,7 +20,9 @@ namespace Modelec {
         enum Step
         {
             GO_TO_TAKE,
+            DOWN,
             TAKE,
+            UP,
             DONE,
         } step_;
 

src/modelec_strat/src/action/down_action.cpp

@@ -8,7 +8,7 @@ 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;
 }
@@ -29,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;
+            if (front_ == FRONT || front_ == BOTH)
-            msg.items[0].start_angle = front_ ? 1.95 : 0;
+            {
-            msg.items[0].end_angle = front_ ? 2.95 : 0;
+                msg.items[0].id = 0;
-            msg.items[0].duration_s = 1;
+                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].id = 1;
-            msg.items[1].start_angle = front_ ? 1.9 : 0;
+                msg.items[1].start_angle = 0.9;
-            msg.items[1].end_angle = front_ ? 0.9 : 0;
+                msg.items[1].end_angle = 1.9;
-            msg.items[1].duration_s = 1;
+                msg.items[1].duration_s = 1;
 
-            msg.items[2].id = front_ ? 2 : 10;
+                msg.items[2].id = 2;
-            msg.items[2].start_angle = front_ ? 0.3 : 0;
+                msg.items[2].start_angle = 0;
-            msg.items[2].end_angle = front_ ? 0 : 0;
+                msg.items[2].end_angle = 0.3;
-            msg.items[2].duration_s = 1;
+                msg.items[2].duration_s = 1;
 
-            msg.items[3].id = front_ ? 3 : 11;
+                msg.items[3].id = 3;
-            msg.items[3].start_angle = front_ ? 2.7 : 0;
+                msg.items[3].start_angle = 3;
-            msg.items[3].end_angle = front_ ? 3 : 0;
+                msg.items[3].end_angle = 2.7;
-            msg.items[3].duration_s = 1;
+                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);
         }
@@ -68,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/up_action.cpp

@@ -8,7 +8,7 @@ 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;
 }
@@ -29,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;
+            if (front_ == FRONT || front_ == BOTH)
-            msg.items[0].start_angle = front_ ? 2.95 : 0;
+            {
-            msg.items[0].end_angle = front_ ? 1.95 : 0;
+                msg.items[0].id = 0;
-            msg.items[0].duration_s = 1;
+                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].id = 1;
-            msg.items[1].start_angle = front_ ? 0.9 : 0;
+                msg.items[1].start_angle = 0.9;
-            msg.items[1].end_angle = front_ ? 1.9 : 0;
+                msg.items[1].end_angle = 1.9;
-            msg.items[1].duration_s = 1;
+                msg.items[1].duration_s = 1;
 
-            msg.items[2].id = front_ ? 2 : 10;
+                msg.items[2].id = 2;
-            msg.items[2].start_angle = front_ ? 0 : 0;
+                msg.items[2].start_angle = 0;
-            msg.items[2].end_angle = front_ ? 0.3 : 0;
+                msg.items[2].end_angle = 0.3;
-            msg.items[2].duration_s = 1;
+                msg.items[2].duration_s = 1;
 
-            msg.items[3].id = front_ ? 3 : 11;
+                msg.items[3].id = 3;
-            msg.items[3].start_angle = front_ ? 3 : 0;
+                msg.items[3].start_angle = 3;
-            msg.items[3].end_angle = front_ ? 2.5 : 0;
+                msg.items[3].end_angle = 2.5;
-            msg.items[3].duration_s = 1;
+                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);
         }
@@ -68,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

@@ -149,7 +149,7 @@ namespace Modelec
         action_ = nullptr;
     }
 
-    void ActionExecutor::Down(bool front) {
+    void ActionExecutor::Down(BaseAction::front front) {
         if (action_done_)
         {
             action_ = std::make_shared<DownAction>(shared_from_this(), front);
@@ -160,7 +160,7 @@ namespace Modelec
         }
     }
 
-    void ActionExecutor::Up(bool front) {
+    void ActionExecutor::Up(BaseAction::front front) {
         if (action_done_)
         {
             action_ = std::make_shared<UPAction>(shared_from_this(), front);

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,9 +26,9 @@ namespace Modelec {
 
         if (!nav_->HasArrived())
         {
-            if ((node_->now() - go_timeout_).seconds() < 2)
+            if ((node_->now() - go_timeout_).seconds() < 5)
             {
-                // nav_->AskWaypoint();
+                nav_->AskWaypoint();
                 return;
             }
             if ((node_->now() - go_timeout_).seconds() < 10)
@@ -55,17 +57,33 @@ namespace Modelec {
                     angle + M_PI), true, Pathfinding::FREE);
 
                 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();
                 deploy_time_ = node_->now();
-
-                step_ = DONE;
             }
+
+            step_ = UP;
+            break;
+        case UP:
+            {
+                action_executor_->Up(BaseAction::FRONT);
+                deploy_time_ = node_->now();
+            }
+
+            step_ = DONE;
             break;
         case DONE:
             {
@@ -88,4 +106,4 @@ namespace Modelec {
         return status_;
     }
 
-}
+}

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,9 +26,9 @@ namespace Modelec {
 
         if (!nav_->HasArrived())
         {
-            if ((node_->now() - go_timeout_).seconds() < 2)
+            if ((node_->now() - go_timeout_).seconds() < 5)
             {
-                // nav_->AskWaypoint();
+                nav_->AskWaypoint();
                 return;
             }
             if ((node_->now() - go_timeout_).seconds() < 10)
@@ -51,11 +53,27 @@ namespace Modelec {
                 go_timeout_ = node_->now();
             }
 
+            step_ = DOWN;
+            break;
+        case DOWN:
+            {
+                action_executor_->Up(BaseAction::FRONT);
+                deploy_time_ = node_->now();
+            }
+
             step_ = TAKE;
             break;
         case TAKE:
             {
-                action_executor_->Up();
+                action_executor_->Take();
+                deploy_time_ = node_->now();
+            }
+
+            step_ = UP;
+            break;
+        case UP:
+            {
+                action_executor_->Up(BaseAction::FRONT);
                 deploy_time_ = node_->now();
             }
 
@@ -78,4 +96,4 @@ namespace Modelec {
         return status_;
     }
 
-}
+}

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
 {
 
@@ -157,6 +159,8 @@ namespace Modelec
                                .count();
                 start_time_pub_->publish(msg);
 
+                action_executor_->Up(BaseAction::Front::BOTH);
+
                 Transition(State::SELECT_MISSION, "Match started");
             }
             break;