Program Listing for File Graph.hpp

↰ Return to documentation for file (/tmp/ws/src/rmf_traffic/rmf_traffic/include/rmf_traffic/agv/Graph.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__GRAPH_HPP
#define RMF_TRAFFIC__AGV__GRAPH_HPP

#include <rmf_traffic/Time.hpp>

#include <Eigen/Geometry>

#include <rmf_utils/impl_ptr.hpp>
#include <rmf_utils/clone_ptr.hpp>

#include <vector>
#include <unordered_map>
#include <unordered_set>
#include <optional>
#include <iostream>

namespace rmf_traffic {
namespace agv {

//==============================================================================
class Graph
{
public:

  class LiftProperties
  {
  public:
    const std::string& name() const;

    Eigen::Vector2d location() const;

    double orientation() const;

    Eigen::Vector2d dimensions() const;

    bool is_in_lift(Eigen::Vector2d position, double envelope = 0.0) const;

    LiftProperties(
      std::string name,
      Eigen::Vector2d location,
      double orientations,
      Eigen::Vector2d dimensions);

    class Implementation;
  private:
    rmf_utils::impl_ptr<Implementation> _pimpl;
  };
  using LiftPropertiesPtr = std::shared_ptr<LiftProperties>;

  class DoorProperties
  {
  public:
    const std::string& name() const;

    Eigen::Vector2d start() const;

    Eigen::Vector2d end() const;

    const std::string& map() const;

    bool intersects(
      Eigen::Vector2d p0,
      Eigen::Vector2d p1,
      double envelope = 0.0) const;

    DoorProperties(
      std::string name,
      Eigen::Vector2d start,
      Eigen::Vector2d end,
      std::string map);

    class Implementation;
  private:
    rmf_utils::impl_ptr<Implementation> _pimpl;
  };
  using DoorPropertiesPtr = std::shared_ptr<DoorProperties>;

  class Waypoint
  {
  public:
    const std::string& get_map_name() const;

    Waypoint& set_map_name(std::string map);

    const Eigen::Vector2d& get_location() const;

    Waypoint& set_location(Eigen::Vector2d location);

    bool is_holding_point() const;

    Waypoint& set_holding_point(bool _is_holding_point);

    bool is_passthrough_point() const;

    Waypoint& set_passthrough_point(bool _is_passthrough);

    bool is_parking_spot() const;

    Waypoint& set_parking_spot(bool _is_parking_spot);

    bool is_charger() const;

    Waypoint& set_charger(bool _is_charger);

    LiftPropertiesPtr in_lift() const;

    Waypoint& set_in_lift(LiftPropertiesPtr properties);

    std::size_t index() const;

    const std::string* name() const;

    std::string name_or_index(
      const std::string& name_format = "%s",
      const std::string& index_format = "#%d") const;

    const std::string& in_mutex_group() const;

    Waypoint& set_in_mutex_group(std::string group_name);

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

    Waypoint& set_merge_radius(std::optional<double> valeu);

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

  class OrientationConstraint
  {
  public:

    static rmf_utils::clone_ptr<OrientationConstraint>
    make(std::vector<double> acceptable_orientations);

    enum class Direction
    {
      Forward,
      Backward,
    };

    static rmf_utils::clone_ptr<OrientationConstraint>
    make(Direction direction, const Eigen::Vector2d& forward_vector);

    virtual bool apply(
      Eigen::Vector3d& position,
      const Eigen::Vector2d& course_vector) const = 0;

    virtual rmf_utils::clone_ptr<OrientationConstraint> clone() const = 0;

    // Default destructor.
    virtual ~OrientationConstraint() = default;
  };

  class Lane
  {
  public:

    class Door
    {
    public:

      Door(std::string name, Duration duration);

      const std::string& name() const;

      Door& name(std::string name);

      Duration duration() const;

      Door& duration(Duration duration);

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

    class DoorOpen : public Door { public: using Door::Door; };
    class DoorClose : public Door { public: using Door::Door; };

    class LiftSession
    {
    public:

      LiftSession(
        std::string lift_name,
        std::string floor_name,
        Duration duration);

      const std::string& lift_name() const;

      LiftSession& lift_name(std::string name);

      const std::string& floor_name() const;

      LiftSession& floor_name(std::string name);

      Duration duration() const;

      LiftSession& duration(Duration duration);

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

    class LiftSessionBegin : public LiftSession
    {
    public:
      using LiftSession::LiftSession;
    };

    class LiftMove : public LiftSession
    {
    public:
      using LiftSession::LiftSession;
    };

    class LiftDoorOpen : public LiftSession
    {
    public:
      using LiftSession::LiftSession;
    };

    class LiftSessionEnd : public LiftSession
    {
    public:
      using LiftSession::LiftSession;
    };

    class Dock
    {
    public:

      Dock(
        std::string dock_name,
        Duration duration);

      const std::string& dock_name() const;

      Dock& dock_name(std::string name);

      Duration duration() const;

      Dock& duration(Duration d);

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

    class Wait
    {
    public:

      Wait(Duration value);

      Duration duration() const;

      Wait& duration(Duration value);

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

    class Executor
    {
    public:

      using DoorOpen = Lane::DoorOpen;
      using DoorClose = Lane::DoorClose;
      using LiftSessionBegin = Lane::LiftSessionBegin;
      using LiftDoorOpen = Lane::LiftDoorOpen;
      using LiftSessionEnd = Lane::LiftSessionEnd;
      using LiftMove = Lane::LiftMove;
      using Dock = Lane::Dock;
      using Wait = Lane::Wait;

      virtual void execute(const DoorOpen& open) = 0;
      virtual void execute(const DoorClose& close) = 0;
      virtual void execute(const LiftSessionBegin& begin) = 0;
      virtual void execute(const LiftDoorOpen& open) = 0;
      virtual void execute(const LiftSessionEnd& end) = 0;
      virtual void execute(const LiftMove& move) = 0;
      virtual void execute(const Dock& dock) = 0;
      virtual void execute(const Wait& wait) = 0;

      virtual ~Executor() = default;
    };

    class Event;
    using EventPtr = rmf_utils::clone_ptr<Event>;

    class Event
    {
    public:

      virtual Duration duration() const = 0;

      template<typename DerivedExecutor>
      DerivedExecutor& execute(DerivedExecutor& executor) const
      {
        return static_cast<DerivedExecutor&>(
          execute(static_cast<Executor&>(executor)));
      }

      virtual Executor& execute(Executor& executor) const = 0;

      virtual EventPtr clone() const = 0;

      virtual ~Event() = default;

      static EventPtr make(DoorOpen open);
      static EventPtr make(DoorClose close);
      static EventPtr make(LiftSessionBegin open);
      static EventPtr make(LiftSessionEnd close);
      static EventPtr make(LiftMove move);
      static EventPtr make(LiftDoorOpen open);
      static EventPtr make(Dock dock);
      static EventPtr make(Wait wait);
    };


    class Node
    {
    public:

      Node(
        std::size_t waypoint_index,
        rmf_utils::clone_ptr<Event> event = nullptr,
        rmf_utils::clone_ptr<OrientationConstraint> orientation = nullptr);

      Node(
        std::size_t waypoint_index,
        rmf_utils::clone_ptr<OrientationConstraint> orientation);

      std::size_t waypoint_index() const;

      const Event* event() const;

      Node& event(rmf_utils::clone_ptr<Event> new_event);

      const OrientationConstraint* orientation_constraint() const;

      class Implementation;
    private:
      // We make the Lane a friend so it can copy and move the Nodes
      friend class Lane;

      // These constructors are private to make sure a user can't modify the
      // waypoint_index of a Node by copying or moving
      Node(const Node&) = default;
      Node(Node&&) = default;
      Node& operator=(const Node&) = default;
      Node& operator=(Node&&) = default;
      rmf_utils::impl_ptr<Implementation> _pimpl;
    };


    class Properties
    {
    public:

      Properties();

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

      Properties& speed_limit(std::optional<double> value);

      const std::string& in_mutex_group() const;

      Properties& set_in_mutex_group(std::string group_name);

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

    Node& entry();

    const Node& entry() const;

    Node& exit();

    const Node& exit() const;

    Properties& properties();

    const Properties& properties() const;

    std::size_t index() const;

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

  Graph();

  Waypoint& add_waypoint(
    std::string map_name,
    Eigen::Vector2d location);

  Waypoint& get_waypoint(std::size_t index);

  const Waypoint& get_waypoint(std::size_t index) const;

  Waypoint* find_waypoint(const std::string& key);

  const Waypoint* find_waypoint(const std::string& key) const;

  bool add_key(const std::string& key, std::size_t wp_index);

  bool remove_key(const std::string& key);

  bool set_key(const std::string& key, std::size_t wp_index);

  const std::unordered_map<std::string, std::size_t>& keys() const;

  std::size_t num_waypoints() const;

  Lane& add_lane(
    const Lane::Node& entry,
    const Lane::Node& exit,
    Lane::Properties properties = Lane::Properties());

  Lane& get_lane(std::size_t index);

  const Lane& get_lane(std::size_t index) const;

  std::size_t num_lanes() const;

  const std::vector<std::size_t>& lanes_from(std::size_t wp_index) const;

  const std::vector<std::size_t>& lanes_into(std::size_t wp_index) const;

  Lane* lane_from(std::size_t from_wp, std::size_t to_wp);

  const Lane* lane_from(std::size_t from_wp, std::size_t to_wp) const;

  LiftPropertiesPtr set_known_lift(LiftProperties lift);

  std::vector<LiftPropertiesPtr> all_known_lifts() const;

  LiftPropertiesPtr find_known_lift(const std::string& name) const;

  DoorPropertiesPtr set_known_door(DoorProperties door);

  std::vector<DoorPropertiesPtr> all_known_doors() const;

  DoorPropertiesPtr find_known_door(const std::string& name) const;

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

} // namespace agv
} // namespace rmf_traffic

#endif // RMF_TRAFFIC__AGV__GRAPH_HPP