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

Browse Source
This commit is contained in:
Pimpest
2025-04-27 20:24:01 +02:00
parent 5ea06dc3cf
commit a9358f6f32
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -30,6 +30,7 @@ namespace mg {
double n_theta = NAN;
double score = 0;
// The angle we will be facing after lookahead
n_theta = w * delta;
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 face = glm::rotate(glm::dvec2(1, 0), theta);
const double angl = glm::angle(face, glm::normalize(target_pos - pos));
const double n_angl = glm::angle(n_face, glm::normalize(target_pos - n_pos));
// Calculate angle to goal post/pre movement
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->ornt_mult * (angl - n_angl);
score += goal->ornt_mult * (glm::abs(angl) - glm::abs(n_angl));
return score;
}