text: /*********************************************************************
   *
   * 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
   *********************************************************************/
  
  #include <teb_local_planner/homotopy_class_planner.h>
  
  #include <limits>
  
  namespace teb_local_planner
  {
  
  HomotopyClassPlanner::HomotopyClassPlanner() : cfg_(NULL), obstacles_(NULL), via_points_(NULL), robot_model_(new PointRobotFootprint()), initial_plan_(NULL), initialized_(false)
  {
  }
  
  HomotopyClassPlanner::HomotopyClassPlanner(const TebConfig &cfg, ObstContainer *obstacles, RobotFootprintModelPtr robot_model,
                                             TebVisualizationPtr visual, const ViaPointContainer *via_points) : initial_plan_(NULL)
  {
      initialize(cfg, obstacles, robot_model, visual, via_points);
  }
  
  HomotopyClassPlanner::~HomotopyClassPlanner()
  {
  }
  
  void HomotopyClassPlanner::initialize(const TebConfig &cfg, ObstContainer *obstacles, RobotFootprintModelPtr robot_model,
                                        TebVisualizationPtr visual, const ViaPointContainer *via_points)
  {
      cfg_ = &cfg;
      obstacles_ = obstacles;
      via_points_ = via_points;
      robot_model_ = robot_model;
  
      if (cfg_->hcp.simple_exploration)
          graph_search_ = boost::shared_ptr<GraphSearchInterface>(new lrKeyPointGraph(*cfg_, this));
      else
          graph_search_ = boost::shared_ptr<GraphSearchInterface>(new ProbRoadmapGraph(*cfg_, this));
  
      initialized_ = true;
  
      setVisualization(visual);
  }
  
  void HomotopyClassPlanner::setVisualization(TebVisualizationPtr visualization)
  {
      visualization_ = visualization;
  }
  
  bool HomotopyClassPlanner::plan(const std::vector<geometry_msgs::PoseStamped> &initial_plan, const geometry_msgs::Twist *start_vel, bool free_goal_vel)
  {
      ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
  
      // store initial plan for further initializations (must be valid for the lifetime of this object or clearPlanner() is called!)
      initial_plan_ = &initial_plan;
  
      PoseSE2 start(initial_plan.front().pose);
      PoseSE2 goal(initial_plan.back().pose);
  
      return plan(start, goal, start_vel, free_goal_vel);
  }
  
  bool HomotopyClassPlanner::plan(const tf::Pose &start, const tf::Pose &goal, const geometry_msgs::Twist *start_vel, bool free_goal_vel)
  {
      ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
      PoseSE2 start_pose(start);
      PoseSE2 goal_pose(goal);
      return plan(start_pose, goal_pose, start_vel, free_goal_vel);
  }
  
  bool HomotopyClassPlanner::plan(const PoseSE2 &start, const PoseSE2 &goal, const geometry_msgs::Twist *start_vel, bool free_goal_vel)
  {
      ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
  
      // Update old TEBs with new start, goal and velocity
      updateAllTEBs(&start, &goal, start_vel);
  
      // Init new TEBs based on newly explored homotopy classes
      exploreEquivalenceClassesAndInitTebs(start, goal, cfg_->obstacles.min_obstacle_dist, start_vel);
      // update via-points if activated
      updateReferenceTrajectoryViaPoints(cfg_->hcp.viapoints_all_candidates);
      // Optimize all trajectories in alternative homotopy classes
      optimizeAllTEBs(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
      // Select which candidate (based on alternative homotopy classes) should be used
      selectBestTeb();
  
      initial_plan_ = nullptr; // clear pointer to any previous initial plan (any previous plan is useless regarding the h-signature);
      return true;
  }
  
  bool HomotopyClassPlanner::getVelocityCommand(double &vx, double &vy, double &omega, int look_ahead_poses) const
  {
      TebOptimalPlannerConstPtr best_teb = bestTeb();
      if (!best_teb)
      {
          vx = 0;
          vy = 0;
          omega = 0;
          return false;
      }
  
      return best_teb->getVelocityCommand(vx, vy, omega, look_ahead_poses);
  }
  
  void HomotopyClassPlanner::visualize()
  {
      if (visualization_)
      {
          // Visualize graph
          if (cfg_->hcp.visualize_hc_graph && graph_search_)
              visualization_->publishGraph(graph_search_->graph_);
  
          // Visualize active tebs as marker
          visualization_->publishTebContainer(tebs_);
  
          // Visualize best teb and feedback message if desired
          TebOptimalPlannerConstPtr best_teb = bestTeb();
          if (best_teb)
          {
              visualization_->publishLocalPlanAndPoses(best_teb->teb());
  
              if (best_teb->teb().sizePoses() > 0) //TODO maybe store current pose (start) within plan method as class field.
                  visualization_->publishRobotFootprintModel(best_teb->teb().Pose(0), *robot_model_);
  
              // feedback message
              if (cfg_->trajectory.publish_feedback)
              {
                  int best_idx = bestTebIdx();
                  if (best_idx >= 0)
                      visualization_->publishFeedbackMessage(tebs_, (unsigned int)best_idx, *obstacles_);
              }
          }
      }
      else
          ROS_DEBUG("Ignoring HomotopyClassPlanner::visualize() call, since no visualization class was instantiated before.");
  }
  
  bool HomotopyClassPlanner::hasEquivalenceClass(const EquivalenceClassPtr &eq_class) const
  {
      // iterate existing h-signatures and check if there is an existing H-Signature similar the candidate
      for (const std::pair<EquivalenceClassPtr, bool> &eqrel : equivalence_classes_)
      {
          if (eq_class->isEqual(*eqrel.first))
              return true; // Found! Homotopy class already exists, therefore nothing added
      }
      return false;
  }
  
  bool HomotopyClassPlanner::addEquivalenceClassIfNew(const EquivalenceClassPtr &eq_class, bool lock)
  {
      if (!eq_class)
          return false;
  
      if (!eq_class->isValid())
      {
          ROS_WARN("HomotopyClassPlanner: Ignoring invalid H-signature");
          return false;
      }
  
      if (hasEquivalenceClass(eq_class))
          return false;
  
      // Homotopy class not found -> Add to class-list, return that the h-signature is new
      equivalence_classes_.push_back(std::make_pair(eq_class, lock));
      return true;
  }
  
  void HomotopyClassPlanner::renewAndAnalyzeOldTebs(bool delete_detours)
  {
      // clear old h-signatures (since they could be changed due to new obstacle positions.
      equivalence_classes_.clear();
  
      // Adding the equivalence class of the latest best_teb_ first
      TebOptPlannerContainer::iterator it_best_teb = best_teb_ ? std::find(tebs_.begin(), tebs_.end(), best_teb_) : tebs_.end();
      bool has_best_teb = it_best_teb != tebs_.end();
      if (has_best_teb)
      {
          std::iter_swap(tebs_.begin(), it_best_teb); // Putting the last best teb at the beginning of the container
          addEquivalenceClassIfNew(calculateEquivalenceClass(best_teb_->teb().poses().begin(),
                                                             best_teb_->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
                                                             best_teb_->teb().timediffs().begin(), best_teb_->teb().timediffs().end()));
      }
      // Collect h-signatures for all existing TEBs and store them together with the corresponding iterator / pointer:
      //   typedef std::list< std::pair<TebOptPlannerContainer::iterator, std::complex<long double> > > TebCandidateType;
      //   TebCandidateType teb_candidates;
  
      // get new homotopy classes and delete multiple TEBs per homotopy class. Skips the best teb if available (added before).
      TebOptPlannerContainer::iterator it_teb = has_best_teb ? std::next(tebs_.begin(), 1) : tebs_.begin();
      while (it_teb != tebs_.end())
      {
          // calculate equivalence class for the current candidate
          EquivalenceClassPtr equivalence_class = calculateEquivalenceClass(it_teb->get()->teb().poses().begin(), it_teb->get()->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
                                                                            it_teb->get()->teb().timediffs().begin(), it_teb->get()->teb().timediffs().end());
  
          //     teb_candidates.push_back(std::make_pair(it_teb,H));
  
          // WORKAROUND until the commented code below works
          // Here we do not compare cost values. Just first come first serve...
          bool new_flag = addEquivalenceClassIfNew(equivalence_class);
          if (!new_flag)
          {
              it_teb = tebs_.erase(it_teb);
              continue;
          }
  
          ++it_teb;
      }
      if (delete_detours)
          deletePlansDetouringBackwards(cfg_->hcp.detours_orientation_tolerance, cfg_->hcp.length_start_orientation_vector);
  
      // Find multiple candidates and delete the one with higher cost
      // TODO: this code needs to be adpated. Erasing tebs from the teb container_ could make iteratores stored in the candidate list invalid!
      //   TebCandidateType::reverse_iterator cand_i = teb_candidates.rbegin();
      //   int test_idx = 0;
      //   while (cand_i != teb_candidates.rend())
      //   {
      //
      //     TebCandidateType::reverse_iterator cand_j = std::find_if(boost::next(cand_i),teb_candidates.rend(), boost::bind(compareH,_1,cand_i->second));
      //     if (cand_j != teb_candidates.rend() && cand_j != cand_i)
      //     {
      //         TebOptimalPlannerPtr pt1 = *(cand_j->first);
      //         TebOptimalPlannerPtr pt2 = *(cand_i->first);
      //         assert(pt1);
      //         assert(pt2);
      //       if ( cand_j->first->get()->getCurrentCost().sum() > cand_i->first->get()->getCurrentCost().sum() )
      //       {
      // 	// found one that has higher cost, therefore erase cand_j
      // 	tebs_.erase(cand_j->first);
      // 	teb_candidates.erase(cand_j);
      //       }
      //       else   // otherwise erase cand_i
      //       {
      // 	tebs_.erase(cand_i->first);
      // 	cand_i = teb_candidates.erase(cand_i);
      //       }
      //     }
      //     else
      //     {
      //         ROS_WARN_STREAM("increase cand_i");
      //         ++cand_i;
      //     }
      //   }
  
      // now add the h-signatures to the internal lookup-table (but only if there is no existing duplicate)
      //   for (TebCandidateType::iterator cand=teb_candidates.begin(); cand!=teb_candidates.end(); ++cand)
      //   {
      //     bool new_flag = addNewHSignatureIfNew(cand->second, cfg_->hcp.h_signature_threshold);
      //     if (!new_flag)
      //     {
      // //       ROS_ERROR_STREAM("getAndFilterHomotopyClassesTEB() - This schould not be happen.");
      //       tebs_.erase(cand->first);
      //     }
      //   }
  }
  
  void HomotopyClassPlanner::updateReferenceTrajectoryViaPoints(bool all_trajectories)
  {
      if ((!all_trajectories && !initial_plan_) || !via_points_ || via_points_->empty() || cfg_->optim.weight_viapoint <= 0)
          return;
  
      if (equivalence_classes_.size() < tebs_.size())
      {
          ROS_ERROR("HomotopyClassPlanner::updateReferenceTrajectoryWithViaPoints(): Number of h-signatures does not match number of trajectories.");
          return;
      }
  
      if (all_trajectories)
      {
          // enable via-points for all tebs
          for (std::size_t i = 0; i < equivalence_classes_.size(); ++i)
          {
              tebs_[i]->setViaPoints(via_points_);
          }
      }
      else
      {
          // enable via-points for teb in the same hommotopy class as the initial_plan and deactivate it for all other ones
          for (std::size_t i = 0; i < equivalence_classes_.size(); ++i)
          {
              if (initial_plan_eq_class_->isEqual(*equivalence_classes_[i].first))
                  tebs_[i]->setViaPoints(via_points_);
              else
                  tebs_[i]->setViaPoints(NULL);
          }
      }
  }
  
  void HomotopyClassPlanner::exploreEquivalenceClassesAndInitTebs(const PoseSE2 &start, const PoseSE2 &goal, double dist_to_obst, const geometry_msgs::Twist *start_vel)
  {
      // first process old trajectories
      renewAndAnalyzeOldTebs(cfg_->hcp.delete_detours_backwards);
  
      // inject initial plan if available and not yet captured
      if (initial_plan_)
      {
          initial_plan_teb_ = addAndInitNewTeb(*initial_plan_, start_vel);
      }
      else
      {
          initial_plan_teb_.reset();
          initial_plan_teb_ = getInitialPlanTEB(); // this method searches for initial_plan_eq_class_ in the teb container (-> if !initial_plan_teb_)
      }
  
      // now explore new homotopy classes and initialize tebs if new ones are found. The appropriate createGraph method is chosen via polymorphism.
      graph_search_->createGraph(start, goal, dist_to_obst, cfg_->hcp.obstacle_heading_threshold, start_vel);
  }
  
  TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const PoseSE2 &start, const PoseSE2 &goal, const geometry_msgs::Twist *start_velocity)
  {
      if (tebs_.size() >= cfg_->hcp.max_number_classes)
          return TebOptimalPlannerPtr();
      TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr(new TebOptimalPlanner(*cfg_, obstacles_, robot_model_, visualization_));
  
      candidate->teb().initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
  
      if (start_velocity)
          candidate->setVelocityStart(*start_velocity);
  
      EquivalenceClassPtr H = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
                                                        candidate->teb().timediffs().begin(), candidate->teb().timediffs().end());
  
      if (addEquivalenceClassIfNew(H))
      {
          tebs_.push_back(candidate);
          return tebs_.back();
      }
  
      // If the candidate constitutes no new equivalence class, return a null pointer
      return TebOptimalPlannerPtr();
  }
  
  TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const std::vector<geometry_msgs::PoseStamped> &initial_plan, const geometry_msgs::Twist *start_velocity)
  {
      if (tebs_.size() >= cfg_->hcp.max_number_classes)
          return TebOptimalPlannerPtr();
      TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr(new TebOptimalPlanner(*cfg_, obstacles_, robot_model_, visualization_));
  
      candidate->teb().initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x,
                                            cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
  
      if (start_velocity)
          candidate->setVelocityStart(*start_velocity);
  
      // store the h signature of the initial plan to enable searching a matching teb later.
      initial_plan_eq_class_ = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
                                                         candidate->teb().timediffs().begin(), candidate->teb().timediffs().end());
  
      if (addEquivalenceClassIfNew(initial_plan_eq_class_, true)) // also prevent candidate from deletion
      {
          tebs_.push_back(candidate);
          return tebs_.back();
      }
  
      // If the candidate constitutes no new equivalence class, return a null pointer
      return TebOptimalPlannerPtr();
  }
  
  void HomotopyClassPlanner::updateAllTEBs(const PoseSE2 *start, const PoseSE2 *goal, const geometry_msgs::Twist *start_velocity)
  {
      // If new goal is too far away, clear all existing trajectories to let them reinitialize later.
      // Since all Tebs are sharing the same fixed goal pose, just take the first candidate:
      if (!tebs_.empty() && ((goal->position() - tebs_.front()->teb().BackPose().position()).norm() >= cfg_->trajectory.force_reinit_new_goal_dist || fabs(g2o::normalize_theta(goal->theta() - tebs_.front()->teb().BackPose().theta())) >= cfg_->trajectory.force_reinit_new_goal_angular))
      {
          ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
          tebs_.clear();
          equivalence_classes_.clear();
      }
  
      // hot-start from previous solutions
      for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
      {
          it_teb->get()->teb().updateAndPruneTEB(*start, *goal);
          if (start_velocity)
              it_teb->get()->setVelocityStart(*start_velocity);
      }
  }
  
  void HomotopyClassPlanner::optimizeAllTEBs(int iter_innerloop, int iter_outerloop)
  {
      // optimize TEBs in parallel since they are independend of each other
      if (cfg_->hcp.enable_multithreading)
      {
          // Must prevent .join_all() from throwing exception if interruption was
          // requested, as this can lead to multiple threads operating on the same
          // TEB, which leads to SIGSEGV
          boost::this_thread::disable_interruption di;
  
          boost::thread_group teb_threads;
          for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
          {
              teb_threads.create_thread(boost::bind(&TebOptimalPlanner::optimizeTEB, it_teb->get(), iter_innerloop, iter_outerloop,
                                                    true, cfg_->hcp.selection_obst_cost_scale, cfg_->hcp.selection_viapoint_cost_scale,
                                                    cfg_->hcp.selection_alternative_time_cost));
          }
          teb_threads.join_all();
      }
      else
      {
          for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
          {
              it_teb->get()->optimizeTEB(iter_innerloop, iter_outerloop, true, cfg_->hcp.selection_obst_cost_scale,
                                         cfg_->hcp.selection_viapoint_cost_scale, cfg_->hcp.selection_alternative_time_cost); // compute cost as well inside optimizeTEB (last argument = true)
          }
      }
  }
  
  TebOptimalPlannerPtr HomotopyClassPlanner::getInitialPlanTEB()
  {
      // first check stored teb object
      if (initial_plan_teb_)
      {
          // check if the teb is still part of the teb container
          if (std::find(tebs_.begin(), tebs_.end(), initial_plan_teb_) != tebs_.end())
              return initial_plan_teb_;
          else
          {
              initial_plan_teb_.reset(); // reset pointer for next call
              ROS_DEBUG("initial teb not found, trying to find a match according to the cached equivalence class");
          }
      }
  
      // reset the locked state for equivalence classes // TODO: this might be adapted if not only the plan containing the initial plan is locked!
      for (int i = 0; i < equivalence_classes_.size(); ++i)
      {
          equivalence_classes_[i].second = false;
      }
  
      // otherwise check if the stored reference equivalence class exist in the list of known classes
      if (initial_plan_eq_class_ && initial_plan_eq_class_->isValid())
      {
          if (equivalence_classes_.size() == tebs_.size())
          {
              for (int i = 0; i < equivalence_classes_.size(); ++i)
              {
                  if (equivalence_classes_[i].first->isEqual(*initial_plan_eq_class_))
                  {
                      equivalence_classes_[i].second = true;
                      return tebs_[i];
                  }
              }
          }
          else
              ROS_ERROR("HomotopyClassPlanner::getInitialPlanTEB(): number of equivalence classes (%lu) and number of trajectories (%lu) does not match.", equivalence_classes_.size(), tebs_.size());
      }
      else
          ROS_DEBUG("HomotopyClassPlanner::getInitialPlanTEB(): initial TEB not found in the set of available trajectories.");
  
      return TebOptimalPlannerPtr();
  }
  
  TebOptimalPlannerPtr HomotopyClassPlanner::selectBestTeb()
  {
      double min_cost = std::numeric_limits<double>::max(); // maximum cost
      double min_cost_last_best = std::numeric_limits<double>::max();
      double min_cost_initial_plan_teb = std::numeric_limits<double>::max();
      TebOptimalPlannerPtr initial_plan_teb = getInitialPlanTEB();
  
      // check if last best_teb is still a valid candidate
      if (best_teb_ && std::find(tebs_.begin(), tebs_.end(), best_teb_) != tebs_.end())
      {
          // get cost of this candidate
          min_cost_last_best = best_teb_->getCurrentCost() * cfg_->hcp.selection_cost_hysteresis; // small hysteresis
          last_best_teb_ = best_teb_;
      }
      else
      {
          last_best_teb_.reset();
      }
  
      if (initial_plan_teb) // the validity was already checked in getInitialPlanTEB()
      {
          // get cost of this candidate
          min_cost_initial_plan_teb = initial_plan_teb->getCurrentCost() * cfg_->hcp.selection_prefer_initial_plan; // small hysteresis
      }
  
      best_teb_.reset(); // reset pointer
  
      for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
      {
          // check if the related TEB leaves the local costmap region
          //      if (tebs_.size()>1 && !(*it_teb)->teb().isTrajectoryInsideRegion(20, -1, 1))
          //      {
          //          ROS_INFO("HomotopyClassPlanner::selectBestTeb(): skipping trajectories that are not inside the local costmap");
          //          continue;
          //      }
  
          double teb_cost;
  
          if (*it_teb == last_best_teb_)
              teb_cost = min_cost_last_best; // skip already known cost value of the last best_teb
          else if (*it_teb == initial_plan_teb)
              teb_cost = min_cost_initial_plan_teb;
          else
              teb_cost = it_teb->get()->getCurrentCost();
  
          if (teb_cost < min_cost)
          {
              // check if this candidate is currently not selected
              best_teb_ = *it_teb;
              min_cost = teb_cost;
          }
      }
  
      // in case we haven't found any teb due to some previous checks, investigate list again
      //   if (!best_teb_ && !tebs_.empty())
      //   {
      //       ROS_DEBUG("all " << tebs_.size() << " tebs rejected previously");
      //       if (tebs_.size()==1)
      //         best_teb_ = tebs_.front();
      //       else // if multiple TEBs are available:
      //       {
      //           // try to use the one that relates to the initial plan
      //           TebOptimalPlannerPtr initial_plan_teb = getInitialPlanTEB();
      //           if (initial_plan_teb)
      //               best_teb_ = initial_plan_teb;
      //           else
      //           {
      //              // now compute the cost for the rest (we haven't computed it before)
      //              for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
      //              {
      //                 double teb_cost = it_teb->get()->getCurrentCost();
      //                 if (teb_cost < min_cost)
      //                 {
      //                     // check if this candidate is currently not selected
      //                     best_teb_ = *it_teb;
      //                     min_cost = teb_cost;
      //                 }
      //              }
      //           }
      //       }
      //   }
  
      // check if we are allowed to change
      if (last_best_teb_ && best_teb_ != last_best_teb_)
      {
          ros::Time now = ros::Time::now();
          if ((now - last_eq_class_switching_time_).toSec() > cfg_->hcp.switching_blocking_period)
          {
              last_eq_class_switching_time_ = now;
          }
          else
          {
              ROS_DEBUG("HomotopyClassPlanner::selectBestTeb(): Switching equivalence classes blocked (check parameter switching_blocking_period.");
              // block switching, so revert best_teb_
              best_teb_ = last_best_teb_;
          }
      }
  
      return best_teb_;
  }
  
  int HomotopyClassPlanner::bestTebIdx() const
  {
      if (tebs_.size() == 1)
          return 0;
  
      if (!best_teb_)
          return -1;
  
      int idx = 0;
      for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb, ++idx)
      {
          if (*it_teb == best_teb_)
              return idx;
      }
      return -1;
  }
  
  bool HomotopyClassPlanner::isTrajectoryFeasible(base_local_planner::CostmapModel *costmap_model, const std::vector<geometry_msgs::Point> &footprint_spec,
                                                  double inscribed_radius, double circumscribed_radius, int look_ahead_idx)
  {
      bool feasible = false;
      while (ros::ok() && !feasible && tebs_.size() > 0)
      {
          TebOptimalPlannerPtr best = findBestTeb();
          if (!best)
          {
              ROS_ERROR("Couldn't retrieve the best plan");
              return false;
          }
          feasible = best->isTrajectoryFeasible(costmap_model, footprint_spec, inscribed_radius, circumscribed_radius, look_ahead_idx);
          if (!feasible)
          {
              removeTeb(best);
              if (last_best_teb_ && (last_best_teb_ == best)) // Same plan as before.
                  return feasible;                            // Not failing could result in oscillations between trajectories.
          }
      }
      return feasible;
  }
  
  TebOptimalPlannerPtr HomotopyClassPlanner::findBestTeb()
  {
      if (tebs_.empty())
          return TebOptimalPlannerPtr();
      if (!best_teb_ || std::find(tebs_.begin(), tebs_.end(), best_teb_) == tebs_.end())
          best_teb_ = selectBestTeb();
      return best_teb_;
  }
  
  TebOptPlannerContainer::iterator HomotopyClassPlanner::removeTeb(TebOptimalPlannerPtr &teb)
  {
      TebOptPlannerContainer::iterator return_iterator = tebs_.end();
      if (equivalence_classes_.size() != tebs_.size())
      {
          ROS_ERROR("removeTeb: size of eq classes != size of tebs");
          return return_iterator;
      }
      auto it_eq_classes = equivalence_classes_.begin();
      for (auto it = tebs_.begin(); it != tebs_.end(); ++it)
      {
          if (*it == teb)
          {
              return_iterator = tebs_.erase(it);
              equivalence_classes_.erase(it_eq_classes);
              break;
          }
          ++it_eq_classes;
      }
      return return_iterator;
  }
  
  void HomotopyClassPlanner::setPreferredTurningDir(RotType dir)
  {
      // set preferred turning dir for all TEBs
      for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
      {
          (*it_teb)->setPreferredTurningDir(dir);
      }
  }
  
  void HomotopyClassPlanner::computeCurrentCost(std::vector<double> &cost, double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
  {
      for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
      {
          it_teb->get()->computeCurrentCost(cost, obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
      }
  }
  
  void HomotopyClassPlanner::deletePlansDetouringBackwards(const double orient_threshold,
                                                           const double len_orientation_vector)
  {
      if (tebs_.size() < 2 || !best_teb_ || std::find(tebs_.begin(), tebs_.end(), best_teb_) == tebs_.end() ||
          best_teb_->teb().sizePoses() < 2)
      {
          return; // A moving direction wasn't chosen yet
      }
      double current_movement_orientation;
      double best_plan_duration = std::max(best_teb_->teb().getSumOfAllTimeDiffs(), 1.0);
      if (!computeStartOrientation(best_teb_, len_orientation_vector, current_movement_orientation))
          return; // The plan is shorter than len_orientation_vector
      for (auto it_teb = tebs_.begin(); it_teb != tebs_.end();)
      {
          if (*it_teb == best_teb_) // The current plan should not be considered a detour
          {
              ++it_teb;
              continue;
          }
          if ((*it_teb)->teb().sizePoses() < 2)
          {
              ROS_DEBUG("Discarding a plan with less than 2 poses");
              it_teb = removeTeb(*it_teb);
              continue;
          }
          double plan_orientation;
          if (!computeStartOrientation(*it_teb, len_orientation_vector, plan_orientation))
          {
              ROS_DEBUG("Failed to compute the start orientation for one of the tebs, likely close to the target");
              it_teb = removeTeb(*it_teb);
              continue;
          }
          if (fabs(g2o::normalize_theta(plan_orientation - current_movement_orientation)) > orient_threshold)
          {
              it_teb = removeTeb(*it_teb); // Plan detouring backwards
              continue;
          }
          if (!it_teb->get()->isOptimized())
          {
              ROS_DEBUG("Removing a teb because it's not optimized");
              it_teb = removeTeb(*it_teb); // Deletes tebs that cannot be optimized (last optim call failed)
              continue;
          }
          if (it_teb->get()->teb().getSumOfAllTimeDiffs() / best_plan_duration > cfg_->hcp.max_ratio_detours_duration_best_duration)
          {
              ROS_DEBUG("Removing a teb because it's duration is much longer than that of the best teb");
              it_teb = removeTeb(*it_teb);
              continue;
          }
          ++it_teb;
      }
  }
  
  bool HomotopyClassPlanner::computeStartOrientation(const TebOptimalPlannerPtr plan, const double len_orientation_vector,
                                                     double &orientation)
  {
      VertexPose start_pose = plan->teb().Pose(0);
      bool second_pose_found = false;
      Eigen::Vector2d start_vector;
      for (auto &pose : plan->teb().poses())
      {
          start_vector = start_pose.position() - pose->position();
          if (start_vector.norm() > len_orientation_vector)
          {
              second_pose_found = true;
              break;
          }
      }
      if (!second_pose_found) // The current plan is too short to make assumptions on the start orientation
          return false;
      orientation = std::atan2(start_vector[1], start_vector[0]);
      return true;
  }
  
  } // namespace teb_local_planner
  