Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Change the way dwa_point behaves if goal is behind #13

Merged
Pimpest merged 1 commits from fix-dwa-point into main 2025-04-27 18:27:57 +00:00
Conversation 0 Commits 1 Files Changed 1 +5 -3
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit a9358f6f32 - Show all commits

8
mg_navigation/include/handlers/dwa_point.hpp
View File

@ -30,6 +30,7 @@ namespace mg {
double n_theta = NAN; double n_theta = NAN;
double score = 0; double score = 0;
// The angle we will be facing after lookahead
n_theta = w * delta; n_theta = w * delta;
if (n_theta <= 1e-6) { //NOLINT if (n_theta <= 1e-6) { //NOLINT
@ -47,11 +48,12 @@ namespace mg {
const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta); const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta);
const glm::dvec2 face = glm::rotate(glm::dvec2(1, 0), theta); const glm::dvec2 face = glm::rotate(glm::dvec2(1, 0), theta);
const double angl = glm::angle(face, glm::normalize(target_pos - pos)); // Calculate angle to goal post/pre movement
const double n_angl = glm::angle(n_face, glm::normalize(target_pos - n_pos)); const double angl = glm::asin(glm::determinant(glm::dmat2{ face, glm::normalize(target_pos - pos) }));
const double n_angl = glm::asin(glm::determinant(glm::dmat2{ n_face, glm::normalize(target_pos - n_pos) }));
score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos)); score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
score += goal->ornt_mult * (angl - n_angl); score += goal->ornt_mult * (glm::abs(angl) - glm::abs(n_angl));
return score; return score;
} }