planning_state.cpp
Go to the documentation of this file.
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 4146 2013-05-13 13:57:41Z garimort@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 <vigir_footstep_planning_lib/modeling/planning_state.h>
00028 
00029 
00030 
00031 namespace vigir_footstep_planning
00032 {
00033 PlanningState::PlanningState(double x, double y, double z, double roll, double pitch, double yaw,
00034                              Leg leg, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state, const PlanningState *succ_state)
00035   : ivState(State(x, y, z, roll, pitch, yaw, leg))
00036   , ivX(state_2_cell(x, cell_size))
00037   , ivY(state_2_cell(y, cell_size))
00038   , ivYaw(angle_state_2_cell(yaw, angle_bin_size))
00039   , ivpPredState(pred_state)
00040   , ivpSuccState(succ_state)
00041   , ivId(-1)
00042 {
00043   unsigned int hash_pred = pred_state ? pred_state->getHashTag() : 0u;
00044   unsigned int hash_succ = succ_state ? succ_state->getHashTag() : 0u;
00045   ivHashTag = calc_hash_tag(ivX, ivY, ivYaw, leg, hash_pred, hash_succ, max_hash_size);
00046 }
00047 
00048 
00049 PlanningState::PlanningState(int x, int y, double z, double roll, double pitch, int yaw,
00050                              Leg leg, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state, const PlanningState *succ_state)
00051   : 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))
00052   , ivX(x)
00053   , ivY(y)
00054   , ivYaw(yaw)
00055   , ivpPredState(pred_state)
00056   , ivpSuccState(succ_state)
00057   , ivId(-1)
00058 {
00059   unsigned int hash_pred = pred_state ? pred_state->getHashTag() : 0u;
00060   unsigned int hash_succ = succ_state ? succ_state->getHashTag() : 0u;
00061   ivHashTag = calc_hash_tag(ivX, ivY, ivYaw, leg, hash_pred, hash_succ, max_hash_size);
00062 }
00063 
00064 PlanningState::PlanningState(const geometry_msgs::Pose& pose,
00065                              Leg leg, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state, const PlanningState *succ_state)
00066   : ivState(State(pose, leg))
00067   , ivX(state_2_cell(pose.position.x, cell_size))
00068   , ivY(state_2_cell(pose.position.y, cell_size))
00069   , ivpPredState(pred_state)
00070   , ivpSuccState(succ_state)
00071   , ivId(-1)
00072 {
00073   ivYaw = angle_state_2_cell(ivState.getYaw(), angle_bin_size);
00074 
00075   unsigned int hash_pred = pred_state ? pred_state->getHashTag() : 0u;
00076   unsigned int hash_succ = succ_state ? succ_state->getHashTag() : 0u;
00077   ivHashTag = calc_hash_tag(ivX, ivY, ivYaw, leg, hash_pred, hash_succ, max_hash_size);
00078 }
00079 
00080 PlanningState::PlanningState(const State& s, double cell_size, double angle_bin_size, int max_hash_size, const PlanningState *pred_state, const PlanningState *succ_state)
00081   : ivState(s)
00082   , ivX(state_2_cell(s.getX(), cell_size))
00083   , ivY(state_2_cell(s.getY(), cell_size))
00084   , ivYaw(angle_state_2_cell(s.getYaw(), angle_bin_size))
00085   , ivpPredState(pred_state)
00086   , ivpSuccState(succ_state)
00087   , ivId(-1)
00088 {
00089   unsigned int hash_pred = pred_state ? pred_state->getHashTag() : 0u;
00090   unsigned int hash_succ = succ_state ? succ_state->getHashTag() : 0u;
00091   ivHashTag = calc_hash_tag(ivX, ivY, ivYaw, s.getLeg(), hash_pred, hash_succ, max_hash_size);
00092 }
00093 
00094 PlanningState::PlanningState(const PlanningState& s)
00095   : ivState(s.getState())
00096   , ivX(s.getX())
00097   , ivY(s.getY())
00098   , ivYaw(s.getYaw())
00099   , ivpPredState(s.getPredState())
00100   , ivpSuccState(s.getSuccState())
00101   , ivId(s.getId())
00102   , ivHashTag(s.getHashTag())
00103 {}
00104 
00105 PlanningState::~PlanningState()
00106 {}
00107 
00108 bool PlanningState::operator==(const PlanningState& s2) const
00109 {
00110   // First test the hash tag. If they differ, the states are definitely
00111   // different.
00112   if (ivHashTag != s2.getHashTag())
00113     return false;
00114 
00116   if (ivpPredState != s2.ivpPredState)
00117     return false;
00118 
00119   if (ivpSuccState != s2.ivpSuccState)
00120     return false;
00121 
00122   // other variables may be ignored, because they are
00123   // selected by x and y
00124   return (ivX == s2.getX() && ivY == s2.getY() && ivYaw == s2.getYaw() &&
00125           ivState.getLeg() == s2.ivState.getLeg());
00126 }
00127 
00128 bool PlanningState::operator!=(const PlanningState& s2) const
00129 {
00130   return !operator==(s2);
00131 }
00132 
00133 const State& PlanningState::getState() const
00134 {
00135   return ivState;
00136 }
00137 
00138 State& PlanningState::getState()
00139 {
00140   return ivState;
00141 }
00142 } // end of namespace


vigir_footstep_planning_lib
Author(s): Alexander Stumpf
autogenerated on Sat Jul 15 2017 02:47:56