diff --git a/simulated_pcb/alim.py b/simulated_pcb/alim.py index 2a22ad1..8776df3 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/12' # Modify this to your serial port (e.g., 'COM3' on Windows) +serial_port = '/dev/pts/11' # 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 fb4c6bf..639d43c 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/6', baud=115200): + def __init__(self, port='/dev/pts/9', baud=115200): self.ser = serial.Serial(port, baud, timeout=1) self.running = True self.start = False @@ -45,7 +45,7 @@ class SimulatedPCB: angle_diff = self.normalize_angle(wp['theta'] - self.theta) if wp['type'] == 1: - if distance < 5.0 and abs(angle_diff) < 0.05: + if distance < 5.0 and abs(angle_diff) < 0.15: self.vx = 0.0 self.vy = 0.0 self.vtheta = 0.0 @@ -54,12 +54,12 @@ class SimulatedPCB: self.waypoint_order.pop(0) del self.waypoints[wp['id']] else: - speed = min(200.0, distance * 1.5) + speed = min(300.0, distance * 3) self.vx = speed * math.cos(angle) self.vy = speed * math.sin(angle) - self.vtheta = max(-1.0, min(1.0, angle_diff)) + self.vtheta = max(-9.0, min(9.0, angle_diff * 2)) else: - speed = min(300.0, distance * 2) + speed = min(600.0, distance * 4) self.vx = speed * math.cos(angle) self.vy = speed * math.sin(angle) self.vtheta = 0.0 diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp index 58e2502..dbf6747 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/13"; // TODO : check the real serial port + request->serial_port = "/dev/pts/12"; // 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 945de25..adab14c 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/7"; // TODO : check the real serial port + request->serial_port = "/dev/pts/10"; // 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_strat/data/config.xml b/src/modelec_strat/data/config.xml index e0a319a..34d98fa 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -3,7 +3,6 @@ 300 300 - 50 @@ -16,7 +15,7 @@ 300 300 - 300 + 100 diff --git a/src/modelec_strat/data/deposite_zone.xml b/src/modelec_strat/data/deposite_zone.xml index 580651c..5601996 100644 --- a/src/modelec_strat/data/deposite_zone.xml +++ b/src/modelec_strat/data/deposite_zone.xml @@ -7,11 +7,12 @@ - + - - + + + @@ -22,11 +23,12 @@ - + - - + + + @@ -38,11 +40,12 @@ - + - - + + + @@ -53,11 +56,12 @@ - + - - + + + diff --git a/src/modelec_strat/data/obstacles.xml b/src/modelec_strat/data/obstacles.xml index 32d1653..e1e2905 100644 --- a/src/modelec_strat/data/obstacles.xml +++ b/src/modelec_strat/data/obstacles.xml @@ -5,16 +5,20 @@ - + - + + + - + - + + + diff --git a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp index d4a9171..3bb3848 100644 --- a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp +++ b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp @@ -1,29 +1,44 @@ #pragma once +#include #include #include #include -namespace Modelec { +namespace Modelec +{ class DepositeZone { public: - DepositeZone(tinyxml2::XMLElement * obstacleElem); + DepositeZone(tinyxml2::XMLElement* obstacleElem); Point GetPosition() const { return position_; } + Point GetNextPotPos() { if (pot_queue_.empty()) - return Point(); - auto elem = pot_queue_.front(); + return Point(std::numeric_limits::min(), std::numeric_limits::min(), 0); + return pot_queue_.front(); + } + + Point ValidNextPotPos() + { + if (pot_queue_.empty()) + return Point(std::numeric_limits::min(), std::numeric_limits::min(), 0); + Point p = pot_queue_.front(); pot_queue_.pop(); - return elem; + return p; } int GetTeam() const { return team_; } int GetId() const { return id_; } int GetMaxPot() const { return max_pot_; } + int RemainingPotPos() const + { + return pot_queue_.size(); + } + protected: int id_, team_, max_pot_; Point position_; diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 1fd0132..e0afadc 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -16,9 +16,9 @@ namespace Modelec { NavigationHelper(const rclcpp::Node::SharedPtr& node); - rclcpp::Node::SharedPtr getNode() const; + rclcpp::Node::SharedPtr GetNode() const; - std::shared_ptr getPathfinding() const; + std::shared_ptr GetPathfinding() const; void SendWaypoint() const; void SendWaypoint(const std::vector &waypoints) const; @@ -33,16 +33,20 @@ namespace Modelec { bool HasArrived() const; - void GoTo(const PosMsg::SharedPtr &goal, bool isClose = false); - void GoTo(int x, int y, double theta, bool isClose = false); + 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); - WaypointListMsg FindPath(const PosMsg::SharedPtr &goal, bool isClose = false); + 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); + + std::pair FindPath(const PosMsg::SharedPtr& goal, bool isClose = false, + int collisionMask = Pathfinding::FREE); PosMsg::SharedPtr GetCurrentPos() const; bool LoadDepositeZoneFromXML(const std::string &filename); - std::shared_ptr GetClosestDepositeZone(const PosMsg::SharedPtr &pos, int teamId); + std::shared_ptr GetClosestDepositeZone(const PosMsg::SharedPtr &pos, int teamId, const std::vector& blacklistedId = {}); protected: void OnWaypointReach(const WaypointReachMsg::SharedPtr msg); diff --git a/src/modelec_strat/include/modelec_strat/obstacle/column.hpp b/src/modelec_strat/include/modelec_strat/obstacle/column.hpp index 3e18c8e..4b833e5 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/column.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/column.hpp @@ -1,6 +1,7 @@ #pragma once #include "obstacle.hpp" +#include namespace Modelec { @@ -17,7 +18,14 @@ namespace Modelec bool IsAtObjective() const; void SetAtObjective(bool atObjective); + Point GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const; + Point GetOptimizedGetPos(const Point& currentPos) const; + + std::vector GetAllPossiblePositions() const; + protected: bool isAtObjective = false; + + std::vector possible_angles_; }; } diff --git a/src/modelec_strat/include/modelec_strat/pathfinding.hpp b/src/modelec_strat/include/modelec_strat/pathfinding.hpp index ec68912..9bed61a 100644 --- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -40,14 +40,23 @@ namespace Modelec { class Pathfinding { public: + enum + { + FREE = 1 << 0, + WALL = 1 << 1, + OBSTACLE = 1 << 2, + ENEMY = 1 << 3, + }; + Pathfinding(); Pathfinding(const rclcpp::Node::SharedPtr& node); rclcpp::Node::SharedPtr getNode() const; - WaypointListMsg FindPath(const PosMsg::SharedPtr& start, - const PosMsg::SharedPtr& goal, bool isClose = false); + std::pair FindPath(const PosMsg::SharedPtr& start, + const PosMsg::SharedPtr& goal, bool isClose = false, + int collisionMask = FREE); //void SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal); @@ -65,7 +74,7 @@ namespace Modelec { typename = std::enable_if_t::value>> std::shared_ptr GetClosestObstacle(const PosMsg::SharedPtr& pos) const; - std::shared_ptr GetClosestColumn(const PosMsg::SharedPtr& pos); + std::shared_ptr GetClosestColumn(const PosMsg::SharedPtr& pos, const std::vector& blacklistedId = {}); protected: void HandleMapRequest( @@ -85,6 +94,8 @@ namespace Modelec { //bool EnemyOnPath(const modelec_interfaces::msg::OdometryPos& enemy_pos); //void Replan(); + bool TestCollision(int x, int y, int collisionMask = FREE); + private: rclcpp::Node::SharedPtr node_; diff --git a/src/modelec_strat/src/missions/prepare_concert_mission.cpp b/src/modelec_strat/src/missions/prepare_concert_mission.cpp index 680b843..e391371 100644 --- a/src/modelec_strat/src/missions/prepare_concert_mission.cpp +++ b/src/modelec_strat/src/missions/prepare_concert_mission.cpp @@ -10,7 +10,7 @@ namespace Modelec void PrepareConcertMission::start(rclcpp::Node::SharedPtr node) { - if (auto col = nav_->getPathfinding()->GetClosestColumn(nav_->GetCurrentPos())) + if (auto col = nav_->GetPathfinding()->GetClosestColumn(nav_->GetCurrentPos())) { column_ = col; } else @@ -21,9 +21,9 @@ namespace Modelec node_ = node; - auto pos = column_->position().GetTakeBasePosition(); + auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); - nav_->GoTo(pos.x, pos.y, pos.theta); + auto res = nav_->GoTo(pos.x, pos.y, pos.theta, false, Pathfinding::FREE | Pathfinding::WALL); status_ = MissionStatus::RUNNING; } @@ -38,31 +38,64 @@ namespace Modelec { case GO_TO_COLUMN: { - auto pos = column_->position().GetTakeClosePosition(); - nav_->GoTo(pos.x, pos.y, pos.theta, true); + auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); + nav_->GoTo(pos.x, pos.y, pos.theta, true, Pathfinding::FREE | Pathfinding::WALL); } 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: { - auto pos = column_->position().GetTakeBasePosition(); - nav_->GoTo(pos.x, pos.y, pos.theta, true); + // TODO + closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), 0); + if (!closestDepoZone_) + { + status_ = MissionStatus::FAILED; + return; + } + + closestDepoZonePoint_ = closestDepoZone_->GetNextPotPos(); + auto p = closestDepoZonePoint_.GetTakeBasePosition(); + auto res = nav_->CanGoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL); + if (res != Pathfinding::FREE) + { + auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); + nav_->GoTo(pos.x, pos.y, pos.theta, true, Pathfinding::FREE | Pathfinding::WALL); + step_ = GO_BACK; + } + else + { + nav_->GoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL); + closestDepoZone_->ValidNextPotPos(); + step_ = GO_TO_PLATFORM; + } } - step_ = GO_BACK; break; case GO_BACK: { closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), 0); - closestDepoZonePoint_ = closestDepoZone_->GetNextPotPos(); + if (!closestDepoZone_) + { + status_ = MissionStatus::FAILED; + return; + } + + closestDepoZonePoint_ = closestDepoZone_->ValidNextPotPos(); auto p = closestDepoZonePoint_.GetTakeBasePosition(); - nav_->GoTo(p.x, p.y, p.theta); + if (nav_->CanGoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL) != Pathfinding::FREE) + { + nav_->GoTo(p.x, p.y, p.theta, true, Pathfinding::FREE | Pathfinding::WALL); + } + else + { + nav_->CanGoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL); + } } step_ = GO_TO_PLATFORM; @@ -82,7 +115,7 @@ namespace Modelec column_->setY(closestDepoZonePoint_.y); column_->setTheta(closestDepoZonePoint_.theta); column_->SetAtObjective(true); - nav_->getPathfinding()->AddObstacle(column_); + nav_->GetPathfinding()->AddObstacle(column_); } step_ = PLACE_PLATFORM; @@ -91,7 +124,10 @@ namespace Modelec case PLACE_PLATFORM: { auto p = closestDepoZonePoint_.GetTakeBasePosition(); - nav_->GoTo(p.x, p.y, p.theta, true); + if (nav_->CanGoTo(p.x, p.y, p.theta) != Pathfinding::FREE) + { + nav_->GoTo(p.x, p.y, p.theta, true, Pathfinding::FREE | Pathfinding::WALL); + } } step_ = GO_BACK_2; diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 3cfbe72..6c899af 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -27,7 +27,7 @@ namespace Modelec { go_to_sub_ = node_->create_subscription( "nav/go_to", 10, [this](const PosMsg::SharedPtr msg) { - GoTo(msg); + GoTo(msg, false, Pathfinding::FREE | Pathfinding::WALL); }); std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/deposite_zone.xml"; @@ -37,12 +37,12 @@ namespace Modelec { } } - rclcpp::Node::SharedPtr NavigationHelper::getNode() const + rclcpp::Node::SharedPtr NavigationHelper::GetNode() const { return node_; } - std::shared_ptr NavigationHelper::getPathfinding() const + std::shared_ptr NavigationHelper::GetPathfinding() const { return pathfinding_; } @@ -166,32 +166,54 @@ namespace Modelec { return waypoints_.back().reached; } - void NavigationHelper::GoTo(const PosMsg::SharedPtr& goal, bool isClose) + int NavigationHelper::GoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask) { + auto [res, wl] = FindPath(goal, isClose, collisionMask); + + if (wl.empty() || res != Pathfinding::FREE) + { + return res; + } + waypoints_.clear(); - auto wml = FindPath(goal, isClose); - - for (auto & w : wml) + for (auto & w : wl) { waypoints_.emplace_back(w); } SendWaypoint(); + + return res; } - void NavigationHelper::GoTo(int x, int y, double theta, bool isClose) + int NavigationHelper::GoTo(int x, int y, double theta, bool isClose, int collisionMask) { PosMsg::SharedPtr goal = std::make_shared(); goal->x = x; goal->y = y; goal->theta = theta; - GoTo(goal, isClose); + return GoTo(goal, isClose, collisionMask); } - WaypointListMsg NavigationHelper::FindPath(const PosMsg::SharedPtr& goal, bool isClose) + int NavigationHelper::CanGoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask) { - return pathfinding_->FindPath(current_pos_, goal, isClose); + return FindPath(goal, isClose, collisionMask).first; + } + + int NavigationHelper::CanGoTo(int x, int y, double theta, bool isClose, int collisionMask) + { + PosMsg::SharedPtr goal = std::make_shared(); + goal->x = x; + goal->y = y; + goal->theta = theta; + return CanGoTo(goal, isClose, collisionMask); + } + + std::pair NavigationHelper::FindPath(const PosMsg::SharedPtr& goal, bool isClose, + int collisionMask) + { + return pathfinding_->FindPath(current_pos_, goal, isClose, collisionMask); } PosMsg::SharedPtr NavigationHelper::GetCurrentPos() const @@ -227,20 +249,20 @@ namespace Modelec { return true; } - std::shared_ptr NavigationHelper::GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId) + std::shared_ptr NavigationHelper::GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId, const std::vector& blacklistedId) { 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_) + for (const auto& [id, zone] : deposite_zones_) { - if (zone.second->GetTeam() == teamId) + if (zone->GetTeam() == teamId && zone->RemainingPotPos() > 0 && blacklistedId.end() == std::find(blacklistedId.begin(), blacklistedId.end(), id)) { - double distance = Point::distance(posPoint, zone.second->GetPosition()); + double distance = Point::distance(posPoint, zone->GetPosition()); if (distance < min_distance) { min_distance = distance; - closest_zone = zone.second; + closest_zone = zone; } } } diff --git a/src/modelec_strat/src/obstacle/column.cpp b/src/modelec_strat/src/obstacle/column.cpp index b3a94e1..bd367a1 100644 --- a/src/modelec_strat/src/obstacle/column.cpp +++ b/src/modelec_strat/src/obstacle/column.cpp @@ -10,6 +10,13 @@ namespace Modelec ColumnObstacle::ColumnObstacle(tinyxml2::XMLElement* obstacleElem) : Obstacle(obstacleElem) { + possible_angles_.push_back(theta_); + for (auto elem = obstacleElem->FirstChildElement("possible-angle"); + elem; + elem = elem->NextSiblingElement("possible-angle")) + { + possible_angles_.push_back(elem->DoubleAttribute("theta")); + } } ColumnObstacle::ColumnObstacle(const modelec_interfaces::msg::Obstacle& msg) : @@ -26,4 +33,38 @@ namespace Modelec { isAtObjective = atObjective; } + + Point ColumnObstacle::GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const + { + Point p = Point(msg->x, msg->y, msg->theta); + return GetOptimizedGetPos(p); + } + + Point ColumnObstacle::GetOptimizedGetPos(const Point& currentPos) const + { + auto distance = std::numeric_limits::max(); + double optimizedAngle = 0; + for (const auto& angle : possible_angles_) + { + auto newPos = position().GetTakePosition(400, angle); + double dist = Point::distance(currentPos, newPos); + if (dist < distance) + { + distance = dist; + optimizedAngle = angle; + } + } + + return Point(x_, y_, optimizedAngle); + } + + std::vector ColumnObstacle::GetAllPossiblePositions() const + { + std::vector positions; + for (const auto & angle : possible_angles_) + { + positions.push_back(Point(x_, y_, angle)); + } + return positions; + } } diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index 72ecfc4..fbc4dd9 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -3,13 +3,13 @@ #include #include -namespace Modelec { - +namespace Modelec +{ struct AStarNode { int x, y; - double g = std::numeric_limits::infinity(); // Cost from start - double f = std::numeric_limits::infinity(); // g + heuristic + double g = std::numeric_limits::infinity(); // Cost from start + double f = std::numeric_limits::infinity(); // g + heuristic int parent_x = -1; int parent_y = -1; @@ -21,7 +21,7 @@ namespace Modelec { double heuristic(int x1, int y1, int x2, int y2) { - return std::hypot(x1 - x2, y1 - y2); // Euclidean distance + return std::hypot(x1 - x2, y1 - y2); // Euclidean distance } Pathfinding::Pathfinding() @@ -45,7 +45,8 @@ namespace Modelec { grid_width_ = Config::get("config.map.size.grid_width", 300); grid_height_ = Config::get("config.map.size.grid_height", 200); - std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/obstacles.xml"; + std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") + + "/data/obstacles.xml"; if (!LoadObstaclesFromXML(obstacles_path)) { RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML"); @@ -53,7 +54,8 @@ namespace Modelec { obstacle_add_sub_ = node_->create_subscription( "obstacle/add", 10, - [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { + [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) + { RCLCPP_INFO(node_->get_logger(), "Obstacle add request received"); AddObstacle(std::make_shared(*msg)); }); @@ -63,7 +65,8 @@ namespace Modelec { obstacle_remove_sub_ = node_->create_subscription( "obstacle/remove", 10, - [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { + [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) + { RCLCPP_INFO(node_->get_logger(), "Obstacle remove request received"); RemoveObstacle(msg->id); }); @@ -74,7 +77,8 @@ namespace Modelec { ask_obstacle_srv_ = node_->create_service( "nav/ask_map_obstacle", [this](const std::shared_ptr request, - const std::shared_ptr response) { + const std::shared_ptr response) + { RCLCPP_INFO(node_->get_logger(), "Ask obstacle request received"); HandleAskObstacleRequest(request, response); }); @@ -82,7 +86,8 @@ namespace Modelec { map_srv_ = node_->create_service( "nav/map", [this](const std::shared_ptr request, - const std::shared_ptr response) { + const std::shared_ptr response) + { RCLCPP_INFO(node_->get_logger(), "Map request received"); HandleMapRequest(request, response); }); @@ -90,14 +95,16 @@ namespace Modelec { map_size_srv_ = node_->create_service( "nav/map_size", [this](const std::shared_ptr request, - const std::shared_ptr response) { + const std::shared_ptr response) + { RCLCPP_INFO(node_->get_logger(), "Map size request received"); HandleMapSizeRequest(request, response); }); enemy_pos_sub_ = node_->create_subscription( "enemy/position", 10, - [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { OnEnemyPosition(msg); }); @@ -139,12 +146,13 @@ namespace Modelec { } }*/ - WaypointListMsg Pathfinding::FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal, bool isClose) + std::pair Pathfinding::FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal, + bool isClose, int collisionMask) { if (!start || !goal) { RCLCPP_WARN(node_->get_logger(), "Start or Goal position is null"); - return WaypointListMsg(); + return {-3, WaypointListMsg()}; } WaypointListMsg waypoints; @@ -161,7 +169,7 @@ namespace Modelec { grid_.resize(grid_height_); for (auto& row : grid_) { - row.assign(grid_width_, 0); // 0 = free + row.assign(grid_width_, FREE); } if (has_enemy_pos_) @@ -169,8 +177,10 @@ namespace Modelec { int ex = (last_enemy_pos_.x / cell_size_mm_x); int ey = ((map_height_mm_ - last_enemy_pos_.y) / cell_size_mm_y); - const int inflate_enemy_x = std::ceil((enemy_margin_mm_ + (enemy_width_mm_/2)) / cell_size_mm_x) + inflate_x; - const int inflate_enemy_y = std::ceil((enemy_margin_mm_ + (enemy_length_mm_/2)) / cell_size_mm_y) + inflate_y; + const int inflate_enemy_x = std::ceil((enemy_margin_mm_ + (enemy_width_mm_ / 2)) / cell_size_mm_x) + + inflate_x; + const int inflate_enemy_y = std::ceil((enemy_margin_mm_ + (enemy_length_mm_ / 2)) / cell_size_mm_y) + + inflate_y; for (int y = ey - inflate_enemy_y; y <= ey + inflate_enemy_y; ++y) { @@ -178,7 +188,7 @@ namespace Modelec { { if (x >= 0 && y >= 0 && x < grid_width_ && y < grid_height_) { - grid_[y][x] = 1; + grid_[y][x] |= ENEMY; } } } @@ -189,8 +199,8 @@ namespace Modelec { { for (int x = 0; x < inflate_x; ++x) { - grid_[y][x] = 1; // bord gauche - grid_[y][grid_width_ - 1 - x] = 1; // bord droit + grid_[y][x] |= WALL; + grid_[y][grid_width_ - 1 - x] |= WALL; } } @@ -199,18 +209,19 @@ namespace Modelec { { for (int y = 0; y < inflate_y; ++y) { - grid_[y][x] = 1; // bord bas - grid_[grid_height_ - 1 - y][x] = 1; // bord haut + grid_[y][x] |= WALL; + grid_[grid_height_ - 1 - y][x] |= WALL; } } // 2. Fill obstacles with inflation + // TODO some bug exist with the inflate 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 width = obstacle->width() + inflate_x * 2 * cell_size_mm_x; + float height = obstacle->height() + inflate_y * 2 * cell_size_mm_y; float theta = M_PI_2 - obstacle->theta(); float dx = width / 2.0f; @@ -244,9 +255,9 @@ namespace Modelec { } int x_start = std::max(0, (int)(min_x / cell_size_mm_x)); - int x_end = std::min(grid_width_ - 1, (int)(max_x / cell_size_mm_x)); + int x_end = std::min(grid_width_ - 1, (int)(max_x / cell_size_mm_x)); int y_start = std::max(0, (int)(min_y / cell_size_mm_y)); - int y_end = std::min(grid_height_ - 1, (int)(max_y / cell_size_mm_y)); + int y_end = std::min(grid_height_ - 1, (int)(max_y / cell_size_mm_y)); // Mark cells that fall inside rotated rectangle for (int y = y_start; y <= y_end; ++y) @@ -269,7 +280,7 @@ namespace Modelec { int gy = (grid_height_ - 1) - y; if (x >= 0 && gy >= 0 && x < grid_width_ && gy < grid_height_) { - grid_[gy][x] = 1; + grid_[gy][x] |= OBSTACLE; } } } @@ -287,34 +298,37 @@ namespace Modelec { goal_x >= grid_width_ || goal_y >= grid_height_) { RCLCPP_WARN(node_->get_logger(), "Start or Goal out of bounds"); - return waypoints; + return {-2, waypoints}; } - if (grid_[start_y][start_x] == 1 || grid_[goal_y][goal_x] == 1) + if (!TestCollision(start_x, start_y, collisionMask) || !TestCollision(goal_x, goal_y, collisionMask)) { - if (grid_[start_y][start_x] == 1) + if (!TestCollision(start_x, start_y, collisionMask)) { RCLCPP_WARN(node_->get_logger(), "Start inside an obstacle"); + return {grid_[start_y][start_x], waypoints}; } else { RCLCPP_WARN(node_->get_logger(), "Goal inside an obstacle"); + return {grid_[goal_y][goal_x], waypoints}; } - return waypoints; } // 4. A* algorithm std::priority_queue, std::greater> open_list; std::unordered_map all_nodes; - auto hash = [](int x, int y) { + auto hash = [](int x, int y) + { return (int64_t)x << 32 | (uint32_t)y; }; - auto neighbors = [](int x, int y) { + auto neighbors = [](int x, int y) + { return std::vector>{ - {x+1, y}, {x-1, y}, {x, y+1}, {x, y-1}, - {x+1, y+1}, {x-1, y+1}, {x+1, y-1}, {x-1, y-1} // diagonals + {x + 1, y}, {x - 1, y}, {x, y + 1}, {x, y - 1}, + {x + 1, y + 1}, {x - 1, y + 1}, {x + 1, y - 1}, {x - 1, y - 1} // diagonals }; }; @@ -343,7 +357,7 @@ namespace Modelec { if (nx < 0 || ny < 0 || ny >= (int)grid_.size() || nx >= (int)grid_[0].size()) continue; - if (grid_[ny][nx] == 1) + if (!TestCollision(nx, ny, collisionMask)) continue; double tentative_g = current.g + heuristic(current.x, current.y, nx, ny); @@ -367,7 +381,7 @@ namespace Modelec { if (!path_found) { RCLCPP_WARN(node_->get_logger(), "No path found"); - return waypoints; + return {-1, waypoints}; } // 5. Reconstruct path @@ -411,7 +425,7 @@ namespace Modelec { while (true) { - if (grid_[y][x] == 1) + if (!TestCollision(x, y, collisionMask)) { clear = false; break; @@ -477,7 +491,7 @@ namespace Modelec { waypoints.back().is_end = true; } - return waypoints; + return {FREE, waypoints}; } /*void Pathfinding::SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal) @@ -518,20 +532,28 @@ namespace Modelec { obstacle_add_pub_->publish(msg); } - std::shared_ptr Pathfinding::GetClosestColumn(const PosMsg::SharedPtr& pos) + std::shared_ptr Pathfinding::GetClosestColumn(const PosMsg::SharedPtr& pos, + const std::vector& blacklistedId) { 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()) + for (const auto& [id, obstacle] : obstacle_map_) + { + if (auto obs = std::dynamic_pointer_cast(obstacle)) + { + if (!obs->IsAtObjective() && std::find(blacklistedId.begin(), blacklistedId.end(), obs->id()) == + blacklistedId.end()) { - auto dist = Point::distance(robotPos, obs->position()); - if (dist < distance) { - distance = dist; - closest_obstacle = obs; + for (auto possiblePos : obs->GetAllPossiblePositions()) + { + auto dist = Point::distance(robotPos, possiblePos); + if (dist < distance) + { + distance = dist; + closest_obstacle = obs; + } } } } @@ -556,16 +578,16 @@ namespace Modelec { } void Pathfinding::HandleMapSizeRequest(const std::shared_ptr, - const std::shared_ptr response) + const std::shared_ptr response) { response->width = grid_width_; response->height = grid_height_; } void Pathfinding::HandleAskObstacleRequest(const std::shared_ptr, - const std::shared_ptr) + const std::shared_ptr) { - for (auto & [index, obs] : obstacle_map_) + for (auto& [index, obs] : obstacle_map_) { obstacle_add_pub_->publish(obs->toMsg()); } @@ -585,6 +607,18 @@ namespace Modelec { }*/ } + bool Pathfinding::TestCollision(int x, int y, int collisionMask) + { + if (y < 0 || y >= static_cast(grid_.size()) || + x < 0 || x >= static_cast(grid_[y].size())) + { + RCLCPP_WARN(node_->get_logger(), "TestCollision: access out of bounds x=%d y=%d", x, y); + return false; // ou true, selon ce que tu veux (false = pas de collision) + } + + return (grid_[y][x] & collisionMask) && !(grid_[y][x] & ~collisionMask); + } + bool Pathfinding::LoadObstaclesFromXML(const std::string& filename) { tinyxml2::XMLDocument doc; diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index e6d0858..884420a 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -64,7 +64,6 @@ namespace Modelec break; case State::WAIT_START: - RCLCPP_INFO_ONCE(get_logger(), "State: WAIT_START"); if (started_) { match_start_time_ = now; @@ -110,6 +109,11 @@ namespace Modelec current_mission_.reset(); transition(State::SELECT_MISSION, "PrepareConcert finished"); } + else if (current_mission_->getStatus() == MissionStatus::FAILED) + { + current_mission_.reset(); + transition(State::SELECT_MISSION, "PrepareConcert failed"); + } break; case State::DO_PROMOTION: @@ -124,6 +128,11 @@ namespace Modelec current_mission_.reset(); transition(State::SELECT_MISSION, "Promotion finished"); } + else if (current_mission_->getStatus() == MissionStatus::FAILED) + { + current_mission_.reset(); + transition(State::SELECT_MISSION, "Promotion failed"); + } break; case State::DO_GO_HOME: diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp index db850d2..410333a 100644 --- a/src/modelec_utils/include/modelec_utils/point.hpp +++ b/src/modelec_utils/include/modelec_utils/point.hpp @@ -12,7 +12,9 @@ namespace Modelec static double distance(const Point& p1, const Point& p2); - Point GetTakeBasePosition(); - Point GetTakeClosePosition(); + Point GetTakePosition(int distance, double angle) const; + + Point GetTakeBasePosition() const; + Point GetTakeClosePosition() const; }; } diff --git a/src/modelec_utils/src/point.cpp b/src/modelec_utils/src/point.cpp index 68ee198..8ccdf7c 100644 --- a/src/modelec_utils/src/point.cpp +++ b/src/modelec_utils/src/point.cpp @@ -8,22 +8,23 @@ namespace Modelec return sqrt(std::pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2)); } - Point Point::GetTakeBasePosition() + Point Point::GetTakePosition(int distance, double angle) const { Point pos; - pos.x = x + 400 * cos(theta); - pos.y = y + 400 * sin(theta); - pos.theta = theta + M_PI; + pos.x = x + distance * cos(angle); + pos.y = y + distance * sin(angle); + pos.theta = angle + M_PI; return pos; } - Point Point::GetTakeClosePosition() + Point Point::GetTakeBasePosition() const { - Point pos; - pos.x = x + 210 * cos(theta); - pos.y = y + 210 * sin(theta); - pos.theta = theta + M_PI; - return pos; + return GetTakePosition(300, theta); + } + + Point Point::GetTakeClosePosition() const + { + return GetTakePosition(210, theta); } }