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>
<refresh_rate>3</refresh_rate>
<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>
</detection>
<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;
int enemy_emergency_distance_ = 0;
bool last_was_close_enemy_ = false;
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);
enemy_emergency_distance_ = Config::get<int>("config.enemy.detection.min_emergency_distance_mm", 350);
SetupSpawn();
waypoint_reach_sub_ = node_->create_subscription<WaypointReachMsg>(
@@ -76,7 +78,7 @@ namespace Modelec
ask_spawn_srv_ = node->create_service<std_srvs::srv::Empty>(
"/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_)
{
@@ -160,22 +162,22 @@ namespace Modelec
waypoint_queue_.pop();
}
/* void NavigationHelper::SendWaypoint() const
{
for (auto& w : waypoints_)
/* void NavigationHelper::SendWaypoint() const
{
waypoint_pub_->publish(w.ToMsg());
for (auto& w : waypoints_)
{
waypoint_pub_->publish(w.ToMsg());
}
}
}
void NavigationHelper::SendWaypoint(const std::vector<WaypointMsg>& waypoints) const
{
for (auto& w : waypoints)
void NavigationHelper::SendWaypoint(const std::vector<WaypointMsg>& waypoints) const
{
waypoint_pub_->publish(w);
for (auto& w : waypoints)
{
waypoint_pub_->publish(w);
}
}
}
*/
*/
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->theta = Config::get<double>("config.home.blue@theta", 0);
}
return home; }
return home;
}
void NavigationHelper::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
pathfinding_->OnEnemyPosition(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...");
@@ -589,7 +593,6 @@ namespace Modelec
void NavigationHelper::OnEnemyPositionClose(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
if (!last_was_close_enemy_)
{
RCLCPP_INFO(node_->get_logger(), "Enemy is close, replanning...");
@@ -633,7 +636,8 @@ namespace Modelec
bool NavigationHelper::EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg)
{
if (!current_pos_) {
if (!current_pos_)
{
return false;
}