default mission to test

src/modelec_core/launch/modelec.launch.py

@@ -109,7 +109,7 @@ def generate_launch_description():
                     executable='pcb_action_interface',
                     name='pcb_action_interface',
                     parameters=[{
-                        'serial_port': "/dev/USB_ACTION",
+                        'serial_port': "/tmp/USB_ACTION",
                         'baudrate': 115200,
                         'name': "pcb_action",
                     }]

src/modelec_strat/CMakeLists.txt

@@ -15,7 +15,7 @@ find_package(ament_index_cpp REQUIRED)
 find_package(modelec_interfaces REQUIRED)
 find_package(modelec_utils REQUIRED)
 
-add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/missions/go_home_mission.cpp src/obstacle/obstacle.cpp src/obstacle/box.cpp src/deposite_zone.cpp)
+add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/missions/go_home_mission.cpp src/missions/take_send_mission.cpp src/obstacle/obstacle.cpp src/obstacle/box.cpp src/deposite_zone.cpp)
 ament_target_dependencies(strat_fsm rclcpp std_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

src/modelec_strat/include/modelec_strat/action_executor.hpp

@@ -14,10 +14,14 @@ namespace Modelec
         enum Action
         {
             NONE,
+            TAKE,
+            SEND,
         };
 
         enum Step
         {
+            TAKE_STEP,
+            SEND_STEP
         };
 
         ActionExecutor();
@@ -32,6 +36,10 @@ namespace Modelec
 
         void ReInit();
 
+        void Send();
+
+        void Take();
+
     protected:
         rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_pub_;
         rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_pub_;

src/modelec_strat/include/modelec_strat/missions/take_send_mission.hpp

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <modelec_strat/missions/mission_base.hpp>
+#include <modelec_strat/navigation_helper.hpp>
+#include <modelec_strat/action_executor.hpp>
+
+namespace Modelec {
+    class TakeSendMission : public Mission {
+    public:
+        TakeSendMission(const std::shared_ptr<NavigationHelper>& nav,
+                      const std::shared_ptr<ActionExecutor>& action_executor);
+
+        void Start(rclcpp::Node::SharedPtr node) override;
+        void Update() override;
+        void Clear() override;
+        MissionStatus GetStatus() const override;
+        std::string GetName() const override { return "GoHome"; }
+
+    private:
+        enum Step
+        {
+            GO_TO_TAKE,
+            TAKE,
+            WAIT_5S,
+            GO_TO_SEND,
+            SEND,
+            DONE,
+        } step_;
+
+        MissionStatus status_;
+        std::shared_ptr<NavigationHelper> nav_;
+        std::shared_ptr<ActionExecutor> action_executor_;
+        rclcpp::Time go_home_time_;
+        rclcpp::Node::SharedPtr node_;
+
+        rclcpp::Time go_timeout_;
+        rclcpp::Time deploy_time_;
+    };
+}

src/modelec_strat/include/modelec_strat/strat_fms.hpp

@@ -5,7 +5,6 @@
 #include "action_executor.hpp"
 #include "navigation_helper.hpp"
 #include "missions/mission_base.hpp"
-#include "missions/go_home_mission.hpp"
 
 #include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/int64.hpp>
@@ -26,6 +25,8 @@ namespace Modelec
         WAIT_START,
         SELECT_MISSION,
 
+        TAKE_SEND_MISSION,
+
         DO_GO_HOME,
         STOP
     };

src/modelec_strat/src/action_executor.cpp

@@ -70,6 +70,26 @@ namespace Modelec
         {
             switch (step_.front())
             {
+            case SEND_STEP:
+                {
+                    modelec_interfaces::msg::ActionServoPos msg;
+                    msg.id = 5;
+                    msg.pos = 1;
+                    servo_move_pub_->publish(msg);
+
+                    step_running_ = 1;
+                }
+                break;
+            case TAKE_STEP:
+                {
+                    modelec_interfaces::msg::ActionServoPos msg;
+                    msg.id = 6;
+                    msg.pos = 1;
+                    servo_move_pub_->publish(msg);
+
+                    step_running_ = 1;
+                }
+                break;
             default:
                 return;
             }
@@ -88,4 +108,30 @@ namespace Modelec
         }
         action_ = NONE;
     }
