Allow an envelope to be specified for is_in_lift

Signed-off-by: Michael X. Grey <[email protected]>
open-rmf · Nov 24, 2023 · a57591d · a57591d
1 parent 9ce2aa0
commit a57591d
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 5 deletions.
diff --git a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp
@@ -57,8 +57,9 @@ class Graph
     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;
+    /// is inside the lift. The envelope will expand the footprint of the lift
+    /// that is used in the calculation.
+    bool is_in_lift(Eigen::Vector2d position, double envelope = 0.0) const;
 
     /// Constructor
     LiftProperties(

diff --git a/rmf_traffic/src/rmf_traffic/agv/Graph.cpp b/rmf_traffic/src/rmf_traffic/agv/Graph.cpp
@@ -61,15 +61,17 @@ Eigen::Vector2d Graph::LiftProperties::dimensions() const
 }
 
 //==============================================================================
-bool Graph::LiftProperties::is_in_lift(Eigen::Vector2d position) const
+bool Graph::LiftProperties::is_in_lift(
+  Eigen::Vector2d position,
+  double envelope) const
 {
   Eigen::Vector2d p_local = _pimpl->tf_inv * position;
   for (int i = 0; i < 2; ++i)
   {
-    if (p_local[i] < -_pimpl->half_dimensions[i])
+    if (p_local[i]  < -_pimpl->half_dimensions[i] - envelope)
       return false;
 
-    if (_pimpl->half_dimensions[i] < p_local[i])
+    if (_pimpl->half_dimensions[i] + envelope < p_local[i])
       return false;
   }