From 501814dab24035f661f8b7a63a7760c0c5888879 Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 5 May 2025 14:20:41 -0400 Subject: [PATCH] replan if enemy on path --- 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 +- .../modelec_strat/navigation_helper.hpp | 20 +++++- .../include/modelec_strat/pathfinding.hpp | 26 ++++---- src/modelec_strat/src/navigation_helper.cpp | 62 +++++++++++++++++++ src/modelec_strat/src/pathfinding.cpp | 15 ----- 8 files changed, 98 insertions(+), 33 deletions(-) diff --git a/simulated_pcb/alim.py b/simulated_pcb/alim.py index 8776df3..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/11' # 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 639d43c..0a751f9 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/9', 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 dbf6747..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/12"; // 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 adab14c..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/10"; // 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_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index b8d20a1..fbce313 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -60,11 +60,27 @@ namespace Modelec { PosMsg::SharedPtr GetHomePosition(); + void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); + + bool DoesLineIntersectCircle(const Point& start, const Point& end, + const Point& center, float radius); + + bool EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg); + + void Replan(); + protected: void OnWaypointReach(const WaypointReachMsg::SharedPtr msg); void OnPos(const PosMsg::SharedPtr msg); + struct + { + PosMsg::SharedPtr goal; + bool isClose; + int collisionMask; + } last_go_to_; + private: rclcpp::Node::SharedPtr node_; @@ -72,7 +88,7 @@ namespace Modelec { int team_id_ = YELLOW; - std::list waypoints_; + std::vector waypoints_; PosMsg::SharedPtr current_pos_; @@ -83,5 +99,7 @@ namespace Modelec { rclcpp::Subscription::SharedPtr go_to_sub_; rclcpp::Subscription::SharedPtr pos_sub_; + + rclcpp::Subscription::SharedPtr enemy_pos_sub_; }; } \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/pathfinding.hpp b/src/modelec_strat/include/modelec_strat/pathfinding.hpp index 9bed61a..3340db8 100644 --- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -48,6 +48,17 @@ namespace Modelec { ENEMY = 1 << 3, }; + int grid_width_ = 0; + int grid_height_ = 0; + int map_width_mm_ = 0; + int map_height_mm_ = 0; + int robot_width_mm_ = 0; + int robot_length_mm_ = 0; + int enemy_width_mm_ = 0; + int enemy_length_mm_ = 0; + int enemy_margin_mm_ = 0; + int margin_mm_ = 0; + Pathfinding(); Pathfinding(const rclcpp::Node::SharedPtr& node); @@ -76,6 +87,8 @@ namespace Modelec { std::shared_ptr GetClosestColumn(const PosMsg::SharedPtr& pos, const std::vector& blacklistedId = {}); + void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); + protected: void HandleMapRequest( const std::shared_ptr request, @@ -89,8 +102,6 @@ namespace Modelec { const std::shared_ptr request, const std::shared_ptr response); - void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); - //bool EnemyOnPath(const modelec_interfaces::msg::OdometryPos& enemy_pos); //void Replan(); @@ -100,17 +111,6 @@ namespace Modelec { rclcpp::Node::SharedPtr node_; std::vector> grid_; - int grid_width_ = 0; - int grid_height_ = 0; - int map_width_mm_ = 0; - int map_height_mm_ = 0; - int robot_width_mm_ = 0; - int robot_length_mm_ = 0; - int enemy_width_mm_ = 0; - int enemy_length_mm_ = 0; - int enemy_margin_mm_ = 0; - int margin_mm_ = 0; - std::map> obstacle_map_; diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index ef5324a..3d6feda 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -32,6 +32,13 @@ namespace Modelec { GoTo(msg, false, Pathfinding::FREE | Pathfinding::WALL); }); + enemy_pos_sub_ = node_->create_subscription( + "enemy/position", 10, + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { + OnEnemyPosition(msg); + }); + std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/deposite_zone.xml"; if (!LoadDepositeZoneFromXML(deposite_zone_path)) { @@ -175,6 +182,8 @@ namespace Modelec { int NavigationHelper::GoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask) { + last_go_to_ = {goal, isClose, collisionMask}; + auto [res, wl] = FindPath(goal, isClose, collisionMask); if (wl.empty() || res != Pathfinding::FREE) @@ -306,6 +315,59 @@ namespace Modelec { return home; } + void NavigationHelper::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { + pathfinding_->OnEnemyPosition(msg); + + if (EnemyOnPath(*msg)) + { + RCLCPP_INFO(node_->get_logger(), "Enemy is blocking the path, replanning..."); + Replan(); + } + } + + bool NavigationHelper::EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg) + { + for (size_t i = -1; i + 1 < waypoints_.size(); ++i) + { + auto wp = i == -1 ? Waypoint(*current_pos_, -1, false) : waypoints_[i]; + auto next_wp = waypoints_[i + 1]; + if (DoesLineIntersectCircle( + Point(wp.x, wp.y, wp.theta), + Point(next_wp.x, next_wp.y, next_wp.theta), + Point(msg.x, msg.y, msg.theta), (pathfinding_->enemy_length_mm_ + pathfinding_->robot_length_mm_ + pathfinding_->enemy_margin_mm_) / 2.0f)) + { + return true; + } + } + + return false; + } + + bool NavigationHelper::DoesLineIntersectCircle(const Point& start, const Point& end, const Point& center, float radius) + { + float num = std::abs((end.y - start.y) * center.x - (end.x - start.x) * center.y + end.x * start.y - end.y * start.x); + float den = std::sqrt((end.y - start.y) * (end.y - start.y) + (end.x - start.x) * (end.x - start.x)); + float dist = num / den; + return dist < radius; + } + + void NavigationHelper::Replan() + { + if (last_go_to_.goal) + { + if (GoTo(last_go_to_.goal, last_go_to_.isClose, last_go_to_.collisionMask) != Pathfinding::FREE) + { + if (GoTo(last_go_to_.goal, true, last_go_to_.collisionMask) != Pathfinding::FREE) + { + // TODO : change to reset all the waypoints + GoTo(current_pos_, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE | Pathfinding::ENEMY); + // TODO : Handle case where no path is found + } + } + } + } + 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 7566381..dac7b2e 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -101,13 +101,6 @@ namespace Modelec HandleMapSizeRequest(request, response); }); - enemy_pos_sub_ = node_->create_subscription( - "enemy/position", 10, - [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) - { - OnEnemyPosition(msg); - }); - waypoint_pub_ = node_->create_publisher( "odometry/add_waypoint", 100); } @@ -608,14 +601,6 @@ namespace Modelec { last_enemy_pos_ = *msg; has_enemy_pos_ = true; - - RCLCPP_INFO(node_->get_logger(), "Enemy position updated: x=%ld, y=%ld", msg->x, msg->y); - - /*if (EnemyOnPath(last_enemy_pos_)) - { - RCLCPP_INFO(node_->get_logger(), "Enemy is blocking the path, replanning..."); - Replan(); - }*/ } bool Pathfinding::TestCollision(int x, int y, int collisionMask)