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

update enemy detection

Browse Source
This commit is contained in:
acki
2025-05-05 21:21:17 -04:00
parent 14792579d8
commit 43c57a7789
11 changed files with 80 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/modelec_strat/data/config.xml
View File

@@ -8,7 +8,7 @@
<detection>
<min_move_threshold_mm>50</min_move_threshold_mm>
<refresh_rate>0.5</refresh_rate>
<max_stationary_time_s>10</max_stationary_time_s>
<max_stationary_time_s>5</max_stationary_time_s>
</detection>
<factor_close_enemy>-0.3</factor_close_enemy>
</enemy>
@@ -31,8 +31,12 @@
</size>
</map>
<spawn>
<home>
<blue x="2650" y="1600" theta="-1.5708" />
<yellow x="350" y="1600" theta="-1.5708" />
</home>
<spawn>
<blue x="100" y="1600" theta="-1.5708" />
<yellow x="2650" y="1600" theta="-1.5708" />
</spawn>
</config>

32
src/modelec_strat/data/deposite_zone.xml
View File

@@ -1,14 +1,14 @@
<Map>
<!-- YELLOW ZONE -->
<DepositeZone id="0" team="0" max_pot="1">
<Pos x="775" y="75" theta="1.5708" />
<DepositeZone id="100" team="0" max_pot="1">
<Pos x="775" y="75" theta="1.5708" w="450" h="150" />
<PotPos>
<Pos x="775" y="75" theta="1.5708" />
</PotPos>
</DepositeZone>
<DepositeZone id="1" team="0" max_pot="3">
<Pos x="1225" y="225" theta="1.5708" />
<DepositeZone id="101" team="0" max_pot="3">
<Pos x="1225" y="225" theta="1.5708" w="450" h="450" />
<PotPos>
<Pos x="1225" y="75" theta="1.5708" />
<Pos x="1225" y="225" theta="1.5708" />
@@ -16,15 +16,15 @@
</PotPos>
</DepositeZone>
<DepositeZone id="2" team="0" max_pot="1">
<Pos x="2775" y="75" theta="1.5708" />
<DepositeZone id="102" team="0" max_pot="1">
<Pos x="2775" y="75" theta="1.5708" w="450" h="150" />
<PotPos>
<Pos x="2775" y="75" theta="1.5708" />
</PotPos>
</DepositeZone>
<DepositeZone id="3" team="0" max_pot="3">
<Pos x="2775" y="875" theta="3.1415" />
<DepositeZone id="103" team="0" max_pot="3">
<Pos x="2775" y="875" theta="3.1415" w="450" h="450" />
<PotPos>
<Pos x="2925" y="875" theta="3.1415" />
<Pos x="2775" y="875" theta="3.1415" />
@@ -33,15 +33,15 @@
</DepositeZone>
<!-- BLUE ZONE -->
<DepositeZone id="10" team="1" max_pot="1">
<Pos x="2225" y="75" theta="1.5708" />
<DepositeZone id="110" team="1" max_pot="1">
<Pos x="2225" y="75" theta="1.5708" w="450" h="150" />
<PotPos>
<Pos x="2225" y="75" theta="1.5708" />
</PotPos>
</DepositeZone>
<DepositeZone id="11" team="1" max_pot="3">
<Pos x="1775" y="225" theta="1.5708" />
<DepositeZone id="111" team="1" max_pot="3">
<Pos x="1775" y="225" theta="1.5708" w="450" h="450" />
<PotPos>
<Pos x="1775" y="75" theta="1.5708" />
<Pos x="1775" y="225" theta="1.5708" />
@@ -49,15 +49,15 @@
</PotPos>
</DepositeZone>
<DepositeZone id="12" team="1" max_pot="1">
<Pos x="225" y="75" theta="1.5708" />
<DepositeZone id="112" team="1" max_pot="1">
<Pos x="225" y="75" theta="1.5708" w="450" h="150" />
<PotPos>
<Pos x="225" y="75" theta="1.5708" />
</PotPos>
</DepositeZone>
<DepositeZone id="13" team="1" max_pot="3">
<Pos x="225" y="875" theta="0" />
<DepositeZone id="113" team="1" max_pot="3">
<Pos x="225" y="875" theta="0" w="450" h="450" />
<PotPos>
<Pos x="75" y="875" theta="0" />
<Pos x="225" y="875" theta="0" />

4
src/modelec_strat/include/modelec_strat/deposite_zone.hpp
View File

@@ -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<Point> pot_queue_;

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

@@ -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<PosMsg>::SharedPtr pos_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_long_time_sub_;
modelec_interfaces::msg::OdometryPos last_enemy_pos_;

2
src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp
View File

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

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

@@ -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<modelec_interfaces::srv::Map::Request> request,

2
src/modelec_strat/src/deposite_zone.cpp
View File

@@ -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");

2
src/modelec_strat/src/enemy_manager.cpp
View File

@@ -44,7 +44,7 @@ namespace Modelec
});
enemy_long_time_pub_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
"enemy/long-time", 10);
"/enemy/long-time", 10);
timer_ = this->create_wall_timer(
std::chrono::seconds(1),

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

@@ -41,6 +41,13 @@ namespace Modelec {
OnEnemyPosition(msg);
});
enemy_pos_long_time_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
"/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<PosMsg>();
// TODO : handle Team Id
if (team_id_ == YELLOW)
{
home->x = Config::get<int>("config.spawn.yellow@x", 0);
home->y = Config::get<int>("config.spawn.yellow@y", 0);
home->theta = Config::get<double>("config.spawn.yellow@theta", 0);
home->x = Config::get<int>("config.home.yellow@x", 0);
home->y = Config::get<int>("config.home.yellow@y", 0);
home->theta = Config::get<double>("config.home.yellow@theta", 0);
}
else
{
home->x = Config::get<int>("config.spawn.blue@x", 0);
home->y = Config::get<int>("config.spawn.blue@y", 0);
home->theta = Config::get<double>("config.spawn.blue@theta", 0);
home->x = Config::get<int>("config.home.blue@x", 0);
home->y = Config::get<int>("config.home.blue@y", 0);
home->theta = Config::get<double>("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<Obstacle> obs = std::make_shared<Obstacle>(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)

2
src/modelec_strat/src/obstacle/column.cpp
View File

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

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

@@ -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<ColumnObstacle>(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<int>(grid_.size()) ||