Fix/relative distance on looped map

simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp

@@ -168,6 +168,11 @@ class HdMapUtils
 
   auto getLaneletPolygon(const lanelet::Id) const -> std::vector<geometry_msgs::msg::Point>;
 
+  auto getLaneDistances(
+    const traffic_simulator_msgs::msg::LaneletPose & from,
+    const traffic_simulator_msgs::msg::LaneletPose & to) const
+    -> std::optional<std::pair<double, double>>;
+
   auto getLateralDistance(
     const traffic_simulator_msgs::msg::LaneletPose & from,
     const traffic_simulator_msgs::msg::LaneletPose & to) const -> std::optional<double>;

simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp

@@ -1437,39 +1437,105 @@ auto HdMapUtils::getLateralDistance(
   const traffic_simulator_msgs::msg::LaneletPose & from,
   const traffic_simulator_msgs::msg::LaneletPose & to) const -> std::optional<double>
 {
-  const auto route = getRoute(from.lanelet_id, to.lanelet_id);
-  if (route.empty()) {
+  if (const auto distances = getLaneDistances(from, to)) {
+    return distances->second;
+  } else {
     return std::nullopt;
   }
-  return to.offset - from.offset;
 }
 
-auto HdMapUtils::getLongitudinalDistance(
+auto HdMapUtils::getLaneDistances(
   const traffic_simulator_msgs::msg::LaneletPose & from,
-  const traffic_simulator_msgs::msg::LaneletPose & to) const -> std::optional<double>
+  const traffic_simulator_msgs::msg::LaneletPose & to) const
+  -> std::optional<std::pair<double, double>>
 {
   if (from.lanelet_id == to.lanelet_id) {
     if (from.s > to.s) {
       return std::nullopt;
     } else {
-      return to.s - from.s;
+      return std::make_optional<std::pair<double, double>>(
+        {to.s - from.s, to.offset - from.offset});
     }
-  }
-  const auto route = getRoute(from.lanelet_id, to.lanelet_id);
-  if (route.empty()) {
-    return std::nullopt;
-  }
-  double distance = 0;
-  for (const auto lanelet_id : route) {
-    if (lanelet_id == from.lanelet_id) {
-      distance = getLaneletLength(from.lanelet_id) - from.s;
-    } else if (lanelet_id == to.lanelet_id) {
-      distance = distance + to.s;
+  } else {
+    // list up side lanelets of "from" lanelet
+    lanelet::Ids from_side_lanelets;
+    traffic_simulator_msgs::msg::EntityType type;
+    type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE;
+    from_side_lanelets.push_back(from.lanelet_id);
+    if (auto left_lanelets = getLeftLaneletIds(from.lanelet_id, type); not left_lanelets.empty()) {
+      from_side_lanelets.insert(
+        from_side_lanelets.end(), left_lanelets.begin(), left_lanelets.end());
+    }
+    if (auto right_lanelets = getRightLaneletIds(from.lanelet_id, type);
+        not right_lanelets.empty()) {
+      from_side_lanelets.insert(
+        from_side_lanelets.end(), right_lanelets.begin(), right_lanelets.end());
+    }
+
+    auto get_route_length = [&](const auto & route, const auto & from, const auto & to) -> double {
+      double distance = 0.;
+      for (const auto lanelet_id : route) {
+        if (lanelet_id == from.lanelet_id) {
+          distance = getLaneletLength(from.lanelet_id) - from.s;
+        } else if (lanelet_id == to.lanelet_id) {
+          distance = distance + to.s;
+        } else {
+          distance = distance + getLaneletLength(lanelet_id);
+        }
+      }
+      return distance;
+    };
+
+    // calculate distance between each side lanelet and "to" lanelet and get the shortest distance
+    // Note: The reason why we calculate distances from not only "from" lanelet but also
+    //       side lanelets of "from" lanelet, is to avoid getting wrong distance in looping lanelet map.
+    //       In looping lanelet map, the routing-graph can route the path without lane-changes
+    //       by going around the map.
+    //       In this case, the distance between "from" and "to" will be wrongly long.
+    std::pair<double, double> distances = {std::numeric_limits<double>::max(), 0.};
+    for (const auto & from_side_lanelet : from_side_lanelets) {
+      // The matching distance of 10.0[m] was arbitrarily determined by @Hans_Robo, and the function may not work well on roads wider than this.
+      traffic_simulator_msgs::msg::LaneletPose from_lanelet_pose_on_side_lanelet =
+        *toLaneletPose(toMapPose(from).pose, from_side_lanelet, 10.0);
+
+      double route_distance = [&]() -> double {
+        if (from_lanelet_pose_on_side_lanelet.lanelet_id == to.lanelet_id) {
+          if (from_lanelet_pose_on_side_lanelet.s > to.s) {
+            return std::numeric_limits<double>::max();
+          } else {
+            return to.s - from_lanelet_pose_on_side_lanelet.s;
+          }
+        } else if (const auto route = getRoute(from_side_lanelet, to.lanelet_id);
+                   not route.empty()) {
+          return get_route_length(route, from_lanelet_pose_on_side_lanelet, to);
+        } else {
+          return std::numeric_limits<double>::max();
+        }
+      }();
+
+      if (route_distance < distances.first) {
+        distances.first = route_distance;
+        distances.second = to.offset - from_lanelet_pose_on_side_lanelet.offset;
+      }
+    }
+
+    if (distances.first != std::numeric_limits<double>::max()) {
+      return std::make_optional(distances);
     } else {
-      distance = distance + getLaneletLength(lanelet_id);
+      return std::nullopt;
     }
   }
-  return distance;
+}
+
+auto HdMapUtils::getLongitudinalDistance(
+  const traffic_simulator_msgs::msg::LaneletPose & from,
+  const traffic_simulator_msgs::msg::LaneletPose & to) const -> std::optional<double>
+{
+  if (const auto distances = getLaneDistances(from, to)) {
+    return distances->first;
+  } else {
+    return std::nullopt;
+  }
 }
 
 auto HdMapUtils::toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin

test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceCondition.yaml

@@ -28,6 +28,16 @@ OpenSCENARIO:
         CatalogReference: *SAMPLE_VEHICLE
       - name: car_4
         CatalogReference: *SAMPLE_VEHICLE
+      - name: car_5
+        CatalogReference: *SAMPLE_VEHICLE
+      - name: car_6
+        CatalogReference: *SAMPLE_VEHICLE
+      - name: car_7
+        CatalogReference: *SAMPLE_VEHICLE
+      - name: car_8
+        CatalogReference: *SAMPLE_VEHICLE
+      - name: car_9
+        CatalogReference: *SAMPLE_VEHICLE
   Storyboard:
     Init:
       Actions:
@@ -89,6 +99,81 @@ OpenSCENARIO:
                       << : *CAR_2_LANE_POSITION
                       s: 0
               - LongitudinalAction: *SPEED_ACTION_ZERO
+          - entityRef: car_5
+            PrivateAction:
+              - TeleportAction:
+                  Position:
+                    LanePosition:
+                      roadId: ""
+                      laneId: 34507
+                      s: 10
+                      offset: 0.0
+                      Orientation:
+                        type: relative
+                        h: 0
+                        p: 0
+                        r: 0
+              - LongitudinalAction: *SPEED_ACTION_ZERO
+          - entityRef: car_6
+            PrivateAction:
+              - TeleportAction:
+                  Position:
+                    LanePosition:
+                      roadId: ""
+                      laneId: 34468
+                      s: 20
+                      offset: 0.0
+                      Orientation:
+                        type: relative
+                        h: 0
+                        p: 0
+                        r: 0
+              - LongitudinalAction: *SPEED_ACTION_ZERO
+          - entityRef: car_7
+            PrivateAction:
+              - TeleportAction:
+                  Position:
+                    LanePosition:
+                      roadId: ""
+                      laneId: 34468
+                      s: 30
+                      offset: 0.0
+                      Orientation:
+                        type: relative
+                        h: 0
+                        p: 0
+                        r: 0
+              - LongitudinalAction: *SPEED_ACTION_ZERO
+          - entityRef: car_8
+            PrivateAction:
+              - TeleportAction:
+                  Position:
+                    LanePosition:
+                      roadId: ""
+                      laneId: 34507
+                      s: 40
+                      offset: 0.0
+                      Orientation:
+                        type: relative
+                        h: 0
+                        p: 0
+                        r: 0
+              - LongitudinalAction: *SPEED_ACTION_ZERO
+          - entityRef: car_9
+            PrivateAction:
+              - TeleportAction:
+                  Position:
+                    LanePosition:
+                      roadId: ""
+                      laneId: 34606
+                      s: 10
+                      offset: 0.0
+                      Orientation:
+                        type: relative
+                        h: 0
+                        p: 0
+                        r: 0
+              - LongitudinalAction: *SPEED_ACTION_ZERO
     Story:
       - name: story
         Act:
@@ -181,6 +266,70 @@ OpenSCENARIO:
                                         relativeDistanceType: lateral
                                         rule: equalTo
                                         value: 0.5
+                                - name: "check longitudinal distance without looped route (correct : 10m, incorrect: over 200m)"
+                                  delay: 0
+                                  conditionEdge: none
+                                  ByEntityCondition:
+                                    TriggeringEntities:
+                                      triggeringEntitiesRule: all
+                                      EntityRef:
+                                        - entityRef: car_5
+                                    EntityCondition:
+                                      RelativeDistanceCondition:
+                                        coordinateSystem: lane
+                                        entityRef: car_6
+                                        freespace: false # True: distance is measured between closest bounding box points. False: reference point distance is used.
+                                        relativeDistanceType: longitudinal
+                                        rule: lessThan
+                                        value: 12.0
+                                - name: "check lateral distance without looped route (correct : -3m, incorrect: 0m)"
+                                  delay: 0
+                                  conditionEdge: none
+                                  ByEntityCondition:
+                                    TriggeringEntities:
+                                      triggeringEntitiesRule: all
+                                      EntityRef:
+                                        - entityRef: car_5
+                                    EntityCondition:
+                                      RelativeDistanceCondition:
+                                        coordinateSystem: lane
+                                        entityRef: car_6
+                                        freespace: false # True: distance is measured between closest bounding box points. False: reference point distance is used.
+                                        relativeDistanceType: lateral
+                                        rule: lessThan
+                                        value: -1.0
+                                - name: "check longitudinal distance that needs lane change and avoids looped measurement (correct : 44m, incorrect: over 150[m])"
+                                  delay: 0
+                                  conditionEdge: none
+                                  ByEntityCondition:
+                                    TriggeringEntities:
+                                      triggeringEntitiesRule: all
+                                      EntityRef:
+                                        - entityRef: car_7
+                                    EntityCondition:
+                                      RelativeDistanceCondition:
+                                        coordinateSystem: lane
+                                        entityRef: car_6
+                                        freespace: false # True: distance is measured between closest bounding box points. False: reference point distance is used.
+                                        relativeDistanceType: longitudinal
+                                        rule: lessThan
+                                        value: 100
+                                - name: ""
+                                  delay: 0
+                                  conditionEdge: none
+                                  ByEntityCondition:
+                                    TriggeringEntities:
+                                      triggeringEntitiesRule: all
+                                      EntityRef:
+                                        - entityRef: car_8
+                                    EntityCondition:
+                                      RelativeDistanceCondition:
+                                        coordinateSystem: lane
+                                        entityRef: car_9
+                                        freespace: false # True: distance is measured between closest bounding box points. False: reference point distance is used.
+                                        relativeDistanceType: longitudinal
+                                        rule: lessThan
+                                        value: 50
                                 - name: ""
                                   delay: 0
                                   conditionEdge: none