From a57591de2c88b6dbba00b2e2a594ea793183024e Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 24 Nov 2023 12:17:44 +0000 Subject: [PATCH] Allow an envelope to be specified for is_in_lift Signed-off-by: Michael X. Grey --- rmf_traffic/include/rmf_traffic/agv/Graph.hpp | 5 +++-- rmf_traffic/src/rmf_traffic/agv/Graph.cpp | 8 +++++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp index 35ae2120..4274ee6f 100644 --- 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 index 136613a5..8005c14d 100644 --- 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; }