65 lines
2.0 KiB
C++
65 lines
2.0 KiB
C++
#pragma once
|
|
#include <string>
|
|
#include <glm/glm.hpp>
|
|
#include "rclcpp/rclcpp.hpp"
|
|
#include "mg_obstacles/permanent_obstacle.hpp"
|
|
#include "mg_obstacles/static_obstacle.hpp"
|
|
|
|
#include "std_msgs/msg/u_int64.hpp"
|
|
#include "std_msgs/msg/string.hpp"
|
|
#include "geometry_msgs/msg/point_stamped.hpp"
|
|
|
|
#include "tf2_ros/buffer.h"
|
|
|
|
namespace mg {
|
|
class ObstacleManager {
|
|
using StaticListMsg = std_msgs::msg::UInt64;
|
|
using DynamicPosMsg = geometry_msgs::msg::PointStamped;
|
|
|
|
public:
|
|
RCLCPP_SMART_PTR_DEFINITIONS(ObstacleManager)
|
|
|
|
enum ObstacleMask { // NOLINT
|
|
DYNAMIC = 1,
|
|
STATIC = 2,
|
|
MOVABLE = 3,
|
|
PERMANENT = 4,
|
|
IMPORTANT = 5,
|
|
ALL = 7,
|
|
};
|
|
|
|
ObstacleManager(rclcpp::Node* node, tf2_ros::Buffer::SharedPtr& buf);
|
|
bool contains(glm::dvec2 pos, double radius, uint mask = ObstacleMask::ALL);
|
|
double dist_to_nearest(glm::dvec2 pos, int mask = ObstacleMask::ALL);
|
|
double box_colide(glm::dvec2 xy, glm::dvec2 size, double rot, int mask = ObstacleMask::ALL);
|
|
|
|
static double dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, const glm::dmat2 rot_mat);
|
|
static double dist_to_bounds(glm::dvec2 pos, glm::dvec2 size, double rotation);
|
|
|
|
void update_dynamic();
|
|
void update_static();
|
|
|
|
private:
|
|
rclcpp::Node* node_;
|
|
|
|
tf2_ros::Buffer::SharedPtr tf_buffer;
|
|
|
|
std::vector<StaticObstacle> static_obstacles_;
|
|
std::vector<PermanentObstacle> permanent_obstacles_;
|
|
|
|
glm::dvec3 oponent_position_ = { 0, 0, 0.20 };
|
|
bool openent_active_ = false;
|
|
|
|
rclcpp::CallbackGroup::SharedPtr no_exec_cbg;
|
|
|
|
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr side_sub_;
|
|
|
|
rclcpp::Subscription<StaticListMsg>::SharedPtr static_obst_sub_;
|
|
rclcpp::Subscription<DynamicPosMsg>::SharedPtr dynamic_obst_sub_;
|
|
|
|
std::string base_path;
|
|
|
|
void load_permanent(Json::Value& json);
|
|
void load_static(Json::Value& json);
|
|
};
|
|
} |