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

Added collision avoidance to MoveGlobal routine

Browse Source
This commit is contained in:
Pimpest
2025-05-05 01:34:53 +02:00
parent a160af8de3
commit ccfd1189c2
30 changed files with 792 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -106,10 +106,13 @@ namespace mg {
pos_query();
target_init();
rclcpp::Time elapsed = baseNode.get_clock()->now();
rclcpp::Rate rate(UPDATE_RATE);
while (target_check() && rclcpp::ok()) {
target_update();
baseNode.obstacle_manager()->update_dynamic();
baseNode.obstacle_manager()->update_static();
if (hgoal->is_canceling()) {
cancel();
return;
@ -138,7 +141,6 @@ namespace mg {
cvl = spacevl[b_ind];
cvr = spacevr[b_ind];
send_speed(step * cvl, step * cvr);
rate.sleep();
}
succeed();

25
mg_navigation/include/handlers/dwa_global.hpp
View File

@ -18,6 +18,9 @@ namespace mg {
inline void DWA<MgNavigationServer::MoveGlobal>::target_init() {
baseNode.path_buffer()->update_path_block(goal);
target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD);
dimy = 4;
dimx = 11;
}
template <>
@ -34,7 +37,7 @@ namespace mg {
template <>
inline double DWA<MgNavigationServer::MoveGlobal>::calc_score(int vl, int vr) {
auto [v, w] = to_vel(step * vl, step * vr);
const double delta = 0.5;
const double delta = 0.3;
glm::dvec2 n_pos;
double n_theta = NAN;
double score = 0;
@ -56,11 +59,25 @@ 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));
// 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));
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) }));
const double dist = baseNode.obstacle_manager()->box_colide(n_pos, { 0.29, 0.33 }, n_theta);
const double dist2 = baseNode.obstacle_manager()->dist_to_nearest(n_pos) - 0.22;
const double obstacle_scoreA = glm::max(0.0, 0.02 - dist);
const double obstacle_scoreB = glm::max(0.0, 0.03 - dist2);
RCLCPP_INFO(baseNode.get_logger(), "Door Stuck %lf", dist);
RCLCPP_INFO(baseNode.get_logger(), "Door Stuck2 %lf", dist2);
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 * (angl - n_angl);
score += goal->ornt_mult * (glm::abs(angl) - glm::abs(n_angl));
score -= goal->obst_mult_a * obstacle_scoreA;
score -= goal->obst_mult_b * obstacle_scoreB;
return score;
}

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

@ -15,6 +15,7 @@ namespace mg {
template <>
inline void DWA<MgNavigationServer::MovePoint>::target_update() {
target_pos = glm::dvec2(goal->x, goal->y);
dimy = 8;
}
template <>
@ -25,7 +26,7 @@ namespace mg {
template <>
inline double DWA<MgNavigationServer::MovePoint>::calc_score(int vl, int vr) {
auto [v, w] = to_vel(step * vl, step * vr);
const double delta = 0.5;
const double delta = 0.3;
glm::dvec2 n_pos;
double n_theta = NAN;
double score = 0;