Class TebOptimalPlanner

Inheritance Relationships

Base Type

Class Documentation

class TebOptimalPlanner : public hateb_local_planner::PlannerInterface

This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework.

For an introduction and further details about the TEB optimization problem refer to:

  • C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012.

  • C. Rösmann et al.: Efficient trajectory optimization using a sparse model, ECMR, 2013.

  • R. Kümmerle et al.: G2o: A general framework for graph optimization, ICRA, 2011.

Hyper-Graph creation and optimization

const HATebConfig *cfg_

Config class that stores and manages all related parameters.

ObstContainer *obstacles_

Store obstacles that are relevant for planning.

const ViaPointContainer *via_points_

Store via points for planning.

const std::map<uint64_t, ViaPointContainer> *agents_via_points_map_

Store via points for multiple agents, where each agent is identified by a unique ID.

TebVisualizationPtr visualization_

Instance of the visualization class.

TimedElasticBand teb_

Actual trajectory object.

std::map<uint64_t, TimedElasticBand> agents_tebs_map_

Map of TEBs of agents.

FootprintModelPtr robot_model_

Robot model.

CircularFootprintPtr agent_model_

Agent Footprint model.

boost::shared_ptr<g2o::SparseOptimizer> optimizer_

g2o optimizer for trajectory optimization

std::map<uint64_t, std::pair<bool, geometry_msgs::Twist>> agents_vel_start_

Initial velocities for each agent.

std::map<uint64_t, std::pair<bool, geometry_msgs::Twist>> agents_vel_goal_

Goal velocities for each agent.

std::pair<bool, geometry_msgs::Twist> vel_start_

Store the initial velocity at the start pose.

std::pair<bool, geometry_msgs::Twist> vel_goal_

Store the final velocity at the goal pose.

std::vector<geometry_msgs::Pose> static_agents_

Store poses of static agents in the environment that do not move but need consideration for visibility and safety.

bool initialized_

Keeps track about the correct initialization of this class.

bool optimized_

This variable is true as long as the last optimization has been completed successful.

double agent_radius_

Radius of the circular footprint used for agent collision checking and safety distances.

double robot_radius_

Radius of the robot’s circular footprint used for collision checking and safety calculations.

int isMode_

Planning Mode.

std::vector<double> agent_nominal_vels_

Nominal agent velocities calculated using moving average filter.

double current_agent_robot_min_dist_

Controls addition of edges.

double cost_

Store cost value of the current hyper-graph.

RotType prefer_rotdir_

Store whether to prefer a specific initial rotation in optimization (might be activated in case the robot oscillates)

bool buildGraph(double weight_multiplier = 1.0)

Build the hyper-graph representing the TEB optimization problem.

This method creates the optimization problem according to the hyper-graph formulation.

For more details refer to the literature cited in the

TebOptimalPlanner class description.

See also

optimizeGraph

See also

clearGraph

Parameters:

weight_multiplier – Specify a weight multipler for selected weights in optimizeGraph This might be used for weight adapation strategies. Currently, only obstacle collision weights are considered.

Returns:

true, if the graph was created successfully, false otherwise.

bool optimizeGraph(int no_iterations, bool clear_after = true)

Optimize the previously constructed hyper-graph to deform / optimize the TEB.

This method invokes the g2o framework to solve the optimization problem considering dedicated sparsity patterns.

The current implementation calls a non-constrained sparse Levenberg-Marquardt algorithm. Constraints are considered by utilizing penalty approximations. Refer to the literature cited in the

TebOptimalPlanner class description.

See also

buildGraph

See also

clearGraph

Parameters:

  • no_iterations – Number of solver iterations

  • clear_after – Clear the graph after optimization.

Returns:

true, if optimization terminates successfully, false otherwise.

void clearGraph()

Clear an existing internal hyper-graph.

See also

buildGraph

See also

optimizeGraph

void AddTEBVertices()

Add all relevant vertices to the hyper-graph as optimizable variables.

Vertices (if unfixed) represent the variables that will be optimized.

In case of the Timed-Elastic-Band poses and time differences form the vertices of the hyper-graph.

The order of insertion of vertices (to the graph) is important for efficiency, since it affect the sparsity pattern of the underlying hessian computed for optimization.

See also

VertexPose

See also

VertexTimeDiff

See also

buildGraph

See also

optimizeGraph

void AddEdgesVelocity()

Add all edges (local cost functions) for limiting the translational and angular velocity.

See also

EdgeVelocity

