state_space.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2018, Alexander Stumpf, TU Darmstadt
00003 // Based on http://wiki.ros.org/footstep_planner by Johannes Garimort and Armin Hornung
00004 // All rights reserved.
00005 
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00014 //       group, TU Darmstadt nor the names of its contributors may be used to
00015 //       endorse or promote products derived from this software without
00016 //       specific prior written permission.
00017 
00018 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00022 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00023 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00024 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00025 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00026 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00027 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 //=================================================================================================
00029 
00030 #ifndef VIGIR_FOOTSTEP_PLANNING_STATE_SPACE_H__
00031 #define VIGIR_FOOTSTEP_PLANNING_STATE_SPACE_H__
00032 
00033 #include <ros/ros.h>
00034 
00035 #include <boost/thread/mutex.hpp>
00036 
00037 #include <tr1/unordered_set>
00038 #include <tr1/hashtable.h>
00039 
00040 #include <sbpl/headers.h>
00041 
00042 #include <vigir_footstep_planning_lib/modeling/footstep.h>
00043 #include <vigir_footstep_planning_lib/modeling/planning_state.h>
00044 
00045 #include <vigir_footstep_planner/environment_parameters.h>
00046 
00047 
00048 
00049 namespace vigir_footstep_planning
00050 {
00051 // specialization of hash<int,int>, similar to standard boost::hash on pairs?
00052 struct IntPairHash{
00053 public:
00054   size_t operator()(std::pair<int, int> x) const throw() {
00055     size_t seed = std::tr1::hash<int>()(x.first);
00056     return std::tr1::hash<int>()(x.second) + 0x9e3779b9 + (seed<<6) + (seed>>2);
00057   }
00058 };
00059 
00060 struct StepCostPair {
00061   StepCostPair(const PlanningState* state, const int cost)
00062     : state(state)
00063     , cost(cost)
00064   {}
00065 
00066   bool operator < (const StepCostPair& other)
00067   {
00068     return cost < other.cost;
00069   }
00070 
00071   const PlanningState* state;
00072   const int cost;
00073 };
00074 
00075 typedef std::vector<int> exp_states_t;
00076 typedef exp_states_t::const_iterator exp_states_iter_t;
00077 typedef std::tr1::unordered_set<std::pair<int,int>, IntPairHash > exp_states_2d_t;
00078 typedef exp_states_2d_t::const_iterator exp_states_2d_iter_t;
00079 
00080 
00081 
00082 class StateSpace
00083 {
00084 public:
00085   StateSpace(const EnvironmentParameters& params, std::vector<int*>& state_ID2_index_mapping);
00086 
00087   virtual ~StateSpace();
00088 
00089   void reset();
00090 
00091   // typedefs
00092   typedef boost::shared_ptr<StateSpace> Ptr;
00093   typedef boost::shared_ptr<StateSpace> ConstPtr;
00094 
00095   void setFrameId(const std::string& frame_id);
00096 
00097   std::pair<int, int> updateGoal(const State& foot_left, const State& foot_right);
00098   std::pair<int, int> updateStart(const State& foot_left, const State& right_right);
00099   void setPlannerStartAndGoal(unsigned int start_foot_selection);
00100 
00106   bool getState(unsigned int id, State &s) const;
00107 
00108   bool getStartState(State &left, State &right) const;
00109   bool getStartState(State &robot) const;
00110   bool getGoalState(State &left, State &right) const;
00111   bool getGoalState(State &robot) const;
00112 
00113   exp_states_iter_t getRandomStatesStart()
00114   {
00115     return ivRandomStates.begin();
00116   }
00117 
00118   exp_states_iter_t getRandomStatesEnd()
00119   {
00120     return ivRandomStates.end();
00121   }
00122 
00123 
00125   PlanningState* createNewHashEntry(const State& s);
00126 
00134   PlanningState *createNewHashEntry(const PlanningState& s);
00135 
00137   PlanningState* getHashEntry(const State& s);
00138 
00143   PlanningState* getHashEntry(const PlanningState& s);
00144 
00145   PlanningState *createHashEntryIfNotExists(const PlanningState& s);
00146 
00151   bool closeToGoal(const PlanningState& from) const;
00152 
00157   bool closeToStart(const PlanningState& from) const;
00158 
00159 
00161   bool getStepCost(const State& stand_foot, const State& swing_foot_before, const State& swing_foot_after, double& cost, double& risk) const;
00162   bool getStepCost(const State& stand_foot, const State& swing_foot_before, const State& swing_foot_after, int& cost, int& risk) const;
00163   bool getStepCost(const State& stand_foot, const State& swing_foot_before, const State& swing_foot_after, int& cost) const;
00164 
00165   const EnvironmentParameters& params;
00166   std::string frame_id;
00167 
00169   int ivIdPlanningStart;
00170   int ivIdPlanningGoal;
00171 
00172   PlanningState* start_foot_left;
00173   PlanningState* start_foot_right;
00174 
00175   int ivIdStartFootLeft;
00176   int ivIdStartFootRight;
00177 
00178   PlanningState* goal_foot_left;
00179   PlanningState* goal_foot_right;
00180 
00181   int ivIdGoalFootLeft;
00182   int ivIdGoalFootRight;
00183 
00184   std::vector<int> ivStateArea;
00185 
00186   std::vector<int*>& state_ID2_index_mapping;
00187 
00192   std::vector<const PlanningState*> ivStateId2State;
00193 
00199   std::vector<PlanningState*>* ivpStateHash2State;
00200 
00202   bool ivHeuristicExpired;
00203 
00205   const int ivRandomNodeDist;
00206 
00207   exp_states_t ivRandomStates;  
00208 
00209   mutable boost::shared_mutex hash_table_shared_mutex;
00210 };
00211 }
00212 
00213 #endif


vigir_footstep_planner
Author(s): Alexander Stumpf
autogenerated on Thu Jun 6 2019 18:38:04