+
+    void ActionExecutor::Send() {
+        if (action_done_)
+        {
+            action_ = SEND;
+            action_done_ = false;
+            step_running_ = 0;
+
+            step_.push(SEND_STEP);
+
+            Update();
+        }
+    }
+
+    void ActionExecutor::Take() {
+        if (action_done_)
+        {
+            action_ = TAKE;
+            action_done_ = false;
+            step_running_ = 0;
+
+            step_.push(TAKE_STEP);
+
+            Update();
+        }
+    }
 }

src/modelec_strat/src/missions/take_send_mission.cpp

@@ -0,0 +1,78 @@
+#include <modelec_strat/missions/take_send_mission.hpp>
+
+namespace Modelec {
+    TakeSendMission::TakeSendMission(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)  {
+    }
+
+    void TakeSendMission::Start(rclcpp::Node::SharedPtr node)
+    {
+        node_ = node;
+
+
+        nav_->GoToRotateFirst(500, 1200, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
+
+        go_timeout_ = node_->now();
+
+        status_ = MissionStatus::RUNNING;
+    }
+
+    void TakeSendMission::Update()
+    {
+        if (!action_executor_->IsActionDone())
+        {
+            return;
+        }
+
+        if (!nav_->HasArrived())
+        {
+            if ((node_->now() - go_timeout_).seconds() < 4)
+            {
+                return;
+            }
+        }
+
+        switch (step_)
+        {
+        case GO_TO_TAKE:
+            action_executor_->Take();
+
+            step_ = TAKE;
+            break;
+        case TAKE:
+            if ((node_->now() - deploy_time_).seconds() >= 5)
+            {
+                step_ = WAIT_5S;
+            }
+
+            break;
+        case WAIT_5S:
+            nav_->GoToRotateFirst(1100, 500, 0, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
+
+            step_ = GO_TO_SEND;
+            break;
+        case GO_TO_SEND:
+            action_executor_->Send();
+
+            step_ = SEND;
+            break;
+        case SEND:
+
+            step_ = DONE;
+            status_ = MissionStatus::DONE;
+            break;
+        default:
+            break;
+        }
+    }
+
+    void TakeSendMission::Clear()
+    {
+    }
+
+    MissionStatus TakeSendMission::GetStatus() const
+    {
+        return status_;
+    }
+
+}

src/modelec_strat/src/strat_fms.cpp

@@ -2,6 +2,9 @@
 #include <modelec_utils/config.hpp>
 #include <modelec_strat/strat_fms.hpp>
 
+#include <modelec_strat/missions/go_home_mission.hpp>
+#include <modelec_strat/missions/take_send_mission.hpp>
+
 namespace Modelec
 {
 
@@ -156,6 +159,15 @@ namespace Modelec
                     Transition(State::STOP, "All missions done");
                 }
 
+                if (elapsed.seconds() < 70)
+                {
+                    Transition(State::TAKE_SEND_MISSION, "Proceed to concert");
+                }
+                else
+                {
+                    Transition(State::DO_GO_HOME, "Cleanup and finish match");
+                }
+
                 /*if (elapsed.seconds() >= 2)
                 {
                     Transition(State::DO_GO_HOME, "Go Home");
@@ -169,6 +181,20 @@ namespace Modelec
                 break;
             }
 
+        case State::TAKE_SEND_MISSION:
+            if (!current_mission_)
+            {
+                current_mission_ = std::make_unique<TakeSendMission>(nav_, action_executor_);
+                current_mission_->Start(shared_from_this());
+            }
+            current_mission_->Update();
+            if (current_mission_->GetStatus() == MissionStatus::DONE)
+            {
+                current_mission_.reset();
+                Transition(State::SELECT_MISSION, "Take and Send done");
+            }
+            break;
+
         case State::DO_GO_HOME:
             if (!current_mission_)
             {