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

Modified LookAt to not care about side

Browse Source
This commit is contained in:
Pimpest
2025-05-08 22:27:02 +02:00
parent 85c1ce6b88
commit bd47553b86
1 changed files with 5 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
mg_navigation/include/handlers/dwa_lookat.hpp
View File

@ -13,8 +13,9 @@ namespace mg {
const double a = glm::abs(theta - target_ornt);
const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
const double c = (b > glm::pi<double>() / 2) ? glm::pi<double>() - b : b;
return b > goal->tolerance;
return c > goal->tolerance;
}
template <>
@ -56,6 +57,9 @@ namespace mg {
dist_old = (dist_old > glm::pi<double>()) ? glm::two_pi<double>() - dist_old : dist_old;
dist_new = (dist_new > glm::pi<double>()) ? glm::two_pi<double>() - dist_new : dist_new;
dist_old = (dist_old > glm::pi<double>() / 2) ? glm::pi<double>() - dist_old : dist_old;
dist_new = (dist_new > glm::pi<double>() / 2) ? glm::pi<double>() - dist_new : dist_new;
const double score = goal->ornt_mult * (dist_old - dist_new);
return score;