FootstepPlanner.h

Go to the documentation of this file.

/*
 * A footstep planner for humanoid robots.
 *
 * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
 * http://www.ros.org/wiki/footstep_planner
 *
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */


#ifndef FOOTSTEP_PLANNER_FOOTSTEPPLANNER_H_
#define FOOTSTEP_PLANNER_FOOTSTEPPLANNER_H_

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <humanoid_nav_msgs/PlanFootsteps.h>
#include <humanoid_nav_msgs/PlanFootstepsBetweenFeet.h>
#include <footstep_planner/helper.h>
#include <footstep_planner/PathCostHeuristic.h>
#include <footstep_planner/FootstepPlannerEnvironment.h>
#include <footstep_planner/PlanningStateChangeQuery.h>
#include <footstep_planner/State.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <tf/tf.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <XmlRpcValue.h>
#include <XmlRpcException.h>

#include <assert.h>
#include <time.h>


namespace footstep_planner
{
typedef std::vector<State>::const_iterator state_iter_t;

class FootstepPlanner
{
public:
  FootstepPlanner();
  virtual ~FootstepPlanner();

  bool plan(bool force_new_plan=true);

  bool plan(const geometry_msgs::PoseStampedConstPtr start,
            const geometry_msgs::PoseStampedConstPtr goal);

  bool plan(float start_x, float start_y, float start_theta,
            float goal_x, float goal_y, float goal_theta);

  bool replan();

  bool planService(humanoid_nav_msgs::PlanFootsteps::Request &req,
                   humanoid_nav_msgs::PlanFootsteps::Response &resp);

  bool planFeetService(humanoid_nav_msgs::PlanFootstepsBetweenFeet::Request &req,
                   humanoid_nav_msgs::PlanFootstepsBetweenFeet::Response &resp);

  bool setGoal(const State& left_foot, const State& right_foot);

  bool setGoal(const geometry_msgs::PoseStampedConstPtr goal_pose);

  bool setGoal(float x, float y, float theta);

  bool setStart(const geometry_msgs::PoseStampedConstPtr start_pose);

  bool setStart(float x, float y, float theta);

  bool setStart(const State& left_foot, const State& right_foot);

  bool updateMap(const gridmap_2d::GridMap2DPtr map);

  void setMarkerNamespace(const std::string& ns)
  {
    ivMarkerNamespace = ns;
  };

  void setMaxSearchTime(int search_time)
  {
    ivMaxSearchTime = search_time;
  };

  void goalPoseCallback(
      const geometry_msgs::PoseStampedConstPtr& goal_pose);
  void startPoseCallback(
      const geometry_msgs::PoseWithCovarianceStampedConstPtr& start_pose);

  void mapCallback(const nav_msgs::OccupancyGridConstPtr& occupancy_map);

  void clearFootstepPathVis(unsigned num_footsteps=0);

  double getPathCosts() const { return ivPathCost; };

  size_t getNumExpandedStates() const
  {
    return ivPlannerPtr->get_n_expands();
  };

  size_t getNumFootPoses() const { return ivPath.size(); };

  state_iter_t getPathBegin() const { return ivPath.begin(); };
  state_iter_t getPathEnd() const { return ivPath.end(); };

  int getPathSize() { return ivPath.size(); };

  State getStartFootLeft() { return ivStartFootLeft; };
  State getStartFootRight() { return ivStartFootRight; };

  void reset();

  void resetTotally();

  bool pathExists() { return (bool)ivPath.size(); };

  environment_params ivEnvironmentParams;

protected:
  void broadcastExpandedNodesVis();
  void broadcastRandomNodesVis();
  void broadcastFootstepPathVis();
  void broadcastHeuristicPathVis();
  void broadcastPathVis();

  void extractFootstepsSrv(std::vector<humanoid_nav_msgs::StepTarget> & footsteps) const;

  bool pathIsNew(const std::vector<int>& new_path);

  bool extractPath(const std::vector<int>& state_ids);

  void footPoseToMarker(const State& footstep,
                        visualization_msgs::Marker* marker);

  bool run();

  State getFootPose(const State& robot, Leg side);

  void setPlanner();

  void updateEnvironment(const gridmap_2d::GridMap2DPtr old_map);

  boost::shared_ptr<FootstepPlannerEnvironment> ivPlannerEnvironmentPtr;
  gridmap_2d::GridMap2DPtr ivMapPtr;
  boost::shared_ptr<SBPLPlanner> ivPlannerPtr;

  boost::shared_ptr<const PathCostHeuristic> ivPathCostHeuristicPtr;

  std::vector<State> ivPath;

  State ivStartFootLeft;
  State ivStartFootRight;
  State ivGoalFootLeft;
  State ivGoalFootRight;

  ros::Publisher  ivExpandedStatesVisPub;
  ros::Publisher  ivFootstepPathVisPub;
  ros::Publisher  ivRandomStatesVisPub;
  ros::Subscriber ivGridMapSub;
  ros::Publisher  ivHeuristicPathVisPub;
  ros::Publisher  ivPathVisPub;
  ros::Publisher  ivStartPoseVisPub;
  ros::ServiceServer ivFootstepPlanService;
  ros::ServiceServer ivFootstepPlanFeetService;

  double ivFootSeparation;
  double ivMaxStepWidth;
  int    ivCollisionCheckAccuracy;

  bool   ivStartPoseSetUp, ivGoalPoseSetUp;
  int    ivLastMarkerMsgSize;
  double ivPathCost;
  bool   ivSearchUntilFirstSolution;
  double ivMaxSearchTime;
  double ivInitialEpsilon;

   int ivChangedCellsLimit;

  std::string ivPlannerType;
  std::string ivMarkerNamespace;

  std::vector<int> ivPlanningStatesIds;
};
}

#endif  // FOOTSTEP_PLANNER_FOOTSTEPPLANNER_H_