planning_state.h
Go to the documentation of this file.
00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/footstep_planner/include/footstep_planner/planning_state.h $
00002 // SVN $Id: planning_state.h 3298 2012-09-28 11:37:38Z hornunga@informatik.uni-freiburg.de $
00003 
00004 /*
00005  * A footstep planner for humanoid robots
00006  *
00007  * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
00008  * http://www.ros.org/wiki/footstep_planner
00009  *
00010  *
00011  * This program is free software: you can redistribute it and/or modify
00012  * it under the terms of the GNU General Public License as published by
00013  * the Free Software Foundation, version 3.
00014  *
00015  * This program is distributed in the hope that it will be useful,
00016  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  * GNU General Public License for more details.
00019  *
00020  * You should have received a copy of the GNU General Public License
00021  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00022  */
00023 
00024 #ifndef FOOTSTEP_PLANNER_PLANNINGSTATE_H_
00025 #define FOOTSTEP_PLANNER_PLANNINGSTATE_H_
00026 
00027 #include <vigir_footstep_planning_lib/math.h>
00028 #include <vigir_footstep_planning_lib/modeling/state.h>
00029 
00030 
00031 
00032 namespace vigir_footstep_planning
00033 {
00049 class PlanningState
00050 {
00051 public:
00052   // typedefs
00053   typedef boost::shared_ptr<PlanningState> Ptr;
00054   typedef boost::shared_ptr<const PlanningState> ConstPtr;
00055 
00067   PlanningState(double x, double y, double z, double roll, double pitch, double yaw,
00068                 Leg leg, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state = nullptr, const PlanningState *succ_state = nullptr);
00069 
00074   PlanningState(int x, int y, double z, double roll, double pitch, int yaw,
00075                 Leg leg, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state = nullptr, const PlanningState *succ_state = nullptr);
00076 
00077   PlanningState(const geometry_msgs::Pose& pose, Leg leg, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state = nullptr, const PlanningState *succ_state = nullptr);
00078 
00080   PlanningState(const State& s, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state = nullptr, const PlanningState *succ_state = nullptr);
00081 
00083   PlanningState(const PlanningState& s);
00084 
00085   ~PlanningState();
00086 
00091   bool operator==(const PlanningState& s2) const;
00092 
00097   bool operator!=(const PlanningState& s2) const;
00098 
00104   void setId(unsigned int id) { ivId = id; }
00105 
00106   int getX() const { return ivX; }
00107   int getY() const { return ivY; }
00108   int getYaw() const { return ivYaw; }
00109 
00110   double getSwingHeight() const { return ivState.getSwingHeight(); }
00111 
00112   double getSwayDuration() const { return ivState.getSwayDuration(); }
00113   double getStepDuration() const { return ivState.getStepDuration(); }
00114 
00115   Leg getLeg() const { return ivState.getLeg(); }
00116 
00117   void setPredState(const PlanningState* pred_state) { ivpPredState = pred_state; }
00118   const PlanningState* getPredState() const { return ivpPredState; }
00119 
00120   void setSuccState(const PlanningState* succ_state) { ivpSuccState = succ_state; }
00121   const PlanningState* getSuccState() const { return ivpSuccState; }
00122 
00127   unsigned int getHashTag() const { return ivHashTag; }
00128 
00133   int getId() const { return ivId; }
00134 
00136   const State& getState() const;
00137   State& getState();
00138 
00139 private:
00141   State ivState;
00142 
00144   int ivX;
00146   int ivY;
00148   int ivYaw;
00149 
00150   const PlanningState *ivpPredState;
00151   const PlanningState *ivpSuccState;
00152 
00154   int ivId;
00155 
00160   unsigned int ivHashTag;
00161 };
00162 }
00163 #endif  // FOOTSTEP_PLANNER_PLANNINGSTATE_H_


vigir_footstep_planning_lib
Author(s): Alexander Stumpf
autogenerated on Sat Jun 8 2019 19:01:44