diff --git a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp index 116a2fd2..842573c2 100644 --- a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp +++ b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp @@ -27,6 +27,7 @@ #include #include +#include #include namespace rmf_traffic { @@ -37,10 +38,45 @@ class Graph { public: - class Waypoint + /// Properties related to lifts (elevators) that exist in the graph + class LiftProperties { public: + /// Get the name of the lift. + const std::string& name() const; + + /// Get the (x, y) location of the lift in RMF canonical coordinates. + Eigen::Vector2d location() const; + + /// Get the orientation (in radians) of the lift in RMF canonical + /// coordinates. + double orientation() const; + + /// Get the dimensions of the lift, aligned with the lift's local (x, y) + /// coordinates. + Eigen::Vector2d dimensions() const; + + /// Get whether the specified position, given in RMF canonical coordinates, + /// is inside the lift. + bool is_in_lift(Eigen::Vector2d position) const; + + /// Constructor + LiftProperties( + std::string name, + Eigen::Vector2d location, + double orientations, + Eigen::Vector2d dimensions); + + private: + class Implementation; + rmf_utils::impl_ptr _pimpl; + }; + using LiftPropertiesPtr = std::shared_ptr; + /// Properties assigned to each waypoint (vertex) in the graph + class Waypoint + { + public: /// Get the name of the map that this Waypoint exists on. const std::string& get_map_name() const; @@ -88,12 +124,12 @@ class Graph Waypoint& set_charger(bool _is_charger); /// If this waypoint is inside the lift then this will return a pointer to - /// a string of the lift's name. Otherwise this will be a nullptr. - const std::string* in_lift() const; + /// the properties of the lift. Otherwise this will be a nullptr. + LiftPropertiesPtr in_lift() const; - /// Set the name of the lift that the waypoint is inside of, or provide a - /// nullopt if it is not inside a lift. - Waypoint& set_in_lift(std::optional lift_name); + /// Set the properties of the lift that the waypoint is inside of, or + /// provide a nullptr if it is not inside a lift. + Waypoint& set_in_lift(LiftPropertiesPtr properties); /// The index of this waypoint within the Graph. This cannot be changed /// after the waypoint is created. @@ -596,6 +632,15 @@ class Graph /// const-qualified lane_from() const Lane* lane_from(std::size_t from_wp, std::size_t to_wp) const; + /// Get the lifts that are known to exist for this graph. + /// NOTE: There is no mechanism to automatically keep known_lifts synced with + /// the actual lifts used by the vertices, so this must be kept in sync + /// manually. + const std::unordered_set& known_lifts() const; + + /// Mutable reference to the known lifts + std::unordered_set& known_lifts(); + class Implementation; private: rmf_utils::impl_ptr _pimpl; diff --git a/rmf_traffic/src/rmf_traffic/agv/Graph.cpp b/rmf_traffic/src/rmf_traffic/agv/Graph.cpp index 75b31a40..a97bb5bb 100644 --- a/rmf_traffic/src/rmf_traffic/agv/Graph.cpp +++ b/rmf_traffic/src/rmf_traffic/agv/Graph.cpp @@ -25,6 +25,84 @@ namespace rmf_traffic { namespace agv { +//============================================================================== +class Graph::LiftProperties::Implementation +{ +public: + std::string name; + Eigen::Vector2d location; + double orientation; + Eigen::Vector2d half_dimensions; + Eigen::Isometry2d tf_inv; +}; + +//============================================================================== +const std::string& Graph::LiftProperties::name() const +{ + return _pimpl->name; +} + +//============================================================================== +Eigen::Vector2d Graph::LiftProperties::location() const +{ + return _pimpl->location; +} + +//============================================================================== +double Graph::LiftProperties::orientation() const +{ + return _pimpl->orientation; +} + +//============================================================================== +Eigen::Vector2d Graph::LiftProperties::dimensions() const +{ + return 2.0 * _pimpl->half_dimensions; +} + +//============================================================================== +bool Graph::LiftProperties::is_in_lift(Eigen::Vector2d position) const +{ + Eigen::Vector2d p_local = _pimpl->tf_inv * position; + for (int i = 0; i < 2; ++i) + { + if (p_local[i] < -_pimpl->half_dimensions[i]) + return false; + + if (_pimpl->half_dimensions[i] < p_local[i]) + return false; + } + + return true; +} + +//============================================================================== +Eigen::Isometry2d make_lift_tf_inv(Eigen::Vector2d location, double orientation) +{ + Eigen::Isometry2d tf = Eigen::Isometry2d::Identity(); + tf.translate(location); + tf.rotate(orientation); + return tf.inverse(); +} + +//============================================================================== +Graph::LiftProperties::LiftProperties( + std::string name, + Eigen::Vector2d location, + double orientation, + Eigen::Vector2d dimensions) +: _pimpl(rmf_utils::make_impl( + Implementation { + std::move(name), + location, + orientation, + dimensions / 2.0, + make_lift_tf_inv(location, orientation) + })) +{ + // Do nothing +} + //============================================================================== class Graph::Waypoint::Implementation { @@ -46,7 +124,7 @@ class Graph::Waypoint::Implementation bool charger = false; - std::optional in_lift; + LiftPropertiesPtr in_lift = nullptr; template static Waypoint make(Args&& ... args) @@ -143,18 +221,15 @@ auto Graph::Waypoint::set_charger(bool _is_charger) -> Waypoint& } //============================================================================== -const std::string* Graph::Waypoint::in_lift() const +auto Graph::Waypoint::in_lift() const -> LiftPropertiesPtr { - if (_pimpl->in_lift.has_value()) - return &*_pimpl->in_lift; - return nullptr; + return _pimpl->in_lift; } //============================================================================== -auto Graph::Waypoint::set_in_lift( - std::optional lift_name) -> Waypoint& +auto Graph::Waypoint::set_in_lift(LiftPropertiesPtr lift) -> Waypoint& { - _pimpl->in_lift = lift_name; + _pimpl->in_lift = lift; return *this; } @@ -967,6 +1042,18 @@ auto Graph::lane_from(std::size_t from_wp, std::size_t to_wp) -> Lane* return &_pimpl->lanes.at(it->second); } +//============================================================================== +auto Graph::known_lifts() const -> const std::unordered_set& +{ + return _pimpl->lifts; +} + +//============================================================================== +auto Graph::known_lifts() -> std::unordered_set& +{ + return _pimpl->lifts; +} + //============================================================================== auto Graph::lane_from(std::size_t from_wp, std::size_t to_wp) const -> const Lane* diff --git a/rmf_traffic/src/rmf_traffic/agv/internal_Graph.hpp b/rmf_traffic/src/rmf_traffic/agv/internal_Graph.hpp index d1c2385f..fc321f41 100644 --- a/rmf_traffic/src/rmf_traffic/agv/internal_Graph.hpp +++ b/rmf_traffic/src/rmf_traffic/agv/internal_Graph.hpp @@ -31,6 +31,7 @@ class Graph::Implementation std::vector waypoints; std::vector lanes; std::unordered_map keys; + std::unordered_set lifts; // A map from a waypoint index to the set of lanes that can exit from it std::vector> lanes_from;