Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef HECTOR_EXPLORATION_PLANNER_H___
00030 #define HECTOR_EXPLORATION_PLANNER_H___
00031
00032 #include <ros/ros.h>
00033 #include <costmap_2d/costmap_2d.h>
00034 #include <costmap_2d/costmap_2d_ros.h>
00035 #include <geometry_msgs/PoseStamped.h>
00036
00037 #include <dynamic_reconfigure/server.h>
00038
00039 #include <hector_exploration_planner/exploration_transform_vis.h>
00040
00041
00042
00043 namespace hector_exploration_planner{
00044
00045 class ExplorationPlannerConfig;
00046
00047 class HectorExplorationPlanner {
00048 public:
00049 HectorExplorationPlanner();
00050 ~HectorExplorationPlanner();
00051 HectorExplorationPlanner(std::string name,costmap_2d::Costmap2DROS *costmap_ros);
00052 void initialize(std::string name,costmap_2d::Costmap2DROS *costmap_ros);
00053
00054 void dynRecParamCallback(hector_exploration_planner::ExplorationPlannerConfig &config, uint32_t level);
00055
00063 bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &original_goal, std::vector<geometry_msgs::PoseStamped> &plan);
00064
00070 bool doExploration(const geometry_msgs::PoseStamped &start,std::vector<geometry_msgs::PoseStamped> &plan);
00071
00078 bool doInnerExploration(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> &plan);
00079
00080 bool getObservationPose(const geometry_msgs::PoseStamped& observation_pose, const double desired_distance, geometry_msgs::PoseStamped& new_observation_pose);
00081
00082 bool doAlternativeExploration(const geometry_msgs::PoseStamped &start,std::vector<geometry_msgs::PoseStamped> &plan, std::vector<geometry_msgs::PoseStamped> &oldplan);
00083 bool findFrontiers(std::vector<geometry_msgs::PoseStamped> &frontiers, std::vector<geometry_msgs::PoseStamped> &noFrontiers);
00084 bool findFrontiers(std::vector<geometry_msgs::PoseStamped> &frontiers);
00085 bool findInnerFrontier(std::vector<geometry_msgs::PoseStamped> &innerFrontier);
00086
00087 float angleDifferenceWall(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal);
00088 bool exploreWalls(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> &goals);
00089
00090 private:
00091
00092 enum LastMode{
00093 FRONTIER_EXPLORE,
00094 INNER_EXPLORE
00095 } last_mode_;
00096
00100 void setupMapData();
00101 void deleteMapData();
00102 bool buildobstacle_trans_array_(bool use_inflated_obstacles);
00103 bool buildexploration_trans_array_(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> goals,bool useAnglePenalty);
00104 bool getTrajectory(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> goals, std::vector<geometry_msgs::PoseStamped> &plan);
00105 bool recoveryMakePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,std::vector<geometry_msgs::PoseStamped> &plan);
00106 unsigned int cellDanger(int point);
00107 unsigned int angleDanger(float angle);
00108
00109 void saveMaps(std::string path);
00110 void resetMaps();
00111 void clearFrontiers();
00112 bool isValid(int point);
00113 bool isFree(int point);
00114 bool isFrontier(int point);
00115 float angleDifference(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal);
00116 float getDistanceWeight(const geometry_msgs::PoseStamped &point1, const geometry_msgs::PoseStamped &point2);
00117 double getYawToUnknown(int point);
00118 bool isFrontierReached(int point);
00119 bool isSameFrontier(int frontier_point1,int frontier_point2);
00120
00121 void getStraightPoints(int point, int points[]);
00122 void getDiagonalPoints(int point, int points[]);
00123 void getAdjacentPoints(int point, int points[]);
00124 int left(int point);
00125 int upleft(int point);
00126 int up(int point);
00127 int upright(int point);
00128 int right(int point);
00129 int downright(int point);
00130 int down(int point);
00131 int downleft(int point);
00132
00133 ros::Publisher visualization_pub_;
00134 ros::ServiceClient path_service_client_;
00135 costmap_2d::Costmap2DROS* costmap_ros_;
00136 costmap_2d::Costmap2D costmap_;
00137
00138 const unsigned char* occupancy_grid_array_;
00139 unsigned int* exploration_trans_array_;
00140 unsigned int* obstacle_trans_array_;
00141 int* frontier_map_array_;
00142 bool* is_goal_array_;
00143
00144 bool initialized_;
00145 int previous_goal_;
00146
00147 std::string name;
00148 unsigned int map_width_;
00149 unsigned int map_height_;
00150 unsigned int num_map_cells_;
00151
00152
00153 bool p_plan_in_unknown_;
00154 bool p_use_inflated_obs_;
00155 int p_goal_angle_penalty_;
00156 int p_min_obstacle_dist_;
00157 int p_min_frontier_size_;
00158 double p_alpha_;
00159 double p_dist_for_goal_reached_;
00160 double p_same_frontier_dist_;
00161
00162 boost::shared_ptr<dynamic_reconfigure::Server<hector_exploration_planner::ExplorationPlannerConfig> > dyn_rec_server_;
00163
00164 boost::shared_ptr<ExplorationTransformVis> vis_;
00165 boost::shared_ptr<ExplorationTransformVis> inner_vis_;
00166
00167 };
00168 }
00169
00170 #endif
00171
00172