PlanningState.cpp
Go to the documentation of this file.
00001 // SVN $HeadURL$
00002 // SVN $Id$
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Tue Oct 15 2013 10:06:52