homotopy_class_planner.h

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2016,
 *  TU Dortmund - Institute of Control Theory and Systems Engineering.
 *  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 institute 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 OWNER OR CONTRIBUTORS 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.
 *
 * Author: Christoph Rösmann
 *********************************************************************/

#ifndef HOMOTOPY_CLASS_PLANNER_H_
#define HOMOTOPY_CLASS_PLANNER_H_

#include <math.h>
#include <algorithm>
#include <functional>
#include <vector>
#include <iterator>

#ifdef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
  #include <boost/graph/adjacency_list.hpp>
#else
  // Workaround for a bug in boost graph library (concerning directed graphs), boost version 1.48:
  // boost::add_vertex requires a move constructor/assignment operator in one of the underlying boost objects if C++11 is activated,
  // but they are missing. The compiler fails due to an implicit deletion. We just deactivate C++11 default functions for now.
  #define BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
  #include <boost/graph/adjacency_list.hpp>
  #undef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
#endif

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/random.hpp>
#include <boost/utility.hpp>


#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/ColorRGBA.h>

#include <ros/console.h>
#include <ros/ros.h>

#include <teb_local_planner/planner_interface.h>
#include <teb_local_planner/teb_config.h>
#include <teb_local_planner/obstacles.h>
#include <teb_local_planner/optimal_planner.h>
#include <teb_local_planner/visualization.h>
#include <teb_local_planner/robot_footprint_model.h>
#include <teb_local_planner/equivalence_relations.h>


namespace teb_local_planner
{
  
 
struct HcGraphVertex
{ 
public:
  Eigen::Vector2d pos; // position of vertices in the map
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

typedef boost::adjacency_list < boost::listS, boost::vecS, boost::directedS, HcGraphVertex, boost::no_property > HcGraph;
typedef boost::graph_traits<HcGraph>::vertex_descriptor HcGraphVertexType;
typedef boost::graph_traits<HcGraph>::edge_descriptor HcGraphEdgeType;
typedef boost::graph_traits<HcGraph>::vertex_iterator HcGraphVertexIterator;
typedef boost::graph_traits<HcGraph>::edge_iterator HcGraphEdgeIterator;
typedef boost::graph_traits<HcGraph>::adjacency_iterator HcGraphAdjecencyIterator;

class HomotopyClassPlanner : public PlannerInterface
{
public:

  HomotopyClassPlanner();
  
  HomotopyClassPlanner(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
                       TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
    
  virtual ~HomotopyClassPlanner();
  
  void initialize(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
                  TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
  
  
  
  
  virtual bool plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false);
  
  virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false);
  
  virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_vel = NULL, bool free_goal_vel=false);
  
  virtual bool getVelocityCommand(double& vx, double& vy, double& omega) const;
  
  TebOptimalPlannerPtr bestTeb() const {return tebs_.empty() ? TebOptimalPlannerPtr() : tebs_.size()==1 ? tebs_.front() : best_teb_;}
    
  virtual bool isTrajectoryFeasible(base_local_planner::CostmapModel* costmap_model, const std::vector<geometry_msgs::Point>& footprint_spec,
                                    double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1);
  
  
  
  void setVisualization(TebVisualizationPtr visualization);
  
  virtual void visualize();
  
  
    
 
  void exploreEquivalenceClassesAndInitTebs(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, const geometry_msgs::Twist* start_vel);

  
  void deleteTebDetours(double threshold=0.0);
 
  
  template<typename BidirIter, typename Fun>
  TebOptimalPlannerPtr addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::Twist* start_velocity); 
  
  TebOptimalPlannerPtr addAndInitNewTeb(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_velocity); 
  
  TebOptimalPlannerPtr addAndInitNewTeb(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_velocity);
  
  void updateAllTEBs(const PoseSE2* start, const PoseSE2* goal, const geometry_msgs::Twist* start_velocity);
  
  
  void optimizeAllTEBs(int iter_innerloop, int iter_outerloop);
  
  TebOptimalPlannerPtr getInitialPlanTEB();
  
  TebOptimalPlannerPtr selectBestTeb();
  
  
  void clearPlanner() {graph_.clear(); equivalence_classes_.clear(); tebs_.clear(); initial_plan_ = NULL;}
  
  virtual bool isHorizonReductionAppropriate(const std::vector<geometry_msgs::PoseStamped>& initial_plan) const;
  
  template<typename BidirIter, typename Fun>
  EquivalenceClassPtr calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles = NULL);
  
  const TebOptPlannerContainer& getTrajectoryContainer() const {return tebs_;};
  
  virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);  
  
  inline static bool isHSignatureSimilar(const std::complex<long double>& h1, const std::complex<long double>& h2, double threshold)
  {
      double diff_real = std::abs(h2.real() - h1.real());
      double diff_imag = std::abs(h2.imag() - h1.imag());
      if (diff_real<=threshold && diff_imag<=threshold)
        return true; // Found! Homotopy class already exists, therefore nothing added  
      return false;
  }
  
   
  const TebConfig* config() const {return cfg_;}  
    
  const ObstContainer* obstacles() const {return obstacles_;}
  
  bool isInitialized() const {return initialized_;}
  
  void clearGraph() {graph_.clear();}
  
  int bestTebIdx() const;
  
    
protected:
  
  
  void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::Twist* start_velocity);
  
  void createProbRoadmapGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, int no_samples, double obstacle_heading_threshold, const geometry_msgs::Twist* start_velocity);
  
  bool hasEquivalenceClass(const EquivalenceClassPtr& eq_class) const;
  
  bool addEquivalenceClassIfNew(const EquivalenceClassPtr& eq_class, bool lock=false);
  
  void renewAndAnalyzeOldTebs(bool delete_detours);
  
  void updateReferenceTrajectoryViaPoints(bool all_trajectories);
  
  void DepthFirst(HcGraph& g, std::vector<HcGraphVertexType>& visited, const HcGraphVertexType& goal, double start_orientation, double goal_orientation, const geometry_msgs::Twist* start_velocity);
   
  
    
  // external objects (store weak pointers)
  const TebConfig* cfg_; 
  ObstContainer* obstacles_; 
  const ViaPointContainer* via_points_; 
  
  // internal objects (memory management owned)
  TebVisualizationPtr visualization_; 
  TebOptimalPlannerPtr best_teb_; 
  RobotFootprintModelPtr robot_model_; 
  
  const std::vector<geometry_msgs::PoseStamped>* initial_plan_; 
  EquivalenceClassPtr initial_plan_eq_class_; 
  TebOptimalPlannerPtr initial_plan_teb_; 
  
  TebOptPlannerContainer tebs_; 
  
  HcGraph graph_; 
 
  using EquivalenceClassContainer = std::vector< std::pair<EquivalenceClassPtr, bool> >;
  EquivalenceClassContainer equivalence_classes_; 
                                                                            //   The second parameter denotes whether to exclude the class from detour deletion or not (true: force keeping).
  
  boost::random::mt19937 rnd_generator_; 
      
  bool initialized_; 
  
  

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    
};

typedef boost::shared_ptr<HomotopyClassPlanner> HomotopyClassPlannerPtr;


} // namespace teb_local_planner

// include template implementations / definitions
#include <teb_local_planner/homotopy_class_planner.hpp>

#endif /* HOMOTOPY_CLASS_PLANNER_H_ */