Class Planner

Nested Relationships

Nested Types

Class Documentation

class Planner

Public Types

using StartSet = std::vector<Start>

Public Functions

Planner(Configuration config, Options default_options)

Constructor

Parameters:

  • config – [in] This is the Configuration for the Planner. The Planner instance will maintain a cache while it performs planning requests. This cache will offer potential speed ups to subsequent planning requests, but the correctness of the cache depends on the fields in the Configuration to remain constant. Therefore you are not permitted to modify a Planner’s Configuration after the Planner is constructed. To change the planning Configuration, you will need to create a new Planner instance with the desired Configuration.

  • default_options – [in] Unlike the Configuration, you are allowed to change a Planner’s Options. The parameter given here will be used as the default options, so you can set them here and then forget about them. These options can be overriden each time you request a plan.

const Configuration &get_configuration() const

Get a const reference to the configuration for this Planner. Note that the configuration of a planner cannot be changed once it is set.

Note

The Planner maintains a cache that allows searches to become progressively faster. This cache depends on the fields in the Planner’s configuration, so those fields cannot be changed without invalidating that cache. To plan using a different configuration, you should create a new Planner instance with the desired configuration.

Planner &set_default_options(Options default_options)

Change the default planning options.

Options &get_default_options()

Get a mutable reference to the default planning options.

const Options &get_default_options() const

Get a const reference to the default planning options.

Result plan(const Start &start, Goal goal) const

Produce a plan for the given starting conditions and goal. The default Options of this Planner instance will be used.

Parameters:

  • start – [in] The starting conditions

  • goal – [in] The goal conditions

Result plan(const Start &start, Goal goal, Options options) const

Product a plan for the given start and goal conditions. Override the default options.

Parameters:

  • start – [in] The starting conditions

  • goal – [in] The goal conditions

  • options – [in] The Options to use for this plan. This overrides the default Options of the Planner instance.

Result plan(const StartSet &starts, Goal goal) const

Produces a plan for the given set of starting conditions and goal. The default Options of this Planner instance will be used.

The planner will choose the start condition that allows for the shortest plan (not the one that finishes the soonest according to wall time).

At least one start must be specified or else this is guaranteed to return a nullopt.

Parameters:

  • starts – [in] The set of available starting conditions

  • goal – [in] The goal conditions

Result plan(const StartSet &starts, Goal goal, Options options) const

Produces a plan for the given set of starting conditions and goal. Override the default options.

The planner will choose the start condition that allows for the shortest plan (not the one that finishes the soonest according to wall time).

At least one start must be specified or else this is guaranteed to return a nullopt.

Parameters:

  • starts – [in] The starting conditions

  • goal – [in] The goal conditions

  • options – [in] The options to use for this plan. This overrides the default Options of the Planner instance.

Result setup(const Start &start, Goal goal) const

Set up a planning job, but do not start iterating.

See also

plan(const Start&, Goal)

Result setup(const Start &start, Goal goal, Options options) const

Set up a planning job, but do not start iterating.

See also

plan(const Start&, Goal, Options)

Result setup(const StartSet &starts, Goal goal) const

Set up a planning job, but do not start iterating.

See also

plan(const StartSet&, Goal)

Result setup(const StartSet &starts, Goal goal, Options options) const

Set up a planning job, but do not start iterating.

See also

plan(const StartSet&, Goal, Options)

std::optional<QuickestPath> quickest_path(const StartSet &start, std::size_t goal_vertex) const

Calculate the QuickestPath.

If the start set is left empty, the function will return a nullopt.

Parameters:

  • start – [in] One or more start conditions to plan from. This function ignores time and orientation information. Lane information is only used to know the speed limit to use when moving from the start location to the initial waypoint.

  • goal_vertex – [in] The goal vertex to plan to.

Returns:

The quickest path from the start to the finish, or a nullopt if there is no path that connects the start to the finish.

class Configuration

The Configuration class contains planning parameters that are immutable for each Planner instance.

These parameters generally describe the capabilities or behaviors of the AGV that is being planned for, so they shouldn’t need to change in between plans anyway.

Public Functions

Configuration(Graph graph, VehicleTraits traits, Interpolate::Options interpolation = Interpolate::Options())

Constructor

