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/CMakeLists.txt
View File

@ -30,6 +30,7 @@ set(PACKAGE_DEPS
add_executable(mg_nav_server
src/mg_navigation.cpp
src/path_buffer.cpp
)
ament_target_dependencies(mg_nav_server ${PACKAGE_DEPS})

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_;
};
}

10
mg_navigation/src/mg_navigation.cpp
View File

@ -50,9 +50,19 @@ namespace mg {
tf2_buffer = std::make_shared<tf2_ros::Buffer>(get_clock());
tf2_listener = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer);
path_buffer_ = std::make_shared<PathBuffer>(this);
pub_twist = create_publisher<Geometry::TwistStamped>("diffdrive_controller/cmd_vel", 2);
using namespace std::placeholders;
sv_move_global = rclcpp_action::create_server<MoveGlobal>(
this,
"MoveGlobal",
std::bind(&MgNavigationServer::handle_goal<MoveGlobal>, this, _1, _2, "MoveGlobal"),
std::bind(&MgNavigationServer::handle_cancel<MoveGlobal>, this, _1, "MoveGlobal"),
std::bind(&MgNavigationServer::handle_accepted<MoveGlobal>, this, _1, "MoveGlobal"));
sv_move_point = rclcpp_action::create_server<MovePoint>(
this,
"MovePoint",

78
mg_navigation/src/path_buffer.cpp Normal file
View File

@ -0,0 +1,78 @@
#include "path_buffer.hpp"
#include <glm/gtx/norm.hpp>
namespace mg {
PathBuffer::PathBuffer(rclcpp::Node* node) : current(-1), node_(node) {
client_ = node_->create_client<PathService>("GeneratePath");
}
glm::vec2 PathBuffer::get_next(glm::vec2 pos, const double lookahead) {
if (current < 0) {
current++;
while (current < (int)path_msg_->indicies.size()
&& glm::distance2(pos, IdToCoords(path_msg_->indicies[current])) > lookahead * lookahead) {
current++;
}
}
while (current < (int)path_msg_->indicies.size()
&& glm::distance2(pos, IdToCoords(path_msg_->indicies[current])) < lookahead * lookahead) {
current++;
}
if (current == (int)path_msg_->indicies.size()) {
double best = 10000000;
int id = 0;
for (int i = 0; i < (int)goal_->x.size(); i++) {
const double dist2 = glm::distance2({ goal_->x[i], goal_->y[i] }, pos);
if (dist2 < best) {
best = dist2;
id = i;
}
}
return { goal_->x[id], goal_->y[id] };
}
return IdToCoords(path_msg_->indicies[current]);
}
bool PathBuffer::update_path(PathGoal goal) {
if (goal) {
goal_ = goal;
client_->prune_pending_requests();
auto req = std::make_shared<PathService::Request>();
req->x = goal_->x;
req->y = goal_->y;
resp_ = client_->async_send_request(req).share();
}
if (resp_.valid()) {
path_msg_ = resp_.get();
auto req = std::make_shared<PathService::Request>();
req->x = goal_->x;
req->y = goal_->y;
resp_ = client_->async_send_request(req).share();
current = -1;
return true;
}
return false;
}
void PathBuffer::update_path_block(PathGoal goal) {
if (goal) {
goal_ = goal;
client_->prune_pending_requests();
auto req = std::make_shared<PathService::Request>();
req->x = goal_->x;
req->y = goal_->y;
resp_ = client_->async_send_request(req).share();
}
path_msg_ = resp_.get();
auto req = std::make_shared<PathService::Request>();
req->x = goal_->x;
req->y = goal_->y;
resp_ = client_->async_send_request(req).share();
current = -1;
}
}