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

ENEMY ON DA COUNTER

Browse Source 
+ Rotate
This commit is contained in:
acki
2025-05-05 19:56:45 -04:00
parent 3f2ec7082f
commit 14792579d8
10 changed files with 193 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -8,7 +8,9 @@
<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>
</detection>
<factor_close_enemy>-0.3</factor_close_enemy>
</enemy>
<robot>

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

@@ -3,6 +3,7 @@
#include <rclcpp/rclcpp.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <modelec_interfaces/msg/strat_state.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
namespace Modelec
@@ -23,9 +24,13 @@ namespace Modelec
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr current_pos_sub_;
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 enemy_long_time_pub_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time last_movement_time_;
modelec_interfaces::msg::OdometryPos current_pos_;
modelec_interfaces::msg::OdometryPos last_enemy_pos_;
@@ -34,5 +39,8 @@ namespace Modelec
float min_move_threshold_mm_ = 0.0f;
float refresh_rate_s_ = 0.0f;
bool game_stated_ = false;
float max_stationary_time_s_ = 10.0f;
};
}

1
src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp
View File

@@ -19,6 +19,7 @@ namespace Modelec
private:
enum Step {
ROTATE_TO_HOME,
GO_HOME,
GO_CLOSE,
DONE

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

@@ -41,10 +41,18 @@ namespace Modelec {
bool HasArrived() const;
bool RotateTo(const PosMsg::SharedPtr& pos);
bool RotateTo(const Point& pos);
void Rotate(double angle);
int GoTo(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
int GoTo(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE);
int GoTo(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
int GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
int GoToRotateFirst(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE);
int GoToRotateFirst(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
int CanGoTo(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
int CanGoTo(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE);
int CanGoTo(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
@@ -88,6 +96,8 @@ namespace Modelec {
int team_id_ = YELLOW;
float factor_close_enemy_ = 0;
std::vector<Waypoint> waypoints_;
PosMsg::SharedPtr current_pos_;
@@ -103,5 +113,8 @@ namespace Modelec {
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
modelec_interfaces::msg::OdometryPos last_enemy_pos_;
bool await_rotate_ = false;
std::vector<Waypoint> send_back_waypoints_;
};
}

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

@@ -58,6 +58,7 @@ namespace Modelec {
int enemy_length_mm_ = 0;
int enemy_margin_mm_ = 0;
int margin_mm_ = 0;
float factor_close_enemy_ = 0;
Pathfinding();

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

@@ -17,6 +17,8 @@ namespace Modelec
refresh_rate_s_ = Config::get<float>("config.enemy.detection.refresh_rate", 1.0);
max_stationary_time_s_ = Config::get<float>("config.enemy.detection.max_stationary_time_s", 10.0);
current_pos_sub_ = this->create_subscription<modelec_interfaces::msg::OdometryPos>(
"odometry/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
@@ -32,6 +34,18 @@ namespace Modelec
enemy_pos_pub_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
"enemy/position", 10);
state_sub_ = create_subscription<modelec_interfaces::msg::StratState>("/strat/state", 10,
[this](const modelec_interfaces::msg::StratState::SharedPtr msg)
{
if (!game_stated_ && msg->state == modelec_interfaces::msg::StratState::SELECT_MISSION)
{
game_stated_ = true;
}
});
enemy_long_time_pub_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
"enemy/long-time", 10);
timer_ = this->create_wall_timer(
std::chrono::seconds(1),
std::bind(&EnemyManager::TimerCallback, this)
@@ -128,9 +142,21 @@ namespace Modelec
{
last_enemy_pos_ = enemy_pos;
last_publish_time_ = this->now();
last_movement_time_ = this->now(); // Mise à jour du dernier vrai mouvement
enemy_pos_pub_->publish(enemy_pos);
RCLCPP_INFO(this->get_logger(), "Enemy moved: x=%ld, y=%ld", enemy_pos.x, enemy_pos.y);
}
else
{
auto now = this->now();
if ((now - last_movement_time_).seconds() > max_stationary_time_s_)
{
enemy_long_time_pub_->publish(last_enemy_pos_);
RCLCPP_WARN(this->get_logger(), "Enemy has been stationary for too long at x=%ld y=%ld", last_enemy_pos_.x, last_enemy_pos_.y);
last_movement_time_ = now;
}
}
}
else
{

26
src/modelec_strat/src/missions/go_home_mission.cpp
View File

@@ -2,7 +2,7 @@
namespace Modelec
{
GoHomeMission::GoHomeMission(const std::shared_ptr<NavigationHelper>& nav, const rclcpp::Time& start_time) : step_(GO_HOME), status_(MissionStatus::READY), nav_(nav), start_time_(start_time)
GoHomeMission::GoHomeMission(const std::shared_ptr<NavigationHelper>& nav, const rclcpp::Time& start_time) : step_(ROTATE_TO_HOME), status_(MissionStatus::READY), nav_(nav), start_time_(start_time)
{
}
@@ -12,7 +12,15 @@ namespace Modelec
auto pos = nav_->GetHomePosition();
home_point_ = Point(pos->x, pos->y, pos->theta);
nav_->GoTo(home_point_.GetTakeBasePosition());
if (nav_->CanGoTo(home_point_.GetTakeBasePosition()) != Pathfinding::FREE)
{
if (nav_->CanGoTo(home_point_.GetTakeBasePosition(), true) != Pathfinding::FREE)
{
status_ = MissionStatus::FAILED;
return;
}
}
nav_->RotateTo(home_point_);
status_ = MissionStatus::RUNNING;
}
@@ -23,6 +31,20 @@ namespace Modelec
switch (step_)
{
case ROTATE_TO_HOME:
{
if (nav_->GoTo(home_point_.GetTakeBasePosition()) != Pathfinding::FREE)
{
if (nav_->GoTo(home_point_.GetTakeBasePosition(), true) != Pathfinding::FREE)
{
status_ = MissionStatus::FAILED;
return;
}
}
}
step_ = GO_HOME;
break;
case GO_HOME:
if ((node_->now() - start_time_).seconds() < 94)
{

16
src/modelec_strat/src/missions/prepare_concert_mission.cpp
View File

@@ -30,7 +30,7 @@ namespace Modelec
auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
auto res = nav_->GoTo(pos.x, pos.y, pos.theta, false, Pathfinding::FREE | Pathfinding::WALL);
auto res = nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL);
if (res != Pathfinding::FREE)
{
blacklistId.push_back(column_->id());
@@ -64,7 +64,7 @@ namespace Modelec
case GO_TO_COLUMN:
{
auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition();
nav_->GoTo(pos.x, pos.y, pos.theta, true, Pathfinding::FREE | Pathfinding::WALL);
nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::WALL);
}
step_ = GO_CLOSE_TO_COLUMN;
@@ -86,16 +86,16 @@ namespace Modelec
closestDepoZonePoint_ = closestDepoZone_->GetNextPotPos();
auto p = closestDepoZonePoint_.GetTakeBasePosition();
auto res = nav_->CanGoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL);
auto res = nav_->CanGoTo(p, false, Pathfinding::FREE | Pathfinding::WALL);
if (res != Pathfinding::FREE)
{
auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
nav_->GoTo(pos.x, pos.y, pos.theta, true, Pathfinding::FREE | Pathfinding::WALL);
nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::WALL);
step_ = GO_BACK;
}
else
{
nav_->GoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL);
nav_->GoToRotateFirst(p, false, Pathfinding::FREE | Pathfinding::WALL);
closestDepoZone_->ValidNextPotPos();
step_ = GO_TO_PLATFORM;
}
@@ -113,13 +113,13 @@ namespace Modelec
closestDepoZonePoint_ = closestDepoZone_->ValidNextPotPos();
auto p = closestDepoZonePoint_.GetTakeBasePosition();
if (nav_->CanGoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL) != Pathfinding::FREE)
if (nav_->CanGoTo(p, false, Pathfinding::FREE | Pathfinding::WALL) != Pathfinding::FREE)
{
nav_->GoTo(p.x, p.y, p.theta, true, Pathfinding::FREE | Pathfinding::WALL);
nav_->GoToRotateFirst(p, true, Pathfinding::FREE | Pathfinding::WALL);
}
else
{
nav_->CanGoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL);
nav_->GoToRotateFirst(p, false, Pathfinding::FREE | Pathfinding::WALL);
}
}

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

@@ -13,6 +13,8 @@ namespace Modelec {
{
pathfinding_ = std::make_shared<Pathfinding>(node);
factor_close_enemy_ = Config::get<float>("config.enemy.factor_close_enemy", -0.5f);
waypoint_reach_sub_ = node_->create_subscription<WaypointReachMsg>(
"odometry/waypoint_reach", 10,
[this](const WaypointReachMsg::SharedPtr msg) {
@@ -180,6 +182,45 @@ namespace Modelec {
return waypoints_.back().reached;
}
bool NavigationHelper::RotateTo(const PosMsg::SharedPtr& pos)
{
double angle = std::atan2(pos->y - current_pos_->y, pos->x - current_pos_->x);
if (std::abs(angle - current_pos_->theta) > M_PI / 3)
{
Rotate(angle);
return true;
}
return false;
}
bool NavigationHelper::RotateTo(const Point& pos)
{
double angle = std::atan2(pos.y - current_pos_->y, pos.x - current_pos_->x);
if (std::abs(angle - current_pos_->theta) > M_PI / 3)
{
Rotate(angle);
return true;
}
return false;
}
void NavigationHelper::Rotate(double angle)
{
waypoints_.clear();
WaypointMsg startAngle;
startAngle.x = current_pos_->x;
startAngle.y = current_pos_->y;
startAngle.theta = angle;
startAngle.id = 0;
startAngle.is_end = true;
waypoints_.emplace_back(startAngle);
SendWaypoint();
}
int NavigationHelper::GoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask)
{
last_go_to_ = {goal, isClose, collisionMask};
@@ -217,6 +258,58 @@ namespace Modelec {
return GoTo(goal.x, goal.y, goal.theta, isClose, collisionMask);
}
int NavigationHelper::GoToRotateFirst(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)
{
return res;
}
auto p = Point(wl[0].x, wl[0].y, wl[0].theta);
if (RotateTo(p))
{
await_rotate_ = true;
send_back_waypoints_.clear();
for (auto & w : wl)
{
send_back_waypoints_.emplace_back(w);
}
}
else
{
waypoints_.clear();
for (auto & w : wl)
{
waypoints_.emplace_back(w);
}
SendWaypoint();
}
return res;
}
int NavigationHelper::GoToRotateFirst(int x, int y, double theta, bool isClose, int collisionMask)
{
PosMsg::SharedPtr goal = std::make_shared<PosMsg>();
goal->x = x;
goal->y = y;
goal->theta = theta;
return GoToRotateFirst(goal, isClose, collisionMask);
}
int NavigationHelper::GoToRotateFirst(const Point& goal, bool isClose, int collisionMask)
{
return GoToRotateFirst(goal.x, goal.y, goal.theta, isClose, collisionMask);
}
int NavigationHelper::CanGoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask)
{
return FindPath(goal, isClose, collisionMask).first;
@@ -271,7 +364,7 @@ namespace Modelec {
deposite_zones_[obs->GetId()] = obs;
}
RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", deposite_zones_.size());
RCLCPP_INFO(node_->get_logger(), "Loaded %zu zone from XML", deposite_zones_.size());
return true;
}
@@ -289,7 +382,7 @@ namespace Modelec {
{
double distance = Point::distance(posPoint, zone->GetPosition());
double enemy_distance = Point::distance(enemyPos, zone->GetPosition());
double s = distance * 2 + enemy_distance;
double s = distance + enemy_distance * factor_close_enemy_;
if (s < score)
{
score = s;
@@ -381,6 +474,18 @@ namespace Modelec {
if (waypoint.id == msg->id)
{
waypoint.reached = true;
if (await_rotate_)
{
await_rotate_ = false;
waypoints_.clear();
for (auto & w : send_back_waypoints_)
{
waypoints_.emplace_back(w);
}
SendWaypoint();
}
}
}
}

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

@@ -42,6 +42,8 @@ namespace Modelec
enemy_margin_mm_ = Config::get<int>("config.enemy.size.margin_mm", 50);
factor_close_enemy_ = Config::get<float>("config.enemy.factor_close_enemy", -0.5f);
grid_width_ = Config::get<int>("config.map.size.grid_width", 300);
grid_height_ = Config::get<int>("config.map.size.grid_height", 200);
@@ -498,18 +500,6 @@ namespace Modelec
return {FREE, waypoints};
}
/*void Pathfinding::SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal)
{
current_start_ = start;
current_goal_ = goal;
current_waypoints_ = FindPath(start, goal);
for (const auto& wp : current_waypoints_)
{
waypoint_pub_->publish(wp);
}
}*/
void Pathfinding::SetCurrentPos(const PosMsg::SharedPtr& pos)
{
current_start_ = pos;
@@ -557,7 +547,7 @@ namespace Modelec
auto dist = Point::distance(robotPos, possiblePos);
auto distEnemy = Point::distance(enemyPos, possiblePos);
auto s = dist * 2 + distEnemy;
auto s = dist + distEnemy * factor_close_enemy_;
if (s < score)
{