Parameters:

  • vehicle_traits – [in] The traits of the vehicle that is being planned for

  • graph – [in] The graph which is being planned over

  • interpolation – [in] The options for how the planner will perform trajectory interpolation

Configuration &graph(Graph graph)

Set the graph to use for planning.

Graph &graph()

Get a mutable reference to the graph.

const Graph &graph() const

Get a const reference to the graph.

Configuration &vehicle_traits(VehicleTraits traits)

Set the vehicle traits to use for planning.

VehicleTraits &vehicle_traits()

Get a mutable reference to the vehicle traits.

const VehicleTraits &vehicle_traits() const

Get a const reference to the vehicle traits.

Configuration &interpolation(Interpolate::Options interpolate)

Set the interpolation options for the planner.

Interpolate::Options &interpolation()

Get a mutable reference to the interpolation options.

const Interpolate::Options &interpolation() const

Get a const reference to the interpolation options.

Configuration &lane_closures(LaneClosure closures)

Set the lane closures for the graph. The planner will not attempt to expand down any lanes that are closed.

LaneClosure &lane_closures()

Get a mutable reference to the LaneClosure setting.

const LaneClosure &lane_closures() const

Get a const reference to the LaneClosure setting.

Configuration &traversal_cost_per_meter(double value)

How much the cost should increase per meter travelled. Besides this, cost is measured by the number of seconds spent travelling.

double traversal_cost_per_meter() const

Get the traversal cost.

class Debug

This class exists only for debugging purposes. It is not to be used in live production, and its API is to be considered unstable at all times. Any minor version increment

Public Types

using ConstNodePtr = std::shared_ptr<const Node>

Public Functions

Debug(const Planner &planner)

Create a debugger for a planner.

Progress begin(const std::vector<Start> &starts, Goal goal, Options options) const

Begin debugging a plan. Call step() on the Progress object until it returns a plan or until the queue is empty (the Progress object can be treated as a boolean for this purpose).

Public Static Functions

static std::size_t queue_size(const Planner::Result &result)

Get the current size of the frontier queue of a Planner Result.

static std::size_t expansion_count(const Planner::Result &result)

Get the number of search nodes that have been expanded for a Planner Result

static std::size_t node_count(const Planner::Result &result)

Get the current number of nodes that have been created for this Planner Result. This is equal to queue_size(r) + expansion_count(r).

struct Node

A Node in the planning search. A final Planning solution will be a chain of these Nodes, aggregated into a Plan data structure.

Public Types

using SearchQueue = std::priority_queue<ConstNodePtr, std::vector<ConstNodePtr>, Compare>
using Vector = std::vector<ConstNodePtr>

Public Members

ConstNodePtr parent

The parent of this Node. If this is a nullptr, then this was a starting node.

std::vector<Route> route_from_parent

The route that goes from the parent Node to this Node.

double remaining_cost_estimate

An estimate of the remaining cost, based on the heuristic.

double current_cost

The actual cost that has accumulated on the way to this Node.

rmf_utils::optional<std::size_t> waypoint

The waypoint that this Node stops on.

double orientation

The orientation that this Node ends with.

agv::Graph::Lane::EventPtr event

A pointer to an event that occured on the way to this Node.

rmf_utils::optional<std::size_t> start_set_index

If this is a starting node, then this will be the index.

std::size_t id

A unique ID that sticks with this node for its entire lifetime. This will also (roughly) reflect the order of node creation.

struct Compare

Public Functions

inline bool operator()(const ConstNodePtr &a, const ConstNodePtr &b)
class Progress

Public Functions

rmf_utils::optional<Plan> step()

Step the planner forward one time. This will expand the current highest priority Node in the queue and move it to the back of expanded_nodes. The nodes that result from the expansion will all be added to the queue.

inline operator bool() const

Implicitly cast the Progress instance to a boolean. The value will be true if the plan can keep expanding, and it will be false if it cannot expand any further.

After finding a solution, it may be possible to continue expanding, but there is no point because the first solution returned is guaranteed to be the optimal one.

const Node::SearchQueue &queue() const

A priority queue of unexpanded Nodes. They are sorted based on g(n)+h(n) in ascending order (see Node::Compare).

const Node::Vector &expanded_nodes() const

The set of Nodes that have been expanded. They are sorted in the order that they were chosen for expansion.

const Node::Vector &terminal_nodes() const

