planning_state.h
Go to the documentation of this file.
1 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/footstep_planner/include/footstep_planner/planning_state.h $
2 // SVN $Id: planning_state.h 3298 2012-09-28 11:37:38Z hornunga@informatik.uni-freiburg.de $
3 
4 /*
5  * A footstep planner for humanoid robots
6  *
7  * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
8  * http://www.ros.org/wiki/footstep_planner
9  *
10  *
11  * This program is free software: you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation, version 3.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License
21  * along with this program. If not, see <http://www.gnu.org/licenses/>.
22  */
23 
24 #ifndef FOOTSTEP_PLANNER_PLANNINGSTATE_H_
25 #define FOOTSTEP_PLANNER_PLANNINGSTATE_H_
26 
29 
30 
31 
33 {
50 {
51 public:
52  // typedefs
55 
67  PlanningState(double x, double y, double z, double roll, double pitch, double yaw,
68  Leg leg, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state = nullptr, const PlanningState *succ_state = nullptr);
69 
74  PlanningState(int x, int y, double z, double roll, double pitch, int yaw,
75  Leg leg, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state = nullptr, const PlanningState *succ_state = nullptr);
76 
77  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);
78 
80  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);
81 
83  PlanningState(const PlanningState& s);
84 
86 
91  bool operator==(const PlanningState& s2) const;
92 
97  bool operator!=(const PlanningState& s2) const;
98 
104  void setId(unsigned int id) { ivId = id; }
105 
106  int getX() const { return ivX; }
107  int getY() const { return ivY; }
108  int getYaw() const { return ivYaw; }
109 
110  double getSwingHeight() const { return ivState.getSwingHeight(); }
111 
112  double getSwayDuration() const { return ivState.getSwayDuration(); }
113  double getStepDuration() const { return ivState.getStepDuration(); }
114 
115  Leg getLeg() const { return ivState.getLeg(); }
116 
117  void setPredState(const PlanningState* pred_state) { ivpPredState = pred_state; }
118  const PlanningState* getPredState() const { return ivpPredState; }
119 
120  void setSuccState(const PlanningState* succ_state) { ivpSuccState = succ_state; }
121  const PlanningState* getSuccState() const { return ivpSuccState; }
122 
127  unsigned int getHashTag() const { return ivHashTag; }
128 
133  int getId() const { return ivId; }
134 
136  const State& getState() const;
137  State& getState();
138 
139 private:
142 
144  int ivX;
146  int ivY;
148  int ivYaw;
149 
152 
154  int ivId;
155 
160  unsigned int ivHashTag;
161 };
162 }
163 #endif // FOOTSTEP_PLANNER_PLANNINGSTATE_H_
State ivState
pointer to original state
int ivYaw
The robot&#39;s orientation.
double getSwingHeight() const
Definition: state.h:107
boost::shared_ptr< PlanningState > Ptr
void setPredState(const PlanningState *pred_state)
A class representing the robot&#39;s pose (i.e. position and orientation) in the underlying SBPL...
int ivX
Value of the grid cell the position&#39;s x value is fitted into.
double getStepDuration() const
Definition: state.h:109
int ivId
The (unique) ID of the planning state.
const PlanningState * getPredState() const
int ivY
Value of the grid cell the position&#39;s y value is fitted into.
A class representing the robot&#39;s pose (i.e. position and orientation) in the (continuous) world view...
Definition: state.h:49
void setSuccState(const PlanningState *succ_state)
const PlanningState * getSuccState() const
bool operator==(const PlanningState &s2) const
Compare two states on equality of x, y, theta, leg. Makes first use of the non-unique hash tag to rul...
void setId(unsigned int id)
Used to attach such an unique ID to the planning state. (This cannot be done in the constructor since...
boost::shared_ptr< const PlanningState > ConstPtr
PlanningState(double x, double y, double z, double roll, double pitch, double yaw, Leg leg, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state=nullptr, const PlanningState *succ_state=nullptr)
x, y and theta represent the global (continuous) position and orientation of the robot&#39;s support leg...
const State & getState() const
gets the continuous State the PlanningState represents.
bool operator!=(const PlanningState &s2) const
Compare two states on inequality of x, y, theta, leg by comparing the hash tags of the states...
double getSwayDuration() const
Definition: state.h:108


vigir_footstep_planning_lib
Author(s): Alexander Stumpf
autogenerated on Mon Jun 10 2019 15:47:33