Go to the documentation of this file.00001 #ifndef BOUNDED_EXPLORE_LAYER_H_
00002 #define BOUNDED_EXPLORE_LAYER_H_
00003
00004 #include <ros/ros.h>
00005 #include <costmap_2d/layer.h>
00006 #include <costmap_2d/GenericPluginConfig.h>
00007 #include <dynamic_reconfigure/server.h>
00008
00009 #include <geometry_msgs/Polygon.h>
00010 #include <frontier_exploration/Frontier.h>
00011 #include <frontier_exploration/UpdateBoundaryPolygon.h>
00012 #include <frontier_exploration/GetNextFrontier.h>
00013
00014 namespace frontier_exploration
00015 {
00016
00022 class BoundedExploreLayer : public costmap_2d::Layer, public costmap_2d::Costmap2D
00023 {
00024 public:
00025 BoundedExploreLayer();
00026 ~BoundedExploreLayer();
00027
00031 virtual void onInitialize();
00032
00036 virtual void updateBounds(double origin_x, double origin_y, double origin_yaw, double* polygon_min_x, double* polygon_min_y, double* polygon_max_x,
00037 double* polygon_max_y);
00038
00042 virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
00043 bool isDiscretized()
00044 {
00045 return true;
00046 }
00047
00051 virtual void matchSize();
00052
00056 virtual void reset();
00057
00058 protected:
00059
00066 bool updateBoundaryPolygonService(frontier_exploration::UpdateBoundaryPolygon::Request &req, frontier_exploration::UpdateBoundaryPolygon::Response &res);
00067
00073 bool updateBoundaryPolygon(geometry_msgs::PolygonStamped polygon_stamped);
00074
00081 bool getNextFrontierService(frontier_exploration::GetNextFrontier::Request &req, frontier_exploration::GetNextFrontier::Response &res);
00082
00089 bool getNextFrontier(geometry_msgs::PoseStamped start_pose, geometry_msgs::PoseStamped &next_frontier);
00090
00091 private:
00092
00097 void mapUpdateKeepObstacles(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
00098
00099 void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
00100
00101 dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
00102 ros::ServiceServer polygonService_;
00103 ros::ServiceServer frontierService_;
00104 geometry_msgs::Polygon polygon_;
00105 tf::TransformListener tf_listener_;
00106
00107 ros::Publisher frontier_cloud_pub;
00108
00109 bool configured_, marked_;
00110
00111 std::string frontier_travel_point_;
00112 bool resize_to_boundary_;
00113
00114 };
00115
00116 }
00117 #endif