The set of Nodes which terminated, meaning it was not possible to expand from them.

class Goal

Describe the goal conditions of a plan.

Public Functions

Goal(std::size_t goal_waypoint)

Constructor

Note

With this constructor, any final orientation will be accepted.

Parameters:

goal_waypoint – [in] The waypoint that the AGV needs to reach.

Goal(std::size_t goal_waypoint, double goal_orientation)

Constructor

Parameters:

  • goal_waypoint – [in] The waypoint that the AGV needs to reach.

  • goal_orientation – [in] The orientation that the AGV needs to end with.

Goal(std::size_t goal_waypoint, std::optional<rmf_traffic::Time> minimum_time, std::optional<double> goal_orientation = std::nullopt)

Constructor

Parameters:

  • goal_waypoint – [in] The waypoint that the AGV needs to reach.

  • minimum_time – [in] The AGV must be on the goal waypoint at or after this time for the plan to be successful. This is useful if a robot needs to wait at a location, but you want it to give way for other robots.

  • goal_orientation – [in] An optional goal orientation that the AGV needs to end with.

Goal &waypoint(std::size_t goal_waypoint)

Set the goal waypoint.

std::size_t waypoint() const

Get the goal waypoint.

Goal &orientation(double goal_orientation)

Set the goal orientation.

Goal &any_orientation()

Accept any orientation for the final goal.

const double *orientation() const

Get a reference to the goal orientation (or a nullptr if any orientation is acceptable).

Goal &minimum_time(std::optional<rmf_traffic::Time> value)

Set the minimum time for the goal. Pass in a nullopt to remove the minimum time.

std::optional<rmf_traffic::Time> minimum_time() const

Get the minimum time for the goal (or a nullopt is there is no minimum time).

class Options

The Options class contains planning parameters that can change between each planning attempt.

Public Functions

Options(rmf_utils::clone_ptr<RouteValidator> validator, Duration min_hold_time = DefaultMinHoldingTime, std::shared_ptr<const std::atomic_bool> interrupt_flag = nullptr, std::optional<double> maximum_cost_estimate = std::nullopt, std::optional<std::size_t> saturation_limit = std::nullopt)

Constructor

Parameters:

  • validator – [in] A validator to check the validity of the planner’s branching options.

  • min_hold_time – [in] The minimum amount of time that the planner should spend waiting at holding points. Smaller values will make the plan more aggressive about being time-optimal, but the plan may take longer to produce. Larger values will add some latency to the execution of the plan as the robot may wait at a holding point longer than necessary, but the plan will usually be generated more quickly.

  • interrupt_flag – [in] A pointer to a flag that should be used to interrupt the planner if it has been running for too long. If the planner should run indefinitely, then pass in a nullptr.

  • maximum_cost_estimate – [in] A cap on how high the best possible solution’s cost can be. If the cost of the best possible solution ever exceeds this value, then the planner will interrupt itself, no matter what the state of the interrupt_flag is. Set this to nullopt to specify that there should not be a cap.

  • saturation_limit – [in] A cap on how many search nodes the planner is allowed to produce.

Options(rmf_utils::clone_ptr<RouteValidator> validator, Duration min_hold_time, std::function<bool()> interrupter, std::optional<double> maximum_cost_estimate = std::nullopt, std::optional<std::size_t> saturation_limit = std::nullopt)

Constructor

Parameters:

  • validator – [in] A validator to check the validity of the planner’s branching options.

  • validator – [in] A validator to check the validity of the planner’s branching options.

  • min_hold_time – [in] The minimum amount of time that the planner should spend waiting at holding points. Smaller values will make the plan more aggressive about being time-optimal, but the plan may take longer to produce. Larger values will add some latency to the execution of the plan as the robot may wait at a holding point longer than necessary, but the plan will usually be generated more quickly.

  • interrupter – [in] A function that can determine whether the planning should be interrupted. This is an alternative to using the interrupt_flag.

  • maximum_cost_estimate – [in] A cap on how high the best possible solution’s cost can be. If the cost of the best possible solution ever exceeds this value, then the planner will interrupt itself, no matter what the state of the interrupt_flag is. Set this to nullopt to specify that there should not be a cap.

  • saturation_limit – [in] A cap on how many search nodes the planner is allowed to produce.

