FootstepPlanner.h
Go to the documentation of this file.
00001 /*
00002  * A footstep planner for humanoid robots.
00003  *
00004  * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
00005  * http://www.ros.org/wiki/footstep_planner
00006  *
00007  *
00008  * This program is free software: you can redistribute it and/or modify
00009  * it under the terms of the GNU General Public License as published by
00010  * the Free Software Foundation, version 3.
00011  *
00012  * This program is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  * GNU General Public License for more details.
00016  *
00017  * You should have received a copy of the GNU General Public License
00018  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00021 
00022 #ifndef FOOTSTEP_PLANNER_FOOTSTEPPLANNER_H_
00023 #define FOOTSTEP_PLANNER_FOOTSTEPPLANNER_H_
00024 
00025 #include <geometry_msgs/Pose.h>
00026 #include <geometry_msgs/PoseStamped.h>
00027 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00028 #include <humanoid_nav_msgs/PlanFootsteps.h>
00029 #include <humanoid_nav_msgs/PlanFootstepsBetweenFeet.h>
00030 #include <footstep_planner/helper.h>
00031 #include <footstep_planner/PathCostHeuristic.h>
00032 #include <footstep_planner/FootstepPlannerEnvironment.h>
00033 #include <footstep_planner/PlanningStateChangeQuery.h>
00034 #include <footstep_planner/State.h>
00035 #include <nav_msgs/Path.h>
00036 #include <nav_msgs/OccupancyGrid.h>
00037 #include <ros/ros.h>
00038 #include <sensor_msgs/PointCloud.h>
00039 #include <tf/tf.h>
00040 #include <visualization_msgs/Marker.h>
00041 #include <visualization_msgs/MarkerArray.h>
00042 #include <XmlRpcValue.h>
00043 #include <XmlRpcException.h>
00044 
00045 #include <assert.h>
00046 #include <time.h>
00047 
00048 
00049 namespace footstep_planner
00050 {
00051 typedef std::vector<State>::const_iterator state_iter_t;
00052 
00057 class FootstepPlanner
00058 {
00059 public:
00060   FootstepPlanner();
00061   virtual ~FootstepPlanner();
00062 
00070   bool plan(bool force_new_plan=true);
00071 
00073   bool plan(const geometry_msgs::PoseStampedConstPtr start,
00074             const geometry_msgs::PoseStampedConstPtr goal);
00075 
00077   bool plan(float start_x, float start_y, float start_theta,
00078             float goal_x, float goal_y, float goal_theta);
00079 
00087   bool replan();
00088 
00090   bool planService(humanoid_nav_msgs::PlanFootsteps::Request &req,
00091                    humanoid_nav_msgs::PlanFootsteps::Response &resp);
00092 
00094   bool planFeetService(humanoid_nav_msgs::PlanFootstepsBetweenFeet::Request &req,
00095                    humanoid_nav_msgs::PlanFootstepsBetweenFeet::Response &resp);
00096 
00102   bool setGoal(const State& left_foot, const State& right_foot);
00103 
00109   bool setGoal(const geometry_msgs::PoseStampedConstPtr goal_pose);
00110 
00116   bool setGoal(float x, float y, float theta);
00117 
00123   bool setStart(const geometry_msgs::PoseStampedConstPtr start_pose);
00124 
00130   bool setStart(float x, float y, float theta);
00131 
00137   bool setStart(const State& left_foot, const State& right_foot);
00138 
00145   bool updateMap(const gridmap_2d::GridMap2DPtr map);
00146 
00147   void setMarkerNamespace(const std::string& ns)
00148   {
00149     ivMarkerNamespace = ns;
00150   }
00151 
00153   void setMaxSearchTime(int search_time)
00154   {
00155     ivMaxSearchTime = search_time;
00156   }
00157 
00167   void goalPoseCallback(
00168       const geometry_msgs::PoseStampedConstPtr& goal_pose);
00178   void startPoseCallback(
00179       const geometry_msgs::PoseWithCovarianceStampedConstPtr& start_pose);
00180 
00186   void mapCallback(const nav_msgs::OccupancyGridConstPtr& occupancy_map);
00187 
00192   void clearFootstepPathVis(unsigned num_footsteps=0);
00193 
00195   double getPathCosts() const { return ivPathCost; }
00196 
00198   size_t getNumExpandedStates() const
00199   {
00200     return ivPlannerPtr->get_n_expands();
00201   }
00202 
00204   size_t getNumFootPoses() const { return ivPath.size(); }
00205 
00206   state_iter_t getPathBegin() const { return ivPath.begin(); }
00207   state_iter_t getPathEnd() const { return ivPath.end(); }
00208 
00210   int getPathSize() { return ivPath.size(); }
00211 
00212   State getStartFootLeft() { return ivStartFootLeft; }
00213   State getStartFootRight() { return ivStartFootRight; }
00214 
00216   void reset();
00217 
00219   void resetTotally();
00220 
00222   bool pathExists() { return (bool)ivPath.size(); }
00223 
00225   environment_params ivEnvironmentParams;
00226 
00227 protected:
00228   void broadcastExpandedNodesVis();
00229   void broadcastRandomNodesVis();
00230   void broadcastFootstepPathVis();
00231   void broadcastHeuristicPathVis();
00232   void broadcastPathVis();
00233 
00235   void extractFootstepsSrv(std::vector<humanoid_nav_msgs::StepTarget> & footsteps) const;
00236 
00241   bool pathIsNew(const std::vector<int>& new_path);
00242 
00247   bool extractPath(const std::vector<int>& state_ids);
00248 
00250   void footPoseToMarker(const State& footstep,
00251                         visualization_msgs::Marker* marker);
00252 
00259   bool run();
00260 
00262   State getFootPose(const State& robot, Leg side);
00263 
00265   void setPlanner();
00266 
00268   void updateEnvironment(const gridmap_2d::GridMap2DPtr old_map);
00269 
00270   boost::shared_ptr<FootstepPlannerEnvironment> ivPlannerEnvironmentPtr;
00271   gridmap_2d::GridMap2DPtr ivMapPtr;
00272   boost::shared_ptr<SBPLPlanner> ivPlannerPtr;
00273 
00274   boost::shared_ptr<const PathCostHeuristic> ivPathCostHeuristicPtr;
00275 
00276   std::vector<State> ivPath;
00277 
00278   State ivStartFootLeft;
00279   State ivStartFootRight;
00280   State ivGoalFootLeft;
00281   State ivGoalFootRight;
00282 
00283   ros::Publisher  ivExpandedStatesVisPub;
00284   ros::Publisher  ivFootstepPathVisPub;
00285   ros::Publisher  ivRandomStatesVisPub;
00286   ros::Subscriber ivGridMapSub;
00287   ros::Publisher  ivHeuristicPathVisPub;
00288   ros::Publisher  ivPathVisPub;
00289   ros::Publisher  ivStartPoseVisPub;
00290   ros::ServiceServer ivFootstepPlanService;
00291   ros::ServiceServer ivFootstepPlanFeetService;
00292 
00293   double ivFootSeparation;
00294   double ivMaxStepWidth;
00295   int    ivCollisionCheckAccuracy;
00296 
00297   bool   ivStartPoseSetUp, ivGoalPoseSetUp;
00298   int    ivLastMarkerMsgSize;
00299   double ivPathCost;
00300   bool   ivSearchUntilFirstSolution;
00301   double ivMaxSearchTime;
00302   double ivInitialEpsilon;
00303 
00308    int ivChangedCellsLimit;
00309 
00310   std::string ivPlannerType;
00311   std::string ivMarkerNamespace;
00312 
00313   std::vector<int> ivPlanningStatesIds;
00314 };
00315 }
00316 
00317 #endif  // FOOTSTEP_PLANNER_FOOTSTEPPLANNER_H_


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Sat Jun 8 2019 20:21:05