diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 58d61f2..68fe8f0 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -8,7 +8,7 @@ 50 0.5 - 10 + 5 -0.3 @@ -31,8 +31,12 @@ - + + + + + diff --git a/src/modelec_strat/data/deposite_zone.xml b/src/modelec_strat/data/deposite_zone.xml index 5601996..0c1cc75 100644 --- a/src/modelec_strat/data/deposite_zone.xml +++ b/src/modelec_strat/data/deposite_zone.xml @@ -1,14 +1,14 @@ - - + + - - + + @@ -16,15 +16,15 @@ - - + + - - + + @@ -33,15 +33,15 @@ - - + + - - + + @@ -49,15 +49,15 @@ - - + + - - + + diff --git a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp index 3bb3848..f3423e6 100644 --- a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp +++ b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp @@ -34,6 +34,9 @@ namespace Modelec int GetId() const { return id_; } int GetMaxPot() const { return max_pot_; } + int GetWidth() const { return w_; } + int GetHeight() const { return h_; } + int RemainingPotPos() const { return pot_queue_.size(); @@ -41,6 +44,7 @@ namespace Modelec protected: int id_, team_, max_pot_; + int w_, h_; Point position_; std::queue pot_queue_; diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 37b0e1c..c1f7d56 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -70,6 +70,8 @@ namespace Modelec { void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); + void OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); + bool DoesLineIntersectCircle(const Point& start, const Point& end, const Point& center, float radius); @@ -111,6 +113,7 @@ namespace Modelec { rclcpp::Subscription::SharedPtr pos_sub_; rclcpp::Subscription::SharedPtr enemy_pos_sub_; + rclcpp::Subscription::SharedPtr enemy_pos_long_time_sub_; modelec_interfaces::msg::OdometryPos last_enemy_pos_; diff --git a/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp b/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp index 0e550be..7391edd 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp @@ -28,7 +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_); } + Point GetPosition() const { return Point(x_, y_, theta_); } void setId(int id) { id_ = id; } void setX(int x) { x_ = x; } diff --git a/src/modelec_strat/include/modelec_strat/pathfinding.hpp b/src/modelec_strat/include/modelec_strat/pathfinding.hpp index 8a68fe1..aa6248d 100644 --- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -90,6 +90,8 @@ namespace Modelec { void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); + void OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); + protected: void HandleMapRequest( const std::shared_ptr request, diff --git a/src/modelec_strat/src/deposite_zone.cpp b/src/modelec_strat/src/deposite_zone.cpp index 2110bc6..e724539 100644 --- a/src/modelec_strat/src/deposite_zone.cpp +++ b/src/modelec_strat/src/deposite_zone.cpp @@ -15,6 +15,8 @@ namespace Modelec posElem->QueryIntAttribute("x", &position_.x); posElem->QueryIntAttribute("y", &position_.y); posElem->QueryDoubleAttribute("theta", &position_.theta); + posElem->QueryIntAttribute("w", &w_); + posElem->QueryIntAttribute("h", &h_); } auto posPotList = obstacleElem->FirstChildElement("PotPos"); diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp index 9806e45..963ffe2 100644 --- a/src/modelec_strat/src/enemy_manager.cpp +++ b/src/modelec_strat/src/enemy_manager.cpp @@ -44,7 +44,7 @@ namespace Modelec }); enemy_long_time_pub_ = this->create_publisher( - "enemy/long-time", 10); + "/enemy/long-time", 10); timer_ = this->create_wall_timer( std::chrono::seconds(1), diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index df57caf..314ee4f 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -41,6 +41,13 @@ namespace Modelec { OnEnemyPosition(msg); }); + enemy_pos_long_time_sub_ = node_->create_subscription( + "/enemy/long-time", 10, + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { + OnEnemyPositionLongTime(msg); + }); + std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/deposite_zone.xml"; if (!LoadDepositeZoneFromXML(deposite_zone_path)) { @@ -397,18 +404,17 @@ namespace Modelec { 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); + home->x = Config::get("config.home.yellow@x", 0); + home->y = Config::get("config.home.yellow@y", 0); + home->theta = Config::get("config.home.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); + home->x = Config::get("config.home.blue@x", 0); + home->y = Config::get("config.home.blue@y", 0); + home->theta = Config::get("config.home.blue@theta", 0); } return home; } @@ -425,6 +431,22 @@ namespace Modelec { } } + void NavigationHelper::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { + pathfinding_->OnEnemyPositionLongTime(msg); + + Point enemy_pos(msg->x, msg->y, msg->theta); + for (auto& [id, zone] : deposite_zones_) + { + auto zonePos = zone->GetPosition(); + if (Point::distance(enemy_pos, zonePos) < pathfinding_->robot_width_mm_ + (zone->GetWidth()/2) + pathfinding_->enemy_margin_mm_) + { + std::shared_ptr obs = std::make_shared(id, zonePos.x, zonePos.y, zonePos.theta, zone->GetWidth(), zone->GetHeight(), "enemy-zone"); + pathfinding_->AddObstacle(obs); + } + } + } + bool NavigationHelper::EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg) { for (size_t i = -1; i + 1 < waypoints_.size(); ++i) diff --git a/src/modelec_strat/src/obstacle/column.cpp b/src/modelec_strat/src/obstacle/column.cpp index bd367a1..b6ea17a 100644 --- a/src/modelec_strat/src/obstacle/column.cpp +++ b/src/modelec_strat/src/obstacle/column.cpp @@ -46,7 +46,7 @@ namespace Modelec double optimizedAngle = 0; for (const auto& angle : possible_angles_) { - auto newPos = position().GetTakePosition(400, angle); + auto newPos = GetPosition().GetTakePosition(400, angle); double dist = Point::distance(currentPos, newPos); if (dist < distance) { diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index 68e737d..04f3c47 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -599,6 +599,21 @@ namespace Modelec has_enemy_pos_ = true; } + void Pathfinding::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { + Point enemyPos(msg->x, msg->y, msg->theta); + for (auto& [index, obs] : obstacle_map_) + { + if (auto column = std::dynamic_pointer_cast(obs)) + { + if (Point::distance(enemyPos, column->GetPosition()) < enemy_width_mm_ + (column->width() / 2) + enemy_margin_mm_) + { + RemoveObstacle(column->id()); + } + } + } + } + bool Pathfinding::TestCollision(int x, int y, int collisionMask) { if (y < 0 || y >= static_cast(grid_.size()) ||