Options &validator(rmf_utils::clone_ptr<RouteValidator> v)

Set the route validator.

const rmf_utils::clone_ptr<RouteValidator> &validator() const

Get the route validator.

Options &minimum_holding_time(Duration holding_time)

Set the minimum amount of time to spend waiting at holding points.

Duration minimum_holding_time() const

Get the minimum amount of time to spend waiting at holding points.

Options &interrupter(std::function<bool()> cb)

Set an interrupter callback that can indicate to the planner if it should stop trying to plan.

Warning

Using this function will replace anything that was given to interrupt_flag, and it will nullify the interrupt_flag() field.

const std::function<bool()> &interrupter() const

Get the interrupter that will be used in these Options.

Options &interrupt_flag(std::shared_ptr<const std::atomic_bool> flag)

Set an interrupt flag to stop this planner if it has run for too long.

Warning

Using this function will replace anything that was given to interrupter.

const std::shared_ptr<const std::atomic_bool> &interrupt_flag() const

Get the interrupt flag that will stop this planner if it has run for too long.

Options &maximum_cost_estimate(std::optional<double> value)

Set the maximum cost estimate that the planner should allow. If the cost estimate of the best possible plan that the planner could produce ever exceeds this value, the planner will pause itself (but this will not be considered an interruption).

std::optional<double> maximum_cost_estimate() const

Get the maximum cost estimate that the planner will allow.

Options &saturation_limit(std::optional<std::size_t> value)

Set the saturation limit for the planner. If the planner produces more search nodes than this limit, then the planning will stop.

std::optional<std::size_t> saturation_limit() const

Get the saturation limit.

Options &dependency_window(std::optional<Duration> value)

Set the dependency window for generated plans. Any potential conflicts with the generated plan that happen within this window will be added as dependencies to the plan waypoints. If set to a nullopt, the plan will not have any dependencies.

std::optional<Duration> dependency_window() const

Dependency window for the planner.

Options &dependency_resolution(Duration value)

Set the dependency resolution for generated plans. To check for dependencies, the planner will step the generated routes back in time by this value and check for conflicts. Detected conflicts get added to the list of dependencies. This backstepping happens until dependency_window is reached. If dependency_window is nullopt, this value will not be used.

Duration dependency_resolution() const

Get the dependency resolution for generated plans.

Public Static Attributes

static constexpr Duration DefaultMinHoldingTime = std::chrono::seconds(1)
class QuickestPath

The quickest path is a simplified version of the Planner::Result class that can return faster but takes fewer factors into consideration. It does not account for translational acceleration/deceleration of the agent, nor does it consider rotational velocity or rotational acceleration. The traffic schedule is also not accounted for in this option.

QuickestPath will not provide detailed timing information, instead only giving a simplified estimate of the cost and the graph indices for the quickest path.

This quickest path does consider speed limits on lanes when estimating the quickest path. Besides the graph topology, lane closures, event duration estimates, and speed limits, no other planner Configuration or Option values are considered for the quickest path.

This information is used internally as a heuristic for the full planner. The results of using this feature are cached and shared with the full planner, and vice-versa.

Public Functions

double cost() const

The cost of following this path.

const std::vector<std::size_t> &path() const

The quickest path that was found.

class Result

Public Functions

bool success() const

True if a plan was found and this Result can be dereferenced to obtain a plan.

bool disconnected() const

True if there is no feasible path that connects the start to the goal. In this case, a plan will never be found.

operator bool() const

Implicitly cast the result to a boolean. It will return true if a plan was found, otherwise it will return false.

const Plan *operator->() const

If the Result was successful, drill into the plan.

const Plan &operator*() const &

If the Result was successful, get a reference to the plan.

Plan &&operator*() &&

If the Result was successful, move the plan.

const Plan &&operator*() const &&

If the Result was successful, get a reference to the plan.

Result replan(const Start &new_start) const

Replan to the same goal from a new start location using the same options as before.

Parameters:

new_start – [in] The starting conditions that should be used for replanning.

Result replan(const Start &new_start, Options new_options) const

Replan to the same goal from a new start location using a new set of options.

Parameters:

  • new_start – [in] The starting conditions that should be used for replanning.

  • new_options – [in] The options that should be used for replanning.

Result replan(const StartSet &new_starts) const

Replan to the same goal from a new set of start locations using the same options.

