From 5526e1694c71cccfab5540d7b193cd81f5278e0b Mon Sep 17 00:00:00 2001 From: acki Date: Sun, 4 May 2025 01:47:49 -0400 Subject: [PATCH] start to be very nice :) --- .../launch/test.no_strat.modelec.launch.yml | 26 ++++++++++++++ src/modelec_strat/data/config.xml | 5 +++ .../missions/go_home_mission.hpp | 13 +++++-- .../modelec_strat/missions/mission_base.hpp | 3 ++ .../missions/prepare_concert_mission.hpp | 1 + .../missions/promotion_mission.hpp | 4 +-- .../modelec_strat/navigation_helper.hpp | 14 ++++++++ .../include/modelec_strat/strat_fms.hpp | 1 + .../src/missions/go_home_mission.cpp | 33 ++++++++++++++--- .../src/missions/prepare_concert_mission.cpp | 24 +++++++++++-- .../src/missions/promotion_mission.cpp | 4 +++ src/modelec_strat/src/navigation_helper.cpp | 36 +++++++++++++++++++ src/modelec_strat/src/pathfinding.cpp | 15 ++++++-- src/modelec_strat/src/strat_fms.cpp | 8 ++++- .../include/modelec_utils/point.hpp | 1 + src/modelec_utils/src/point.cpp | 5 +++ 16 files changed, 179 insertions(+), 14 deletions(-) create mode 100644 src/modelec_core/launch/test.no_strat.modelec.launch.yml diff --git a/src/modelec_core/launch/test.no_strat.modelec.launch.yml b/src/modelec_core/launch/test.no_strat.modelec.launch.yml new file mode 100644 index 0000000..b122144 --- /dev/null +++ b/src/modelec_core/launch/test.no_strat.modelec.launch.yml @@ -0,0 +1,26 @@ +launch: + +- node: + pkg: 'modelec_com' + exec: "serial_listener" + name: "serial_listener" + +- node: + pkg: 'modelec_com' + exec: 'pcb_odo_interface' + name: 'pcb_odo_interface' + +- node: + pkg: 'modelec_com' + exec: 'pcb_alim_interface' + name: 'pcb_alim_interface' + +- node: + pkg: 'modelec_gui' + exec: "modelec_gui" + name: "modelec_gui" + +- node: + pkg: 'modelec_core' + exec: 'speed_result' + name: 'speed_result' diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 34d98fa..7b52919 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -28,4 +28,9 @@ 2000 + + + + + diff --git a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp index 6be662f..17fb1ec 100644 --- a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp @@ -1,24 +1,33 @@ #pragma once +#include #include -#include "mission_base.hpp" namespace Modelec { class GoHomeMission : public Mission { public: - GoHomeMission(const std::shared_ptr& nav); + GoHomeMission(const std::shared_ptr& nav, const rclcpp::Time& start_time); void start(rclcpp::Node::SharedPtr node) override; void update() override; + void clear() override; MissionStatus getStatus() const override; std::string name() const override { return "GoHome"; } private: + enum Step { + GO_HOME, + GO_CLOSE, + DONE + } step_; + MissionStatus status_; std::shared_ptr nav_; rclcpp::Node::SharedPtr node_; + rclcpp::Time start_time_; + Point home_point_; }; } diff --git a/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp b/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp index 0a76550..a0ac3e7 100644 --- a/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp @@ -3,6 +3,7 @@ #include #include + namespace Modelec { enum class MissionStatus @@ -10,6 +11,7 @@ namespace Modelec READY, RUNNING, DONE, + FINISH_ALL, FAILED }; @@ -19,6 +21,7 @@ namespace Modelec virtual ~Mission() = default; virtual void start(rclcpp::Node::SharedPtr node) = 0; virtual void update() = 0; + virtual void clear() = 0; virtual MissionStatus getStatus() const = 0; virtual std::string name() const = 0; }; diff --git a/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp index c96fe84..eddbab0 100644 --- a/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp @@ -12,6 +12,7 @@ namespace Modelec { void start(rclcpp::Node::SharedPtr node) override; void update() override; + void clear() override; MissionStatus getStatus() const override; std::string name() const override { return "PrepareConcert"; } diff --git a/src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp index 571ca2b..d4a0c7b 100644 --- a/src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp @@ -1,9 +1,8 @@ #pragma once +#include #include -#include "mission_base.hpp" - namespace Modelec { /* @@ -17,6 +16,7 @@ namespace Modelec void start(rclcpp::Node::SharedPtr node) override; void update() override; + void clear() override; MissionStatus getStatus() const override; std::string name() const override { return "Promotion"; } diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index e0afadc..b8d20a1 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -12,6 +12,12 @@ namespace Modelec { class NavigationHelper { public: + enum + { + YELLOW = 0, + BLUE = 1, + }; + NavigationHelper(); NavigationHelper(const rclcpp::Node::SharedPtr& node); @@ -20,6 +26,8 @@ namespace Modelec { std::shared_ptr GetPathfinding() const; + int GetTeamId() const; + void SendWaypoint() const; void SendWaypoint(const std::vector &waypoints) const; @@ -35,9 +43,11 @@ namespace Modelec { 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 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); + int CanGoTo(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); std::pair FindPath(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); @@ -48,6 +58,8 @@ namespace Modelec { std::shared_ptr GetClosestDepositeZone(const PosMsg::SharedPtr &pos, int teamId, const std::vector& blacklistedId = {}); + PosMsg::SharedPtr GetHomePosition(); + protected: void OnWaypointReach(const WaypointReachMsg::SharedPtr msg); @@ -58,6 +70,8 @@ namespace Modelec { std::shared_ptr pathfinding_; + int team_id_ = YELLOW; + std::list waypoints_; PosMsg::SharedPtr current_pos_; diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index 6e61f1b..a2eb56d 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -56,6 +56,7 @@ namespace Modelec rclcpp::Time match_start_time_; bool started_ = false; std::unique_ptr current_mission_; + int team_id_ = 0; std::shared_ptr nav_; std::unique_ptr mission_manager_; diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index bc7d328..bfcc579 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -2,7 +2,7 @@ namespace Modelec { - GoHomeMission::GoHomeMission(const std::shared_ptr& nav) : status_(MissionStatus::READY), nav_(nav) + GoHomeMission::GoHomeMission(const std::shared_ptr& nav, const rclcpp::Time& start_time) : step_(GO_HOME), status_(MissionStatus::READY), nav_(nav), start_time_(start_time) { } @@ -10,19 +10,44 @@ namespace Modelec { node_ = node; - nav_->GoTo(400, 1600.0, -M_PI_2); + auto pos = nav_->GetHomePosition(); + home_point_ = Point(pos->x, pos->y, pos->theta); + nav_->GoTo(home_point_.GetTakeBasePosition()); status_ = MissionStatus::RUNNING; } void GoHomeMission::update() { - if (nav_->HasArrived()) + if (!nav_->HasArrived()) return; + + switch (step_) { - status_ = MissionStatus::DONE; + case GO_HOME: + if ((node_->now() - start_time_).seconds() < 94) + { + break; + } + + nav_->GoTo(home_point_.GetTakePosition(0), true); + + step_ = GO_CLOSE; + break; + + case GO_CLOSE: + step_ = DONE; + status_ = MissionStatus::DONE; + + break; + default: + break; } } + void GoHomeMission::clear() + { + } + MissionStatus GoHomeMission::getStatus() const { return status_; diff --git a/src/modelec_strat/src/missions/prepare_concert_mission.cpp b/src/modelec_strat/src/missions/prepare_concert_mission.cpp index e391371..6f6962b 100644 --- a/src/modelec_strat/src/missions/prepare_concert_mission.cpp +++ b/src/modelec_strat/src/missions/prepare_concert_mission.cpp @@ -10,12 +10,19 @@ namespace Modelec void PrepareConcertMission::start(rclcpp::Node::SharedPtr node) { + + if (!nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId())) + { + status_ = MissionStatus::FINISH_ALL; + return; + } + if (auto col = nav_->GetPathfinding()->GetClosestColumn(nav_->GetCurrentPos())) { column_ = col; } else { - status_ = MissionStatus::FAILED; + status_ = MissionStatus::FINISH_ALL; return; } @@ -24,6 +31,12 @@ namespace Modelec auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); auto res = nav_->GoTo(pos.x, pos.y, pos.theta, false, Pathfinding::FREE | Pathfinding::WALL); + if (res != Pathfinding::FREE) + { + RCLCPP_WARN(node_->get_logger(), "Can't go to column %d", column_->id()); + status_ = MissionStatus::FAILED; + return; + } status_ = MissionStatus::RUNNING; } @@ -52,7 +65,7 @@ namespace Modelec case TAKE_COLUMN: { // TODO - closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), 0); + closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId()); if (!closestDepoZone_) { status_ = MissionStatus::FAILED; @@ -79,7 +92,7 @@ namespace Modelec break; case GO_BACK: { - closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), 0); + closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId()); if (!closestDepoZone_) { status_ = MissionStatus::FAILED; @@ -143,6 +156,11 @@ namespace Modelec } } + void PrepareConcertMission::clear() + { + // TODO : if is doing construction, stop everything and release everything + } + MissionStatus PrepareConcertMission::getStatus() const { return status_; diff --git a/src/modelec_strat/src/missions/promotion_mission.cpp b/src/modelec_strat/src/missions/promotion_mission.cpp index 7845ce2..ec003d7 100644 --- a/src/modelec_strat/src/missions/promotion_mission.cpp +++ b/src/modelec_strat/src/missions/promotion_mission.cpp @@ -45,6 +45,10 @@ namespace Modelec } } + void PromotionMission::clear() + { + } + MissionStatus PromotionMission::getStatus() const { return status_; diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 6c899af..ef5324a 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -2,6 +2,8 @@ #include #include +#include "../../modelec_utils/include/modelec_utils/config.hpp" + namespace Modelec { NavigationHelper::NavigationHelper() { @@ -47,6 +49,11 @@ namespace Modelec { return pathfinding_; } + int NavigationHelper::GetTeamId() const + { + return team_id_; + } + void NavigationHelper::SendWaypoint() const { for (auto & w : waypoints_) @@ -196,6 +203,11 @@ namespace Modelec { return GoTo(goal, isClose, collisionMask); } + int NavigationHelper::GoTo(const Point& goal, bool isClose, int collisionMask) + { + return GoTo(goal.x, goal.y, goal.theta, isClose, collisionMask); + } + int NavigationHelper::CanGoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask) { return FindPath(goal, isClose, collisionMask).first; @@ -210,6 +222,11 @@ namespace Modelec { return CanGoTo(goal, isClose, collisionMask); } + int NavigationHelper::CanGoTo(const Point& goal, bool isClose, int collisionMask) + { + return CanGoTo(goal.x, goal.y, goal.theta, isClose, collisionMask); + } + std::pair NavigationHelper::FindPath(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask) { @@ -270,6 +287,25 @@ namespace Modelec { return closest_zone; } + PosMsg::SharedPtr NavigationHelper::GetHomePosition() + { + PosMsg::SharedPtr home = std::make_shared(); + // TODO : handle Team Id + if (team_id_ == YELLOW) + { + home->x = Config::get("config.spawn.yellow@x", 0); + home->y = Config::get("config.spawn.yellow@y", 0); + home->theta = Config::get("config.spawn.yellow@theta", 0); + } + else + { + home->x = Config::get("config.spawn.blue@x", 0); + home->y = Config::get("config.spawn.blue@y", 0); + home->theta = Config::get("config.spawn.blue@theta", 0); + } + return home; + } + void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr msg) { for (auto & waypoint : waypoints_) diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index fbc4dd9..7566381 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -161,8 +161,19 @@ namespace Modelec const float cell_size_mm_y = map_height_mm_ / grid_height_; // Robot dimensions - const int inflate_x = isClose ? 0 : std::ceil(((robot_width_mm_ + margin_mm_) / 2.0f) / cell_size_mm_x); - const int inflate_y = isClose ? 0 : std::ceil(((robot_length_mm_ + margin_mm_) / 2.0f) / cell_size_mm_y); + int inflate_x; + int inflate_y; + + if (isClose) + { + inflate_x = std::ceil(((robot_width_mm_) / 2.0f) / cell_size_mm_x); + inflate_y = std::ceil(((robot_length_mm_) / 2.0f) / cell_size_mm_y); + } + else + { + inflate_x = std::ceil(((robot_width_mm_ + margin_mm_) / 2.0f) / cell_size_mm_x); + inflate_y = std::ceil(((robot_length_mm_ + margin_mm_) / 2.0f) / cell_size_mm_y); + } // 1. Build fresh empty grid grid_.clear(); diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 884420a..42e6cb3 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -111,9 +111,15 @@ namespace Modelec } else if (current_mission_->getStatus() == MissionStatus::FAILED) { + current_mission_->clear(); current_mission_.reset(); transition(State::SELECT_MISSION, "PrepareConcert failed"); } + else if (current_mission_->getStatus() == MissionStatus::FINISH_ALL) + { + current_mission_.reset(); + transition(State::DO_GO_HOME, "Finish all finished"); + } break; case State::DO_PROMOTION: @@ -138,7 +144,7 @@ namespace Modelec case State::DO_GO_HOME: if (!current_mission_) { - current_mission_ = std::make_unique(nav_); + current_mission_ = std::make_unique(nav_, match_start_time_); current_mission_->start(shared_from_this()); } current_mission_->update(); diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp index 410333a..70ef661 100644 --- a/src/modelec_utils/include/modelec_utils/point.hpp +++ b/src/modelec_utils/include/modelec_utils/point.hpp @@ -13,6 +13,7 @@ namespace Modelec static double distance(const Point& p1, const Point& p2); Point GetTakePosition(int distance, double angle) const; + Point GetTakePosition(int distance) const; Point GetTakeBasePosition() const; Point GetTakeClosePosition() const; diff --git a/src/modelec_utils/src/point.cpp b/src/modelec_utils/src/point.cpp index 8ccdf7c..d5e40c0 100644 --- a/src/modelec_utils/src/point.cpp +++ b/src/modelec_utils/src/point.cpp @@ -17,6 +17,11 @@ namespace Modelec return pos; } + Point Point::GetTakePosition(int distance) const + { + return GetTakePosition(distance, theta); + } + Point Point::GetTakeBasePosition() const { return GetTakePosition(300, theta);