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

1by1

Browse Source
This commit is contained in:
acki
2025-05-28 16:44:06 -04:00
parent ebfbf22083
commit 5d1ff1c71c
5 changed files with 99 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -36,8 +36,9 @@ namespace Modelec
int GetTeamId() const;
void SendWaypoint() const;
void SendWaypoint(const std::vector<WaypointMsg>& waypoints) const;
// void SendWaypoint() const;
// void SendWaypoint(const std::vector<WaypointMsg>& 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<Waypoint> waypoints_;
std::queue<Waypoint> waypoint_queue_;
PosMsg::SharedPtr current_pos_;

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

@@ -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;

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

@@ -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;
}
}

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

@@ -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;
}

4
src/modelec_strat/src/strat_fms.cpp
View File

@@ -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)
{