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

detection min

Browse Source
This commit is contained in:
acki
2025-05-30 06:38:13 -04:00
parent 6fed7a33da
commit 066fbd5312
3 changed files with 23 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -9,7 +9,7 @@
<min_move_threshold_mm>50</min_move_threshold_mm> <min_move_threshold_mm>50</min_move_threshold_mm>
<refresh_rate>3</refresh_rate> <refresh_rate>3</refresh_rate>
<max_stationary_time_s>0.5</max_stationary_time_s> <max_stationary_time_s>0.5</max_stationary_time_s>
<min_emergency_distance_mm>500</min_emergency_distance_mm> <min_emergency_distance_mm>370</min_emergency_distance_mm>
<margin_detection_table_mm>50</margin_detection_table_mm> <margin_detection_table_mm>50</margin_detection_table_mm>
</detection> </detection>
<factor_close_enemy>-0.3</factor_close_enemy> <factor_close_enemy>-0.3</factor_close_enemy>

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

@@ -129,6 +129,8 @@ namespace Modelec
float factor_close_enemy_ = 0; float factor_close_enemy_ = 0;
int enemy_emergency_distance_ = 0;
bool last_was_close_enemy_ = false; bool last_was_close_enemy_ = false;
std::vector<Waypoint> waypoints_; std::vector<Waypoint> waypoints_;

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

@@ -16,6 +16,8 @@ namespace Modelec
factor_close_enemy_ = Config::get<float>("config.enemy.factor_close_enemy", -0.5f); factor_close_enemy_ = Config::get<float>("config.enemy.factor_close_enemy", -0.5f);
enemy_emergency_distance_ = Config::get<int>("config.enemy.detection.min_emergency_distance_mm", 350);
SetupSpawn(); SetupSpawn();
waypoint_reach_sub_ = node_->create_subscription<WaypointReachMsg>( waypoint_reach_sub_ = node_->create_subscription<WaypointReachMsg>(
@@ -76,7 +78,7 @@ namespace Modelec
ask_spawn_srv_ = node->create_service<std_srvs::srv::Empty>( ask_spawn_srv_ = node->create_service<std_srvs::srv::Empty>(
"/nav/ask_spawn", [this](const std_srvs::srv::Empty::Request::SharedPtr, "/nav/ask_spawn", [this](const std_srvs::srv::Empty::Request::SharedPtr,
const std_srvs::srv::Empty::Response::SharedPtr) const std_srvs::srv::Empty::Response::SharedPtr)
{ {
for (auto& ys : spawn_yellow_) for (auto& ys : spawn_yellow_)
{ {
@@ -160,22 +162,22 @@ namespace Modelec
waypoint_queue_.pop(); waypoint_queue_.pop();
} }
/* void NavigationHelper::SendWaypoint() const /* void NavigationHelper::SendWaypoint() const
{
for (auto& w : waypoints_)
{ {
waypoint_pub_->publish(w.ToMsg()); for (auto& w : waypoints_)
{
waypoint_pub_->publish(w.ToMsg());
}
} }
}
void NavigationHelper::SendWaypoint(const std::vector<WaypointMsg>& waypoints) const void NavigationHelper::SendWaypoint(const std::vector<WaypointMsg>& waypoints) const
{
for (auto& w : waypoints)
{ {
waypoint_pub_->publish(w); for (auto& w : waypoints)
{
waypoint_pub_->publish(w);
}
} }
} */
*/
void NavigationHelper::AddWaypoint(const PosMsg& pos, int index) void NavigationHelper::AddWaypoint(const PosMsg& pos, int index)
{ {
@@ -543,14 +545,16 @@ namespace Modelec
home->y = Config::get<int>("config.home.blue@y", 0); home->y = Config::get<int>("config.home.blue@y", 0);
home->theta = Config::get<double>("config.home.blue@theta", 0); home->theta = Config::get<double>("config.home.blue@theta", 0);
} }
return home; } return home;
}
void NavigationHelper::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) void NavigationHelper::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{ {
pathfinding_->OnEnemyPosition(msg); pathfinding_->OnEnemyPosition(msg);
last_enemy_pos_ = *msg; last_enemy_pos_ = *msg;
if (Point::distance(Point(msg->x, msg->y, msg->theta), Point(current_pos_->x, current_pos_->y, current_pos_->theta)) < 600) if (Point::distance(Point(msg->x, msg->y, msg->theta),
Point(current_pos_->x, current_pos_->y, current_pos_->theta)) < enemy_emergency_distance_)
{ {
RCLCPP_INFO(node_->get_logger(), "Enemy is close, stopping odometry..."); RCLCPP_INFO(node_->get_logger(), "Enemy is close, stopping odometry...");
@@ -589,7 +593,6 @@ namespace Modelec
void NavigationHelper::OnEnemyPositionClose(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) void NavigationHelper::OnEnemyPositionClose(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{ {
if (!last_was_close_enemy_) if (!last_was_close_enemy_)
{ {
RCLCPP_INFO(node_->get_logger(), "Enemy is close, replanning..."); RCLCPP_INFO(node_->get_logger(), "Enemy is close, replanning...");
@@ -633,7 +636,8 @@ namespace Modelec
bool NavigationHelper::EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg) bool NavigationHelper::EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg)
{ {
if (!current_pos_) { if (!current_pos_)
{
return false; return false;
} }