diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 7a6c49e..7d00291 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -36,8 +36,9 @@ namespace Modelec int GetTeamId() const; - void SendWaypoint() const; - void SendWaypoint(const std::vector& waypoints) const; + // void SendWaypoint() const; + // void SendWaypoint(const std::vector& waypoints) const; + void SendGoTo(); void AddWaypoint(const PosMsg& pos, int index); void AddWaypoint(const WaypointMsg& waypoint); @@ -128,6 +129,7 @@ namespace Modelec bool last_was_close_enemy_ = false; std::vector waypoints_; + std::queue waypoint_queue_; PosMsg::SharedPtr current_pos_; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index fd0443c..aef58d6 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -197,7 +197,7 @@ namespace Modelec top_pot_msg.pos = 1; servo_move_pub_->publish(top_pot_msg); - step_running_ = 6; + step_running_ = 2; } break; @@ -229,7 +229,7 @@ namespace Modelec top_pot_msg.pos = 1; servo_move_pub_->publish(top_pot_msg); - step_running_ = 2; + step_running_ = 1; } break; @@ -270,7 +270,7 @@ namespace Modelec top_pot_msg.pos = 0; servo_move_pub_->publish(top_pot_msg); - step_running_ = 6; + step_running_ = 5; } break; diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 61251f5..5d3435e 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -124,7 +124,25 @@ namespace Modelec return team_id_; } - void NavigationHelper::SendWaypoint() const + void NavigationHelper::SendGoTo() + { + while (!waypoint_queue_.empty()) + { + waypoint_queue_.pop(); + } + + for (auto w : waypoints_) + { + w.id = 0; + w.is_end = true; + waypoint_queue_.push(w); + } + + waypoint_pub_->publish(waypoint_queue_.front().ToMsg()); + waypoint_queue_.pop(); + } + +/* void NavigationHelper::SendWaypoint() const { for (auto& w : waypoints_) { @@ -139,6 +157,7 @@ namespace Modelec waypoint_pub_->publish(w); } } +*/ void NavigationHelper::AddWaypoint(const PosMsg& pos, int index) { @@ -244,7 +263,7 @@ namespace Modelec { return true; } - return waypoints_.back().reached; + return waypoint_queue_.empty() && waypoints_.back().reached; } bool NavigationHelper::RotateTo(const PosMsg::SharedPtr& pos) @@ -283,7 +302,8 @@ namespace Modelec startAngle.is_end = true; waypoints_.emplace_back(startAngle); - SendWaypoint(); + // SendWaypoint(); + SendGoTo(); } int NavigationHelper::GoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask) @@ -304,7 +324,8 @@ namespace Modelec waypoints_.emplace_back(w); } - SendWaypoint(); + // SendWaypoint(); + SendGoTo(); return res; } @@ -355,7 +376,8 @@ namespace Modelec waypoints_.emplace_back(w); } - SendWaypoint(); + // SendWaypoint(); + SendGoTo(); } return res; @@ -651,9 +673,9 @@ namespace Modelec return spawn_; } - void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr msg) + void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr) { - for (auto& waypoint : waypoints_) + /*for (auto& waypoint : waypoints_) { if (waypoint.id == msg->id) { @@ -671,6 +693,18 @@ namespace Modelec SendWaypoint(); } } + }*/ + + if (!waypoint_queue_.empty()) + { + waypoint_pub_->publish(waypoint_queue_.front().ToMsg()); + waypoint_queue_.pop(); + + waypoints_[waypoints_.size() - waypoint_queue_.size() - 1].reached = true; + } + else + { + waypoints_.back().reached = true; } } diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index 451416d..9179dd1 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -463,7 +463,7 @@ namespace Modelec } // 7. Fill Waypoints (reconvertir grille -> millimètres) - int id = 0; + /*int id = 0; for (size_t i = 0; i < smooth_path.size(); ++i) { const auto& [x, y] = smooth_path[i]; @@ -490,6 +490,52 @@ namespace Modelec waypoints.push_back(wp); } if (!waypoints.empty()) + { + waypoints.back().is_end = true; + }*/ + + // 7. Fill Waypoints (reconvertir grille -> millimètres) + int id = 0; + for (size_t i = 0; i < smooth_path.size(); ++i) + { + const auto& [x, y] = smooth_path[i]; + + // Skip first point if it's too close to robot position + if (i == 0) + { + float world_x = x * cell_size_mm_x; + float world_y = (grid_height_ - 1 - y) * cell_size_mm_y; + + float dx = std::abs(world_x - start->x); + float dy = std::abs(world_y - start->y); + + if (dx < 50 && dy < 50) // <== seuil de 50mm + continue; + } + + WaypointMsg wp; + wp.x = x * cell_size_mm_x; + wp.y = (grid_height_ - 1 - y) * cell_size_mm_y; + + // Calculer l'angle entre le point actuel et le prochain point + if (i < smooth_path.size() - 1) // Si ce n'est pas le dernier point + { + const auto& [next_x, next_y] = smooth_path[i + 1]; + // Calcul de l'angle entre (x, y) et (next_x, next_y) + float delta_x = next_x * cell_size_mm_x - wp.x; + float delta_y = (grid_height_ - 1 - next_y) * cell_size_mm_y - wp.y; + wp.theta = std::atan2(delta_y, delta_x); // Calcul de l'angle en radians + } + else + { + wp.theta = goal->theta; + } + + wp.id = id++; + wp.is_end = false; + waypoints.push_back(wp); + } + if (!waypoints.empty()) { waypoints.back().is_end = true; } diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 2e976a5..0dd6058 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -149,6 +149,10 @@ namespace Modelec { Transition(State::DO_PROMOTION, "Start promotion"); } + /*else + { + Transition(State::STOP, "Start prepare concert"); + }*/ // TODO : check the time needed by the mission else if (elapsed.seconds() < 75) {