00001 #include <costmap_2d/costmap_2d_ros.h> 00002 #include <ros/ros.h> 00003 00004 namespace obstacle_finder 00005 { 00006 class Obstacle 00007 { 00008 public: 00009 double x, y; 00010 double dist; 00011 00012 Obstacle(double x, double y, double dist); 00013 00014 Obstacle(); 00015 }; 00016 00017 class ObstacleFinder 00018 { 00019 public: 00020 ObstacleFinder(costmap_2d::Costmap2DROS* costmap, double robot_odom_x, double robot_odom_y); 00021 00028 Obstacle nearestObstacle(costmap_2d::Costmap2DROS* new_costmap, double robot_odom_x, double robot_odom_y); 00029 00030 Obstacle nearestObstacle(double robot_odom_x, double robot_odom_y); 00031 00032 Obstacle nearestObstacle(); 00033 00034 private: 00035 costmap_2d::Costmap2DROS* costmap_; 00036 double robot_odom_x_; 00037 double robot_odom_y_; 00038 }; 00039 }