optimal_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 OPTIMAL_PLANNER_H_
#define OPTIMAL_PLANNER_H_

#include <math.h>


// teb stuff
#include <teb_local_planner/teb_config.h>
#include <teb_local_planner/misc.h>
#include <teb_local_planner/timed_elastic_band.h>
#include <teb_local_planner/planner_interface.h>
#include <teb_local_planner/visualization.h>
#include <teb_local_planner/robot_footprint_model.h>

// g2o lib stuff
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"

// g2o custom edges and vertices for the TEB planner
#include <teb_local_planner/g2o_types/edge_velocity.h>
#include <teb_local_planner/g2o_types/edge_acceleration.h>
#include <teb_local_planner/g2o_types/edge_kinematics.h>
#include <teb_local_planner/g2o_types/edge_time_optimal.h>
#include <teb_local_planner/g2o_types/edge_obstacle.h>
#include <teb_local_planner/g2o_types/edge_dynamic_obstacle.h>
#include <teb_local_planner/g2o_types/edge_via_point.h>
#include <teb_local_planner/g2o_types/edge_prefer_rotdir.h>

// messages
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_datatypes.h>
#include <teb_local_planner/TrajectoryMsg.h>

#include <nav_msgs/Odometry.h>
#include <limits.h>

namespace teb_local_planner
{

typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> >  TEBBlockSolver;

typedef g2o::LinearSolverCSparse<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;
//typedef g2o::LinearSolverCholmod<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;

typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ViaPointContainer;


class TebOptimalPlanner : public PlannerInterface
{
public:
    
  TebOptimalPlanner();
  
  TebOptimalPlanner(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
                    TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
  
  virtual ~TebOptimalPlanner();
  
  void initialize(const TebConfig& cfg, ObstContainer* obstacles = NULL, RobotFootprintModelPtr robot_model = boost::make_shared<PointRobotFootprint>(),
                  TebVisualizationPtr visual = 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;
  
  
  bool optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards = false,
                   double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
  
  
  
  
  
  void setVelocityStart(const geometry_msgs::Twist& vel_start);
  
  void setVelocityGoal(const geometry_msgs::Twist& vel_goal);
  
  void setVelocityGoalFree() {vel_goal_.first = false;}
  
  
  
  
  
  void setObstVector(ObstContainer* obst_vector) {obstacles_ = obst_vector;}
  
  const ObstContainer& getObstVector() const {return *obstacles_;}

  
  
  
  void setViaPoints(const ViaPointContainer* via_points) {via_points_ = via_points;}
  
  const ViaPointContainer& getViaPoints() const {return *via_points_;}

          
  
  
  void setVisualization(TebVisualizationPtr visualization);
  
  virtual void visualize();
  
  
  
        
  virtual void clearPlanner() 
  {
    clearGraph();
    teb_.clearTimedElasticBand();
  }
  
  virtual void setPreferredTurningDir(RotType dir) {prefer_rotdir_=dir;}
  
  static void registerG2OTypes();
  
  TimedElasticBand& teb() {return teb_;};
  
  const TimedElasticBand& teb() const {return teb_;};
  
  boost::shared_ptr<g2o::SparseOptimizer> optimizer() {return optimizer_;};
  
  boost::shared_ptr<const g2o::SparseOptimizer> optimizer() const {return optimizer_;};
  
  bool isOptimized() const {return optimized_;};
        
  void computeCurrentCost(double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
  
  virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)
  {
    computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
    cost.push_back( getCurrentCost() );
  }
  
  double getCurrentCost() const {return cost_;}
  
    
  inline void extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, double& vx, double& vy, double& omega) const;
  
  void getVelocityProfile(std::vector<geometry_msgs::Twist>& velocity_profile) const;
  
  void getFullTrajectory(std::vector<TrajectoryPointMsg>& trajectory) const;
  
  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);
  
  
  virtual bool isHorizonReductionAppropriate(const std::vector<geometry_msgs::PoseStamped>& initial_plan) const;
  
  
protected:
  
  
  bool buildGraph(double weight_multiplier=1.0);
  
  bool optimizeGraph(int no_iterations, bool clear_after=true);
  
  void clearGraph();
  
  void AddTEBVertices();
  
  void AddEdgesVelocity();
  
  void AddEdgesAcceleration();
  
  void AddEdgesTimeOptimal();
  
  void AddEdgesObstacles(double weight_multiplier=1.0);
  
  void AddEdgesObstaclesLegacy(double weight_multiplier=1.0);
  
  void AddEdgesViaPoints();
  
  void AddEdgesDynamicObstacles(double weight_multiplier=1.0);

  void AddEdgesKinematicsDiffDrive();
  
  void AddEdgesKinematicsCarlike();
  
  void AddEdgesPreferRotDir(); 
  
  
  
  boost::shared_ptr<g2o::SparseOptimizer> initOptimizer();
    

  // external objects (store weak pointers)
  const TebConfig* cfg_; 
  ObstContainer* obstacles_; 
  const ViaPointContainer* via_points_; 
  
  double cost_; 
  RotType prefer_rotdir_; 
  
  // internal objects (memory management owned)
  TebVisualizationPtr visualization_; 
  TimedElasticBand teb_; 
  RobotFootprintModelPtr robot_model_; 
  boost::shared_ptr<g2o::SparseOptimizer> optimizer_; 
  std::pair<bool, geometry_msgs::Twist> vel_start_; 
  std::pair<bool, geometry_msgs::Twist> vel_goal_; 

  bool initialized_; 
  bool optimized_; 
  
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW    
};

typedef boost::shared_ptr<TebOptimalPlanner> TebOptimalPlannerPtr;
typedef boost::shared_ptr<const TebOptimalPlanner> TebOptimalPlannerConstPtr;
typedef std::vector< TebOptimalPlannerPtr > TebOptPlannerContainer;

} // namespace teb_local_planner

#endif /* OPTIMAL_PLANNER_H_ */