$search
00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/footstep_planner/src/PlanningState.cpp $ 00002 // SVN $Id: PlanningState.cpp 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 * D* Lite (Koenig et al. 2002) partly based on the implementation 00011 * by J. Neufeld (http://code.google.com/p/dstarlite/) 00012 * 00013 * 00014 * This program is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU General Public License as published by 00016 * the Free Software Foundation, version 3. 00017 * 00018 * This program is distributed in the hope that it will be useful, 00019 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00020 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00021 * GNU General Public License for more details. 00022 * 00023 * You should have received a copy of the GNU General Public License 00024 * along with this program. If not, see <http://www.gnu.org/licenses/>. 00025 */ 00026 00027 #include <footstep_planner/PlanningState.h> 00028 00029 00030 namespace footstep_planner 00031 { 00032 PlanningState::PlanningState(double x, double y, double theta, Leg leg, 00033 double cell_size, int num_angle_bins, 00034 int max_hash_size) 00035 : ivX(state_2_cell(x, cell_size)), 00036 ivY(state_2_cell(y, cell_size)), 00037 ivTheta(angle_state_2_cell(theta, num_angle_bins)), 00038 ivLeg(leg), 00039 ivId(-1), 00040 ivHashTag(calc_hash_tag(ivX, ivY, ivTheta, ivLeg, max_hash_size)) 00041 {} 00042 00043 00044 PlanningState::PlanningState(int x, int y, int theta, Leg leg, 00045 int max_hash_size) 00046 : ivX(x), 00047 ivY(y), 00048 ivTheta(theta), 00049 ivLeg(leg), 00050 ivId(-1), 00051 ivHashTag(calc_hash_tag(ivX, ivY, ivTheta, ivLeg, max_hash_size)) 00052 {} 00053 00054 00055 PlanningState::PlanningState(const State& s, double cell_size, 00056 int num_angle_bins, int max_hash_size) 00057 : ivX(state_2_cell(s.getX(), cell_size)), 00058 ivY(state_2_cell(s.getY(), cell_size)), 00059 ivTheta(angle_state_2_cell(s.getTheta(), num_angle_bins)), 00060 ivLeg(s.getLeg()), 00061 ivId(-1), 00062 ivHashTag(calc_hash_tag(ivX, ivY, ivTheta, ivLeg, max_hash_size)) 00063 {} 00064 00065 00066 PlanningState::PlanningState(const PlanningState& s) 00067 : ivX(s.getX()), 00068 ivY(s.getY()), 00069 ivTheta(s.getTheta()), 00070 ivLeg(s.getLeg()), 00071 ivId(s.getId()), 00072 ivHashTag(s.getHashTag()) 00073 {} 00074 00075 00076 PlanningState::~PlanningState() 00077 {} 00078 00079 00080 bool 00081 PlanningState::operator ==(const PlanningState& s2) 00082 const 00083 { 00084 // First test the hash tag. If they differ, the states are definitely 00085 // different. 00086 if (ivHashTag != s2.getHashTag()) 00087 return false; 00088 00089 return (ivX == s2.getX() && ivY == s2.getY() && 00090 ivTheta == s2.getTheta() && ivLeg == s2.getLeg()); 00091 } 00092 00093 00094 bool 00095 PlanningState::operator !=(const PlanningState& s2) 00096 const 00097 { 00098 return ivHashTag != s2.getHashTag(); 00099 } 00100 00101 00102 State 00103 PlanningState::getState(double cell_size, int num_angle_bins) 00104 const 00105 { 00106 return State(cell_2_state(ivX, cell_size), 00107 cell_2_state(ivY, cell_size), 00108 angles::normalize_angle( 00109 angle_cell_2_state(ivTheta, num_angle_bins)), 00110 ivLeg); 00111 } 00112 } // end of namespace