See also

buildGraph

See also

optimizeGraph

void AddEdgesVelocityForAgents()

Add all edges (local cost functions) for limiting the translational and angular velocity for agents.

See also

EdgeVelocity

See also

buildGraph

See also

optimizeGraph

void AddEdgesAcceleration()

Add all edges (local cost functions) for limiting the translational and angular acceleration.

See also

EdgeAcceleration

See also

EdgeAccelerationStart

See also

EdgeAccelerationGoal

See also

buildGraph

See also

optimizeGraph

void AddEdgesAccelerationForAgents()

Add all edges (local cost functions) for limiting the translational and angular acceleration for agents.

See also

EdgeAcceleration

See also

EdgeAccelerationStart

See also

EdgeAccelerationGoal

See also

buildGraph

See also

optimizeGraph

void AddEdgesTimeOptimal()

Add all edges (local cost functions) for minimizing the transition time (resp. minimize time differences)

See also

EdgeTimeOptimal

See also

buildGraph

See also

optimizeGraph

void AddEdgesTimeOptimalForAgents()

Add all edges (local cost functions) for minimizing the transition time (resp. minimize time differences) for agents.

See also

EdgeTimeOptimal

See also

buildGraph

See also

optimizeGraph

void AddEdgesShortestPath()

Add all edges (local cost functions) for minimizing the path length.

See also

EdgeShortestPath

See also

buildGraph

See also

optimizeGraph

void AddEdgesObstacles(double weight_multiplier = 1.0)

Add all edges (local cost functions) related to keeping a distance from static obstacles.

See also

EdgeObstacle

See also

buildGraph

See also

optimizeGraph

Warning

do not combine with AddEdgesInflatedObstacles

Parameters:

weight_multiplier – Specify an additional weight multipler (in addition to the the config weight)

void AddEdgesObstaclesLegacy(double weight_multiplier = 1.0)

Add all edges (local cost functions) related to keeping a distance from static obstacles (legacy association strategy)

See also

EdgeObstacle

See also

buildGraph

See also

optimizeGraph

Warning

do not combine with AddEdgesInflatedObstacles

Parameters:

weight_multiplier – Specify an additional weight multipler (in addition to the the config weight)

void AddEdgesObstaclesForAgents()

Add edges to the graph to maintain distance between agents and static obstacles.

This method adds edges (cost functions) that help maintain safe distances between each agent and static obstacles in the environment. Similar to AddEdgesObstacles() but specifically handles the agent trajectories from the agents_tebs_map_.

See also

EdgeObstacle

See also

buildGraph

See also

optimizeGraph

void AddEdgesViaPoints()

Add all edges (local cost functions) related to minimizing the distance to via-points.

See also

EdgeViaPoint

See also

buildGraph

See also

optimizeGraph

void AddEdgesViaPointsForAgents()

Add all edges (local cost functions) related to minimizing the distance to via-points for agents.

See also

EdgeViaPoint

See also

buildGraph

See also

optimizeGraph

void AddEdgesDynamicObstacles(double weight_multiplier = 1.0)

Add all edges (local cost functions) related to keeping a distance from dynamic (moving) obstacles.

See also

EdgeDynamicObstacle

See also

buildGraph

See also

optimizeGraph

Parameters:

weight_multiplier – Specify an additional weight multipler (in addition to the the config weight)

void AddEdgesDynamicObstaclesForAgents(double weight_multiplier = 1.0)

Add all edges (local cost functions) related to keeping a distance from dynamic (moving) obstacles in case of humans.

See also

EdgeDynamicObstacle

See also

buildGraph

See also

optimizeGraph

Parameters:

weight_multiplier – Specify an additional weight multipler (in addition to the the config weight)

void AddEdgesInvisibleHumans(double weight_multiplier = 1.0)

Add edges to consider invisible humans in trajectory optimization.

Adds cost function edges to handle regions where humans might be present but not directly visible to the robot’s sensors. This helps create safer trajectories in areas with potential occlusions or blind spots.

See also

EdgeInvisibleHuman

See also

buildGraph

See also

optimizeGraph

Parameters:

weight_multiplier – Scaling factor for the invisible human cost weights

void AddEdgesStaticAgentVisibility()

Add edges to consider visibility constraints for static agents. Penelizes robot for entering a human fov from behind and very closely.

See also

EdgeStaticAgentVisibility

See also

buildGraph

See also

optimizeGraph

void AddEdgesKinematicsDiffDrive()

