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>
#include <random>

#include <boost/shared_ptr.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>
#include <teb_local_planner/graph_search.h>


namespace teb_local_planner
{

inline std::complex<long double> getCplxFromVertexPosePtr(const VertexPose* pose)
{
  return std::complex<long double>(pose->x(), pose->y());
};


inline std::complex<long double> getCplxFromMsgPoseStamped(const geometry_msgs::PoseStamped& pose)
{
  return std::complex<long double>(pose.pose.position.x, pose.pose.position.y);
};

class HomotopyClassPlanner : public PlannerInterface
{
public:

  using EquivalenceClassContainer = std::vector< std::pair<EquivalenceClassPtr, bool> >;

  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);


  void updateRobotModel(RobotFootprintModelPtr robot_model );


  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, int look_ahead_poses) 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);

  TebOptimalPlannerPtr findBestTeb();

  TebOptPlannerContainer::iterator removeTeb(TebOptimalPlannerPtr& teb);



  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, bool free_goal_vel = false);

  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, bool free_goal_vel = false);

  TebOptimalPlannerPtr addAndInitNewTeb(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist* start_velocity, bool free_goal_vel = false);

  TebOptimalPlannerPtr addAndInitNewTeb(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_velocity, bool free_goal_vel = false);

  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();


  virtual void clearPlanner() {clearGraph(); equivalence_classes_.clear(); tebs_.clear(); initial_plan_ = NULL;}


  virtual void setPreferredTurningDir(RotType dir);

  template<typename BidirIter, typename Fun>
  EquivalenceClassPtr calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles = NULL,
                                                boost::optional<TimeDiffSequence::iterator> timediff_start = boost::none, boost::optional<TimeDiffSequence::iterator> timediff_end = boost::none);

  const TebOptPlannerContainer& getTrajectoryContainer() const {return tebs_;}

  bool hasDiverged() const override;

  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;
  }
  void deletePlansDetouringBackwards(const double orient_threshold, const double len_orientation_vector);
  bool computeStartOrientation(const TebOptimalPlannerPtr plan, const double len_orientation_vector, double& orientation);


  const TebConfig* config() const {return cfg_;}

  const ObstContainer* obstacles() const {return obstacles_;}

  bool isInitialized() const {return initialized_;}

  void clearGraph() {if(graph_search_) graph_search_->clearGraph();}

  int bestTebIdx() const;


  bool addEquivalenceClassIfNew(const EquivalenceClassPtr& eq_class, bool lock=false);

  const EquivalenceClassContainer& getEquivalenceClassRef() const  {return equivalence_classes_;}

  bool isInBestTebClass(const EquivalenceClassPtr& eq_class) const;

  int numTebsInClass(const EquivalenceClassPtr& eq_class) const;

  int numTebsInBestTebClass() const;

  void randomlyDropTebs();

protected:


  bool hasEquivalenceClass(const EquivalenceClassPtr& eq_class) const;


  void renewAndAnalyzeOldTebs(bool delete_detours);

  void updateReferenceTrajectoryViaPoints(bool all_trajectories);



  // external objects (store weak pointers)
  const TebConfig* cfg_; 
  ObstContainer* obstacles_; 
  const ViaPointContainer* via_points_; 

  // internal objects (memory management owned)
  TebVisualizationPtr visualization_; 
  TebOptimalPlannerPtr best_teb_; 
  EquivalenceClassPtr best_teb_eq_class_; 
  RobotFootprintModelPtr robot_model_; 

  const std::vector<geometry_msgs::PoseStamped>* initial_plan_; 
  EquivalenceClassPtr initial_plan_eq_class_; 
  TebOptimalPlannerPtr initial_plan_teb_; 

  TebOptPlannerContainer tebs_; 

  EquivalenceClassContainer equivalence_classes_; 
                                                                            //   The second parameter denotes whether to exclude the class from detour deletion or not (true: force keeping).

  boost::shared_ptr<GraphSearchInterface> graph_search_;

  ros::Time last_eq_class_switching_time_; 

  std::default_random_engine random_;
  bool initialized_; 

  TebOptimalPlannerPtr last_best_teb_;  



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_ */