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

First set of behaviors

Browse Source
This commit is contained in:
Pimpest
2025-05-08 12:10:13 +02:00
parent da3d1d9d15
commit bce371dba2
13 changed files with 710 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
mg_navigation/include/handlers/dwa_core.hpp
View File

@ -125,11 +125,14 @@ namespace mg {
pos_query();
populate_candidates(spacevl, spacevr, dimx, dimy);
double b_score = calc_score(spacevl[0], spacevr[0]);
uint b_ind = 0;
double b_score = calc_score(spacevl[0], spacevr[0]) - 1;
int b_ind = -1;
for (uint i = 1; i < spacevl.size(); i++) {
const double score = calc_score(spacevl[i], spacevr[i]);
if (score == NAN) {
continue;
}
if (score > b_score) {
b_score = score;
b_ind = i;
@ -138,8 +141,13 @@ namespace mg {
calc_score(spacevl[b_ind], spacevr[b_ind]); // Called here again so it sticks out in logs
cvl = spacevl[b_ind];
cvr = spacevr[b_ind];
if (b_ind >= 0) {
cvl = spacevl[b_ind];
cvr = spacevr[b_ind];
} else {
cvl = 0;
cvr = 0;
}
send_speed(step * cvl, step * cvr);
rate.sleep();
}