60 lines
1.5 KiB
C++
60 lines
1.5 KiB
C++
#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)) / 1000.0F; }
|
|
|
|
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_;
|
|
};
|
|
|
|
} |