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

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