planning_state.cpp
Go to the documentation of this file.
1 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/footstep_planner/src/PlanningState.cpp $
2 // SVN $Id: PlanningState.cpp 4146 2013-05-13 13:57:41Z garimort@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  * D* Lite (Koenig et al. 2002) partly based on the implementation
11  * by J. Neufeld (http://code.google.com/p/dstarlite/)
12  *
13  *
14  * This program is free software: you can redistribute it and/or modify
15  * it under the terms of the GNU General Public License as published by
16  * the Free Software Foundation, version 3.
17  *
18  * This program is distributed in the hope that it will be useful,
19  * but WITHOUT ANY WARRANTY; without even the implied warranty of
20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21  * GNU General Public License for more details.
22  *
23  * You should have received a copy of the GNU General Public License
24  * along with this program. If not, see <http://www.gnu.org/licenses/>.
25  */
26 
28 
29 
30 
32 {
33 PlanningState::PlanningState(double x, double y, double z, double roll, double pitch, double yaw,
34  Leg leg, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state, const PlanningState *succ_state)
35  : ivState(State(x, y, z, roll, pitch, yaw, leg))
36  , ivX(state_2_cell(x, cell_size))
37  , ivY(state_2_cell(y, cell_size))
38  , ivYaw(angle_state_2_cell(yaw, angle_bin_size))
39  , ivpPredState(pred_state)
40  , ivpSuccState(succ_state)
41  , ivId(-1)
42 {
43  unsigned int hash_pred = pred_state ? pred_state->getHashTag() : 0u;
44  unsigned int hash_succ = succ_state ? succ_state->getHashTag() : 0u;
45  ivHashTag = calc_hash_tag(ivX, ivY, ivYaw, leg, hash_pred, hash_succ, max_hash_size);
46 }
47 
48 
49 PlanningState::PlanningState(int x, int y, double z, double roll, double pitch, int yaw,
50  Leg leg, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state, const PlanningState *succ_state)
51  : ivState(State(cell_2_state(x, cell_size), cell_2_state(y, cell_size), z, roll, pitch, angles::normalize_angle(angle_cell_2_state(yaw, angle_bin_size)), leg))
52  , ivX(x)
53  , ivY(y)
54  , ivYaw(yaw)
55  , ivpPredState(pred_state)
56  , ivpSuccState(succ_state)
57  , ivId(-1)
58 {
59  unsigned int hash_pred = pred_state ? pred_state->getHashTag() : 0u;
60  unsigned int hash_succ = succ_state ? succ_state->getHashTag() : 0u;
61  ivHashTag = calc_hash_tag(ivX, ivY, ivYaw, leg, hash_pred, hash_succ, max_hash_size);
62 }
63 
64 PlanningState::PlanningState(const geometry_msgs::Pose& pose,
65  Leg leg, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state, const PlanningState *succ_state)
66  : ivState(State(pose, leg))
67  , ivX(state_2_cell(pose.position.x, cell_size))
68  , ivY(state_2_cell(pose.position.y, cell_size))
69  , ivpPredState(pred_state)
70  , ivpSuccState(succ_state)
71  , ivId(-1)
72 {
73  ivYaw = angle_state_2_cell(ivState.getYaw(), angle_bin_size);
74 
75  unsigned int hash_pred = pred_state ? pred_state->getHashTag() : 0u;
76  unsigned int hash_succ = succ_state ? succ_state->getHashTag() : 0u;
77  ivHashTag = calc_hash_tag(ivX, ivY, ivYaw, leg, hash_pred, hash_succ, max_hash_size);
78 }
79 
80 PlanningState::PlanningState(const State& s, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state, const PlanningState *succ_state)
81  : ivState(s)
82  , ivX(state_2_cell(s.getX(), cell_size))
83  , ivY(state_2_cell(s.getY(), cell_size))
84  , ivYaw(angle_state_2_cell(s.getYaw(), angle_bin_size))
85  , ivpPredState(pred_state)
86  , ivpSuccState(succ_state)
87  , ivId(-1)
88 {
89  unsigned int hash_pred = pred_state ? pred_state->getHashTag() : 0u;
90  unsigned int hash_succ = succ_state ? succ_state->getHashTag() : 0u;
91  ivHashTag = calc_hash_tag(ivX, ivY, ivYaw, s.getLeg(), hash_pred, hash_succ, max_hash_size);
92 }
93 
95  : ivState(s.getState())
96  , ivX(s.getX())
97  , ivY(s.getY())
98  , ivYaw(s.getYaw())
101  , ivId(s.getId())
102  , ivHashTag(s.getHashTag())
103 {}
104 
106 {}
107 
109 {
110  // First test the hash tag. If they differ, the states are definitely
111  // different.
112  if (ivHashTag != s2.getHashTag())
113  return false;
114 
116  if (ivpPredState != s2.ivpPredState)
117  return false;
118 
119  if (ivpSuccState != s2.ivpSuccState)
120  return false;
121 
122  // other variables may be ignored, because they are
123  // selected by x and y
124  return (ivX == s2.getX() && ivY == s2.getY() && ivYaw == s2.getYaw() &&
125  ivState.getLeg() == s2.ivState.getLeg());
126 }
127 
129 {
130  return !operator==(s2);
131 }
132 
134 {
135  return ivState;
136 }
137 
139 {
140  return ivState;
141 }
142 } // end of namespace
int state_2_cell(float value, float cell_size)
Discretize a (continuous) state value into a cell. (Should be used to discretize a State to a Plannin...
Definition: math.h:191
int angle_state_2_cell(double angle, double angle_bin_size)
Discretize a (continuous) angle into a bin.
Definition: math.h:167
State ivState
pointer to original state
int ivYaw
The robot&#39;s orientation.
double getYaw() const
Definition: state.h:102
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.
TFSIMD_FORCE_INLINE const tfScalar & y() const
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
const PlanningState * getSuccState() const
double cell_2_state(int value, double cell_size)
Calculate the respective (continuous) state value for a cell. (Should be used to get a State from a d...
Definition: math.h:202
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...
TFSIMD_FORCE_INLINE const tfScalar & x() const
def normalize_angle(angle)
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...
unsigned int calc_hash_tag(int x, int y, int theta, int leg, unsigned int hash_pred, unsigned int hash_succ, int max_hash_size)
Definition: math.h:229
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 angle_cell_2_state(int angle, double angle_bin_size)
Calculate the respective (continuous) angle for a bin.
Definition: math.h:178


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