43 lines
1.0 KiB
C++
43 lines
1.0 KiB
C++
#pragma once
|
|
|
|
#include <array>
|
|
#include <glm/fwd.hpp>
|
|
#include <memory>
|
|
#include <rclcpp/client.hpp>
|
|
#include <rclcpp/service.hpp>
|
|
#include "mg_planner/a_star/a_star.hpp"
|
|
#include "mg_obstacles/mg_obstacles.hpp"
|
|
#include "rclcpp/rclcpp.hpp"
|
|
|
|
#include "tf2_ros/buffer.h"
|
|
#include "tf2_ros/transform_listener.h"
|
|
|
|
#include "mg_msgs/msg/path.hpp"
|
|
#include "mg_msgs/srv/calc_path.hpp"
|
|
|
|
#define TREE_SIZE 9
|
|
|
|
namespace mg {
|
|
|
|
class PlannerNode : public rclcpp::Node {
|
|
public:
|
|
PlannerNode();
|
|
|
|
void fill_obstacles() { }
|
|
|
|
ObstacleManager::SharedPtr getObstacleManager() { return obstacle_manager_; }
|
|
|
|
glm::ivec2 get_pos();
|
|
|
|
private:
|
|
rclcpp::Service<mg_msgs::srv::CalcPath>::SharedPtr astar_service;
|
|
rclcpp::Publisher<mg_msgs::msg::Path>::SharedPtr path_pub_;
|
|
|
|
AStar astar_;
|
|
ObstacleManager::SharedPtr obstacle_manager_;
|
|
|
|
std::shared_ptr<tf2_ros::Buffer> tf_buf_;
|
|
std::shared_ptr<tf2_ros::TransformListener> tf_listener;
|
|
};
|
|
|
|
} |