modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-01-18 16:47:43 +01:00
Code Issues Packages Projects Releases Wiki Activity

replan if enemy on path

Browse Source
This commit is contained in:
acki
2025-05-05 14:20:41 -04:00
parent 7a299626eb
commit 501814dab2
8 changed files with 98 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
simulated_pcb/alim.py
View File

@@ -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

2
simulated_pcb/odo.py
View File

@@ -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

2
src/modelec_com/src/pcb_alim_interface.cpp
View File

@@ -10,7 +10,7 @@ namespace Modelec
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
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<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{

2
src/modelec_com/src/pcb_odo_interface.cpp
View File

@@ -10,7 +10,7 @@ namespace Modelec
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
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<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{

20
src/modelec_strat/include/modelec_strat/navigation_helper.hpp
View File

@@ -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<Waypoint> waypoints_;
std::vector<Waypoint> waypoints_;
PosMsg::SharedPtr current_pos_;
@@ -83,5 +99,7 @@ namespace Modelec {
rclcpp::Subscription<PosMsg>::SharedPtr go_to_sub_;
rclcpp::Subscription<PosMsg>::SharedPtr pos_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
};
}

26
src/modelec_strat/include/modelec_strat/pathfinding.hpp
View File

@@ -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<ColumnObstacle> GetClosestColumn(const PosMsg::SharedPtr& pos, const std::vector<int>& blacklistedId = {});
void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
protected:
void HandleMapRequest(
const std::shared_ptr<modelec_interfaces::srv::Map::Request> request,
@@ -89,8 +102,6 @@ namespace Modelec {
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
const std::shared_ptr<std_srvs::srv::Empty::Response> 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<std::vector<int>> 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<int, std::shared_ptr<Obstacle>> obstacle_map_;

62
src/modelec_strat/src/navigation_helper.cpp
View File

@@ -32,6 +32,13 @@ namespace Modelec {
GoTo(msg, false, Pathfinding::FREE | Pathfinding::WALL);
});
enemy_pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
"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_)

15
src/modelec_strat/src/pathfinding.cpp
View File

@@ -101,13 +101,6 @@ namespace Modelec
HandleMapSizeRequest(request, response);
});
enemy_pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
"enemy/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
OnEnemyPosition(msg);
});
waypoint_pub_ = node_->create_publisher<WaypointMsg>(
"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)