.. _program_listing_file__tmp_ws_src_rmf_traffic_rmf_traffic_include_rmf_traffic_agv_Graph.hpp: Program Listing for File Graph.hpp ================================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/rmf_traffic/rmf_traffic/include/rmf_traffic/agv/Graph.hpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp /* * 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 #include #include #include #include #include #include namespace rmf_traffic { namespace agv { //============================================================================== class Graph { public: 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); 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; class Implementation; private: Waypoint(); rmf_utils::impl_ptr _pimpl; }; class OrientationConstraint { public: static rmf_utils::clone_ptr make(std::vector acceptable_orientations); enum class Direction { Forward, Backward, }; static rmf_utils::clone_ptr 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 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 _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 _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 _pimpl; }; class Wait { public: Wait(Duration value); Duration duration() const; Wait& duration(Duration value); class Implementation; private: rmf_utils::impl_ptr _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; class Event { public: virtual Duration duration() const = 0; template DerivedExecutor& execute(DerivedExecutor& executor) const { return static_cast(execute( static_cast(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 = nullptr, rmf_utils::clone_ptr orientation = nullptr); Node( std::size_t waypoint_index, rmf_utils::clone_ptr orientation); std::size_t waypoint_index() const; const Event* event() const; Node& event(rmf_utils::clone_ptr 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 _pimpl; }; class Properties { public: Properties(); std::optional speed_limit() const; Properties& speed_limit(std::optional value); class Implementation; private: rmf_utils::impl_ptr _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 _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& 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& lanes_from(std::size_t wp_index) const; const std::vector& 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; class Implementation; private: rmf_utils::impl_ptr _pimpl; }; } // namespace agv } // namespace rmf_traffic #endif // RMF_TRAFFIC__AGV__GRAPH_HPP