From f11360a62a32bb0436f136820a7d4866fced2fac Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 19 Nov 2025 18:26:30 +0100 Subject: [PATCH] default mission to test --- src/modelec_core/launch/modelec.launch.py | 2 +- src/modelec_strat/CMakeLists.txt | 2 +- .../include/modelec_strat/action_executor.hpp | 8 ++ .../missions/take_send_mission.hpp | 39 ++++++++++ .../include/modelec_strat/strat_fms.hpp | 3 +- src/modelec_strat/src/action_executor.cpp | 46 +++++++++++ .../src/missions/take_send_mission.cpp | 78 +++++++++++++++++++ src/modelec_strat/src/strat_fms.cpp | 26 +++++++ 8 files changed, 201 insertions(+), 3 deletions(-) create mode 100644 src/modelec_strat/include/modelec_strat/missions/take_send_mission.hpp create mode 100644 src/modelec_strat/src/missions/take_send_mission.cpp diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 00be8b6..288321c 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/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", }] diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index 023ec64..7ad1a6e 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/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 diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 0b864b0..3430b34 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/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::SharedPtr asc_move_pub_; rclcpp::Publisher::SharedPtr servo_move_pub_; diff --git a/src/modelec_strat/include/modelec_strat/missions/take_send_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/take_send_mission.hpp new file mode 100644 index 0000000..c023f35 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/missions/take_send_mission.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include + +namespace Modelec { + class TakeSendMission : public Mission { + public: + TakeSendMission(const std::shared_ptr& nav, + const std::shared_ptr& 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 nav_; + std::shared_ptr action_executor_; + rclcpp::Time go_home_time_; + rclcpp::Node::SharedPtr node_; + + rclcpp::Time go_timeout_; + rclcpp::Time deploy_time_; + }; +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index cd02b81..0a5ccc9 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/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 #include @@ -26,6 +25,8 @@ namespace Modelec WAIT_START, SELECT_MISSION, + TAKE_SEND_MISSION, + DO_GO_HOME, STOP }; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index f43812b..6247e3a 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/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(); + } + } } diff --git a/src/modelec_strat/src/missions/take_send_mission.cpp b/src/modelec_strat/src/missions/take_send_mission.cpp new file mode 100644 index 0000000..97698d3 --- /dev/null +++ b/src/modelec_strat/src/missions/take_send_mission.cpp @@ -0,0 +1,78 @@ +#include + +namespace Modelec { + TakeSendMission::TakeSendMission(const std::shared_ptr& nav, const std::shared_ptr& 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_; + } + +} \ No newline at end of file diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 7e25206..d782cda 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -2,6 +2,9 @@ #include #include +#include +#include + 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(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_) {