32 #ifndef COLLISION_CHECKER_H 33 #define COLLISION_CHECKER_H 37 #include <sensor_msgs/Range.h> 38 #include <sensor_msgs/LaserScan.h> 65 void draw_line(
const tf2::Vector3 &p1,
const tf2::Vector3 &p2,
66 float r,
float g,
float b,
int id);
69 void check_dist(
float x,
bool forward,
float& min_dist)
const;
71 bool left,
float& min_dist)
const;
73 float degrees(
float radians)
const;
83 float obstacle_dist(
bool forward,
float &left_dist,
float &right_dist,
84 tf2::Vector3 &fl, tf2::Vector3 &fr);
float robot_back_length_sq
void check_dist(float x, bool forward, float &min_dist) const
void check_angle(float theta, float x, float y, bool left, float &min_dist) const
float robot_front_length_sq
float obstacle_arc_angle(double linear, double angular)
tf2_ros::Buffer & tf_buffer
ObstaclePoints & ob_points
float degrees(float radians) const
float obstacle_angle(bool left)
std::mutex obstacle_mutex
float obstacle_dist(bool forward, float &left_dist, float &right_dist, tf2::Vector3 &fl, tf2::Vector3 &fr)
void draw_line(const tf2::Vector3 &p1, const tf2::Vector3 &p2, float r, float g, float b, int id)
CollisionChecker(ros::NodeHandle &nh, tf2_ros::Buffer &tf_buffer, ObstaclePoints &op)