RJD-1509 Optimize number of distance::longitudinalDistance calls

Author: gmajrobotec

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -331,26 +331,21 @@ auto ActionNode::getDistanceToTargetEntity(
       target_lanelet_pose) {
     const auto & from_lanelet_pose = canonicalized_entity_status->getCanonicalizedLaneletPose();
     const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox();
-    if (const auto bounding_box_distance =
-          traffic_simulator::distance::boundingBoxLaneLongitudinalDistance(
+    if (const auto bounding_box_distances =
+          traffic_simulator::distance::laneLongitudinalDistances(
             *from_lanelet_pose, from_bounding_box, *target_lanelet_pose, target_bounding_box,
             include_adjacent_lanelet, include_opposite_direction, routing_configuration,
             hdmap_utils);
-        !bounding_box_distance || bounding_box_distance.value() < 0.0) {
-      return std::nullopt;
-    } else if (const auto position_distance = traffic_simulator::distance::longitudinalDistance(
-                 *from_lanelet_pose, *target_lanelet_pose, include_adjacent_lanelet,
-                 include_opposite_direction, routing_configuration, hdmap_utils);
-               !position_distance) {
+        !bounding_box_distances || bounding_box_distances.value().first < 0.0) {
       return std::nullopt;
     } else {
       const auto target_bounding_box_distance =
-        bounding_box_distance.value() + from_bounding_box.dimensions.x / 2.0;
+        bounding_box_distances.value().first + from_bounding_box.dimensions.x / 2.0;
 
       /// @note if the distance of the target entity to the spline is smaller than the width of the reference entity
       if (const auto target_to_spline_distance = traffic_simulator::distance::distanceToSpline(
             static_cast<geometry_msgs::msg::Pose>(*target_lanelet_pose), target_bounding_box,
-            spline, position_distance.value());
+            spline, bounding_box_distances.value().second);
           target_to_spline_distance <= from_bounding_box.dimensions.y / 2.0) {
         return target_bounding_box_distance;
       }

simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp

@@ -71,6 +71,15 @@ auto boundingBoxLaneLongitudinalDistance(
   const traffic_simulator::RoutingConfiguration & routing_configuration,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;
 
+auto laneLongitudinalDistances(
+  const CanonicalizedLaneletPose & from,
+  const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box,
+  const CanonicalizedLaneletPose & to,
+  const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, bool include_adjacent_lanelet,
+  bool include_opposite_direction,
+  const traffic_simulator::RoutingConfiguration & routing_configuration,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<std::pair<double,double>>;
+
 // Bounds
 auto distanceToLaneBound(
   const geometry_msgs::msg::Pose & map_pose,

simulation/traffic_simulator/src/utils/distance.cpp

@@ -197,6 +197,36 @@ auto boundingBoxLaneLongitudinalDistance(
   return std::nullopt;
 }
 
+auto laneLongitudinalDistances(
+  const CanonicalizedLaneletPose & from,
+  const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box,
+  const CanonicalizedLaneletPose & to,
+  const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, bool include_adjacent_lanelet,
+  bool include_opposite_direction,
+  const traffic_simulator::RoutingConfiguration & routing_configuration,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<std::pair<double,double>>
+{
+  if (const auto longitudinal_distance = longitudinalDistance(
+        from, to, include_adjacent_lanelet, include_opposite_direction, routing_configuration,
+        hdmap_utils_ptr);
+      longitudinal_distance) {
+    const auto from_bounding_box_distances =
+      math::geometry::getDistancesFromCenterToEdge(from_bounding_box);
+    const auto to_bounding_box_distances =
+      math::geometry::getDistancesFromCenterToEdge(to_bounding_box);
+    auto bounding_box_distance = 0.0;
+    if (longitudinal_distance.value() > 0.0) {
+      bounding_box_distance =
+        -std::abs(from_bounding_box_distances.front) - std::abs(to_bounding_box_distances.rear);
+    } else if (longitudinal_distance.value() < 0.0) {
+      bounding_box_distance =
+        +std::abs(from_bounding_box_distances.rear) + std::abs(to_bounding_box_distances.front);
+    }
+    return std::make_pair(longitudinal_distance.value() + bounding_box_distance, longitudinal_distance.value());
+  }
+  return std::nullopt;
+}
+
 // Bounds
 auto distanceToLeftLaneBound(
   const geometry_msgs::msg::Pose & map_pose,