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

enemy close

Browse Source
This commit is contained in:
acki
2025-05-27 21:42:31 -04:00
parent 46afb6324f
commit 33b7795e4a
5 changed files with 45 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -9,6 +9,7 @@
<min_move_threshold_mm>50</min_move_threshold_mm>
<refresh_rate>0.5</refresh_rate>
<max_stationary_time_s>5</max_stationary_time_s>
<min_emergency_distance_mm>100</min_emergency_distance_mm>
</detection>
<factor_close_enemy>-0.3</factor_close_enemy>
</enemy>

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

@@ -25,6 +25,7 @@ namespace Modelec
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_scan_sub_;
rclcpp::Subscription<modelec_interfaces::msg::StratState>::SharedPtr state_sub_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_pub_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr close_enemy_pos_pub_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_long_time_pub_;
rclcpp::TimerBase::SharedPtr timer_;
@@ -46,5 +47,6 @@ namespace Modelec
float robot_width_ = 0;
float robot_length_ = 0;
double robot_radius_ = 0;
float min_emergency_distance_ = 0.0f;
};
}

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

@@ -80,6 +80,7 @@ namespace Modelec
PosMsg::SharedPtr GetHomePosition();
void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
void OnEnemyPositionClose(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
void OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
@@ -124,6 +125,8 @@ namespace Modelec
float factor_close_enemy_ = 0;
bool last_was_close_enemy_ = false;
std::vector<Waypoint> waypoints_;
PosMsg::SharedPtr current_pos_;
@@ -138,6 +141,7 @@ namespace Modelec
rclcpp::Publisher<PosMsg>::SharedPtr pos_pub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr close_enemy_pos_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_long_time_sub_;
modelec_interfaces::msg::OdometryPos last_enemy_pos_;

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

@@ -26,6 +26,8 @@ namespace Modelec
robot_length_ = Config::get<float>("config.robot.size.length_mm", 500.0);
robot_radius_ = std::max(robot_width_, robot_length_) / 2.0;
min_emergency_distance_ = Config::get<float>("config.enemy.detection.min_emergency_distance_mm", 100.0);
RCLCPP_INFO(get_logger(), "Configuration loaded:");
RCLCPP_INFO(get_logger(), " min_move_threshold_mm: %f", min_move_threshold_mm_);
RCLCPP_INFO(get_logger(), " refresh_rate_s: %f", refresh_rate_s_);
@@ -35,6 +37,7 @@ namespace Modelec
RCLCPP_INFO(get_logger(), " robot_width_mm: %f", robot_width_);
RCLCPP_INFO(get_logger(), " robot_length_mm: %f", robot_length_);
RCLCPP_INFO(get_logger(), " robot_radius_mm: %f", robot_radius_);
RCLCPP_INFO(get_logger(), " min_emergency_distance_mm: %f", min_emergency_distance_);
current_pos_sub_ = this->create_subscription<modelec_interfaces::msg::OdometryPos>(
"odometry/position", 10,
@@ -53,6 +56,9 @@ namespace Modelec
enemy_pos_pub_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
"enemy/position", 10);
close_enemy_pos_pub_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
"enemy/position/emergency", 10);
state_sub_ = create_subscription<modelec_interfaces::msg::StratState>("/strat/state", 10,
[this](
const

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

@@ -49,6 +49,13 @@ namespace Modelec
OnEnemyPosition(msg);
});
close_enemy_pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
"enemy/position/emergency", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
OnEnemyPositionClose(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)
@@ -474,18 +481,9 @@ namespace Modelec
PosMsg::SharedPtr NavigationHelper::GetHomePosition()
{
PosMsg::SharedPtr home = std::make_shared<PosMsg>();
if (team_id_ == YELLOW)
{
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.home.blue@x", 0);
home->y = Config::get<int>("config.home.blue@y", 0);
home->theta = Config::get<double>("config.home.blue@theta", 0);
}
home->x = spawn_.x;
home->y = spawn_.y;
home->theta = spawn_.theta;
return home;
}
@@ -501,6 +499,28 @@ namespace Modelec
}
}
void NavigationHelper::OnEnemyPositionClose(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
last_was_close_enemy_ = true;
pathfinding_->OnEnemyPosition(msg);
last_enemy_pos_ = *msg;
// TODO : try to replan next frame
waypoints_.clear();
WaypointMsg emptyWaypoint;
emptyWaypoint.x = current_pos_->x;
emptyWaypoint.y = current_pos_->y;
emptyWaypoint.theta = current_pos_->theta;
emptyWaypoint.id = 0;
emptyWaypoint.is_end = true;
waypoints_.emplace_back(emptyWaypoint);
SendWaypoint();
}
void NavigationHelper::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
pathfinding_->OnEnemyPositionLongTime(msg);