Provide lift properties

Signed-off-by: Michael X. Grey <[email protected]>

rmf_traffic/include/rmf_traffic/agv/Graph.hpp

@@ -27,6 +27,7 @@
 
 #include <vector>
 #include <unordered_map>
+#include <unordered_set>
 #include <optional>
 
 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<Implementation> _pimpl;
+  };
+  using LiftPropertiesPtr = std::shared_ptr<const LiftProperties>;
 
+  /// 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<std::string> 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<LiftPropertiesPtr>& known_lifts() const;
+
+  /// Mutable reference to the known lifts
+  std::unordered_set<LiftPropertiesPtr>& known_lifts();
+
   class Implementation;
 private:
   rmf_utils::impl_ptr<Implementation> _pimpl;

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>(
+      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<std::string> in_lift;
+  LiftPropertiesPtr in_lift = nullptr;
 
   template<typename... Args>
   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<std::string> 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<LiftPropertiesPtr>&
+{
+  return _pimpl->lifts;
+}
+
+//==============================================================================
+auto Graph::known_lifts() -> std::unordered_set<LiftPropertiesPtr>&
+{
+  return _pimpl->lifts;
+}
+
 //==============================================================================
 auto Graph::lane_from(std::size_t from_wp, std::size_t to_wp) const
 -> const Lane*

rmf_traffic/src/rmf_traffic/agv/internal_Graph.hpp

@@ -31,6 +31,7 @@ class Graph::Implementation
   std::vector<Waypoint> waypoints;
   std::vector<Lane> lanes;
   std::unordered_map<std::string, std::size_t> keys;
+  std::unordered_set<LiftPropertiesPtr> lifts;
 
   // A map from a waypoint index to the set of lanes that can exit from it
   std::vector<std::vector<std::size_t>> lanes_from;