Add all edges (local cost functions) for satisfying kinematic constraints of a differential drive robot.

See also

AddEdgesKinematicsCarlike

See also

buildGraph

See also

optimizeGraph

Warning

do not combine with AddEdgesKinematicsCarlike()

void AddEdgesKinematicsDiffDriveForAgents()

Add all edges (local cost functions) for satisfying kinematic constraints of a differential drive agents.

See also

AddEdgesKinematicsCarlike

See also

buildGraph

See also

optimizeGraph

Warning

do not combine with AddEdgesKinematicsCarlike()

void AddEdgesKinematicsCarlike()

Add all edges (local cost functions) for satisfying kinematic constraints of a carlike robot.

See also

AddEdgesKinematicsDiffDrive

See also

buildGraph

See also

optimizeGraph

Warning

do not combine with AddEdgesKinematicsDiffDrive()

void AddEdgesKinematicsCarlikeForAgents()

Add all edges (local cost functions) for satisfying kinematic constraints of a carlike agent.

See also

AddEdgesKinematicsDiffDrive

See also

buildGraph

See also

optimizeGraph

Warning

do not combine with AddEdgesKinematicsDiffDrive()

void AddEdgesPreferRotDir()

Add all edges (local cost functions) for prefering a specifiy turning direction (by penalizing the other one)

See also

buildGraph

See also

optimizeGraph

void AddEdgesAgentRobotSafety()

Add edges to maintain safety distance between agents and robot.

Adds cost function edges that ensure a minimum safety distance is maintained between the robot and all agents during trajectory optimization.

See also

EdgeAgentRobotSafety

See also

buildGraph

See also

optimizeGraph

void AddEdgesAgentAgentSafety()

Add edges to maintain safety distances between different agents.

Adds cost function edges that ensure minimum safety distances are maintained between different agents during trajectory optimization to avoid inter-agent collisions.

See also

EdgeAgentAgentSafety

See also

buildGraph

See also

optimizeGraph

void AddEdgesAgentRobotRelVelocity()

Add edges to control relative velocities between agents and robot.

Adds cost function edges that help the robot to exploit the existing space and robot’s velocity to increase human comfort. This makes the robot reduce its velocity in constrained spaces and move farther away with maximum velocity in open spaces.

See also

EdgeAgentRobotRelVelocity

See also

buildGraph

See also

optimizeGraph

void AddEdgesAgentRobotVisibility()

Add edges to maintain visibility constraints between agents and robot.

Adds cost function edges that encourage the robot to enter the human’s FOV at an appropriate angle and distance, helping ensure that agents (especially humans) can see and predict the robot’s movements.

See also

EdgeAgentRobotVisibility

See also

buildGraph

See also

optimizeGraph

static boost::shared_ptr<g2o::SparseOptimizer> initOptimizer()

Initialize and configure the g2o sparse optimizer.

Returns:

shared pointer to the g2o::SparseOptimizer instance

Plan a trajectory <br>

virtual bool plan(const std::vector<geometry_msgs::PoseStamped> &initial_plan, const geometry_msgs::Twist *start_vel = nullptr, bool free_goal_vel = false, const AgentPlanVelMap *initial_agent_plan_vel_map = nullptr, hateb_local_planner::OptimizationCostArray *op_costs = nullptr, double dt_ref = 0.4, double dt_hyst = 0.1, int Mode = 0) override

Plan a trajectory based on an initial reference plan.

Call this method to create and optimize a trajectory that is initialized according to an initial reference plan (given as a container of poses).

The method supports hot-starting from previous solutions, if avaiable:

Parameters:

  • initial_plan – vector of geometry_msgs::PoseStamped

  • start_vel – Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)

  • free_goal_vel – if true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)

Returns:

true if planning was successful, false otherwise

virtual bool plan(const tf::Pose &start, const tf::Pose &goal, const geometry_msgs::Twist *start_vel = nullptr, bool free_goal_vel = false, hateb_local_planner::OptimizationCostArray *op_costs = nullptr, double dt_ref = 0.4, double dt_hyst = 0.1, int Mode = 0) override

Plan a trajectory between a given start and goal pose (tf::Pose version)

Call this method to create and optimize a trajectory that is initialized between a given start and goal pose.

The method supports hot-starting from previous solutions, if avaiable:

Parameters:

  • start – tf::Pose containing the start pose of the trajectory

  • goal – tf::Pose containing the goal pose of the trajectory

  • start_vel – Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)

  • free_goal_vel – if true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)

