footstep_planner_environment.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_FOOTSTEP_PLANNER_ENVIRONMENT_H__
00031 #define VIGIR_FOOTSTEP_PLANNING_FOOTSTEP_PLANNER_ENVIRONMENT_H__
00032 
00033 #include <math.h>
00034 
00035 #include <vector>
00036 
00037 #include <boost/function.hpp>
00038 
00039 #include <sbpl/headers.h>
00040 
00041 #include <vigir_pluginlib/plugin_manager.h>
00042 
00043 #include <vigir_foot_pose_transformer/foot_pose_transformer.h>
00044 
00045 #include <vigir_footstep_planning_lib/helper.h>
00046 
00047 #include <vigir_footstep_planning_plugins/plugin_aggregators/world_model.h>
00048 #include <vigir_footstep_planning_plugins/plugin_aggregators/step_cost_estimator.h>
00049 #include <vigir_footstep_planning_plugins/plugin_aggregators/heuristic.h>
00050 
00051 #include <vigir_footstep_planner/environment_parameters.h>
00052 #include <vigir_footstep_planner/state_space/state_space.h>
00053 
00054 
00055 
00056 namespace vigir_footstep_planning
00057 {
00065 class FootstepPlannerEnvironment
00066   : public DiscreteSpaceInformation
00067 {
00068 public:
00069   FootstepPlannerEnvironment(const EnvironmentParameters& params, const FootPoseTransformer& foot_pose_transformer, boost::function<void (msgs::PlanningFeedback)>& feedback_cb);
00070 
00071   virtual ~FootstepPlannerEnvironment();
00072 
00073   void setFrameId(const std::string& frame_id);
00074 
00075   const StateSpace::Ptr& getStateSpace() { return state_space; }
00076 
00078   int getNumExpandedStates() { return ivNumExpandedStates; }
00079 
00084   void reset();
00085 
00086   void stateExpanded(const PlanningState& s);
00087   void stateVisited(const PlanningState& s);
00088 
00089   exp_states_2d_iter_t getExpandedStatesStart()
00090   {
00091     return ivExpandedStates.begin();
00092   }
00093 
00094   exp_states_2d_iter_t getExpandedStatesEnd()
00095   {
00096     return ivExpandedStates.end();
00097   }
00098 
00103   int GetFromToHeuristic(int FromStateID, int ToStateID);
00104 
00109   int GetGoalHeuristic(int stateID);
00110 
00115   int GetStartHeuristic(int stateID);
00116 
00122   void GetSuccs(int SourceStateID, std::vector<int> *SuccIDV, std::vector<int> *CostV);
00123 
00129   void GetPreds(int TargetStateID, std::vector<int> *PredIDV, std::vector<int> *CostV);
00130 
00136   virtual void GetRandomSuccsatDistance(int SourceStateID,
00137                                         std::vector<int>* SuccIDV,
00138                                         std::vector<int>* CLowV);
00139 
00145   virtual void GetRandomPredsatDistance(int TargetStateID,
00146                                         std::vector<int>* PredIDV,
00147                                         std::vector<int>* CLowV);
00148 
00150   bool AreEquivalent(int StateID1, int StateID2);
00151 
00152   bool InitializeEnv(const char *sEnvFile);
00153 
00154   bool InitializeMDPCfg(MDPConfig *MDPCfg);
00155 
00156   void PrintEnv_Config(FILE *fOut);
00157 
00158   void PrintState(int stateID, bool bVerbose, FILE *fOut);
00159 
00160   void SetAllActionsandAllOutcomes(CMDPSTATE *state);
00161 
00162   void SetAllPreds(CMDPSTATE *state);
00163 
00164   int SizeofCreatedEnv();
00165 
00171   void updateHeuristicValues();
00172 
00173 protected:
00178   int GetFromToHeuristic(const PlanningState& from, const PlanningState& to, const PlanningState& start, const PlanningState& goal);
00179 
00181   struct less
00182   {
00183     bool operator()(const PlanningState* a, const PlanningState* b) const;
00184   };
00185 
00186   StateSpace::Ptr state_space;
00187 
00188   // local instance of foot pose transformer
00189   const FootPoseTransformer& foot_pose_transformer;
00190 
00191   // feedback callback
00192   boost::function<void (msgs::PlanningFeedback)>& feedback_cb;
00193 
00195   const EnvironmentParameters& params;
00196 
00197   exp_states_2d_t ivExpandedStates;
00198   size_t ivNumExpandedStates;
00199 
00200   std::string frame_id;
00201   std::vector<msgs::Step> visited_steps;
00202   ros::Time last_feedback_flush;
00203 };
00204 }
00205 
00206 #endif


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