Parameters:

new_starts – [in] The set of starting conditions that should be used for replanning.

Result replan(const StartSet &new_starts, Options new_options) const

Replan to the same goal from a new set of start locations using a new set of options.

Parameters:

  • new_starts – [in] The set of starting conditions that should be used for replanning.

  • new_options – [in] The options that should be used for replanning.

Result setup(const Start &new_start) const

Set up a new planning job to the same goal, but do not start iterating.

See also

replan(const Start&)

Result setup(const Start &new_start, Options new_options) const

Set up a new planning job to the same goal, but do not start iterating.

See also

replan(const Start&, Options)

Result setup(const StartSet &new_starts) const

Set up a new planning job to the same goal, but do not start iterating.

See also

replan(const StartSet&)

Result setup(const StartSet &new_starts, Options new_options) const

Set up a new planning job to the same goal, but do not start iterating.

See also

replan(const StartSet&, Options)

bool resume()

Resume planning if the planner was paused.

Returns:

true if a plan has been found, false otherwise.

bool resume(std::shared_ptr<const std::atomic_bool> interrupt_flag)

Resume planning if the planner was paused.

Parameters:

interrupt_flag – [in] A new interrupt flag to listen to while planning.

Returns:

true if a plan has been found, false otherwise.

Options &options()

Get a mutable reference to the options that will be used by this planning task.

const Options &options() const

Get the options that will be used by this planning task.

Result &options(Options new_options)

Change the options to be used by this planning task.

std::optional<double> cost_estimate() const

Get the best cost estimate of the current state of this planner result. This is the value of the lowest f(n)=g(n)+h(n) in the planner’s queue. If the node queue of this planner result is empty, this will return a nullopt.

double initial_cost_estimate() const

Get the cost estimate that was initially computed for this plan. If no valid starts were provided, then this will return infinity.

std::optional<double> ideal_cost() const

Get the cost that this plan would have if there is no traffic. If the plan is impossible (e.g. the starts are disconnected from the goal) this will return a nullopt.

const std::vector<Start> &get_starts() const

Get the start conditions that were given for this planning task.

const Goal &get_goal() const

Get the goal for this planning task.

const Configuration &get_configuration() const

If this Plan is valid, this will return the Planner::Configuration that was used to produce it.

If replan() is called, this Planner::Configuration will be used to produce the new Plan.

bool interrupted() const

This will return true if the planning failed because it was interrupted. Otherwise it will return false.

bool saturated() const

This will return true if the planner has reached its saturation limit.

std::vector<schedule::ParticipantId> blockers() const

This is a list of schedule Participants who blocked the planning effort. Blockers do not necessarily prevent a solution from being found, but they do prevent the optimal solution from being available.

class Start

Describe the starting conditions of a plan.

Public Functions

Start(Time initial_time, std::size_t initial_waypoint, double initial_orientation, std::optional<Eigen::Vector2d> location = std::nullopt, std::optional<std::size_t> initial_lane = std::nullopt)

Constructor

Parameters:

  • inital_time – [in] The starting time of the plan.

  • initial_waypoint – [in] The waypoint index that the plan will begin from.

  • initial_orientation – [in] The orientation that the AGV will start with.

  • initial_location – [in] Optional field to specify if the robot is not starting directly on the initial_waypoint location. When planning from this initial_location to the initial_waypoint the planner will assume it has an unconstrained lane.

  • initial_lane – [in] Optional field to specify if the robot is starting in a certain lane. This will only be used if an initial_location is specified.

Start &time(Time initial_time)

Set the starting time of a plan.

Time time() const

Get the starting time.

Start &waypoint(std::size_t initial_waypoint)

Set the starting waypoint of a plan.

std::size_t waypoint() const

Get the starting waypoint.

Start &orientation(double initial_orientation)

Set the starting orientation of a plan.

double orientation() const

Get the starting orientation.

const std::optional<Eigen::Vector2d> &location() const

Get the starting location, if one was specified.

Start &location(std::optional<Eigen::Vector2d> initial_location)

Set the starting location, or remove it by using std::nullopt.

const std::optional<std::size_t> &lane() const

Get the starting lane, if one was specified.

Start &lane(std::optional<std::size_t> initial_lane)

Set the starting lane, or remove it by using std::nullopt.