Returns:

true if planning was successful, false otherwise

virtual bool plan(const PoseSE2 &start, const PoseSE2 &goal, const geometry_msgs::Twist *start_vel = nullptr, bool free_goal_vel = false, double pre_plan_time = 0.0, hateb_local_planner::OptimizationCostArray *op_costs = nullptr, double dt_ref = 0.4, double dt_hyst = 0.1, int Mode = 0) override

Plan a trajectory between a given start and goal pose.

Call this method to create and optimize a trajectory that is initialized between a given start and goal pose.

The method supports hot-starting from previous solutions, if avaiable:

Parameters:

  • start – PoseSE2 containing the start pose of the trajectory

  • goal – PoseSE2 containing the goal pose of the trajectory

  • start_vel – Initial velocity at the start pose (twist message containing the translational and angular velocity).

  • free_goal_vel – if true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)

Returns:

true if planning was successful, false otherwise

virtual bool getVelocityCommand(double &vx, double &vy, double &omega, int look_ahead_poses, double dt_ref) const override

Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.

Warning

Call plan() first and check if the generated plan is feasible.

Parameters:

  • vx – [out] translational velocity [m/s]

  • vy – [out] strafing velocity which can be nonzero for holonomic robots[m/s]

  • omega – [out] rotational velocity [rad/s]

Returns:

true if command is valid, false otherwise

bool optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards = true, double obst_cost_scale = 1.0, double viapoint_cost_scale = 1.0, bool alternative_time_cost = false, hateb_local_planner::OptimizationCostArray *op_costs = nullptr, double dt_ref = 0.4, double dt_hyst = 0.1)

Optimize a previously initialized trajectory (actual TEB optimization loop).

optimizeTEB implements the main optimization loop.

It consist of two nested loops:

  • The outer loop resizes the trajectory according to the temporal resolution by invoking TimedElasticBand::autoResize(). Afterwards the internal method optimizeGraph() is called that constitutes the innerloop.

  • The inner loop calls the solver (g2o framework, resp. sparse Levenberg-Marquardt) and iterates a specified number of optimization calls (iterations_innerloop).

The outer loop is repeated iterations_outerloop

times.

The ratio of inner and outer loop iterations significantly defines the contraction behavior and convergence rate of the trajectory optimization. Based on our experiences, 2-6 innerloop iterations are sufficient.

The number of outer loop iterations should be determined by considering the maximum CPU time required to match the control rate.

Optionally, the cost vector can be calculated by specifying

compute_cost_afterwards, see computeCurrentCost().

Remark

This method is usually called from a plan() method

Parameters:

  • iterations_innerloop – Number of iterations for the actual solver loop

  • iterations_outerloop – Specifies how often the trajectory should be resized followed by the inner solver loop.

  • compute_cost_afterwards – if true Calculate the cost vector according to computeCurrentCost(), the vector can be accessed afterwards using getCurrentCost().

  • obst_cost_scale – Specify extra scaling for obstacle costs (only used if compute_cost_afterwards is true)

  • viapoint_cost_scale – Specify extra scaling for via-point costs (only used if compute_cost_afterwards is true)

  • alternative_time_cost – Replace the cost for the time optimal objective by the actual (weighted) transition time (only used if compute_cost_afterwards is true).

Returns:

true if the optimization terminates successfully, false otherwise

bool optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards = true, double obst_cost_scale = 1.0, double viapoint_cost_scale = 1.0, bool alternative_time_cost = false, hateb_local_planner::OptimizationCostArray *op_costs = nullptr)

Desired initial and final velocity

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].

Remark

Calling this function is not neccessary if the initial velocity is passed via the plan() method

Parameters:

vel_start – Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used, for holonomic robots also linear.y)

void setVelocityGoal(const geometry_msgs::Twist &vel_goal)

Set the desired final velocity at the trajectory’s goal pose.

Remark

Call this function only if a non-zero velocity is desired and if free_goal_vel is set to false in plan()

Parameters:

vel_goal – twist message containing the translational and angular final velocity

inline void setVelocityGoalFree()

Set the desired final velocity at the trajectory’s goal pose to be the maximum velocity limit.

Remark

Calling this function is not neccessary if free_goal_vel is set to false in plan()

Take obstacles into account

inline void setObstVector(ObstContainer *obst_vector)

Assign a new set of obstacles.

Remark

This method overrids the obstacle container optinally assigned in the constructor.

Parameters:

obst_vector – pointer to an obstacle container (can also be a nullptr)

