82 lines
2.7 KiB
C++
82 lines
2.7 KiB
C++
#include "path_buffer.hpp"
|
|
|
|
#include <glm/gtx/norm.hpp>
|
|
|
|
#include <chrono>
|
|
using namespace std::chrono_literals;
|
|
|
|
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_.wait_for(0s) == std::future_status::ready) {
|
|
|
|
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();
|
|
}
|
|
|
|
resp_.wait();
|
|
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;
|
|
}
|
|
} |