FootstepNavigation.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_FOOTSTEPNAVIGATION_H_
#define FOOTSTEP_PLANNER_FOOTSTEPNAVIGATION_H_

#include <actionlib/client/simple_action_client.h>
#include <footstep_planner/FootstepPlanner.h>
#include <footstep_planner/State.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <humanoid_nav_msgs/ClipFootstep.h>
#include <humanoid_nav_msgs/ExecFootstepsAction.h>
#include <humanoid_nav_msgs/ExecFootstepsFeedback.h>
#include <humanoid_nav_msgs/PlanFootsteps.h>
#include <humanoid_nav_msgs/StepTargetService.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>

#include <assert.h>


namespace footstep_planner
{
class FootstepNavigation
{
public:
  FootstepNavigation();
  virtual ~FootstepNavigation();

  bool setGoal(const geometry_msgs::PoseStampedConstPtr goal_pose);

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

  void goalPoseCallback(
  const geometry_msgs::PoseStampedConstPtr& goal_pose);

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

protected:
  bool plan();

  bool replan();

  void startExecution();

  bool getFootTransform(const std::string& foot_id,
                        const std::string& world_frame_id,
                        const ros::Time& time,
                        const ros::Duration& waiting_time,
                        tf::Transform* foot);

  bool getFootstep(const tf::Pose& from, const State& from_planned,
                               const State& to, humanoid_nav_msgs::StepTarget* footstep);

  bool getFootstepsFromPath(
      const State& current_support_leg, int starting_step_num,
      std::vector<humanoid_nav_msgs::StepTarget>& footsteps);

  bool updateStart();

  void executeFootsteps();

  void executeFootstepsFast();

  void activeCallback();

  void doneCallback(
  const actionlib::SimpleClientGoalState& state,
      const humanoid_nav_msgs::ExecFootstepsResultConstPtr& result);

  void feedbackCallback(
  const humanoid_nav_msgs::ExecFootstepsFeedbackConstPtr& fb);

  bool performable(const humanoid_nav_msgs::StepTarget& footstep);
  bool performable(float step_x, float step_y);

  bool performanceValid(const humanoid_nav_msgs::ClipFootstep& footstep);

  bool performanceValid(const State& planned, const State& executed);

  bool performanceValid(float a_x, float a_y, float a_theta,
                        float b_x, float b_y, float b_theta);

  FootstepPlanner ivPlanner;

  ros::Subscriber ivGridMapSub;
  ros::Subscriber ivRobotPoseSub;
  ros::Subscriber ivGoalPoseSub;

  ros::ServiceClient ivFootstepSrv;
  ros::ServiceClient ivClipFootstepSrv;

  tf::TransformListener ivTransformListener;

  boost::mutex ivExecutionLock;

  boost::shared_ptr<boost::thread> ivFootstepExecutionPtr;

  std::string ivIdFootRight;
  std::string ivIdFootLeft;
  std::string ivIdMapFrame;

  double ivAccuracyX;
  double ivAccuracyY;
  double ivAccuracyTheta;
  double ivCellSize;
  int    ivNumAngleBins;

  bool ivForwardSearch;

  bool ivExecutingFootsteps;

  double ivFeedbackFrequency;

  actionlib::SimpleActionClient<
      humanoid_nav_msgs::ExecFootstepsAction> ivFootstepsExecution;

  const int ivExecutionShift;

  int ivControlStepIdx;

  int ivResetStepIdx;

  bool ivSafeExecution;

  double ivMaxStepX;
  double ivMaxStepY;
  double ivMaxStepTheta;
  double ivMaxInvStepX;
  double ivMaxInvStepY;
  double ivMaxInvStepTheta;

  std::vector<std::pair<double, double> > ivStepRange;
};
}
#endif  // FOOTSTEP_PLANNER_FOOTSTEPNAVIGATION_H_