PlanningState.cpp
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  * D* Lite (Koenig et al. 2002) partly based on the implementation
00008  * by J. Neufeld (http://code.google.com/p/dstarlite/)
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 #include <footstep_planner/PlanningState.h>
00025 
00026 
00027 namespace footstep_planner
00028 {
00029 PlanningState::PlanningState(double x, double y, double theta, Leg leg,
00030                              double cell_size, int num_angle_bins,
00031                              int max_hash_size)
00032 : ivX(state_2_cell(x, cell_size)),
00033   ivY(state_2_cell(y, cell_size)),
00034   ivTheta(angle_state_2_cell(theta, num_angle_bins)),
00035   ivLeg(leg),
00036   ivId(-1),
00037   ivHashTag(calc_hash_tag(ivX, ivY, ivTheta, ivLeg, max_hash_size))
00038 {}
00039 
00040 
00041 PlanningState::PlanningState(int x, int y, int theta, Leg leg,
00042                              int max_hash_size)
00043 :  ivX(x),
00044    ivY(y),
00045    ivTheta(theta),
00046    ivLeg(leg),
00047    ivId(-1),
00048    ivHashTag(calc_hash_tag(ivX, ivY, ivTheta, ivLeg, max_hash_size))
00049 {}
00050 
00051 
00052 PlanningState::PlanningState(const State& s, double cell_size,
00053                              int num_angle_bins, int max_hash_size)
00054 : ivX(state_2_cell(s.getX(), cell_size)),
00055   ivY(state_2_cell(s.getY(), cell_size)),
00056   ivTheta(angle_state_2_cell(s.getTheta(), num_angle_bins)),
00057   ivLeg(s.getLeg()),
00058   ivId(-1),
00059   ivHashTag(calc_hash_tag(ivX, ivY, ivTheta, ivLeg, max_hash_size))
00060 {}
00061 
00062 
00063 PlanningState::PlanningState(const PlanningState& s)
00064 : ivX(s.getX()),
00065   ivY(s.getY()),
00066   ivTheta(s.getTheta()),
00067   ivLeg(s.getLeg()),
00068   ivId(s.getId()),
00069   ivHashTag(s.getHashTag())
00070 {}
00071 
00072 
00073 PlanningState::~PlanningState()
00074 {}
00075 
00076 
00077 bool
00078 PlanningState::operator ==(const PlanningState& s2)
00079 const
00080 {
00081   // First test the hash tag. If they differ, the states are definitely
00082   // different.
00083   if (ivHashTag != s2.getHashTag())
00084     return false;
00085 
00086   return (ivX == s2.getX() && ivY == s2.getY() &&
00087     ivTheta == s2.getTheta() && ivLeg == s2.getLeg());
00088 }
00089 
00090 
00091 bool
00092 PlanningState::operator !=(const PlanningState& s2)
00093 const
00094 {
00095   return ivHashTag != s2.getHashTag();
00096 }
00097 
00098 
00099 State
00100 PlanningState::getState(double cell_size, int num_angle_bins)
00101 const
00102 {
00103   return State(cell_2_state(ivX, cell_size),
00104                cell_2_state(ivY, cell_size),
00105                angles::normalize_angle(
00106                    angle_cell_2_state(ivTheta, num_angle_bins)),
00107                    ivLeg);
00108 }
00109 } // end of namespace


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