From 4bd1a1438c304eac6d794f61bcb0748e2a3808c4 Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 3 May 2025 17:20:12 -0400 Subject: [PATCH] strat v0.2.2 : - start of a dynamic strat - enemy isn't part of the strat for the moment (next update) --- simulated_pcb/alim.py | 2 +- simulated_pcb/odo.py | 2 +- src/modelec_com/src/pcb_alim_interface.cpp | 2 +- src/modelec_com/src/pcb_odo_interface.cpp | 2 +- .../include/modelec_gui/pages/map_page.hpp | 11 ++++ src/modelec_gui/src/pages/map_page.cpp | 39 +++++++++++- src/modelec_strat/CMakeLists.txt | 2 +- src/modelec_strat/data/config.xml | 2 +- src/modelec_strat/data/deposite_zone.xml | 63 +++++++++++++++++++ src/modelec_strat/data/obstacles.xml | 20 +++--- .../include/modelec_strat/deposite_zone.hpp | 33 ++++++++++ .../missions/prepare_concert_mission.hpp | 7 ++- .../modelec_strat/navigation_helper.hpp | 9 +++ .../include/modelec_strat/obstacle/column.hpp | 23 +++++++ .../modelec_strat/{ => obstacle}/obstacle.hpp | 15 ++++- .../include/modelec_strat/pathfinding.hpp | 35 +++++++++-- .../include/modelec_strat/strat_fms.hpp | 2 + src/modelec_strat/src/deposite_zone.cpp | 33 ++++++++++ .../src/missions/prepare_concert_mission.cpp | 54 +++++++++++++--- src/modelec_strat/src/navigation_helper.cpp | 61 ++++++++++++++++++ src/modelec_strat/src/obstacle/column.cpp | 29 +++++++++ .../src/{ => obstacle}/obstacle.cpp | 2 +- src/modelec_strat/src/pathfinding.cpp | 61 +++++++++++++----- src/modelec_strat/src/strat_fms.cpp | 11 +++- src/modelec_utils/CMakeLists.txt | 1 + .../include/modelec_utils/point.hpp | 18 ++++++ src/modelec_utils/src/point.cpp | 29 +++++++++ 27 files changed, 516 insertions(+), 52 deletions(-) create mode 100644 src/modelec_strat/data/deposite_zone.xml create mode 100644 src/modelec_strat/include/modelec_strat/deposite_zone.hpp create mode 100644 src/modelec_strat/include/modelec_strat/obstacle/column.hpp rename src/modelec_strat/include/modelec_strat/{ => obstacle}/obstacle.hpp (80%) create mode 100644 src/modelec_strat/src/deposite_zone.cpp create mode 100644 src/modelec_strat/src/obstacle/column.cpp rename src/modelec_strat/src/{ => obstacle}/obstacle.cpp (97%) create mode 100644 src/modelec_utils/include/modelec_utils/point.hpp create mode 100644 src/modelec_utils/src/point.cpp diff --git a/simulated_pcb/alim.py b/simulated_pcb/alim.py index 9884deb..2a22ad1 100644 --- a/simulated_pcb/alim.py +++ b/simulated_pcb/alim.py @@ -4,7 +4,7 @@ import time import serial # Set the parameters for the serial connection -serial_port = '/dev/pts/9' # Modify this to your serial port (e.g., 'COM3' on Windows) +serial_port = '/dev/pts/12' # Modify this to your serial port (e.g., 'COM3' on Windows) baud_rate = 115200 # Modify this to your baud rate # Open the serial connection diff --git a/simulated_pcb/odo.py b/simulated_pcb/odo.py index 13d736c..fb4c6bf 100644 --- a/simulated_pcb/odo.py +++ b/simulated_pcb/odo.py @@ -4,7 +4,7 @@ import math import threading class SimulatedPCB: - def __init__(self, port='/dev/pts/8', baud=115200): + def __init__(self, port='/dev/pts/6', baud=115200): self.ser = serial.Serial(port, baud, timeout=1) self.running = True self.start = False diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp index 8d4e0ca..58e2502 100644 --- a/src/modelec_com/src/pcb_alim_interface.cpp +++ b/src/modelec_com/src/pcb_alim_interface.cpp @@ -10,7 +10,7 @@ namespace Modelec auto request = std::make_shared(); request->name = "pcb_alim"; request->bauds = 115200; - request->serial_port = "/dev/pts/10"; // TODO : check the real serial port + request->serial_port = "/dev/pts/13"; // TODO : check the real serial port auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) { diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 846f339..945de25 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -10,7 +10,7 @@ namespace Modelec auto request = std::make_shared(); request->name = "pcb_odo"; request->bauds = 115200; - request->serial_port = "/dev/pts/9"; // TODO : check the real serial port + request->serial_port = "/dev/pts/7"; // TODO : check the real serial port auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) { diff --git a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp index e834d7d..bfd6e19 100644 --- a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp @@ -18,8 +18,10 @@ #include #include #include +#include #include +#include namespace ModelecGUI { class MapPage : public QWidget @@ -61,6 +63,9 @@ namespace ModelecGUI { QPushButton* tirette_button_; + QLabel* timer_label_; + QLabel* score_label_; + modelec_interfaces::msg::OdometryPos robotPos; std::vector qpoints; //std::vector points; @@ -97,5 +102,11 @@ namespace ModelecGUI { modelec_interfaces::msg::OdometryPos enemy_pos_; rclcpp::Publisher::SharedPtr enemy_pos_pub_; + rclcpp::Subscription::SharedPtr strat_start_sub_; + + bool isGameStarted_ = false; + long int start_time_ = 0; + + rclcpp::Subscription::SharedPtr strat_state_sub_; }; } diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index 943a52c..8743b7c 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -17,9 +17,20 @@ namespace ModelecGUI tirette_button_ = new QPushButton("Tirette", this); + timer_label_ = new QLabel("00", this); + timer_label_->setAlignment(Qt::AlignCenter); + timer_label_->setFont(QFont("Arial", 24)); + timer_label_->setStyleSheet("QLabel { color: white; }"); + score_label_ = new QLabel("Score: 0", this); + score_label_->setAlignment(Qt::AlignCenter); + score_label_->setFont(QFont("Arial", 24)); + score_label_->setStyleSheet("QLabel { color: white; }"); + h_layout = new QHBoxLayout(this); h_layout->addStretch(); + h_layout->addWidget(score_label_); h_layout->addWidget(tirette_button_); + h_layout->addWidget(timer_label_); h_layout->addStretch(); v_layout->addLayout(h_layout); @@ -39,6 +50,8 @@ namespace ModelecGUI enemy_length_ = Modelec::Config::get("config.enemy.size.length_mm", 300); enemy_width_ = Modelec::Config::get("config.enemy.size.width_mm", 300); + + add_waypoint_sub_ = node_->create_subscription("odometry/add_waypoint", 100, [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) { if (lastWapointWasEnd) @@ -75,14 +88,27 @@ namespace ModelecGUI obstacle_.erase(msg->id); }); - - go_to_pub_ = node_->create_publisher("nav/go_to", 10); enemy_pos_pub_ = node_->create_publisher("enemy/position", 10); tirette_pub_ = node_->create_publisher("tirette", 10); + strat_start_sub_ = node_->create_subscription("/strat/start_time", 10, + [this](const std_msgs::msg::Int64::SharedPtr msg){ + isGameStarted_ = true; + start_time_ = msg->data; + }); + + strat_state_sub_ = node_->create_subscription("/strat/state", 10, + [this](const modelec_interfaces::msg::StratState::SharedPtr msg){ + if (msg->state == modelec_interfaces::msg::StratState::STOP) + { + RCLCPP_INFO(node_->get_logger(), "Game stop"); + isGameStarted_ = false; + } + }); + connect(tirette_button_, &QPushButton::clicked, this, [this]() { std_msgs::msg::Bool msg; msg.data = true; @@ -145,6 +171,15 @@ namespace ModelecGUI { QWidget::paintEvent(paint_event); + if (isGameStarted_) + { + auto now = std::chrono::system_clock::now().time_since_epoch(); + auto start = std::chrono::nanoseconds(start_time_); + auto elapsed = now - start; + auto elapsed_s = std::chrono::duration_cast(elapsed).count(); + timer_label_->setText(QString::number(elapsed_s)); + } + QPainter painter(this); renderer->render(&painter, rect()); // Scales SVG to widget size painter.save(); diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index f5277d2..c1fb19f 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/mission_manager.cpp src/missions/prepare_concert_mission.cpp src/missions/promotion_mission.cpp src/missions/go_home_mission.cpp src/obstacle.cpp) +add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/mission_manager.cpp src/missions/prepare_concert_mission.cpp src/missions/promotion_mission.cpp src/missions/go_home_mission.cpp src/obstacle/obstacle.cpp src/obstacle/column.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/data/config.xml b/src/modelec_strat/data/config.xml index cf8c293..e0a319a 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -16,7 +16,7 @@ 300 300 - 0 + 300 diff --git a/src/modelec_strat/data/deposite_zone.xml b/src/modelec_strat/data/deposite_zone.xml new file mode 100644 index 0000000..580651c --- /dev/null +++ b/src/modelec_strat/data/deposite_zone.xml @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/modelec_strat/data/obstacles.xml b/src/modelec_strat/data/obstacles.xml index e4ac05e..32d1653 100644 --- a/src/modelec_strat/data/obstacles.xml +++ b/src/modelec_strat/data/obstacles.xml @@ -5,18 +5,18 @@ - - - - - + + + + + - - - - - + + + + + diff --git a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp new file mode 100644 index 0000000..d4a9171 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include +#include +#include + +namespace Modelec { + class DepositeZone + { + public: + DepositeZone(tinyxml2::XMLElement * obstacleElem); + + Point GetPosition() const { return position_; } + Point GetNextPotPos() + { + if (pot_queue_.empty()) + return Point(); + auto elem = pot_queue_.front(); + pot_queue_.pop(); + return elem; + } + + int GetTeam() const { return team_; } + int GetId() const { return id_; } + int GetMaxPot() const { return max_pot_; } + + protected: + int id_, team_, max_pot_; + Point position_; + + std::queue pot_queue_; + }; +} 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 1ff15b5..c96fe84 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 @@ -2,6 +2,7 @@ #include #include +#include namespace Modelec { @@ -19,9 +20,11 @@ namespace Modelec { GO_TO_COLUMN, GO_CLOSE_TO_COLUMN, TAKE_COLUMN, + GO_BACK, GO_TO_PLATFORM, GO_CLOSE_TO_PLATFORM, PLACE_PLATFORM, + GO_BACK_2, DONE } step_; @@ -29,7 +32,9 @@ namespace Modelec { std::shared_ptr nav_; rclcpp::Node::SharedPtr node_; - Obstacle column_; + std::shared_ptr column_; + std::shared_ptr closestDepoZone_; + Point closestDepoZonePoint_; }; } diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index b6cc160..1fd0132 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -5,6 +5,7 @@ #include #include +#include "deposite_zone.hpp" #include "pathfinding.hpp" namespace Modelec { @@ -37,6 +38,12 @@ namespace Modelec { WaypointListMsg FindPath(const PosMsg::SharedPtr &goal, bool isClose = false); + PosMsg::SharedPtr GetCurrentPos() const; + + bool LoadDepositeZoneFromXML(const std::string &filename); + + std::shared_ptr GetClosestDepositeZone(const PosMsg::SharedPtr &pos, int teamId); + protected: void OnWaypointReach(const WaypointReachMsg::SharedPtr msg); @@ -51,6 +58,8 @@ namespace Modelec { PosMsg::SharedPtr current_pos_; + std::map> deposite_zones_; + rclcpp::Subscription::SharedPtr waypoint_reach_sub_; rclcpp::Publisher::SharedPtr waypoint_pub_; diff --git a/src/modelec_strat/include/modelec_strat/obstacle/column.hpp b/src/modelec_strat/include/modelec_strat/obstacle/column.hpp new file mode 100644 index 0000000..3e18c8e --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/obstacle/column.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include "obstacle.hpp" + +namespace Modelec +{ + class ColumnObstacle : public Obstacle + { + public: + ColumnObstacle() = default; + ColumnObstacle(const ColumnObstacle&) = default; + + ColumnObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type); + ColumnObstacle(tinyxml2::XMLElement * obstacleElem); + ColumnObstacle(const modelec_interfaces::msg::Obstacle& msg); + + bool IsAtObjective() const; + void SetAtObjective(bool atObjective); + + protected: + bool isAtObjective = false; + }; +} diff --git a/src/modelec_strat/include/modelec_strat/obstacle.hpp b/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp similarity index 80% rename from src/modelec_strat/include/modelec_strat/obstacle.hpp rename to src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp index fec9daf..0e550be 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp @@ -6,6 +6,8 @@ #include +#include + namespace Modelec { class Obstacle { public: @@ -15,7 +17,9 @@ namespace Modelec { Obstacle(const modelec_interfaces::msg::Obstacle& msg); Obstacle(const Obstacle& other) = default; - modelec_interfaces::msg::Obstacle toMsg() const; + virtual ~Obstacle() = default; + + virtual modelec_interfaces::msg::Obstacle toMsg() const; int id() const { return id_; } int x() const { return x_; } @@ -24,6 +28,7 @@ namespace Modelec { int width() const { return w_; } int height() const { return h_; } const std::string& type() const { return type_; } + Point position() const { return Point(x_, y_, theta_); } void setId(int id) { id_ = id; } void setX(int x) { x_ = x; } @@ -39,11 +44,17 @@ namespace Modelec { theta_ = theta; } + void setPosition(const Point& p) { + x_ = p.x; + y_ = p.y; + theta_ = p.theta; + } + void setSize(int w, int h) { w_ = w; h_ = h; } - private: + protected: int id_, x_, y_, w_, h_; double theta_; std::string type_; diff --git a/src/modelec_strat/include/modelec_strat/pathfinding.hpp b/src/modelec_strat/include/modelec_strat/pathfinding.hpp index b7d98ce..ec68912 100644 --- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -14,7 +14,9 @@ #include #include -#include "obstacle.hpp" +#include + +#include "obstacle/column.hpp" namespace Modelec { @@ -53,11 +55,17 @@ namespace Modelec { void SetCurrentPos(const PosMsg::SharedPtr& pos); - Obstacle GetObstacle(int id) const; + std::shared_ptr GetObstacle(int id) const; void RemoveObstacle(int id); - void AddObstacle(const Obstacle& obstacle); + void AddObstacle(const std::shared_ptr& obstacle); + + template ::value>> + std::shared_ptr GetClosestObstacle(const PosMsg::SharedPtr& pos) const; + + std::shared_ptr GetClosestColumn(const PosMsg::SharedPtr& pos); protected: void HandleMapRequest( @@ -93,7 +101,7 @@ namespace Modelec { int margin_mm_ = 0; - std::map obstacle_map_; + std::map> obstacle_map_; PosMsg::SharedPtr current_start_; PosMsg::SharedPtr current_goal_; @@ -116,4 +124,23 @@ namespace Modelec { rclcpp::Service::SharedPtr ask_obstacle_srv_; }; + template + std::shared_ptr Pathfinding::GetClosestObstacle(const PosMsg::SharedPtr& pos) const + { + std::shared_ptr closest_obstacle = nullptr; + auto robotPos = Point(pos->x, pos->y, pos->theta); + float distance = std::numeric_limits::max(); + + for (const auto& obstacle : obstacle_map_) { + if (auto obs = std::dynamic_pointer_cast(obstacle.second)) { + auto dist = Point::distance(robotPos, obs->position()); + if (dist < distance) { + distance = dist; + closest_obstacle = obs; + } + } + } + + return closest_obstacle; + } } diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index 902784a..6e61f1b 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -11,6 +11,7 @@ #include "missions/go_home_mission.hpp" #include "std_msgs/msg/bool.hpp" +#include #include "modelec_interfaces/msg/strat_state.hpp" @@ -62,5 +63,6 @@ namespace Modelec rclcpp::Subscription::SharedPtr tirette_sub_; rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::Publisher::SharedPtr start_time_pub_; }; } diff --git a/src/modelec_strat/src/deposite_zone.cpp b/src/modelec_strat/src/deposite_zone.cpp new file mode 100644 index 0000000..2110bc6 --- /dev/null +++ b/src/modelec_strat/src/deposite_zone.cpp @@ -0,0 +1,33 @@ +#include +#include + +namespace Modelec +{ + DepositeZone::DepositeZone(tinyxml2::XMLElement* obstacleElem) + { + obstacleElem->QueryIntAttribute("id", &id_); + obstacleElem->QueryIntAttribute("team", &team_); + obstacleElem->QueryIntAttribute("max_pot", &max_pot_); + + auto posElem = obstacleElem->FirstChildElement("Pos"); + if (posElem) + { + posElem->QueryIntAttribute("x", &position_.x); + posElem->QueryIntAttribute("y", &position_.y); + posElem->QueryDoubleAttribute("theta", &position_.theta); + } + + auto posPotList = obstacleElem->FirstChildElement("PotPos"); + if (posPotList) + { + for (auto elemPos = posPotList->FirstChildElement("Pos"); elemPos; elemPos = elemPos->NextSiblingElement("Pos")) + { + Point pos; + elemPos->QueryIntAttribute("x", &pos.x); + elemPos->QueryIntAttribute("y", &pos.y); + elemPos->QueryDoubleAttribute("theta", &pos.theta); + pot_queue_.emplace(pos); + } + } + } +} diff --git a/src/modelec_strat/src/missions/prepare_concert_mission.cpp b/src/modelec_strat/src/missions/prepare_concert_mission.cpp index a890d05..680b843 100644 --- a/src/modelec_strat/src/missions/prepare_concert_mission.cpp +++ b/src/modelec_strat/src/missions/prepare_concert_mission.cpp @@ -1,4 +1,5 @@ #include +#include namespace Modelec { @@ -9,11 +10,20 @@ namespace Modelec void PrepareConcertMission::start(rclcpp::Node::SharedPtr node) { - column_ = nav_->getPathfinding()->GetObstacle(14); + if (auto col = nav_->getPathfinding()->GetClosestColumn(nav_->GetCurrentPos())) + { + column_ = col; + } else + { + status_ = MissionStatus::FAILED; + return; + } node_ = node; - nav_->GoTo(775, 480, -M_PI_2); + auto pos = column_->position().GetTakeBasePosition(); + + nav_->GoTo(pos.x, pos.y, pos.theta); status_ = MissionStatus::RUNNING; } @@ -27,26 +37,40 @@ namespace Modelec switch (step_) { case GO_TO_COLUMN: - nav_->GoTo(775, 460, -M_PI_2, true); + { + auto pos = column_->position().GetTakeClosePosition(); + nav_->GoTo(pos.x, pos.y, pos.theta, true); + } step_ = GO_CLOSE_TO_COLUMN; break; case GO_CLOSE_TO_COLUMN: - nav_->getPathfinding()->RemoveObstacle(column_.id()); + nav_->getPathfinding()->RemoveObstacle(column_->id()); step_ = TAKE_COLUMN; break; case TAKE_COLUMN: { - nav_->GoTo(2500, 875, 0); + auto pos = column_->position().GetTakeBasePosition(); + nav_->GoTo(pos.x, pos.y, pos.theta, true); + } + + step_ = GO_BACK; + break; + case GO_BACK: + { + closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), 0); + closestDepoZonePoint_ = closestDepoZone_->GetNextPotPos(); + auto p = closestDepoZonePoint_.GetTakeBasePosition(); + nav_->GoTo(p.x, p.y, p.theta); } step_ = GO_TO_PLATFORM; break; - case GO_TO_PLATFORM: { - nav_->GoTo(2700, 875, 0, true); + auto p = closestDepoZonePoint_.GetTakeClosePosition(); + nav_->GoTo(p.x, p.y, p.theta, true); } step_ = GO_CLOSE_TO_PLATFORM; @@ -54,9 +78,10 @@ namespace Modelec case GO_CLOSE_TO_PLATFORM: { - column_.setX(2925); - column_.setY(875); - column_.setTheta(0); + column_->setX(closestDepoZonePoint_.x); + column_->setY(closestDepoZonePoint_.y); + column_->setTheta(closestDepoZonePoint_.theta); + column_->SetAtObjective(true); nav_->getPathfinding()->AddObstacle(column_); } @@ -64,6 +89,15 @@ namespace Modelec break; case PLACE_PLATFORM: + { + auto p = closestDepoZonePoint_.GetTakeBasePosition(); + nav_->GoTo(p.x, p.y, p.theta, true); + } + + step_ = GO_BACK_2; + break; + + case GO_BACK_2: step_ = DONE; status_ = MissionStatus::DONE; break; diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 37897bf..3cfbe72 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -1,5 +1,6 @@ #include #include +#include namespace Modelec { NavigationHelper::NavigationHelper() @@ -28,6 +29,12 @@ namespace Modelec { "nav/go_to", 10, [this](const PosMsg::SharedPtr msg) { GoTo(msg); }); + + std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/deposite_zone.xml"; + if (!LoadDepositeZoneFromXML(deposite_zone_path)) + { + RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML"); + } } rclcpp::Node::SharedPtr NavigationHelper::getNode() const @@ -187,6 +194,60 @@ namespace Modelec { return pathfinding_->FindPath(current_pos_, goal, isClose); } + PosMsg::SharedPtr NavigationHelper::GetCurrentPos() const + { + return current_pos_; + } + + bool NavigationHelper::LoadDepositeZoneFromXML(const std::string& filename) + { + tinyxml2::XMLDocument doc; + if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacle XML file: %s", filename.c_str()); + return false; + } + + tinyxml2::XMLElement* root = doc.FirstChildElement("Map"); + if (!root) + { + RCLCPP_ERROR(node_->get_logger(), "No root element in file"); + return false; + } + + for (tinyxml2::XMLElement* elem = root->FirstChildElement("DepositeZone"); + elem != nullptr; + elem = elem->NextSiblingElement("DepositeZone")) + { + std::shared_ptr obs = std::make_shared(elem); + deposite_zones_[obs->GetId()] = obs; + } + + RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", deposite_zones_.size()); + return true; + } + + std::shared_ptr NavigationHelper::GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId) + { + std::shared_ptr closest_zone = nullptr; + double min_distance = std::numeric_limits::max(); + auto posPoint = Point(pos->x, pos->y, pos->theta); + for (const auto& zone : deposite_zones_) + { + if (zone.second->GetTeam() == teamId) + { + double distance = Point::distance(posPoint, zone.second->GetPosition()); + if (distance < min_distance) + { + min_distance = distance; + closest_zone = zone.second; + } + } + } + + return closest_zone; + } + void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr msg) { for (auto & waypoint : waypoints_) diff --git a/src/modelec_strat/src/obstacle/column.cpp b/src/modelec_strat/src/obstacle/column.cpp new file mode 100644 index 0000000..b3a94e1 --- /dev/null +++ b/src/modelec_strat/src/obstacle/column.cpp @@ -0,0 +1,29 @@ +#include + +namespace Modelec +{ + ColumnObstacle::ColumnObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type) + : Obstacle(id, x, y, theta, w, h, type) + { + } + + ColumnObstacle::ColumnObstacle(tinyxml2::XMLElement* obstacleElem) + : Obstacle(obstacleElem) + { + } + + ColumnObstacle::ColumnObstacle(const modelec_interfaces::msg::Obstacle& msg) : + Obstacle(msg) + { + } + + bool ColumnObstacle::IsAtObjective() const + { + return isAtObjective; + } + + void ColumnObstacle::SetAtObjective(bool atObjective) + { + isAtObjective = atObjective; + } +} diff --git a/src/modelec_strat/src/obstacle.cpp b/src/modelec_strat/src/obstacle/obstacle.cpp similarity index 97% rename from src/modelec_strat/src/obstacle.cpp rename to src/modelec_strat/src/obstacle/obstacle.cpp index 801d46a..839a2e0 100644 --- a/src/modelec_strat/src/obstacle.cpp +++ b/src/modelec_strat/src/obstacle/obstacle.cpp @@ -1,4 +1,4 @@ -#include +#include namespace Modelec { diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index ee32ca8..72ecfc4 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -1,5 +1,6 @@ #include #include +#include #include namespace Modelec { @@ -54,7 +55,7 @@ namespace Modelec { "obstacle/add", 10, [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { RCLCPP_INFO(node_->get_logger(), "Obstacle add request received"); - AddObstacle(Obstacle(*msg)); + AddObstacle(std::make_shared(*msg)); }); obstacle_add_pub_ = node_->create_publisher( @@ -152,8 +153,8 @@ 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); + 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); // 1. Build fresh empty grid grid_.clear(); @@ -206,11 +207,11 @@ namespace Modelec { // 2. Fill obstacles with inflation for (const auto& [id, obstacle] : obstacle_map_) { - float cx = obstacle.x(); - float cy = obstacle.y(); - float width = obstacle.width() + inflate_x * cell_size_mm_x; - float height = obstacle.height() + inflate_y * cell_size_mm_y; - float theta = M_PI_2 - obstacle.theta(); + float cx = obstacle->x(); + float cy = obstacle->y(); + float width = obstacle->width() + inflate_x * cell_size_mm_x; + float height = obstacle->height() + inflate_y * cell_size_mm_y; + float theta = M_PI_2 - obstacle->theta(); float dx = width / 2.0f; float dy = height / 2.0f; @@ -496,7 +497,7 @@ namespace Modelec { current_start_ = pos; } - Obstacle Pathfinding::GetObstacle(int id) const + std::shared_ptr Pathfinding::GetObstacle(int id) const { return obstacle_map_.at(id); } @@ -510,13 +511,35 @@ namespace Modelec { obstacle_remove_pub_->publish(msg); } - void Pathfinding::AddObstacle(const Obstacle& obstacle) + void Pathfinding::AddObstacle(const std::shared_ptr& obstacle) { - obstacle_map_[obstacle.id()] = obstacle; - modelec_interfaces::msg::Obstacle msg = obstacle.toMsg(); + obstacle_map_[obstacle->id()] = obstacle; + modelec_interfaces::msg::Obstacle msg = obstacle->toMsg(); obstacle_add_pub_->publish(msg); } + std::shared_ptr Pathfinding::GetClosestColumn(const PosMsg::SharedPtr& pos) + { + std::shared_ptr closest_obstacle = nullptr; + auto robotPos = Point(pos->x, pos->y, pos->theta); + float distance = std::numeric_limits::max(); + + for (const auto& obstacle : obstacle_map_) { + if (auto obs = std::dynamic_pointer_cast(obstacle.second)) { + if (!obs->IsAtObjective()) + { + auto dist = Point::distance(robotPos, obs->position()); + if (dist < distance) { + distance = dist; + closest_obstacle = obs; + } + } + } + } + + return closest_obstacle; + } + void Pathfinding::HandleMapRequest(const std::shared_ptr, const std::shared_ptr response) { @@ -544,7 +567,7 @@ namespace Modelec { { for (auto & [index, obs] : obstacle_map_) { - obstacle_add_pub_->publish(obs.toMsg()); + obstacle_add_pub_->publish(obs->toMsg()); } } @@ -582,8 +605,16 @@ namespace Modelec { obstacleElem != nullptr; obstacleElem = obstacleElem->NextSiblingElement("Obstacle")) { - Obstacle obs(obstacleElem); - obstacle_map_[obs.id()] = obs; + std::shared_ptr obs = std::make_shared(obstacleElem); + obstacle_map_[obs->id()] = obs; + } + + for (tinyxml2::XMLElement* obstacleElem = root->FirstChildElement("Gradin"); + obstacleElem != nullptr; + obstacleElem = obstacleElem->NextSiblingElement("Gradin")) + { + std::shared_ptr obs = std::make_shared(obstacleElem); + obstacle_map_[obs->id()] = obs; } RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", obstacle_map_.size()); diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index c1572cc..e6d0858 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -18,6 +18,8 @@ namespace Modelec state_pub_ = create_publisher("/strat/state", 10); + start_time_pub_ = create_publisher("/strat/start_time", 10); + std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; if (!Config::load(config_path)) { @@ -66,6 +68,13 @@ namespace Modelec if (started_) { match_start_time_ = now; + + std_msgs::msg::Int64 msg; + msg.data = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); + start_time_pub_->publish(msg); + transition(State::SELECT_MISSION, "Match started"); } break; @@ -78,7 +87,7 @@ namespace Modelec { transition(State::DO_PROMOTION, "Start promotion"); } - else if (elapsed.seconds() < 20) + else if (elapsed.seconds() < 80) { transition(State::DO_PREPARE_CONCERT, "Proceed to concert"); } diff --git a/src/modelec_utils/CMakeLists.txt b/src/modelec_utils/CMakeLists.txt index cc16e95..8cf6e42 100644 --- a/src/modelec_utils/CMakeLists.txt +++ b/src/modelec_utils/CMakeLists.txt @@ -25,6 +25,7 @@ target_link_libraries(config PUBLIC tinyxml2::tinyxml2) # === modelec::utils === add_library(utils src/utils.cpp + src/point.cpp ) target_include_directories(utils PUBLIC diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp new file mode 100644 index 0000000..db850d2 --- /dev/null +++ b/src/modelec_utils/include/modelec_utils/point.hpp @@ -0,0 +1,18 @@ +#pragma once + +namespace Modelec +{ + struct Point { + int x; + int y; + double theta; + + Point() : x(0), y(0), theta(0) {} + Point(int x, int y, double theta) : x(x), y(y), theta(theta) {} + + static double distance(const Point& p1, const Point& p2); + + Point GetTakeBasePosition(); + Point GetTakeClosePosition(); + }; +} diff --git a/src/modelec_utils/src/point.cpp b/src/modelec_utils/src/point.cpp new file mode 100644 index 0000000..68ee198 --- /dev/null +++ b/src/modelec_utils/src/point.cpp @@ -0,0 +1,29 @@ +#include +#include + +namespace Modelec +{ + double Point::distance(const Point& p1, const Point& p2) + { + return sqrt(std::pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2)); + } + + Point Point::GetTakeBasePosition() + { + Point pos; + pos.x = x + 400 * cos(theta); + pos.y = y + 400 * sin(theta); + pos.theta = theta + M_PI; + return pos; + } + + Point Point::GetTakeClosePosition() + { + Point pos; + pos.x = x + 210 * cos(theta); + pos.y = y + 210 * sin(theta); + pos.theta = theta + M_PI; + return pos; + } + +}