diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index b1f5886e8b4..7dc8d5ddd6d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/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; + auto getLaneDistances( + const traffic_simulator_msgs::msg::LaneletPose & from, + const traffic_simulator_msgs::msg::LaneletPose & to) const + -> std::optional>; + auto getLateralDistance( const traffic_simulator_msgs::msg::LaneletPose & from, const traffic_simulator_msgs::msg::LaneletPose & to) const -> std::optional; diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 7c2b6be091c..4d221de3adc 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/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 { - 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 + const traffic_simulator_msgs::msg::LaneletPose & to) const + -> std::optional> { 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>( + {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 distances = {std::numeric_limits::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::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::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::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 +{ + if (const auto distances = getLaneDistances(from, to)) { + return distances->first; + } else { + return std::nullopt; + } } auto HdMapUtils::toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceCondition.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceCondition.yaml index 70e69ae3cb9..0525787f780 100644 --- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceCondition.yaml +++ b/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