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;

8
mg_navigation/include/mg_navigation.hpp
View File

@ -17,6 +17,8 @@
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "mg_obstacles/mg_obstacles.hpp"
#include "path_buffer.hpp"
namespace mg {
@ -47,12 +49,14 @@ namespace mg {
MgNavigationServer(rclcpp::NodeOptions& opts);
std::shared_ptr<PathBuffer> path_buffer() { return path_buffer_; }
std::shared_ptr<PathBuffer> path_buffer() { return path_buffer_; }
std::shared_ptr<ObstacleManager>& obstacle_manager() { return obstacle_manager_; }
private:
bool is_processing = false;
std::shared_ptr<PathBuffer> path_buffer_;
std::shared_ptr<PathBuffer> path_buffer_;
std::shared_ptr<ObstacleManager> obstacle_manager_;
template <typename T>
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,

2
mg_navigation/include/path_buffer.hpp
View File

@ -28,7 +28,7 @@ namespace mg {
return glm::vec2{ grid.x, grid.y } * glm::vec2{ GRID_X_SIZE, GRID_Y_SIZE } + glm::vec2{ MIN_X, MIN_Y };
}
inline glm::ivec2 IdToGrid(const int id) { return { id % GRID_X, id / GRID_X }; }
inline glm::vec2 IdToCoords(const int id) { return GridToCoords(IdToGrid(id)); }
inline glm::vec2 IdToCoords(const int id) { return GridToCoords(IdToGrid(id)) / 1000.0F; }
class PathBuffer {
using PathService = mg_msgs::srv::CalcPath;