footstep_planner_environment.h

Go to the documentation of this file.

//=================================================================================================
// Copyright (c) 2018, Alexander Stumpf, TU Darmstadt
// Based on http://wiki.ros.org/footstep_planner by Johannes Garimort and Armin Hornung
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Simulation, Systems Optimization and Robotics
//       group, TU Darmstadt nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#ifndef VIGIR_FOOTSTEP_PLANNING_FOOTSTEP_PLANNER_ENVIRONMENT_H__
#define VIGIR_FOOTSTEP_PLANNING_FOOTSTEP_PLANNER_ENVIRONMENT_H__

#include <math.h>

#include <vector>

#include <boost/function.hpp>

#include <sbpl/headers.h>

#include <vigir_pluginlib/plugin_manager.h>

#include <vigir_foot_pose_transformer/foot_pose_transformer.h>

#include <vigir_footstep_planning_lib/helper.h>

#include <vigir_footstep_planning_plugins/plugin_aggregators/world_model.h>
#include <vigir_footstep_planning_plugins/plugin_aggregators/step_cost_estimator.h>
#include <vigir_footstep_planning_plugins/plugin_aggregators/heuristic.h>

#include <vigir_footstep_planner/environment_parameters.h>
#include <vigir_footstep_planner/state_space/state_space.h>



namespace vigir_footstep_planning
{
class FootstepPlannerEnvironment
  : public DiscreteSpaceInformation
{
public:
  FootstepPlannerEnvironment(const EnvironmentParameters& params, const FootPoseTransformer& foot_pose_transformer, boost::function<void (msgs::PlanningFeedback)>& feedback_cb);

  virtual ~FootstepPlannerEnvironment();

  void setFrameId(const std::string& frame_id);

  const StateSpace::Ptr& getStateSpace() { return state_space; }

  int getNumExpandedStates() { return ivNumExpandedStates; }

  void reset();

  void stateExpanded(const PlanningState& s);
  void stateVisited(const PlanningState& s);

  exp_states_2d_iter_t getExpandedStatesStart()
  {
    return ivExpandedStates.begin();
  }

  exp_states_2d_iter_t getExpandedStatesEnd()
  {
    return ivExpandedStates.end();
  }

  int GetFromToHeuristic(int FromStateID, int ToStateID);

  int GetGoalHeuristic(int stateID);

  int GetStartHeuristic(int stateID);

  void GetSuccs(int SourceStateID, std::vector<int> *SuccIDV, std::vector<int> *CostV);

  void GetPreds(int TargetStateID, std::vector<int> *PredIDV, std::vector<int> *CostV);

  virtual void GetRandomSuccsatDistance(int SourceStateID,
                                        std::vector<int>* SuccIDV,
                                        std::vector<int>* CLowV);

  virtual void GetRandomPredsatDistance(int TargetStateID,
                                        std::vector<int>* PredIDV,
                                        std::vector<int>* CLowV);

  bool AreEquivalent(int StateID1, int StateID2);

  bool InitializeEnv(const char *sEnvFile);

  bool InitializeMDPCfg(MDPConfig *MDPCfg);

  void PrintEnv_Config(FILE *fOut);

  void PrintState(int stateID, bool bVerbose, FILE *fOut);

  void SetAllActionsandAllOutcomes(CMDPSTATE *state);

  void SetAllPreds(CMDPSTATE *state);

  int SizeofCreatedEnv();

  void updateHeuristicValues();

protected:
  int GetFromToHeuristic(const PlanningState& from, const PlanningState& to, const PlanningState& start, const PlanningState& goal);

  struct less
  {
    bool operator()(const PlanningState* a, const PlanningState* b) const;
  };

  StateSpace::Ptr state_space;

  // local instance of foot pose transformer
  const FootPoseTransformer& foot_pose_transformer;

  // feedback callback
  boost::function<void (msgs::PlanningFeedback)>& feedback_cb;

  const EnvironmentParameters& params;

  exp_states_2d_t ivExpandedStates;
  size_t ivNumExpandedStates;

  std::string frame_id;
  std::vector<msgs::Step> visited_steps;
  ros::Time last_feedback_flush;
};
}

#endif