SBPLPlanner2D.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013 Armin Hornung, University of Freiburg
00003  *
00004  * Redistribution and use in source and binary forms, with or without
00005  * modification, are permitted provided that the following conditions are met:
00006  *
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 University of Freiburg nor the names of its
00013  *       contributors may be used to endorse or promote products derived from
00014  *       this software without specific prior written permission.
00015  *
00016  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00017  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00018  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00019  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00020  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00021  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00022  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00023  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00024  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00025  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00026  * POSSIBILITY OF SUCH DAMAGE.
00027  */
00028 
00029 #ifndef HUMANOID_PLANNER_2D_SBPL_2D_PLANNER_
00030 #define HUMANOID_PLANNER_2D_SBPL_2D_PLANNER_
00031 
00032 #include <ros/ros.h>
00033 #include <geometry_msgs/PoseStamped.h>
00034 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00035 #include <sbpl/headers.h>
00036 #include <visualization_msgs/Marker.h>
00037 #include <nav_msgs/Path.h>
00038 #include <gridmap_2d/GridMap2D.h>
00039 
00040 
00041 class SBPLPlanner2D {
00042 public:
00043   SBPLPlanner2D();
00044   virtual ~SBPLPlanner2D();
00046   void goalCallback(const geometry_msgs::PoseStampedConstPtr& goal);
00048   void startCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& start);
00050   void mapCallback(const nav_msgs::OccupancyGridConstPtr& occupancy_map);
00052   bool updateMap(gridmap_2d::GridMap2DPtr map);
00053 
00062   bool plan(const geometry_msgs::Pose& start, const geometry_msgs::Pose& goal);
00063   
00064   inline const nav_msgs::Path& getPath() const{return path_;};
00065 
00066 protected:
00067   bool plan();
00068   void setPlanner(); 
00069   ros::NodeHandle nh_;
00070   ros::Subscriber goal_sub_, start_sub_, map_sub_;
00071   ros::Publisher path_pub_;
00072   boost::shared_ptr<SBPLPlanner> planner_;
00073   boost::shared_ptr<EnvironmentNAV2D> planner_environment_;
00074   gridmap_2d::GridMap2DPtr map_;
00075 
00076   std::string planner_type_;
00077   double allocated_time_;
00078   double initial_epsilon_;
00079   bool search_until_first_solution_;
00080   bool forward_search_;
00081   double robot_radius_;
00082 
00083   bool start_received_, goal_received_;
00084   geometry_msgs::Pose start_pose_, goal_pose_;
00085   nav_msgs::Path path_;
00086   
00087   static const unsigned char OBSTACLE_COST = 20;
00088 
00089   //double m_wayPointDistance;
00090 };
00091 
00092 #endif 


humanoid_planner_2d
Author(s): Armin Hornung
autogenerated on Mon Oct 6 2014 00:42:04