inline const ObstContainer &getObstVector() const

Access the internal obstacle container.

Returns:

Const reference to the obstacle container

Take via-points into account

inline void setViaPoints(const ViaPointContainer *via_points)

Assign a new set of via-points.

Any previously set container will be overwritten.

Parameters:

via_points – pointer to a via_point container (can also be a nullptr)

inline const ViaPointContainer &getViaPoints() const

Access the internal via-point container.

Returns:

Const reference to the via-point container

Visualization

void setVisualization(TebVisualizationPtr visualization)

Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence)

See also

visualize

Parameters:

visualization – shared pointer to a TebVisualization instance

virtual void visualize() override

Publish the local plan and pose sequence via ros topics (e.g. subscribe with rviz).

Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor.

See also

setVisualization

Utility methods and more

inline virtual void clearPlanner() override

Reset the planner by clearing the internal graph and trajectory.

inline virtual void setPreferredTurningDir(RotType dir) override

Prefer a desired initial turning direction (by penalizing the opposing one)

A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty. Initial means that the penalty is applied only to the first few poses of the trajectory.

Parameters:

dir – This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none)

inline TimedElasticBand &teb()

Access the internal TimedElasticBand trajectory.

Warning

In general, the underlying teb must not be modified directly. Use with care…

Returns:

reference to the teb

inline const TimedElasticBand &teb() const

Access the internal TimedElasticBand trajectory (read-only).

Returns:

const reference to the teb

inline boost::shared_ptr<g2o::SparseOptimizer> optimizer()

Access the internal g2o optimizer.

Warning

In general, the underlying optimizer must not be modified directly. Use with care…

Returns:

const shared pointer to the g2o sparse optimizer

inline boost::shared_ptr<const g2o::SparseOptimizer> optimizer() const

Access the internal g2o optimizer (read-only).

Returns:

const shared pointer to the g2o sparse optimizer

inline bool isOptimized() const

Check if last optimization was successful.

Returns:

true if the last optimization returned without errors, otherwise false (also if no optimization has been called before).

void computeCurrentCost(double obst_cost_scale = 1.0, double viapoint_cost_scale = 1.0, bool alternative_time_cost = false, hateb_local_planner::OptimizationCostArray *op_costs = NULL)

Compute the cost vector of a given optimization problen (hyper-graph must exist).

Use this method to obtain information about the current edge errors / costs (local cost functions).

The vector of cost values is composed according to the different edge types (time_optimal, obstacles, …).

Refer to the method declaration for the detailed composition.

The cost for the edges that minimize time differences (

EdgeTimeOptimal) corresponds to the sum of all single squared time differneces: \( \sum_i \Delta T_i^2 \). Sometimes, the user may want to get a value that is proportional or identical to the actual trajectory transition time \( \sum_i \Delta T_i \)

.

Set

alternative_time_cost to true in order to get the cost calculated using the latter equation, but check the implemented definition, if the value is scaled to match the magnitude of other cost values.

See also

getCurrentCost

See also

optimizeTEB

Parameters:

  • obst_cost_scale – Specify extra scaling for obstacle costs.

  • viapoint_cost_scale – Specify extra scaling for via points.

  • alternative_time_cost – Replace the cost for the time optimal objective by the actual (weighted) transition time.

Returns:

TebCostVec containing the cost values

inline virtual void computeCurrentCost(std::vector<double> &cost, double obst_cost_scale = 1.0, double viapoint_cost_scale = 1.0, bool alternative_time_cost = false)

Compute and return the cost of the current optimization graph (supports multiple trajectories)

Parameters:

  • cost – [out] current cost value for each trajectory [for a planner with just a single trajectory: size=1, vector will not be cleared]

  • obst_cost_scale – Specify extra scaling for obstacle costs

  • viapoint_cost_scale – Specify extra scaling for via points.

  • alternative_time_cost – Replace the cost for the time optimal objective by the actual (weighted) transition time

inline double getCurrentCost() const

Access the cost vector.

The accumulated cost value previously calculated using computeCurrentCost or by calling optimizeTEB with enabled cost flag.

Returns:

const reference to the TebCostVec.

inline 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 holonomic robots)

The velocity is extracted using finite differences. The direction of the translational velocity is also determined.

Parameters:

  • pose1 – pose at time k

  • pose2 – consecutive pose at time k+1

  • dt – actual time difference between k and k+1 (must be >0 !!!)

  • vx – [out] translational velocity

  • vy – [out] strafing velocity which can be nonzero for holonomic robots

  • omega – [out] rotational velocity

