Program Listing for File Planner.hpp

↰ Return to documentation for file (/tmp/ws/src/rmf_traffic/rmf_traffic/include/rmf_traffic/agv/Planner.hpp)

/*
 * Copyright (C) 2019 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#ifndef RMF_TRAFFIC__AGV__PLANNER_HPP
#define RMF_TRAFFIC__AGV__PLANNER_HPP

#include <rmf_traffic/Trajectory.hpp>

#include <rmf_traffic/agv/Graph.hpp>
#include <rmf_traffic/agv/LaneClosure.hpp>
#include <rmf_traffic/agv/Interpolate.hpp>
#include <rmf_traffic/agv/RouteValidator.hpp>
#include <rmf_traffic/agv/VehicleTraits.hpp>

#include <rmf_utils/optional.hpp>

#include <atomic>

namespace rmf_traffic {
namespace agv {

//==============================================================================
// Forward declaration
class Plan;

//==============================================================================
class Planner
{
public:

  class Configuration
  {
  public:

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

    Configuration& graph(Graph graph);

    Graph& graph();

    const Graph& graph() const;

    Configuration& vehicle_traits(VehicleTraits traits);

    VehicleTraits& vehicle_traits();

    const VehicleTraits& vehicle_traits() const;

    Configuration& interpolation(Interpolate::Options interpolate);

    Interpolate::Options& interpolation();

    const Interpolate::Options& interpolation() const;

    Configuration& lane_closures(LaneClosure closures);

    LaneClosure& lane_closures();

    const LaneClosure& lane_closures() const;

    Configuration& traversal_cost_per_meter(double value);

    double traversal_cost_per_meter() const;

    // TODO(MXG): Add a field to specify whether multi-start planning problems
    // should choose the plan that takes the least amount of time (according to
    // plan duration) or the plan that finishes the earliest (according to the
    // wall clock).

    class Implementation;
  private:
    rmf_utils::impl_ptr<Implementation> _pimpl;
  };

  class Options
  {
  public:

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

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

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

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

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

    Options& minimum_holding_time(Duration holding_time);

    Duration minimum_holding_time() const;

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

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

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

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

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

    std::optional<double> maximum_cost_estimate() const;

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

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

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

    std::optional<Duration> dependency_window() const;

    Options& dependency_resolution(Duration value);

    Duration dependency_resolution() const;

    class Implementation;
  private:
    rmf_utils::impl_ptr<Implementation> _pimpl;
  };

  class Start
  {
  public:

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

    Start& time(Time initial_time);

    Time time() const;

    Start& waypoint(std::size_t initial_waypoint);

    std::size_t waypoint() const;

    Start& orientation(double initial_orientation);

    double orientation() const;

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

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

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

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

    class Implementation;
  private:
    rmf_utils::impl_ptr<Implementation> _pimpl;
  };

  class Goal
  {
  public:

    // TODO(MXG): Consider using optional for the goal orientation

    // TODO(MXG): Consider supporting goals that have multiple acceptable goal
    // orientations.

    Goal(std::size_t goal_waypoint);

    Goal(std::size_t goal_waypoint, double goal_orientation);

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

    Goal& waypoint(std::size_t goal_waypoint);

    std::size_t waypoint() const;

    Goal& orientation(double goal_orientation);

    Goal& any_orientation();

    const double* orientation() const;

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

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

    class Implementation;
  private:
    rmf_utils::impl_ptr<Implementation> _pimpl;
  };

  class Result;

  Planner(
    Configuration config,
    Options default_options);

  const Configuration& get_configuration() const;

  Planner& set_default_options(Options default_options);

  Options& get_default_options();

  const Options& get_default_options() const;

  using StartSet = std::vector<Start>;

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

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


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

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

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

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

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

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

  class QuickestPath
  {
  public:
    double cost() const;

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

    class Implementation;
  private:
    QuickestPath();
    rmf_utils::impl_ptr<Implementation> _pimpl;
  };

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

  class Implementation;
  class Debug;
private:
  rmf_utils::impl_ptr<Implementation> _pimpl;
};

//==============================================================================
class Planner::Result
{
public:

  bool success() const;

  bool disconnected() const;

  operator bool() const;

  const Plan* operator->() const;

  const Plan& operator*() const&;

  Plan&& operator*() &&;

  const Plan&& operator*() const&&;

  Result replan(const Start& new_start) const;

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

  Result replan(const StartSet& new_starts) const;

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

  Result setup(const Start& new_start) const;

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

  Result setup(const StartSet& new_starts) const;

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

  bool resume();

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

  Options& options();

  const Options& options() const;

  Result& options(Options new_options);

  std::optional<double> cost_estimate() const;

  [[deprecated("Use ideal_cost() instead")]]
  double initial_cost_estimate() const;

  std::optional<double> ideal_cost() const;

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

  const Goal& get_goal() const;

  const Configuration& get_configuration() const;

  bool interrupted() const;

  bool saturated() const;

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

  class Implementation;
private:
  Result();
  rmf_utils::impl_ptr<Implementation> _pimpl;
};

//==============================================================================
class Plan
{
public:

  using Start = Planner::Start;
  using StartSet = Planner::StartSet;
  using Goal = Planner::Goal;
  using Options = Planner::Options;
  using Configuration = Planner::Configuration;
  using Result = Planner::Result;

  struct Checkpoint
  {
    RouteId route_id;
    CheckpointId checkpoint_id;
  };
  using Checkpoints = std::vector<Checkpoint>;

  struct Progress
  {
    std::size_t graph_index;
    Checkpoints checkpoints;
    rmf_traffic::Time time;
  };

  class Waypoint
  {
  public:

    const Eigen::Vector3d& position() const;

    rmf_traffic::Time time() const;

    std::optional<std::size_t> graph_index() const;

    const std::vector<std::size_t>& approach_lanes() const;

    const std::vector<Progress>& progress_checkpoints() const;

    const Checkpoints& arrival_checkpoints() const;

    [[deprecated("Use arrival_checkpoints().back().route_id instead")]]
    std::size_t itinerary_index() const;

    [[deprecated("Use arrival_checkpoints().back().checkpoint_id instead")]]
    std::size_t trajectory_index() const;

    const Graph::Lane::Event* event() const;

    const Dependencies& dependencies() const;

    class Implementation;
  private:
    Waypoint();
    rmf_utils::impl_ptr<Implementation> _pimpl;
  };

  const std::vector<Route>& get_itinerary() const;

  const std::vector<Waypoint>& get_waypoints() const;

  const Start& get_start() const;

  double get_cost() const;

  // TODO(MXG): Create a feature that can diff two plans to produce the most
  // efficient schedule::Database::Change to get from the original plan to the
  // new plan.

  class Implementation;
private:
  Plan();
  rmf_utils::impl_ptr<Implementation> _pimpl;
};


std::vector<Plan::Start> compute_plan_starts(
  const rmf_traffic::agv::Graph& graph,
  const std::string& map_name,
  const Eigen::Vector3d pose,
  const rmf_traffic::Time start_time,
  const double max_merge_waypoint_distance = 0.1,
  const double max_merge_lane_distance = 1.0,
  const double min_lane_length = 1e-8);

} // namespace agv
} // namespace rmf_traffic

#endif // RMF_TRAFFIC__AGV__PLANNER_HPP