FootstepNavigation.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 #ifndef FOOTSTEP_PLANNER_FOOTSTEPNAVIGATION_H_
00022 #define FOOTSTEP_PLANNER_FOOTSTEPNAVIGATION_H_
00023 
00024 #include <actionlib/client/simple_action_client.h>
00025 #include <footstep_planner/FootstepPlanner.h>
00026 #include <footstep_planner/State.h>
00027 #include <geometry_msgs/Pose.h>
00028 #include <geometry_msgs/PoseStamped.h>
00029 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00030 #include <humanoid_nav_msgs/ClipFootstep.h>
00031 #include <humanoid_nav_msgs/ExecFootstepsAction.h>
00032 #include <humanoid_nav_msgs/ExecFootstepsFeedback.h>
00033 #include <humanoid_nav_msgs/PlanFootsteps.h>
00034 #include <humanoid_nav_msgs/StepTargetService.h>
00035 #include <nav_msgs/Path.h>
00036 #include <nav_msgs/OccupancyGrid.h>
00037 #include <ros/ros.h>
00038 #include <tf/tf.h>
00039 #include <tf/transform_listener.h>
00040 
00041 #include <assert.h>
00042 
00043 
00044 namespace footstep_planner
00045 {
00050 class FootstepNavigation
00051 {
00052 public:
00053   FootstepNavigation();
00054   virtual ~FootstepNavigation();
00055 
00057   bool setGoal(const geometry_msgs::PoseStampedConstPtr goal_pose);
00058 
00060   bool setGoal(float x, float y, float theta);
00061 
00067   void goalPoseCallback(
00068   const geometry_msgs::PoseStampedConstPtr& goal_pose);
00069 
00075   void mapCallback(const nav_msgs::OccupancyGridConstPtr& occupancy_map);
00076 
00077 protected:
00084   bool plan();
00085 
00094   bool replan();
00095 
00097   void startExecution();
00098 
00104   bool getFootTransform(const std::string& foot_id,
00105                         const std::string& world_frame_id,
00106                         const ros::Time& time,
00107                         const ros::Duration& waiting_time,
00108                         tf::Transform* foot);
00109 
00116   bool getFootstep(const tf::Pose& from, const State& from_planned,
00117                                const State& to, humanoid_nav_msgs::StepTarget* footstep);
00118 
00132   bool getFootstepsFromPath(
00133       const State& current_support_leg, int starting_step_num,
00134       std::vector<humanoid_nav_msgs::StepTarget>& footsteps);
00135 
00137   bool updateStart();
00138 
00140   void executeFootsteps();
00141 
00146   void executeFootstepsFast();
00147 
00152   void activeCallback();
00153 
00158   void doneCallback(
00159   const actionlib::SimpleClientGoalState& state,
00160       const humanoid_nav_msgs::ExecFootstepsResultConstPtr& result);
00161 
00166   void feedbackCallback(
00167   const humanoid_nav_msgs::ExecFootstepsFeedbackConstPtr& fb);
00168 
00169   bool performable(const humanoid_nav_msgs::StepTarget& footstep);
00170   bool performable(float step_x, float step_y);
00171 
00180   bool performanceValid(const humanoid_nav_msgs::ClipFootstep& footstep);
00181 
00183   bool performanceValid(const State& planned, const State& executed);
00184 
00186   bool performanceValid(float a_x, float a_y, float a_theta,
00187                         float b_x, float b_y, float b_theta);
00188 
00189   FootstepPlanner ivPlanner;
00190 
00191   ros::Subscriber ivGridMapSub;
00192   ros::Subscriber ivRobotPoseSub;
00193   ros::Subscriber ivGoalPoseSub;
00194 
00195   ros::ServiceClient ivFootstepSrv;
00196   ros::ServiceClient ivClipFootstepSrv;
00197 
00198   tf::TransformListener ivTransformListener;
00199 
00200   boost::mutex ivExecutionLock;
00201 
00202   boost::shared_ptr<boost::thread> ivFootstepExecutionPtr;
00203 
00204   std::string ivIdFootRight;
00205   std::string ivIdFootLeft;
00206   std::string ivIdMapFrame;
00207 
00208   double ivAccuracyX;
00209   double ivAccuracyY;
00210   double ivAccuracyTheta;
00211   double ivCellSize;
00212   int    ivNumAngleBins;
00213 
00215   bool ivForwardSearch;
00216 
00218   bool ivExecutingFootsteps;
00219 
00221   double ivFeedbackFrequency;
00222 
00224   actionlib::SimpleActionClient<
00225       humanoid_nav_msgs::ExecFootstepsAction> ivFootstepsExecution;
00226 
00228   const int ivExecutionShift;
00229 
00234   int ivControlStepIdx;
00235 
00240   int ivResetStepIdx;
00241 
00243   bool ivSafeExecution;
00244 
00245   double ivMaxStepX;
00246   double ivMaxStepY;
00247   double ivMaxStepTheta;
00248   double ivMaxInvStepX;
00249   double ivMaxInvStepY;
00250   double ivMaxInvStepTheta;
00251 
00252   std::vector<std::pair<double, double> > ivStepRange;
00253 };
00254 }
00255 #endif  // FOOTSTEP_PLANNER_FOOTSTEPNAVIGATION_H_


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