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();
}

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

@ -49,6 +49,13 @@ 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 dist = baseNode.obstacle_manager()->box_colide(
n_pos, { 0.29, 0.33 }, n_theta, ObstacleManager::ObstacleMask::DYNAMIC);
if (dist < 0.01) {
return NAN;
}
// 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) }));