39 #ifndef OPTIMAL_PLANNER_H_ 40 #define OPTIMAL_PLANNER_H_ 54 #include "g2o/core/sparse_optimizer.h" 55 #include "g2o/core/block_solver.h" 56 #include "g2o/core/factory.h" 57 #include "g2o/core/optimization_algorithm_gauss_newton.h" 58 #include "g2o/core/optimization_algorithm_levenberg.h" 59 #include "g2o/solvers/csparse/linear_solver_csparse.h" 60 #include "g2o/solvers/cholmod/linear_solver_cholmod.h" 74 #include <nav_msgs/Path.h> 75 #include <geometry_msgs/PoseStamped.h> 77 #include <teb_local_planner/TrajectoryMsg.h> 79 #include <nav_msgs/Odometry.h> 93 typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >
ViaPointContainer;
169 virtual bool plan(
const std::vector<geometry_msgs::PoseStamped>& initial_plan,
const geometry_msgs::Twist* start_vel = NULL,
bool free_goal_vel=
false);
217 virtual bool getVelocityCommand(
double& vx,
double& vy,
double& omega,
int look_ahead_poses)
const;
246 bool optimizeTEB(
int iterations_innerloop,
int iterations_outerloop,
bool compute_cost_afterwards =
false,
247 double obst_cost_scale=1.0,
double viapoint_cost_scale=1.0,
bool alternative_time_cost=
false);
424 void computeCurrentCost(
double obst_cost_scale=1.0,
double viapoint_cost_scale=1.0,
bool alternative_time_cost=
false);
434 virtual void computeCurrentCost(std::vector<double>& cost,
double obst_cost_scale=1.0,
double viapoint_cost_scale=1.0,
bool alternative_time_cost=
false)
511 double circumscribed_radius=0.0,
int look_ahead_idx=-1);
532 bool buildGraph(
double weight_multiplier=1.0);
546 bool optimizeGraph(
int no_iterations,
bool clear_after=
true);
698 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
static void registerG2OTypes()
Register the vertices and edges defined for the TEB to the g2o::Factory.
void setVelocityStart(const geometry_msgs::Twist &vel_start)
Set the initial velocity at the trajectory's start pose (e.g. the robot's velocity) [twist overload]...
bool optimizeGraph(int no_iterations, bool clear_after=true)
Optimize the previously constructed hyper-graph to deform / optimize the TEB.
void setVelocityGoalFree()
Set the desired final velocity at the trajectory's goal pose to be the maximum velocity limit...
std::pair< bool, geometry_msgs::Twist > vel_start_
Store the initial velocity at the start pose.
TimedElasticBand & teb()
Access the internal TimedElasticBand trajectory.
This abstract class defines an interface for local planners.
void clearGraph()
Clear an existing internal hyper-graph.
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)
Check whether the planned trajectory is feasible or not.
void setVisualization(TebVisualizationPtr visualization)
Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and ...
std::pair< bool, geometry_msgs::Twist > vel_goal_
Store the final velocity at the goal pose.
void AddEdgesShortestPath()
Add all edges (local cost functions) for minimizing the path length.
virtual void visualize()
Publish the local plan and pose sequence via ros topics (e.g. subscribe with rviz).
const ObstContainer & getObstVector() const
Access the internal obstacle container.
const ViaPointContainer & getViaPoints() const
Access the internal via-point container.
void AddEdgesTimeOptimal()
Add all edges (local cost functions) for minimizing the transition time (resp. minimize time differen...
Config class for the teb_local_planner and its components.
void AddEdgesAcceleration()
Add all edges (local cost functions) for limiting the translational and angular acceleration.
Class that defines a trajectory modeled as an elastic band with augmented tempoarl information...
boost::shared_ptr< g2o::SparseOptimizer > optimizer()
Access the internal g2o optimizer.
void setVelocityGoal(const geometry_msgs::Twist &vel_goal)
Set the desired final velocity at the trajectory's goal pose.
void setObstVector(ObstContainer *obst_vector)
Assign a new set of obstacles.
double getCurrentCost() const
Access the cost vector.
TebVisualizationPtr visual
void AddEdgesVelocity()
Add all edges (local cost functions) for limiting the translational and angular velocity.
virtual void setPreferredTurningDir(RotType dir)
Prefer a desired initial turning direction (by penalizing the opposing one)
void computeCurrentCost(double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)
Compute the cost vector of a given optimization problen (hyper-graph must exist). ...
boost::shared_ptr< const g2o::SparseOptimizer > optimizer() const
Access the internal g2o optimizer (read-only).
void AddEdgesPreferRotDir()
Add all edges (local cost functions) for prefering a specifiy turning direction (by penalizing the ot...
virtual ~TebOptimalPlanner()
Destruct the optimal planner.
boost::shared_ptr< TebOptimalPlanner > TebOptimalPlannerPtr
Abbrev. for shared instances of the TebOptimalPlanner.
virtual bool plan(const std::vector< geometry_msgs::PoseStamped > &initial_plan, const geometry_msgs::Twist *start_vel=NULL, bool free_goal_vel=false)
Plan a trajectory based on an initial reference plan.
bool buildGraph(double weight_multiplier=1.0)
Build the hyper-graph representing the TEB optimization problem.
void AddTEBVertices()
Add all relevant vertices to the hyper-graph as optimizable variables.
boost::shared_ptr< const TebOptimalPlanner > TebOptimalPlannerConstPtr
Abbrev. for shared const TebOptimalPlanner pointers.
ObstContainer * obstacles_
Store obstacles that are relevant for planning.
TebVisualizationPtr visualization_
Instance of the visualization class.
g2o::BlockSolver< g2o::BlockSolverTraits<-1,-1 > > TEBBlockSolver
Typedef for the block solver utilized for optimization.
RotType
Symbols for left/none/right rotations.
void AddEdgesViaPoints()
Add all edges (local cost functions) related to minimizing the distance to via-points.
bool initialized_
Keeps track about the correct initialization of this class.
void AddEdgesDynamicObstacles(double weight_multiplier=1.0)
Add all edges (local cost functions) related to keeping a distance from dynamic (moving) obstacles...
boost::shared_ptr< TebVisualization > TebVisualizationPtr
Abbrev. for shared instances of the TebVisualization.
std::vector< ObstaclePtr > ObstContainer
Abbrev. for containers storing multiple obstacles.
bool optimized_
This variable is true as long as the last optimization has been completed successful.
TebOptimalPlanner()
Default constructor.
void getFullTrajectory(std::vector< TrajectoryPointMsg > &trajectory) const
Return the complete trajectory including poses, velocity profiles and temporal information.
void setViaPoints(const ViaPointContainer *via_points)
Assign a new set of via-points.
void clearTimedElasticBand()
clear all poses and timediffs from the trajectory. The pose and timediff sequences will be empty and ...
std::vector< TebOptimalPlannerPtr > TebOptPlannerContainer
Abbrev. for containers storing multiple teb optimal planners.
RobotFootprintModelPtr robot_model_
Robot model.
This class implements a pose in the domain SE2: The pose consist of the position x and y and an orie...
void initialize(const TebConfig &cfg, ObstContainer *obstacles=NULL, RobotFootprintModelPtr robot_model=boost::make_shared< PointRobotFootprint >(), TebVisualizationPtr visual=TebVisualizationPtr(), const ViaPointContainer *via_points=NULL)
Initializes the optimal planner.
virtual void clearPlanner()
Reset the planner by clearing the internal graph and trajectory.
void getVelocityProfile(std::vector< geometry_msgs::Twist > &velocity_profile) const
Compute the velocity profile of the trajectory.
virtual bool getVelocityCommand(double &vx, double &vy, double &omega, int look_ahead_poses) const
Get the velocity command from a previously optimized plan to control the robot at the current samplin...
std::vector< ObstaclePtr > obst_vector
This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework.
bool isOptimized() const
Check if last optimization was successful.
double cost_
Store cost value of the current hyper-graph.
void AddEdgesKinematicsCarlike()
Add all edges (local cost functions) for satisfying kinematic constraints of a carlike robot...
void extractVelocity(const PoseSE2 &pose1, const PoseSE2 &pose2, double dt, double &vx, double &vy, double &omega) const
Extract the velocity from consecutive poses and a time difference (including strafing velocity for ho...
const TimedElasticBand & teb() const
Access the internal TimedElasticBand trajectory (read-only).
ViaPointContainer via_points
const ViaPointContainer * via_points_
Store via points for planning.
TimedElasticBand teb_
Actual trajectory object.
boost::shared_ptr< g2o::SparseOptimizer > initOptimizer()
Initialize and configure the g2o sparse optimizer.
g2o::LinearSolverCSparse< TEBBlockSolver::PoseMatrixType > TEBLinearSolver
Typedef for the linear solver utilized for optimization.
void AddEdgesObstacles(double weight_multiplier=1.0)
Add all edges (local cost functions) related to keeping a distance from static obstacles.
const TebConfig * cfg_
Config class that stores and manages all related parameters.
void AddEdgesKinematicsDiffDrive()
Add all edges (local cost functions) for satisfying kinematic constraints of a differential drive rob...
std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > ViaPointContainer
Typedef for a container storing via-points.
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)
Optimize a previously initialized trajectory (actual TEB optimization loop).
boost::shared_ptr< g2o::SparseOptimizer > optimizer_
g2o optimizer for trajectory optimization
void AddEdgesObstaclesLegacy(double weight_multiplier=1.0)
Add all edges (local cost functions) related to keeping a distance from static obstacles (legacy asso...
RotType prefer_rotdir_
Store whether to prefer a specific initial rotation in optimization (might be activated in case the r...
virtual void computeCurrentCost(std::vector< double > &cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)