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

Add a-star based global planner

Browse Source
This commit is contained in:
Pimpest
2025-04-17 19:24:19 +02:00
parent 0fffe915ca
commit ec831bd334
21 changed files with 686 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
mg_navigation/include/handlers/dwa.hpp
View File

@ -2,6 +2,7 @@
#include "handlers/dwa_core.hpp"
#include "handlers/dwa_point.hpp"
#include "handlers/dwa_global.hpp"
#include "handlers/dwa_forward.hpp"
#include "handlers/dwa_lookat.hpp"
#include "handlers/dwa_rotate.hpp"

89
mg_navigation/include/handlers/dwa_global.hpp Normal file
View File

@ -0,0 +1,89 @@
#pragma once
#include "handlers/dwa_core.hpp"
#include "glm/gtx/norm.hpp"
#include "glm/gtx/rotate_vector.hpp"
#include "glm/gtx/vector_angle.hpp"
#define LOOKAHEAD 0.2
namespace mg {
template <>
inline bool DWA<MgNavigationServer::MoveGlobal>::target_check() {
return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
}
template <>
inline void DWA<MgNavigationServer::MoveGlobal>::target_init() {
baseNode.path_buffer()->update_path_block(goal);
target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD);
}
template <>
inline void DWA<MgNavigationServer::MoveGlobal>::target_update() {
baseNode.path_buffer()->update_path(goal);
target_pos = baseNode.path_buffer()->get_next(pos, LOOKAHEAD);
}
template <>
inline bool DWA<MgNavigationServer::MoveGlobal>::condition_check() {
return false;
}
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;
glm::dvec2 n_pos;
double n_theta = NAN;
double score = 0;
n_theta = w * delta;
if (n_theta <= 1e-6) { //NOLINT
n_theta += theta;
const glm::dvec2 dp = glm::rotate(glm::dvec2(1, 0), theta) * delta * v;
n_pos = dp + pos;
} else {
n_theta += theta;
const double r = v / w;
n_pos.x = r * (glm::sin(n_theta) - glm::sin(theta));
n_pos.y = -r * (glm::cos(n_theta) - glm::cos(theta));
n_pos += pos;
}
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));
score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
score += goal->ornt_mult * (angl - n_angl);
return score;
}
template <>
inline void DWA<MgNavigationServer::MoveGlobal>::populate_candidates(std::vector<int>& vl,
std::vector<int>& vr,
int dimx,
int dimy) {
vl.clear();
vr.clear();
for (int i = -dimx / 2; i <= dimx / 2; i++) {
for (int j = -dimy / 2; j <= dimy / 2; j++) {
auto [v, w] = to_vel(step * (cvl + i), step * (cvr + j));
if (step * abs(cvl + i) <= goal->max_wheel_speed && step * abs(cvr + j) <= goal->max_wheel_speed
&& glm::abs(v) < goal->max_vel && glm::abs(w) < goal->max_angular) {
vl.push_back(cvl + i);
vr.push_back(cvr + j);
}
}
}
}
}

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 "path_buffer.hpp"
namespace mg {
namespace Geometry = geometry_msgs::msg;
@ -25,12 +27,14 @@ namespace mg {
public:
RCLCPP_SMART_PTR_ALIASES_ONLY(MgNavigationServer)
using MovePoint = mg_msgs::action::MovePoint;
using MoveGlobal = mg_msgs::action::MoveGlobal;
using MoveStraight = mg_msgs::action::MoveStraight;
using LookAt = mg_msgs::action::LookAt;
using Rotate = mg_msgs::action::Rotate;
rclcpp::Publisher<Geometry::TwistStamped>::SharedPtr pub_twist;
rclcpp_action::Server<MoveGlobal>::SharedPtr sv_move_global;
rclcpp_action::Server<MovePoint>::SharedPtr sv_move_point;
rclcpp_action::Server<MoveStraight>::SharedPtr sv_move_straight;
rclcpp_action::Server<LookAt>::SharedPtr sv_look_at;
@ -43,9 +47,13 @@ namespace mg {
MgNavigationServer(rclcpp::NodeOptions& opts);
std::shared_ptr<PathBuffer> path_buffer() { return path_buffer_; }
private:
bool is_processing = false;
std::shared_ptr<PathBuffer> path_buffer_;
template <typename T>
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
typename T::Goal::ConstSharedPtr,

60
mg_navigation/include/path_buffer.hpp Normal file
View File

@ -0,0 +1,60 @@
#pragma once
#include <vector>
#include <mutex>
#include <thread>
#include <glm/glm.hpp>
#include "mg_msgs/srv/calc_path.hpp"
#include "mg_msgs/action/move_global.hpp"
#include "rclcpp/rclcpp.hpp"
#define AREAX 3000
#define AREAY 2000
#define GRID_Y 66
#define GRID_X 106
#define MIN_X 175
#define MIN_Y 175
#define MAX_X 2825
#define MAX_Y 1825
#define GRID_X_SIZE ((MAX_X - MIN_X) / GRID_X)
#define GRID_Y_SIZE ((MAX_Y - MIN_Y) / GRID_Y)
namespace mg {
inline glm::vec2 GridToCoords(const glm::ivec2 grid) {
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)); }
class PathBuffer {
using PathService = mg_msgs::srv::CalcPath;
using PathGoal = mg_msgs::action::MoveGlobal::Goal::ConstSharedPtr;
using PathFuture = rclcpp::Client<PathService>::SharedFuture;
public:
PathBuffer(rclcpp::Node* node);
// ~PathBuffer();
glm::vec2 get_next(glm::vec2 pos, const double lookahead);
bool update_path(PathGoal goal = nullptr);
void update_path_block(PathGoal goal = nullptr);
private:
int current;
rclcpp::Node* node_;
PathGoal goal_;
PathService::Response::ConstSharedPtr path_msg_;
rclcpp::Client<PathService>::Client::SharedPtr client_;
PathFuture resp_;
};
}