fix(simulator_core, api): change relativeLaneletPose to laneletDistan…

…ce, move it to distance utils and use globally

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -246,10 +246,10 @@ class SimulatorCore
     {
       const bool allow_lane_change = (routing_algorithm == RoutingAlgorithm::value_type::shortest);
       if (prerequisite(from_pose_or_entity_name, to_pose_or_entity_name)) {
-        if (
-          const auto pose = core->relativeLaneletPose(
-            from_pose_or_entity_name, to_pose_or_entity_name, allow_lane_change)) {
-          return pose->getLaneletPose().offset;
+        if (const auto lanelet_distance = core->laneletDistance(
+              from_pose_or_entity_name, to_pose_or_entity_name, allow_lane_change);
+            lanelet_distance.lateral) {
+          return lanelet_distance.lateral.value();
         }
       }
       return std::numeric_limits<double>::quiet_NaN();
@@ -262,10 +262,10 @@ class SimulatorCore
     {
       const bool allow_lane_change = (routing_algorithm == RoutingAlgorithm::value_type::shortest);
       if (prerequisite(from_pose_or_entity_name, to_pose_or_entity_name)) {
-        if (
-          const auto pose = core->relativeLaneletPose(
-            from_pose_or_entity_name, to_pose_or_entity_name, allow_lane_change)) {
-          return pose->getLaneletPose().s;
+        if (const auto lanelet_distance = core->laneletDistance(
+              from_pose_or_entity_name, to_pose_or_entity_name, allow_lane_change);
+            lanelet_distance.longitudinal) {
+          return lanelet_distance.longitudinal.value();
         }
       }
       return std::numeric_limits<double>::quiet_NaN();
@@ -278,10 +278,10 @@ class SimulatorCore
     {
       const bool allow_lane_change = (routing_algorithm == RoutingAlgorithm::value_type::shortest);
       if (prerequisite(from_pose_or_entity_name, to_pose_or_entity_name)) {
-        if (
-          const auto pose = core->boundingBoxRelativeLaneletPose(
-            from_pose_or_entity_name, to_pose_or_entity_name, allow_lane_change)) {
-          return pose->getLaneletPose().offset;
+        if (const auto lanelet_distance = core->boundingBoxLaneletDistance(
+              from_pose_or_entity_name, to_pose_or_entity_name, allow_lane_change);
+            lanelet_distance.lateral) {
+          return lanelet_distance.lateral.value();
         }
       }
       return std::numeric_limits<double>::quiet_NaN();
@@ -294,10 +294,10 @@ class SimulatorCore
     {
       const bool allow_lane_change = (routing_algorithm == RoutingAlgorithm::value_type::shortest);
       if (prerequisite(from_pose_or_entity_name, to_pose_or_entity_name)) {
-        if (
-          const auto pose = core->boundingBoxRelativeLaneletPose(
-            from_pose_or_entity_name, to_pose_or_entity_name, allow_lane_change)) {
-          return pose->getLaneletPose().s;
+        if (const auto lanelet_distance = core->boundingBoxLaneletDistance(
+              from_pose_or_entity_name, to_pose_or_entity_name, allow_lane_change);
+            lanelet_distance.longitudinal) {
+          return lanelet_distance.longitudinal.value();
         }
       }
       return std::numeric_limits<double>::quiet_NaN();

simulation/traffic_simulator/include/traffic_simulator/api/api.hpp

@@ -25,6 +25,7 @@
 #include <traffic_simulator/simulation_clock/simulation_clock.hpp>
 #include <traffic_simulator/traffic/traffic_controller.hpp>
 #include <traffic_simulator/traffic/traffic_source.hpp>
+#include <traffic_simulator/utils/distance.hpp>
 #include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
 
 namespace traffic_simulator
@@ -259,25 +260,25 @@ class API
     const std::string & from_entity_name, const std::string & to_entity_name)
     -> std::optional<geometry_msgs::msg::Pose>;
 
-  auto relativeLaneletPose(
+  auto laneletDistance(
     const std::string & from_entity_name, const std::string & to_entity_name,
-    const bool allow_lane_change) -> std::optional<CanonicalizedLaneletPose>;
+    const bool allow_lane_change) -> LaneletDistance;
 
-  auto relativeLaneletPose(
+  auto laneletDistance(
     const std::string & from_entity_name, const LaneletPose & to_lanelet_pose,
-    const bool allow_lane_change) -> std::optional<CanonicalizedLaneletPose>;
+    const bool allow_lane_change) -> LaneletDistance;
 
-  auto relativeLaneletPose(
+  auto laneletDistance(
     const LaneletPose & from_lanelet_pose, const std::string & to_entity_name,
-    const bool allow_lane_change) -> std::optional<CanonicalizedLaneletPose>;
+    const bool allow_lane_change) -> LaneletDistance;
 
-  auto boundingBoxRelativeLaneletPose(
+  auto boundingBoxLaneletDistance(
     const std::string & from_entity_name, const std::string & to_entity_name,
-    const bool allow_lane_change) -> std::optional<CanonicalizedLaneletPose>;
+    const bool allow_lane_change) -> LaneletDistance;
 
-  auto boundingBoxRelativeLaneletPose(
+  auto boundingBoxLaneletDistance(
     const std::string & from_entity_name, const LaneletPose & to_lanelet_pose,
-    const bool allow_lane_change) -> std::optional<CanonicalizedLaneletPose>;
+    const bool allow_lane_change) -> LaneletDistance;
 
   // traffics, lanelet
   auto getHdmapUtils() const -> const std::shared_ptr<hdmap_utils::HdMapUtils> &;

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

@@ -22,7 +22,13 @@ namespace traffic_simulator
 {
 inline namespace distance
 {
-// Lateral
+// Lanelet
+struct LaneletDistance
+{
+  std::optional<double> longitudinal{std::nullopt};
+  std::optional<double> lateral{std::nullopt};
+};
+
 auto lateralDistance(
   const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to,
   bool allow_lane_change, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
@@ -33,12 +39,16 @@ auto lateralDistance(
   double matching_distance, bool allow_lane_change,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;
 
-// Longitudinal
 auto longitudinalDistance(
   const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to,
   bool include_adjacent_lanelet, bool include_opposite_direction, bool allow_lane_change,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;
 
+auto laneletDistance(
+  const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to,
+  const bool allow_lane_change, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+  -> LaneletDistance;
+
 // BoundingBox
 auto boundingBoxDistance(
   const geometry_msgs::msg::Pose & from,
@@ -61,6 +71,13 @@ auto boundingBoxLaneLongitudinalDistance(
   bool include_opposite_direction, bool allow_lane_change,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;
 
+auto boundingBoxLaneletDistance(
+  const CanonicalizedLaneletPose & from,
+  const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box,
+  const CanonicalizedLaneletPose & to,
+  const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const bool allow_lane_change,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> LaneletDistance;
+
 // Bounds
 auto distanceToLaneBound(
   const geometry_msgs::msg::Pose & map_pose,

simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp

@@ -86,19 +86,6 @@ auto boundingBoxRelativePose(
   const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box)
   -> std::optional<geometry_msgs::msg::Pose>;
 
-// Relative LaneletPose
-auto relativeLaneletPose(
-  const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to,
-  const bool allow_lane_change, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
-  -> CanonicalizedLaneletPose;
-
-auto boundingBoxRelativeLaneletPose(
-  const CanonicalizedLaneletPose & from,
-  const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box,
-  const CanonicalizedLaneletPose & to,
-  const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const bool allow_lane_change,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> CanonicalizedLaneletPose;
-
 // Others
 auto isInLanelet(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id lanelet_id,

simulation/traffic_simulator/src/api/api.cpp

@@ -17,7 +17,6 @@
 #include <geometry/intersection/collision.hpp>
 #include <geometry/quaternion/quaternion_to_euler.hpp>
 #include <traffic_simulator/api/api.hpp>
-#include <traffic_simulator/utils/distance.hpp>
 #include <traffic_simulator/utils/pose.hpp>
 
 namespace traffic_simulator
@@ -448,82 +447,82 @@ auto API::relativePose(
   return pose::relativePose(from_map_pose, to_entity->getMapPose());
 }
 
-auto API::relativeLaneletPose(
+auto API::laneletDistance(
   const std::string & from_entity_name, const std::string & to_entity_name,
-  const bool allow_lane_change) -> std::optional<CanonicalizedLaneletPose>
+  const bool allow_lane_change) -> LaneletDistance
 {
   const auto from_entity = getEntity(from_entity_name);
   const auto to_entity = getEntity(to_entity_name);
   if (from_entity_name != to_entity_name) {
     if (from_entity->isInLanelet() && to_entity->isInLanelet()) {
-      return pose::relativeLaneletPose(
+      return distance::laneletDistance(
         from_entity->getCanonicalizedLaneletPose().value(),
         to_entity->getCanonicalizedLaneletPose().value(), allow_lane_change, getHdmapUtils());
     }
   }
-  return std::nullopt;
+  return LaneletDistance();
 }
 
-auto API::relativeLaneletPose(
+auto API::laneletDistance(
   const std::string & from_entity_name, const LaneletPose & to_lanelet_pose,
-  const bool allow_lane_change) -> std::optional<CanonicalizedLaneletPose>
+  const bool allow_lane_change) -> LaneletDistance
 {
   const auto canonicalized_lanelet_pose = pose::canonicalize(to_lanelet_pose, getHdmapUtils());
   const auto from_entity = getEntity(from_entity_name);
   if (from_entity->isInLanelet()) {
-    return pose::relativeLaneletPose(
+    return distance::laneletDistance(
       from_entity->getCanonicalizedLaneletPose().value(), canonicalized_lanelet_pose,
       allow_lane_change, getHdmapUtils());
   } else {
-    return std::nullopt;
+    return LaneletDistance();
   }
 }
 
-auto API::relativeLaneletPose(
+auto API::laneletDistance(
   const LaneletPose & from_lanelet_pose, const std::string & to_entity_name,
-  const bool allow_lane_change) -> std::optional<CanonicalizedLaneletPose>
+  const bool allow_lane_change) -> LaneletDistance
 {
   const auto canonicalized_lanelet_pose = pose::canonicalize(from_lanelet_pose, getHdmapUtils());
   const auto to_entity = getEntity(to_entity_name);
   if (to_entity->isInLanelet()) {
-    return pose::relativeLaneletPose(
+    return distance::laneletDistance(
       canonicalized_lanelet_pose, to_entity->getCanonicalizedLaneletPose().value(),
       allow_lane_change, getHdmapUtils());
   } else {
-    return std::nullopt;
+    return LaneletDistance();
   }
 }
 
-auto API::boundingBoxRelativeLaneletPose(
+auto API::boundingBoxLaneletDistance(
   const std::string & from_entity_name, const std::string & to_entity_name,
-  const bool allow_lane_change) -> std::optional<CanonicalizedLaneletPose>
+  const bool allow_lane_change) -> LaneletDistance
 {
   const auto from_entity = getEntity(from_entity_name);
   const auto to_entity = getEntity(to_entity_name);
   if (from_entity_name != to_entity_name) {
     if (from_entity->isInLanelet() && to_entity->isInLanelet()) {
-      return pose::boundingBoxRelativeLaneletPose(
+      return distance::boundingBoxLaneletDistance(
         from_entity->getCanonicalizedLaneletPose().value(), from_entity->getBoundingBox(),
         to_entity->getCanonicalizedLaneletPose().value(), to_entity->getBoundingBox(),
         allow_lane_change, getHdmapUtils());
     }
   }
-  return std::nullopt;
+  return LaneletDistance();
 }
 
-auto API::boundingBoxRelativeLaneletPose(
+auto API::boundingBoxLaneletDistance(
   const std::string & from_entity_name, const LaneletPose & to_lanelet_pose,
-  const bool allow_lane_change) -> std::optional<CanonicalizedLaneletPose>
+  const bool allow_lane_change) -> LaneletDistance
 {
   const auto canonicalized_lanelet_pose = pose::canonicalize(to_lanelet_pose, getHdmapUtils());
   const auto from_entity = getEntity(from_entity_name);
   if (from_entity->isInLanelet()) {
-    return pose::boundingBoxRelativeLaneletPose(
+    return distance::boundingBoxLaneletDistance(
       from_entity->getCanonicalizedLaneletPose().value(), from_entity->getBoundingBox(),
       canonicalized_lanelet_pose, traffic_simulator_msgs::msg::BoundingBox(), allow_lane_change,
       getHdmapUtils());
   } else {
-    return std::nullopt;
+    return LaneletDistance();
   }
 }
 

simulation/traffic_simulator/src/utils/distance.cpp

@@ -123,6 +123,29 @@ auto longitudinalDistance(
   }
 }
 
+auto laneletDistance(
+  const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to,
+  const bool allow_lane_change, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+  -> LaneletDistance
+{
+  constexpr bool include_adjacent_lanelet{false};
+  constexpr bool include_opposite_direction{true};
+
+  LaneletDistance lanelet_distance;
+  // here the s and offset are intentionally assigned independently, even if
+  // it is not possible to calculate one of them - it happens that one is sufficient
+  if (
+    const auto longitudinal_distance = longitudinalDistance(
+      from, to, include_adjacent_lanelet, include_opposite_direction, allow_lane_change,
+      hdmap_utils_ptr)) {
+    lanelet_distance.longitudinal = longitudinal_distance.value();
+  }
+  if (const auto lateral_distance = lateralDistance(from, to, allow_lane_change, hdmap_utils_ptr)) {
+    lanelet_distance.lateral = lateral_distance.value();
+  }
+  return lanelet_distance;
+}
+
 auto boundingBoxDistance(
   const geometry_msgs::msg::Pose & from,
   const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box,
@@ -187,6 +210,33 @@ auto boundingBoxLaneLongitudinalDistance(
   return std::nullopt;
 }
 
+auto boundingBoxLaneletDistance(
+  const CanonicalizedLaneletPose & from,
+  const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box,
+  const CanonicalizedLaneletPose & to,
+  const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const bool allow_lane_change,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> LaneletDistance
+{
+  constexpr bool include_adjacent_lanelet{false};
+  constexpr bool include_opposite_direction{true};
+
+  LaneletDistance lanelet_distance;
+  // here the s and offset are intentionally assigned independently, even if
+  // it is not possible to calculate one of them - it happens that one is sufficient
+  if (
+    const auto longitudinal_bounding_box_distance = boundingBoxLaneLongitudinalDistance(
+      from, from_bounding_box, to, to_bounding_box, include_adjacent_lanelet,
+      include_opposite_direction, allow_lane_change, hdmap_utils_ptr)) {
+    lanelet_distance.longitudinal = longitudinal_bounding_box_distance.value();
+  }
+  if (
+    const auto lateral_bounding_box_distance = boundingBoxLaneLateralDistance(
+      from, from_bounding_box, to, to_bounding_box, allow_lane_change, hdmap_utils_ptr)) {
+    lanelet_distance.lateral = lateral_bounding_box_distance.value();
+  }
+  return lanelet_distance;
+}
+
 auto distanceToLeftLaneBound(
   const geometry_msgs::msg::Pose & map_pose,
   const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id,