void getVelocityProfile(std::vector<geometry_msgs::Twist> &velocity_profile) const

Compute the velocity profile of the trajectory.

This method computes the translational and rotational velocity for the complete planned trajectory. The first velocity is the one that is provided as initial velocity (fixed). Velocities at index k=2…end-1 are related to the transition from pose_{k-1} to pose_k. The last velocity is the final velocity (fixed). The number of Twist objects is therefore sizePoses()+1; In summary: v[0] = v_start, v[1,…end-1] = +-(pose_{k+1}-pose{k})/dt, v(end) = v_goal It can be used for evaluation and debugging purposes or for open-loop control. For computing the velocity required for controlling the robot to the next step refer to getVelocityCommand().

Parameters:

velocity_profile – [out] velocity profile will be written to this vector (after clearing any existing content) with the size=no_poses+1

virtual cohan_msgs::Trajectory getFullTrajectory() const override

Return the complete trajectory including poses, velocity profiles and temporal information.

It is useful for evaluation and debugging purposes or for open-loop control. Since the velocity obtained using difference quotients is the mean velocity between consecutive poses, the velocity at each pose at time stamp k is obtained by taking the average between both velocities. The velocity of the first pose is v_start (provided initial value) and the last one is v_goal (usually zero, if free_goal_vel is off). See getVelocityProfile() for the list of velocities between consecutive points.

Parameters:

trajectory – [out] the resulting trajectory

virtual cohan_msgs::Trajectory getFullAgentTrajectory(uint64_t agent_id) override

Return the complete trajectory for a specific agent including poses, velocity profiles and temporal information.

Similar to getFullTrajectory() but specifically for retrieving an agent’s planned trajectory. The trajectory includes pose sequence, velocity profile, and timing information useful for evaluation and debugging of agent behavior.

Parameters:

agent_id – The unique identifier of the agent whose trajectory should be retrieved

Returns:

The complete trajectory message for the specified agent

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

Check whether the planned trajectory is feasible or not.

This method currently checks only that the trajectory, or a part of the trajectory is collision free. Obstacles are here represented as costmap instead of the internal ObstacleContainer.

Parameters:

  • costmap_model – Pointer to the costmap model

  • footprint_spec – The specification of the footprint of the robot in world coordinates

  • inscribed_radius – The radius of the inscribed circle of the robot

  • circumscribed_radius – The radius of the circumscribed circle of the robot

  • look_ahead_idx – Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.

Returns:

true, if the robot footprint along the first part of the trajectory intersects with any obstacle in the costmap, false otherwise.

static void registerG2OTypes()

Register the vertices and edges defined for the TEB to the g2o::Factory.

This allows the user to export the internal graph to a text file for instance. Access the optimizer() for more details.

Public Functions

TebOptimalPlanner()

Default constructor.

explicit TebOptimalPlanner(const HATebConfig &cfg, ObstContainer *obstacles = nullptr, FootprintModelPtr robot_model = boost::make_shared<PointFootprint>(), TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer *via_points = nullptr, CircularFootprintPtr agent_model = boost::make_shared<CircularFootprint>(), const std::map<uint64_t, ViaPointContainer> *agents_via_points_map = nullptr)

Construct and initialize the TEB optimal planner.

Parameters:

  • cfg – Const reference to the HATebConfig class for internal parameters

  • obstacles – Container storing all relevant obstacles (see Obstacle)

  • robot_model – Shared pointer to the robot shape model used for optimization (optional)

  • visual – Shared pointer to the TebVisualization class (optional)

  • via_points – Container storing via-points (optional)

~TebOptimalPlanner() override

Destruct the optimal planner.

void initialize(const HATebConfig &cfg, ObstContainer *obstacles = nullptr, FootprintModelPtr robot_model = boost::make_shared<PointFootprint>(), TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer *via_points = nullptr, CircularFootprintPtr agent_model = boost::make_shared<CircularFootprint>(), const std::map<uint64_t, ViaPointContainer> *agents_via_points_map = nullptr)

Initializes the optimal planner.

Parameters:

  • cfg – Const reference to the HATebConfig class for internal parameters

  • obstacles – Container storing all relevant obstacles (see Obstacle)

  • robot_model – Shared pointer to the robot shape model used for optimization (optional)

  • visual – Shared pointer to the TebVisualization class (optional)

  • via_points – Container storing via-points (optional)