hector_exploration_planner: hector_exploration_planner: hector_exploration

00001 //=================================================================================================
00002 // Copyright (c) 2012, Mark Sollweck, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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 namespace hector_exploration_planner{
00038 
00039 class HectorExplorationPlanner {
00040 public:
00041   HectorExplorationPlanner();
00042   ~HectorExplorationPlanner();
00043   HectorExplorationPlanner(std::string name,costmap_2d::Costmap2DROS *costmap_ros);
00044   void initialize(std::string name,costmap_2d::Costmap2DROS *costmap_ros);
00045 
00053   bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,std::vector<geometry_msgs::PoseStamped> &plan);
00054 
00060   bool doExploration(const geometry_msgs::PoseStamped &start,std::vector<geometry_msgs::PoseStamped> &plan);
00061 
00068   bool doInnerExploration(const geometry_msgs::PoseStamped &start,std::vector<geometry_msgs::PoseStamped> &plan);
00069 
00070   bool doAlternativeExploration(const geometry_msgs::PoseStamped &start,std::vector<geometry_msgs::PoseStamped> &plan, std::vector<geometry_msgs::PoseStamped> &oldplan);
00071   bool findFrontiers(std::vector<geometry_msgs::PoseStamped> &frontiers, std::vector<geometry_msgs::PoseStamped> &noFrontiers);
00072   bool findFrontiers(std::vector<geometry_msgs::PoseStamped> &frontiers);
00073   bool findInnerFrontier(std::vector<geometry_msgs::PoseStamped> &innerFrontier);
00074   
00075   float angleDifferenceWall(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal);
00076   bool exploreWalls(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> &goals);
00077 
00078 private:
00082   void setupMapData();
00083   void deleteMapData();
00084   bool buildobstacle_trans_array_();
00085   bool buildexploration_trans_array_(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> goals,bool useAnglePenalty);
00086   bool getTrajectory(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> goals, std::vector<geometry_msgs::PoseStamped> &plan);
00087   bool recoveryMakePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,std::vector<geometry_msgs::PoseStamped> &plan);
00088   unsigned int cellDanger(int point);
00089   unsigned int angleDanger(float angle);
00090 
00091   void saveMaps(std::string path);
00092   void resetMaps();
00093   void clearFrontiers();
00094   bool isValid(int point);
00095   bool isFree(int point);
00096   bool isFrontier(int point);
00097   float angleDifference(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal);
00098   float getDistanceWeight(const geometry_msgs::PoseStamped &point1, const geometry_msgs::PoseStamped &point2);
00099   double getYawToUnknown(int point);
00100   bool isFrontierReached(int point);
00101   bool isSameFrontier(int frontier_point1,int frontier_point2);
00102 
00103   void getStraightPoints(int point, int points[]);
00104   void getDiagonalPoints(int point, int points[]);
00105   void getAdjacentPoints(int point, int points[]);
00106   int left(int point);
00107   int upleft(int point);
00108   int up(int point);
00109   int upright(int point);
00110   int right(int point);
00111   int downright(int point);
00112   int down(int point);
00113   int downleft(int point);
00114 
00115   ros::Publisher visualization_pub_;
00116   ros::ServiceClient path_service_client_;
00117   costmap_2d::Costmap2DROS* costmap_ros_;
00118   costmap_2d::Costmap2D costmap_;
00119 
00120   const unsigned char* occupancy_grid_array_;
00121   unsigned int* exploration_trans_array_;
00122   unsigned int* obstacle_trans_array_;
00123   int* frontier_map_array_;
00124   bool* is_goal_array_;
00125 
00126   bool initialized_;
00127   int previous_goal_;
00128 
00129   std::string name;
00130   unsigned int map_width_;
00131   unsigned int map_height_;
00132   unsigned int num_map_cells_;
00133 
00134   // Parameters
00135   bool p_plan_in_unknown_;
00136   bool p_use_inflated_obs_;
00137   int p_goal_angle_penalty_;
00138   int p_min_obstacle_dist_;
00139   int p_min_frontier_size_;
00140   double p_alpha_;
00141   double p_dist_for_goal_reached_;
00142   double p_same_frontier_dist_;
00143 
00144 };
00145 }
00146 
00147 #endif
00148 
00149
hector_exploration_planner.h
