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,
                       TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
 
  virtual ~HomotopyClassPlanner();
 
  void initialize(const TebConfig& cfg, ObstContainer* obstacles = NULL,
                  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, 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, double feasibility_check_lookahead_distance=-1.0);
 
  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_; 
 
  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_ */