FootstepPlanner.h
Go to the documentation of this file.
1 /*
2  * A footstep planner for humanoid robots.
3  *
4  * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
5  * http://www.ros.org/wiki/footstep_planner
6  *
7  *
8  * This program is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, version 3.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program. If not, see <http://www.gnu.org/licenses/>.
19  */
20 
21 
22 #ifndef FOOTSTEP_PLANNER_FOOTSTEPPLANNER_H_
23 #define FOOTSTEP_PLANNER_FOOTSTEPPLANNER_H_
24 
25 #include <geometry_msgs/Pose.h>
26 #include <geometry_msgs/PoseStamped.h>
27 #include <geometry_msgs/PoseWithCovarianceStamped.h>
28 #include <humanoid_nav_msgs/PlanFootsteps.h>
29 #include <humanoid_nav_msgs/PlanFootstepsBetweenFeet.h>
34 #include <footstep_planner/State.h>
35 #include <nav_msgs/Path.h>
36 #include <nav_msgs/OccupancyGrid.h>
37 #include <ros/ros.h>
38 #include <sensor_msgs/PointCloud.h>
39 #include <tf/tf.h>
40 #include <visualization_msgs/Marker.h>
41 #include <visualization_msgs/MarkerArray.h>
42 #include <XmlRpcValue.h>
43 #include <XmlRpcException.h>
44 
45 #include <assert.h>
46 #include <time.h>
47 
48 
49 namespace footstep_planner
50 {
51 typedef std::vector<State>::const_iterator state_iter_t;
52 
58 {
59 public:
61  virtual ~FootstepPlanner();
62 
70  bool plan(bool force_new_plan=true);
71 
73  bool plan(const geometry_msgs::PoseStampedConstPtr start,
74  const geometry_msgs::PoseStampedConstPtr goal);
75 
77  bool plan(float start_x, float start_y, float start_theta,
78  float goal_x, float goal_y, float goal_theta);
79 
87  bool replan();
88 
90  bool planService(humanoid_nav_msgs::PlanFootsteps::Request &req,
91  humanoid_nav_msgs::PlanFootsteps::Response &resp);
92 
94  bool planFeetService(humanoid_nav_msgs::PlanFootstepsBetweenFeet::Request &req,
95  humanoid_nav_msgs::PlanFootstepsBetweenFeet::Response &resp);
96 
102  bool setGoal(const State& left_foot, const State& right_foot);
103 
109  bool setGoal(const geometry_msgs::PoseStampedConstPtr goal_pose);
110 
116  bool setGoal(float x, float y, float theta);
117 
123  bool setStart(const geometry_msgs::PoseStampedConstPtr start_pose);
124 
130  bool setStart(float x, float y, float theta);
131 
137  bool setStart(const State& left_foot, const State& right_foot);
138 
145  bool updateMap(const gridmap_2d::GridMap2DPtr map);
146 
147  void setMarkerNamespace(const std::string& ns)
148  {
149  ivMarkerNamespace = ns;
150  }
151 
153  void setMaxSearchTime(int search_time)
154  {
155  ivMaxSearchTime = search_time;
156  }
157 
167  void goalPoseCallback(
168  const geometry_msgs::PoseStampedConstPtr& goal_pose);
178  void startPoseCallback(
179  const geometry_msgs::PoseWithCovarianceStampedConstPtr& start_pose);
180 
186  void mapCallback(const nav_msgs::OccupancyGridConstPtr& occupancy_map);
187 
192  void clearFootstepPathVis(unsigned num_footsteps=0);
193 
195  double getPathCosts() const { return ivPathCost; }
196 
198  size_t getNumExpandedStates() const
199  {
200  return ivPlannerPtr->get_n_expands();
201  }
202 
204  size_t getNumFootPoses() const { return ivPath.size(); }
205 
206  state_iter_t getPathBegin() const { return ivPath.begin(); }
207  state_iter_t getPathEnd() const { return ivPath.end(); }
208 
210  int getPathSize() { return ivPath.size(); }
211 
214 
216  void reset();
217 
219  void resetTotally();
220 
222  bool pathExists() { return (bool)ivPath.size(); }
223 
226 
227 protected:
232  void broadcastPathVis();
233 
235  void extractFootstepsSrv(std::vector<humanoid_nav_msgs::StepTarget> & footsteps) const;
236 
241  bool pathIsNew(const std::vector<int>& new_path);
242 
247  bool extractPath(const std::vector<int>& state_ids);
248 
250  void footPoseToMarker(const State& footstep,
251  visualization_msgs::Marker* marker);
252 
259  bool run();
260 
262  State getFootPose(const State& robot, Leg side);
263 
265  void setPlanner();
266 
268  void updateEnvironment(const gridmap_2d::GridMap2DPtr old_map);
269 
273 
275 
276  std::vector<State> ivPath;
277 
282 
292 
296 
299  double ivPathCost;
303 
309 
310  std::string ivPlannerType;
311  std::string ivMarkerNamespace;
312 
313  std::vector<int> ivPlanningStatesIds;
314 };
315 }
316 
317 #endif // FOOTSTEP_PLANNER_FOOTSTEPPLANNER_H_
bool setGoal(const State &left_foot, const State &right_foot)
Sets the goal pose as two feet (left / right)
std::vector< int > ivPlanningStatesIds
bool planFeetService(humanoid_nav_msgs::PlanFootstepsBetweenFeet::Request &req, humanoid_nav_msgs::PlanFootstepsBetweenFeet::Response &resp)
Service handle to plan footsteps.
bool plan(bool force_new_plan=true)
Start a planning task from scratch (will delete information of previous planning tasks). Map and start, goal poses need to be set beforehand.
void setMarkerNamespace(const std::string &ns)
void clearFootstepPathVis(unsigned num_footsteps=0)
Clear the footstep path visualization from a previous planning task.
environment_params ivEnvironmentParams
Planning parameters.
ros::ServiceServer ivFootstepPlanFeetService
void goalPoseCallback(const geometry_msgs::PoseStampedConstPtr &goal_pose)
Callback to set the goal pose as a robot pose centered between two feet. If the start pose has been s...
bool updateMap(const gridmap_2d::GridMap2DPtr map)
Updates the map in the planning environment.
void setMaxSearchTime(int search_time)
Set the maximal search time.
state_iter_t getPathEnd() const
A class to control the interaction between ROS and the footstep planner.
void mapCallback(const nav_msgs::OccupancyGridConstPtr &occupancy_map)
Callback to set the map.
bool replan()
Starts a planning task based on previous planning information (note that this method can also be used...
A class representing the robot&#39;s pose (i.e. position and orientation) in the (continuous) world view...
Definition: State.h:34
boost::shared_ptr< SBPLPlanner > ivPlannerPtr
void startPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &start_pose)
Callback to set the start pose as a robot pose centered between two feet. If the goal pose has been s...
void updateEnvironment(const gridmap_2d::GridMap2DPtr old_map)
Updates the environment in case of a changed map.
boost::shared_ptr< FootstepPlannerEnvironment > ivPlannerEnvironmentPtr
state_iter_t getPathBegin() const
bool pathIsNew(const std::vector< int > &new_path)
boost::shared_ptr< const PathCostHeuristic > ivPathCostHeuristicPtr
ros::ServiceServer ivFootstepPlanService
void reset()
Reset the previous planning information.
void extractFootstepsSrv(std::vector< humanoid_nav_msgs::StepTarget > &footsteps) const
helper to create service response
bool run()
Starts the planning task in the underlying SBPL.
State getFootPose(const State &robot, Leg side)
Returns the foot pose of a leg for a given robot pose.
bool planService(humanoid_nav_msgs::PlanFootsteps::Request &req, humanoid_nav_msgs::PlanFootsteps::Response &resp)
Service handle to plan footsteps.
std::vector< State >::const_iterator state_iter_t
void footPoseToMarker(const State &footstep, visualization_msgs::Marker *marker)
Generates a visualization msgs for a foot pose.
void resetTotally()
Reset and reinitialize the environment.
void setPlanner()
Sets the planning algorithm used by SBPL.
bool setStart(const geometry_msgs::PoseStampedConstPtr start_pose)
Sets the start pose as a robot pose centered between two feet.
int ivChangedCellsLimit
If limit of changed cells is reached the planner starts a new task from the scratch.
gridmap_2d::GridMap2DPtr ivMapPtr
bool extractPath(const std::vector< int > &state_ids)
Extracts the path (list of foot poses) from a list of state IDs calculated by the SBPL...


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Mon Jun 10 2019 13:38:24