From 4624a239b64e532d86acadad8a5d27a60c02e317 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 5 Dec 2024 13:10:24 +0100 Subject: [PATCH 1/9] feat(traffic_simulator): expand lanelet_wrapper: add ::distance and necessary parts of ::route, ::lanelet_map, ::traffic_lights --- simulation/traffic_simulator/CMakeLists.txt | 3 + .../lanelet_wrapper/distance.hpp | 83 ++++++ .../lanelet_wrapper/lanelet_map.hpp | 30 ++ .../lanelet_wrapper/route.hpp | 34 +++ .../lanelet_wrapper/traffic_lights.hpp | 45 +++ .../src/lanelet_wrapper/distance.cpp | 266 ++++++++++++++++++ .../src/lanelet_wrapper/lanelet_map.cpp | 103 +++++++ .../src/lanelet_wrapper/route.cpp | 46 +++ .../src/lanelet_wrapper/traffic_lights.cpp | 100 +++++++ 9 files changed, 710 insertions(+) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp create mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp create mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_lights.hpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/route.cpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/traffic_lights.cpp diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index f55539afe1f..2e936014a7b 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -48,10 +48,13 @@ ament_auto_add_library(traffic_simulator SHARED src/helper/helper.cpp src/job/job.cpp src/job/job_list.cpp + src/lanelet_wrapper/distance.cpp src/lanelet_wrapper/lanelet_loader.cpp src/lanelet_wrapper/lanelet_map.cpp src/lanelet_wrapper/lanelet_wrapper.cpp src/lanelet_wrapper/pose.cpp + src/lanelet_wrapper/route.cpp + src/lanelet_wrapper/traffic_lights.cpp src/lanelet_wrapper/traffic_rules.cpp src/simulation_clock/simulation_clock.cpp src/traffic/traffic_controller.cpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp new file mode 100644 index 00000000000..04705251ee3 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp @@ -0,0 +1,83 @@ + +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_SIMULATOR__LANELET_WRAPPER_DISTANCE_HPP_ +#define TRAFFIC_SIMULATOR__LANELET_WRAPPER_DISTANCE_HPP_ + +#include + +#include +#include +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace distance +{ +using Point = geometry_msgs::msg::Point; +using Spline = math::geometry::CatmullRomSpline; +using SplineInterface = math::geometry::CatmullRomSplineInterface; +using LaneletPose = traffic_simulator_msgs::msg::LaneletPose; + +auto lateralDistance( + const LaneletPose & from, const LaneletPose & to, + const RoutingConfiguration & routing_configuration = RoutingConfiguration()) + -> std::optional; + +auto longitudinalDistance( + const LaneletPose & from, const LaneletPose & to, + const RoutingConfiguration & routing_configuration = RoutingConfiguration()) + -> std::optional; + +// StopLine +auto distanceToStopLine(const lanelet::Ids & route_lanelets, const SplineInterface & route_spline) + -> std::optional; + +auto distanceToStopLine( + const lanelet::Ids & route_lanelets, const std::vector & route_waypoints) + -> std::optional; + +auto distanceToStopLine(const std::vector & route_waypoints, const lanelet::Id stop_line_id) + -> std::optional; + +// TrafficLigthStopLine +auto distanceToTrafficLightStopLine( + const SplineInterface & route_spline, const lanelet::Id traffic_light_id) + -> std::optional; + +auto distanceToTrafficLightStopLine( + const std::vector & route_waypoints, const lanelet::Id traffic_light_id) + -> std::optional; + +auto distanceToTrafficLightStopLine( + const lanelet::Ids & route_lanelets, const SplineInterface & route_spline) + -> std::optional; + +auto distanceToTrafficLightStopLine( + const lanelet::Ids & route_lanelets, const std::vector & route_waypoints) + -> std::optional; + +// Crosswalk +auto distanceToCrosswalk(const std::vector & route_waypoints, const lanelet::Id crosswalk_id) + -> std::optional; + +auto distanceToCrosswalk(const SplineInterface & route_spline, const lanelet::Id crosswalk_id) + -> std::optional; +} // namespace distance +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_DISTANCE_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp index dcfcbcf2ca6..e5d31f30f86 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp @@ -26,6 +26,7 @@ namespace lanelet_wrapper { namespace lanelet_map { +// Basics auto isInLanelet(const lanelet::Id lanelet_id, const double lanelet_pose_s) -> bool; auto isInLanelet(const lanelet::Id lanelet_id, const Point point) -> bool; @@ -48,12 +49,14 @@ auto nearbyLaneletIds( const Point & point, const double distance_threshold, const bool include_crosswalk, const std::size_t search_count) -> lanelet::Ids; +// Center points auto centerPoints(const lanelet::Ids & lanelet_ids) -> std::vector; auto centerPoints(const lanelet::Id lanelet_id) -> std::vector; auto centerPointsSpline(const lanelet::Id lanelet_id) -> std::shared_ptr; +// Next lanelet auto nextLaneletIds( const lanelet::Id lanelet_id, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; @@ -70,6 +73,7 @@ auto nextLaneletIds( const lanelet::Ids & lanelet_ids, const std::string & turn_direction, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; +// Previous lanelet auto previousLaneletIds( const lanelet::Id lanelet_id, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; @@ -85,6 +89,32 @@ auto previousLaneletIds( auto previousLaneletIds( const lanelet::Ids & lanelet_ids, const std::string & turn_direction, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +// Bounds +auto leftBound(const lanelet::Id lanelet_id) -> std::vector; + +auto rightBound(const lanelet::Id lanelet_id) -> std::vector; + +// Polygons +auto laneletPolygon(const lanelet::Id lanelet_id) -> std::vector; + +auto stopLinePolygon(const lanelet::Id lanelet_id) -> std::vector; + +auto toPolygon(const lanelet::ConstLineString3d & line_string) -> std::vector; + +// Relations +auto rightOfWayLaneletIds(const lanelet::Ids & lanelet_ids) + -> std::unordered_map; + +auto rightOfWayLaneletIds(const lanelet::Id lanelet_id) -> lanelet::Ids; + +// Objects on path +auto trafficSignsOnPath(const lanelet::Ids & lanelet_ids) + -> std::vector>; + +auto stopLinesOnPath(const lanelet::Ids & lanelet_ids) -> lanelet::ConstLineStrings3d; + +auto stopLineIdsOnPath(const lanelet::Ids & lanelet_ids) -> lanelet::Ids; } // namespace lanelet_map } // namespace lanelet_wrapper } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp new file mode 100644 index 00000000000..b8ac35388c5 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp @@ -0,0 +1,34 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_SIMULATOR__LANELET_WRAPPER_ROUTE_HPP_ +#define TRAFFIC_SIMULATOR__LANELET_WRAPPER_ROUTE_HPP_ + +#include + +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace route +{ +auto route( + const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, + const RoutingConfiguration & routing_configuration = RoutingConfiguration()) -> lanelet::Ids; +} // namespace route +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_ROUTE_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_lights.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_lights.hpp new file mode 100644 index 00000000000..3eb5f603ef2 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_lights.hpp @@ -0,0 +1,45 @@ + + +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_SIMULATOR__LANELET_WRAPPER_TRAFFIC_LIGHTS_HPP_ +#define TRAFFIC_SIMULATOR__LANELET_WRAPPER_TRAFFIC_LIGHTS_HPP_ + +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace traffic_lights +{ +auto trafficLightStopLinesPoints(const lanelet::Id traffic_light_id) + -> std::vector>; + +auto trafficLightIdsOnPath(const lanelet::Ids & route_lanelets) -> lanelet::Ids; + +// used only by this namespace +auto toAutowareTrafficLights(const lanelet::Id traffic_light_id) + -> std::vector; + +auto autowareTrafficLightsOnPath(const lanelet::Ids & lanelet_ids) + -> std::vector; +} // namespace traffic_lights +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_TRAFFIC_LIGHTS_HPP_ diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp new file mode 100644 index 00000000000..631ca1bf734 --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp @@ -0,0 +1,266 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace distance +{ +auto lateralDistance( + const LaneletPose & from, const LaneletPose & to, + const RoutingConfiguration & routing_configuration) -> std::optional +{ + const auto route = route::route(from.lanelet_id, to.lanelet_id, routing_configuration); + if (route.empty()) { + return std::nullopt; + } else if (not routing_configuration.allow_lane_change) { + return to.offset - from.offset; + } else { + constexpr double matching_distance{10.0}; + double lateral_distance_by_lane_change = 0.0; + for (std::size_t i = 0; i < route.size() - 1; i++) { + auto next_lanelet_ids = + lanelet_map::nextLaneletIds(route[i], routing_configuration.routing_graph_type); + if (auto next_lanelet = std::find_if( + next_lanelet_ids.begin(), next_lanelet_ids.end(), + [&route, i](const lanelet::Id & id) { return id == route[i + 1]; }); + next_lanelet == next_lanelet_ids.end()) { + const auto current_lanelet_pose = helper::constructLaneletPose(route[i], 0.0, 0.0); + const auto next_lanelet_pose = helper::constructLaneletPose(route[i + 1], 0.0, 0.0); + if ( + const auto next_lanelet_origin_from_current_lanelet = pose::toLaneletPose( + pose::toMapPose(next_lanelet_pose).pose, route[i], matching_distance)) { + lateral_distance_by_lane_change += next_lanelet_origin_from_current_lanelet->offset; + } else if ( + const auto current_lanelet_origin_from_next_lanelet = pose::toLaneletPose( + pose::toMapPose(current_lanelet_pose).pose, route[i + 1], matching_distance)) { + lateral_distance_by_lane_change -= current_lanelet_origin_from_next_lanelet->offset; + } else { + /// @todo maybe an exception should be thrown here? since the route exists but is + /// incorrect? + return std::nullopt; + } + } + } + return to.offset - from.offset + lateral_distance_by_lane_change; + } +} + +auto longitudinalDistance( + const LaneletPose & from_pose, const LaneletPose & to_pose, + const RoutingConfiguration & routing_configuration) -> std::optional +{ + const auto is_lane_change_required = [&routing_configuration]( + const lanelet::Id current_lanelet, + const lanelet::Id next_lanelet) -> bool { + const auto next_lanelet_ids = + lanelet_map::nextLaneletIds(current_lanelet, routing_configuration.routing_graph_type); + return std::none_of( + next_lanelet_ids.cbegin(), next_lanelet_ids.cend(), + [next_lanelet](const lanelet::Id id) { return id == next_lanelet; }); + }; + + /// @sa https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/DistanceCalculation/ + if (const auto route = + route::route(from_pose.lanelet_id, to_pose.lanelet_id, routing_configuration); + not route.empty() || from_pose.lanelet_id == to_pose.lanelet_id) { + /// @note horizontal bar must intersect with the lanelet spline, so the distance was set arbitrarily to 10 meters. + static constexpr double matching_distance = 10.0; + double accumulated_distance = 0.0; + // accumulate lanelet lengths alongside the route, considering possible lane changes + for (std::size_t i = 1UL; i < route.size(); ++i) { + // if lane change is required, add the distance traveled during the lane change + // if lane change is not required, add the current lanelet length + if (is_lane_change_required(route[i - 1UL], route[i])) { + const auto current_lanelet_spline = lanelet_map::centerPointsSpline(route[i - 1UL]); + const auto next_lanelet_spline = lanelet_map::centerPointsSpline(route[i]); + + // first, lanelets are matched at the start (s = 0.0) of each lanelet; only if this fails, + // matching is performed at a larger s value, which should cover the rest of the cases. + static constexpr double lanelet_starting_s = 0.0; + const auto lanelet_min_middle_s = + std::min(current_lanelet_spline->getLength(), next_lanelet_spline->getLength()) * 0.5; + + const auto current_start_matching_s = current_lanelet_spline->getSValue( + next_lanelet_spline->getPose(lanelet_starting_s), matching_distance); + const auto next_start_matching_s = next_lanelet_spline->getSValue( + current_lanelet_spline->getPose(lanelet_starting_s), matching_distance); + const auto current_middle_matching_s = current_lanelet_spline->getSValue( + next_lanelet_spline->getPose(lanelet_min_middle_s), matching_distance); + const auto next_middle_matching_s = next_lanelet_spline->getSValue( + current_lanelet_spline->getPose(lanelet_min_middle_s), matching_distance); + + if (current_start_matching_s.has_value()) { + accumulated_distance += current_start_matching_s.value(); + } else if (next_start_matching_s.has_value()) { + accumulated_distance -= next_start_matching_s.value(); + } else if (current_middle_matching_s.has_value()) { + accumulated_distance += current_middle_matching_s.value() - lanelet_min_middle_s; + } else if (next_middle_matching_s.has_value()) { + accumulated_distance -= next_middle_matching_s.value() - lanelet_min_middle_s; + } else { + return std::nullopt; + } + } else { + accumulated_distance += lanelet_map::laneletLength(route[i - 1UL]); + } + } + // subtract the distance already traveled on the first lanelet: from_pose.s + // and add the distance that needs to be traveled on the last: to_pose.s. + if (const double distance = accumulated_distance - from_pose.s + to_pose.s; distance >= 0.0) { + return std::make_optional(distance); + } else { + return std::nullopt; + } + } else { + return std::nullopt; + } +} + +// StopLine +auto distanceToStopLine(const lanelet::Ids & route_lanelets, const SplineInterface & route_spline) + -> std::optional +{ + if (route_spline.getLength() <= 0) { + return std::nullopt; + } else { + std::vector collision_points; + // fill in collision_points using stop_lines + const auto stop_lines = lanelet_wrapper::lanelet_map::stopLinesOnPath({route_lanelets}); + for (const auto & stop_line : stop_lines) { + std::vector stop_line_points; + for (const auto & point : stop_line) { + stop_line_points.emplace_back( + geometry_msgs::build().x(point.x()).y(point.y()).z(point.z())); + } + if (const auto collision_point = route_spline.getCollisionPointIn2D(stop_line_points)) { + collision_points.push_back(collision_point.value()); + } + } + return collision_points.empty() + ? std::nullopt + : std::optional(*std::min_element(collision_points.begin(), collision_points.end())); + } +} + +auto distanceToStopLine( + const lanelet::Ids & route_lanelets, const std::vector & route_waypoints) + -> std::optional +{ + if (route_waypoints.empty()) { + return std::nullopt; + } else { + return distanceToStopLine(route_lanelets, Spline{route_waypoints}); + } +} + +auto distanceToStopLine(const std::vector & route_waypoints, const lanelet::Id stop_line_id) + -> std::optional +{ + if (route_waypoints.empty()) { + return std::nullopt; + } else { + const Spline route_spline{route_waypoints}; + return route_spline.getCollisionPointIn2D( + lanelet_wrapper::lanelet_map::stopLinePolygon(stop_line_id)); + } +} + +// TrafficLigthStopLine +auto distanceToTrafficLightStopLine( + const SplineInterface & route_spline, const lanelet::Id traffic_light_id) -> std::optional +{ + if (route_spline.getLength() <= 0) { + return std::nullopt; + } else { + const auto stop_lines = traffic_lights::trafficLightStopLinesPoints(traffic_light_id); + for (const auto & stop_line : stop_lines) { + if (const auto collision_point = route_spline.getCollisionPointIn2D(stop_line)) { + return collision_point; + } + } + return std::nullopt; + } +} + +auto distanceToTrafficLightStopLine( + const std::vector & route_waypoints, const lanelet::Id traffic_light_id) + -> std::optional +{ + if (route_waypoints.empty()) { + return std::nullopt; + } else { + return distanceToTrafficLightStopLine(Spline{route_waypoints}, traffic_light_id); + } +} + +auto distanceToTrafficLightStopLine( + const lanelet::Ids & route_lanelets, const SplineInterface & route_spline) + -> std::optional +{ + if (auto traffic_light_ids = + lanelet_wrapper::traffic_lights::trafficLightIdsOnPath(route_lanelets); + traffic_light_ids.empty()) { + return std::nullopt; + } else { + std::vector collision_points; + for (const auto traffic_light_id : traffic_light_ids) { + const auto collision_point = distanceToTrafficLightStopLine(route_spline, traffic_light_id); + if (collision_point) { + collision_points.push_back(collision_point.value()); + } + } + return collision_points.empty() + ? std::nullopt + : std::optional(*std::min_element(collision_points.begin(), collision_points.end())); + } +} + +auto distanceToTrafficLightStopLine( + const lanelet::Ids & route_lanelets, const std::vector & route_waypoints) + -> std::optional +{ + return distanceToTrafficLightStopLine(route_lanelets, Spline{route_waypoints}); +} + +// crosswalk +auto distanceToCrosswalk(const std::vector & route_waypoints, const lanelet::Id crosswalk_id) + -> std::optional +{ + if (route_waypoints.empty()) { + return std::nullopt; + } else { + const Spline route_spline(route_waypoints); + return route_spline.getCollisionPointIn2D( + lanelet_wrapper::lanelet_map::laneletPolygon(crosswalk_id)); + } +} + +auto distanceToCrosswalk(const SplineInterface & route_spline, const lanelet::Id crosswalk_id) + -> std::optional +{ + return route_spline.getCollisionPointIn2D( + lanelet_wrapper::lanelet_map::laneletPolygon(crosswalk_id), false); +} +} // namespace distance +} // namespace lanelet_wrapper +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 43d24d0cc6e..8fb87707aec 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -28,6 +28,7 @@ namespace lanelet_wrapper { namespace lanelet_map { +// Basics auto isInLanelet(const lanelet::Id lanelet_id, const double lanelet_pose_s) -> bool { return 0 <= lanelet_pose_s and lanelet_pose_s <= laneletLength(lanelet_id); @@ -97,6 +98,7 @@ auto nearbyLaneletIds( } } +// Center points auto centerPoints(const lanelet::Ids & lanelet_ids) -> std::vector { if (lanelet_ids.empty()) { @@ -124,6 +126,7 @@ auto centerPointsSpline(const lanelet::Id lanelet_id) -> std::shared_ptr lanelet_id, LaneletWrapper::map()); } +// Next lanelet auto nextLaneletIds(const lanelet::Id lanelet_id, const RoutingGraphType type) -> lanelet::Ids { lanelet::Ids next_lanelet_ids; @@ -171,6 +174,7 @@ auto nextLaneletIds( return lanelet::Ids(next_lanelet_ids_set.begin(), next_lanelet_ids_set.end()); } +// Previous lanelet auto previousLaneletIds(const lanelet::Id lanelet_id, const RoutingGraphType type) -> lanelet::Ids { lanelet::Ids previous_lanelet_ids; @@ -218,6 +222,105 @@ auto previousLaneletIds( } return lanelet::Ids(previous_lanelet_ids_set.begin(), previous_lanelet_ids_set.end()); } + +// Bounds +auto leftBound(const lanelet::Id lanelet_id) -> std::vector +{ + return toPolygon(LaneletWrapper::map()->laneletLayer.get(lanelet_id).leftBound()); +} + +auto rightBound(const lanelet::Id lanelet_id) -> std::vector +{ + return toPolygon(LaneletWrapper::map()->laneletLayer.get(lanelet_id).rightBound()); +} + +// Polygons +auto laneletPolygon(const lanelet::Id lanelet_id) -> std::vector +{ + std::vector points; + const auto & lanelet_polygon = LaneletWrapper::map()->laneletLayer.get(lanelet_id).polygon3d(); + for (const auto & point : lanelet_polygon) { + points.emplace_back(geometry_msgs::build().x(point.x()).y(point.y()).z(point.z())); + } + return points; +} + +auto stopLinePolygon(const lanelet::Id lanelet_id) -> std::vector +{ + /// @todo here you should probably add a verify if the passed lanelet_id is indeed a stop_line + return toPolygon(LaneletWrapper::map()->lineStringLayer.get(lanelet_id)); +} + +auto toPolygon(const lanelet::ConstLineString3d & line_string) -> std::vector +{ + std::vector points; + for (const auto & point : line_string) { + points.emplace_back(geometry_msgs::build().x(point.x()).y(point.y()).z(point.z())); + } + return points; +} + +// Relations +auto rightOfWayLaneletIds(const lanelet::Id lanelet_id) -> lanelet::Ids +{ + lanelet::Ids right_of_way_lanelets_ids; + const auto & right_of_ways = + LaneletWrapper::map()->laneletLayer.get(lanelet_id).regulatoryElementsAs(); + for (const auto & right_of_way : right_of_ways) { + for (const auto & right_of_way_lanelet : right_of_way->rightOfWayLanelets()) { + if (right_of_way_lanelet.id() != lanelet_id) { + right_of_way_lanelets_ids.push_back(right_of_way_lanelet.id()); + } + } + } + return right_of_way_lanelets_ids; +} + +auto rightOfWayLaneletIds(const lanelet::Ids & lanelet_ids) + -> std::unordered_map +{ + std::unordered_map right_of_way_lanelets_ids; + for (const auto & lanelet_id : lanelet_ids) { + right_of_way_lanelets_ids.emplace(lanelet_id, rightOfWayLaneletIds(lanelet_id)); + } + return right_of_way_lanelets_ids; +} + +// Objects on path +auto trafficSignsOnPath(const lanelet::Ids & lanelet_ids) + -> std::vector> +{ + std::vector> ret; + for (const auto & lanelet_id : lanelet_ids) { + const auto lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); + const auto traffic_signs = lanelet.regulatoryElementsAs(); + for (const auto & traffic_sign : traffic_signs) { + ret.emplace_back(traffic_sign); + } + } + return ret; +} + +auto stopLinesOnPath(const lanelet::Ids & lanelet_ids) -> lanelet::ConstLineStrings3d +{ + lanelet::ConstLineStrings3d stop_lines; + for (const auto & traffic_sign : lanelet_wrapper::lanelet_map::trafficSignsOnPath(lanelet_ids)) { + if (traffic_sign->type() == "stop_sign") { + const auto & ref_lines = traffic_sign->refLines(); + stop_lines.insert(stop_lines.end(), ref_lines.begin(), ref_lines.end()); + } + } + return stop_lines; +} + +auto stopLineIdsOnPath(const lanelet::Ids & lanelet_ids) -> lanelet::Ids +{ + lanelet::Ids stop_line_ids; + for (const auto & ret : stopLinesOnPath(lanelet_ids)) { + stop_line_ids.push_back(ret.id()); + } + return stop_line_ids; +} } // namespace lanelet_map } // namespace lanelet_wrapper } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp new file mode 100644 index 00000000000..2d812b62b40 --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp @@ -0,0 +1,46 @@ + +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace route +{ +auto route( + const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, + const RoutingConfiguration & routing_configuration) -> lanelet::Ids +{ + /// @todo improve architecture in terms of access to cache of the graph + return LaneletWrapper::routeCache(routing_configuration.routing_graph_type) + .getRoute( + from_lanelet_id, to_lanelet_id, LaneletWrapper::map(), routing_configuration, + LaneletWrapper::routingGraph(routing_configuration.routing_graph_type)); +} +} // namespace route +} // namespace lanelet_wrapper +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/traffic_lights.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/traffic_lights.cpp new file mode 100644 index 00000000000..a1f057b92ff --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/traffic_lights.cpp @@ -0,0 +1,100 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace traffic_lights +{ +auto trafficLightStopLinesPoints(const lanelet::Id traffic_light_id) + -> std::vector> +{ + std::vector> stop_lines; + for (const auto & traffic_light : toAutowareTrafficLights(traffic_light_id)) { + stop_lines.emplace_back(std::vector{}); + if (const auto & lanelet_stop_line = traffic_light->stopLine()) { + auto & current_stop_line = stop_lines.back(); + for (const auto & point : lanelet_stop_line.value()) { + current_stop_line.push_back( + geometry_msgs::build().x(point.x()).y(point.y()).z(point.z())); + } + } + } + return stop_lines; +} + +auto trafficLightIdsOnPath(const lanelet::Ids & route_lanelets) -> lanelet::Ids +{ + lanelet::Ids traffic_light_ids; + for (const auto & autoware_traffic_lights : autowareTrafficLightsOnPath(route_lanelets)) { + for (const auto & three_light_bulbs : autoware_traffic_lights->lightBulbs()) { + if (three_light_bulbs.hasAttribute("traffic_light_id")) { + if (const auto traffic_light_id = three_light_bulbs.attribute("traffic_light_id").asId(); + traffic_light_id) { + traffic_light_ids.push_back(traffic_light_id.value()); + } + } + } + } + return traffic_light_ids; +} + +auto toAutowareTrafficLights(const lanelet::Id traffic_light_id) + -> std::vector +{ + auto areBulbsAssignedToTrafficLight = [&traffic_light_id](auto red_yellow_green_bulbs) -> bool { + return red_yellow_green_bulbs.hasAttribute("traffic_light_id") and + red_yellow_green_bulbs.attribute("traffic_light_id").asId() and + red_yellow_green_bulbs.attribute("traffic_light_id").asId().value() == traffic_light_id; + }; + + std::vector autoware_traffic_lights; + const auto & all_lanelets = lanelet::utils::query::laneletLayer(LaneletWrapper::map()); + for (const auto & autoware_traffic_light : + lanelet::utils::query::autowareTrafficLights(all_lanelets)) { + for (auto three_light_bulbs : autoware_traffic_light->lightBulbs()) { + if (areBulbsAssignedToTrafficLight(three_light_bulbs)) { + autoware_traffic_lights.push_back(autoware_traffic_light); + } + } + } + + if (!autoware_traffic_lights.empty()) { + return autoware_traffic_lights; + } else { + THROW_SEMANTIC_ERROR( + traffic_light_id, + " lanelet does not have any regulatory elements with light bulbs assigned!"); + } +} + +auto autowareTrafficLightsOnPath(const lanelet::Ids & lanelet_ids) + -> std::vector +{ + std::vector autoware_traffic_lights; + for (const auto & lanelet_id : lanelet_ids) { + const auto & lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); + for (const auto & traffic_light : + lanelet.regulatoryElementsAs()) { + autoware_traffic_lights.push_back(traffic_light); + } + } + return autoware_traffic_lights; +} +} // namespace traffic_lights +} // namespace lanelet_wrapper +} // namespace traffic_simulator From a83b14d4873fe5d4e63314a6ec3fc8661bc3957d Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 5 Dec 2024 13:21:41 +0100 Subject: [PATCH 2/9] feat(traffic_simulator, behavior_tree_plugin, openscenario_intepreter): use ::distance from lanelet_wrapper instead of hdmap_utils --- .../simulator_core.hpp | 4 +- .../behavior_tree_plugin/action_node.hpp | 7 +- .../behavior_tree_plugin/src/action_node.cpp | 62 +++---- .../follow_polyline_trajectory_action.cpp | 2 +- .../follow_front_entity_action.cpp | 3 +- .../follow_lane_action.cpp | 3 +- .../stop_at_crossing_entity_action.cpp | 3 +- .../stop_at_stop_line_action.cpp | 3 +- .../follow_polyline_trajectory_action.cpp | 2 +- .../behavior/follow_trajectory.hpp | 3 +- .../data_type/lanelet_pose.hpp | 2 +- .../traffic_simulator/utils/distance.hpp | 80 +++++---- .../include/traffic_simulator/utils/pose.hpp | 8 +- .../include/traffic_simulator/utils/route.hpp | 36 ++++ .../src/behavior/follow_trajectory.cpp | 26 +-- .../src/data_type/lanelet_pose.cpp | 12 +- .../src/entity/ego_entity.cpp | 3 +- .../src/entity/entity_base.cpp | 8 +- .../src/entity/entity_manager.cpp | 3 +- .../traffic_simulator/src/utils/distance.cpp | 154 ++++++++++-------- .../traffic_simulator/src/utils/pose.cpp | 38 ++--- 21 files changed, 244 insertions(+), 218 deletions(-) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/utils/route.hpp diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 5fe8749dabc..5aede85657a 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -182,7 +182,7 @@ class SimulatorCore routing_configuration.allow_lane_change = (routing_algorithm == RoutingAlgorithm::value_type::shortest); return traffic_simulator::pose::relativeLaneletPose( - from_lanelet_pose, to_lanelet_pose, routing_configuration, core->getHdmapUtils()); + from_lanelet_pose, to_lanelet_pose, routing_configuration); } static auto makeNativeBoundingBoxRelativeLanePosition( @@ -230,7 +230,7 @@ class SimulatorCore (routing_algorithm == RoutingAlgorithm::value_type::shortest); return traffic_simulator::pose::boundingBoxRelativeLaneletPose( from_lanelet_pose, from_bounding_box, to_lanelet_pose, to_bounding_box, - routing_configuration, core->getHdmapUtils()); + routing_configuration); } static auto makeNativeBoundingBoxRelativeWorldPosition( diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index 8e13996926e..a845083755a 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -55,9 +55,6 @@ class ActionNode : public BT::ActionNodeBase -> double; auto getDistanceToFrontEntity(const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional; - auto getDistanceToStopLine( - const lanelet::Ids & route_lanelets, - const std::vector & waypoints) const -> std::optional; auto getDistanceToTrafficLightStopLine( const lanelet::Ids & route_lanelets, const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional; @@ -65,10 +62,10 @@ class ActionNode : public BT::ActionNodeBase auto getRightOfWayEntities(const lanelet::Ids & following_lanelets) const -> std::vector; auto getYieldStopDistance(const lanelet::Ids & following_lanelets) const -> std::optional; - auto getOtherEntityStatus(lanelet::Id lanelet_id) const - -> std::vector; auto stopEntity() const -> void; auto getHorizon() const -> double; + auto getOtherEntitiesCanonicalizedLaneletPoses() const + -> std::vector; /// throws if the derived class return RUNNING. auto executeTick() -> BT::NodeStatus override; diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 147a9b48ac2..f51f59d88c3 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -90,6 +90,18 @@ auto ActionNode::getBlackBoardValues() -> void } } +auto ActionNode::getOtherEntitiesCanonicalizedLaneletPoses() const + -> std::vector +{ + std::vector other_poses; + for (const auto & [entity_name, entity_status] : other_entity_status) { + if (auto const canonicalized_lanelet_pose = entity_status.getCanonicalizedLaneletPose()) { + other_poses.push_back(canonicalized_lanelet_pose.value()); + } + } + return other_poses; +} + auto ActionNode::getHorizon() const -> double { return std::clamp(canonicalized_entity_status->getTwist().linear.x * 5.0, 20.0, 50.0); @@ -109,43 +121,16 @@ auto ActionNode::setCanonicalizedEntityStatus(const traffic_simulator::EntitySta entity_status, default_matching_distance_for_lanelet_pose_calculation); } -auto ActionNode::getOtherEntityStatus(lanelet::Id lanelet_id) const - -> std::vector -{ - std::vector ret; - for (const auto & status : other_entity_status) { - if ( - status.second.laneMatchingSucceed() && - traffic_simulator::isSameLaneletId(status.second, lanelet_id)) { - ret.emplace_back(status.second); - } - } - return ret; -} - auto ActionNode::getYieldStopDistance(const lanelet::Ids & following_lanelets) const -> std::optional { - std::set distances; - for (const auto & lanelet : following_lanelets) { - const auto right_of_way_ids = hdmap_utils->getRightOfWayLaneletIds(lanelet); - for (const auto right_of_way_id : right_of_way_ids) { - const auto other_status = getOtherEntityStatus(right_of_way_id); - if (!other_status.empty() && canonicalized_entity_status->laneMatchingSucceed()) { - const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); - const auto distance_forward = hdmap_utils->getLongitudinalDistance( - lanelet_pose, traffic_simulator::helper::constructLaneletPose(lanelet, 0)); - const auto distance_backward = hdmap_utils->getLongitudinalDistance( - traffic_simulator::helper::constructLaneletPose(lanelet, 0), lanelet_pose); - if (distance_forward) { - distances.insert(distance_forward.value()); - } else if (distance_backward) { - distances.insert(-distance_backward.value()); - } - } - } - if (distances.size() != 0) { - return *distances.begin(); + if ( + const auto canonicalized_lanelet_pose = + canonicalized_entity_status->getCanonicalizedLaneletPose()) { + if (const auto other_poses = getOtherEntitiesCanonicalizedLaneletPoses(); + !other_poses.empty()) { + traffic_simulator::distance::distanceToYieldStop( + canonicalized_lanelet_pose.value(), following_lanelets, other_poses); } } return std::nullopt; @@ -214,7 +199,7 @@ auto ActionNode::getDistanceToTrafficLightStopLine( if (traffic_lights->isRequiredStopTrafficLightState(traffic_light_id)) { if ( const auto collision_point = - hdmap_utils->getDistanceToTrafficLightStopLine(spline, traffic_light_id)) { + traffic_simulator::distance::distanceToTrafficLightStopLine(spline, traffic_light_id)) { collision_points.insert(collision_point.value()); } } @@ -226,13 +211,6 @@ auto ActionNode::getDistanceToTrafficLightStopLine( return std::nullopt; } -auto ActionNode::getDistanceToStopLine( - const lanelet::Ids & route_lanelets, - const std::vector & waypoints) const -> std::optional -{ - return hdmap_utils->getDistanceToStopLine(route_lanelets, waypoints); -} - auto ActionNode::getDistanceToFrontEntity( const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional { diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 8e722f5afb0..9d25d9b668d 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -77,7 +77,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus } else if ( const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus( static_cast(*canonicalized_entity_status), - *polyline_trajectory, behavior_parameter, hdmap_utils, step_time, + *polyline_trajectory, behavior_parameter, step_time, default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed())) { setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp index 8c30fce024a..0bbd34faf1c 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp @@ -90,7 +90,8 @@ BT::NodeStatus FollowFrontEntityAction::tick() if (trajectory == nullptr) { return BT::NodeStatus::FAILURE; } - auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); + auto distance_to_stopline = + traffic_simulator::distance::distanceToStopLine(route_lanelets, *trajectory); auto distance_to_conflicting_entity = getDistanceToConflictingEntity(route_lanelets, *trajectory); const auto front_entity_name = getFrontEntityName(*trajectory); if (!front_entity_name) { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp index 1b70dd26599..209484678c7 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp @@ -105,7 +105,8 @@ BT::NodeStatus FollowLaneAction::tick() return BT::NodeStatus::FAILURE; } } - auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); + auto distance_to_stopline = + traffic_simulator::distance::distanceToStopLine(route_lanelets, *trajectory); auto distance_to_conflicting_entity = getDistanceToConflictingEntity(route_lanelets, *trajectory); if (distance_to_stopline) { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp index c85e6cdf622..641735fa412 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp @@ -112,7 +112,8 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() return BT::NodeStatus::FAILURE; } distance_to_stop_target_ = getDistanceToConflictingEntity(route_lanelets, *trajectory); - auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); + auto distance_to_stopline = + traffic_simulator::distance::distanceToStopLine(route_lanelets, *trajectory); const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory); if (!distance_to_stop_target_) { in_stop_sequence_ = false; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp index 23011a89e47..0daeb59f1d9 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp @@ -108,7 +108,8 @@ BT::NodeStatus StopAtStopLineAction::tick() if (trajectory == nullptr) { return BT::NodeStatus::FAILURE; } - distance_to_stopline_ = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); + distance_to_stopline_ = + traffic_simulator::distance::distanceToStopLine(route_lanelets, *trajectory); const auto distance_to_stop_target = getDistanceToConflictingEntity(route_lanelets, *trajectory); const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory); if (!distance_to_stopline_) { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index c2c097a0da2..3d84f15781a 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -77,7 +77,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus } else if ( const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus( static_cast(*canonicalized_entity_status), - *polyline_trajectory, behavior_parameter, hdmap_utils, step_time, + *polyline_trajectory, behavior_parameter, step_time, default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed())) { setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp index da3293fc67e..cb6ecf01a51 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp @@ -29,8 +29,7 @@ namespace follow_trajectory auto makeUpdatedStatus( const traffic_simulator_msgs::msg::EntityStatus &, traffic_simulator_msgs::msg::PolylineTrajectory &, - const traffic_simulator_msgs::msg::BehaviorParameter &, - const std::shared_ptr &, double, double, + const traffic_simulator_msgs::msg::BehaviorParameter &, double, double, std::optional target_speed = std::nullopt) -> std::optional; } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp index 11fca6a9dc4..54f429c104c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp @@ -48,7 +48,7 @@ class CanonicalizedLaneletPose auto alignOrientationToLanelet() -> void; auto hasAlternativeLaneletPose() const -> bool { return lanelet_poses_.size() > 1; } auto getAlternativeLaneletPoseBaseOnShortestRouteFrom( - LaneletPose from, const std::shared_ptr & hdmap_utils, + LaneletPose from, const RoutingConfiguration & routing_configuration = RoutingConfiguration()) const -> std::optional; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index 7919a140491..d810952721f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -15,7 +15,11 @@ #ifndef TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_ #define TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_ +#include +#include #include +#include +#include #include namespace traffic_simulator @@ -25,13 +29,12 @@ inline namespace distance // Lateral auto lateralDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - const traffic_simulator::RoutingConfiguration & routing_configuration, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional; + const RoutingConfiguration & routing_configuration) -> std::optional; auto lateralDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - double matching_distance, const traffic_simulator::RoutingConfiguration & routing_configuration, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional; + const double matching_distance, const RoutingConfiguration & routing_configuration) + -> std::optional; // Lateral (unit: lanes) auto countLaneChanges( @@ -43,9 +46,8 @@ auto countLaneChanges( // Longitudinal auto longitudinalDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool include_adjacent_lanelet, bool include_opposite_direction, - const traffic_simulator::RoutingConfiguration & routing_configuration, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional; + const bool include_adjacent_lanelet, const bool include_opposite_direction, + const RoutingConfiguration & routing_configuration) -> std::optional; // BoundingBox auto boundingBoxDistance( @@ -59,59 +61,69 @@ auto boundingBoxLaneLateralDistance( const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, - const traffic_simulator::RoutingConfiguration & routing_configuration, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional; + const RoutingConfiguration & routing_configuration) -> std::optional; auto boundingBoxLaneLongitudinalDistance( 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_ptr) -> std::optional; + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, + const bool include_adjacent_lanelet, const bool include_opposite_direction, + const RoutingConfiguration & routing_configuration) -> std::optional; // Bounds auto distanceToLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, - const std::shared_ptr & hdmap_utils_ptr) -> double; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id) -> double; auto distanceToLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, - const std::shared_ptr & hdmap_utils_ptr) -> double; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids) + -> double; auto distanceToLeftLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, - const std::shared_ptr & hdmap_utils_ptr) -> double; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id) + -> double; auto distanceToLeftLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, - const std::shared_ptr & hdmap_utils_ptr) -> double; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids) + -> double; auto distanceToRightLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, - const std::shared_ptr & hdmap_utils_ptr) -> double; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id) + -> double; auto distanceToRightLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, - const std::shared_ptr & hdmap_utils_ptr) -> double; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids) + -> double; // Other objects -auto distanceToCrosswalk( - const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array, - const lanelet::Id target_crosswalk_id, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional; - -auto distanceToStopLine( - const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array, - const lanelet::Id target_stop_line_id, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional; +template +auto distanceToStopLine(Ts &&... xs) +{ + return lanelet_wrapper::distance::distanceToStopLine(std::forward(xs)...); +} + +template +auto distanceToTrafficLightStopLine(Ts &&... xs) +{ + return lanelet_wrapper::distance::distanceToTrafficLightStopLine( + std::forward(xs)...); +} + +template +auto distanceToCrosswalk(Ts &&... xs) +{ + return lanelet_wrapper::distance::distanceToCrosswalk(std::forward(xs)...); +} + +auto distanceToYieldStop( + const CanonicalizedLaneletPose & reference_pose, const lanelet::Ids & following_lanelets, + const std::vector & other_poses) -> std::optional; } // namespace distance } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 92371213c72..f7b8d801bd1 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -85,21 +85,19 @@ auto boundingBoxRelativePose( // Relative LaneletPose auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - const RoutingConfiguration & routing_configuration, - const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose; + const RoutingConfiguration & routing_configuration) -> LaneletPose; 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 RoutingConfiguration & routing_configuration, - const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose; + const RoutingConfiguration & routing_configuration) -> LaneletPose; // Others auto isInLanelet( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id lanelet_id, - const double tolerance, const std::shared_ptr & hdmap_utils_ptr) -> bool; + const double tolerance) -> bool; auto isInLanelet(const geometry_msgs::msg::Point & point, const lanelet::Id lanelet_id) -> bool; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/route.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/route.hpp new file mode 100644 index 00000000000..c135033b240 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/route.hpp @@ -0,0 +1,36 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_SIMULATOR__UTILS__ROUTE_HPP_ +#define TRAFFIC_SIMULATOR__UTILS__ROUTE_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +inline namespace route +{ +template +auto route(Ts &&... xs) +{ + return lanelet_wrapper::route::route(std::forward(xs)...); +} +} // namespace route +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__UTILS__ROUTE_HPP_ diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 92e1ad14b46..72598a740d3 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -26,6 +26,9 @@ #include #include #include +#include +#include +#include namespace traffic_simulator { @@ -46,9 +49,8 @@ auto any(F f, T && x, Ts &&... xs) auto makeUpdatedStatus( const traffic_simulator_msgs::msg::EntityStatus & entity_status, traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const std::shared_ptr & hdmap_utils, const double step_time, - double matching_distance, std::optional target_speed) -> std::optional + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const double step_time, + const double matching_distance, std::optional target_speed) -> std::optional { using math::arithmetic::isApproximatelyEqualTo; using math::arithmetic::isDefinitelyLessThan; @@ -68,15 +70,15 @@ auto makeUpdatedStatus( auto distance_along_lanelet = [&](const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double { - if (const auto from_lanelet_pose = pose::toCanonicalizedLaneletPose( + if (const auto from_canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( from, entity_status.bounding_box, false, matching_distance); - from_lanelet_pose) { - if (const auto to_lanelet_pose = pose::toCanonicalizedLaneletPose( + from_canonicalized_lanelet_pose) { + if (const auto to_canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( to, entity_status.bounding_box, false, matching_distance); - to_lanelet_pose) { - if (const auto distance = hdmap_utils->getLongitudinalDistance( - static_cast(from_lanelet_pose.value()), - static_cast(to_lanelet_pose.value())); + to_canonicalized_lanelet_pose) { + if (const auto distance = distance::longitudinalDistance( + from_canonicalized_lanelet_pose.value(), to_canonicalized_lanelet_pose.value(), false, + false, RoutingConfiguration()); distance) { return distance.value(); } @@ -118,8 +120,8 @@ auto makeUpdatedStatus( } return makeUpdatedStatus( - entity_status, polyline_trajectory, behavior_parameter, hdmap_utils, step_time, - matching_distance, target_speed); + entity_status, polyline_trajectory, behavior_parameter, step_time, matching_distance, + target_speed); }; auto is_infinity_or_nan = [](auto x) constexpr { return std::isinf(x) or std::isnan(x); }; diff --git a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp index 975129d65c9..22c17c711cb 100644 --- a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp +++ b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp @@ -20,6 +20,7 @@ #include #include #include +#include namespace traffic_simulator { @@ -67,18 +68,17 @@ auto CanonicalizedLaneletPose::operator=(const CanonicalizedLaneletPose & other) } auto CanonicalizedLaneletPose::getAlternativeLaneletPoseBaseOnShortestRouteFrom( - LaneletPose from, const std::shared_ptr & hdmap_utils, - const RoutingConfiguration & routing_configuration) const -> std::optional + LaneletPose from, const RoutingConfiguration & routing_configuration) const + -> std::optional { if (lanelet_poses_.empty()) { return std::nullopt; } - lanelet::Ids shortest_route = - hdmap_utils->getRoute(from.lanelet_id, lanelet_poses_[0].lanelet_id, routing_configuration); + auto shortest_route = + route::route(from.lanelet_id, lanelet_poses_[0].lanelet_id, routing_configuration); LaneletPose alternative_lanelet_pose = lanelet_poses_[0]; for (const auto & laneletPose : lanelet_poses_) { - const auto route = - hdmap_utils->getRoute(from.lanelet_id, laneletPose.lanelet_id, routing_configuration); + const auto route = route::route(from.lanelet_id, laneletPose.lanelet_id, routing_configuration); if (shortest_route.size() > route.size()) { shortest_route = route; alternative_lanelet_pose = laneletPose; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 84b8412555c..c411d39207b 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -152,8 +152,7 @@ void EgoEntity::onUpdate(double current_time, double step_time) const auto non_canonicalized_updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus( static_cast(*status_), *polyline_trajectory_, - behavior_parameter_, hdmap_utils_ptr_, step_time, - getDefaultMatchingDistanceForLaneletPoseCalculation(), + behavior_parameter_, step_time, getDefaultMatchingDistanceForLaneletPoseCalculation(), target_speed_ ? target_speed_.value() : status_->getTwist().linear.x)) { // prefer current lanelet on ss2 side setStatus(non_canonicalized_updated_status.value(), status_->getLaneletIds()); diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 44f55312b13..ffce683243f 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -717,9 +717,9 @@ auto EntityBase::requestSynchronize( RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; - const auto entity_distance = longitudinalDistance( + const auto entity_distance = distance::longitudinalDistance( entity_lanelet_pose.value(), entity_target, true, true, - lane_changeable_routing_configuration, hdmap_utils_ptr_); + lane_changeable_routing_configuration); if (!entity_distance.has_value()) { THROW_SEMANTIC_ERROR( "Failed to get distance between entity and target lanelet pose. Check if the entity has " @@ -732,9 +732,9 @@ auto EntityBase::requestSynchronize( ? THROW_SEMANTIC_ERROR("Failed to find target entity. Check if the target entity exists.") : other_status_.find(target_name)->second.getLaneletPose(); - const auto target_entity_distance = longitudinalDistance( + const auto target_entity_distance = distance::longitudinalDistance( CanonicalizedLaneletPose(target_entity_lanelet_pose), target_sync_pose, true, true, - lane_changeable_routing_configuration, hdmap_utils_ptr_); + lane_changeable_routing_configuration); if (!target_entity_distance.has_value() || target_entity_distance.value() < 0.0) { RCLCPP_WARN_ONCE( rclcpp::get_logger("traffic_simulator"), diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 4211cc33c93..f7e78c81173 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -266,8 +266,7 @@ bool EntityManager::isInLanelet( { if (const auto entity = getEntity(name)) { if (const auto canonicalized_lanelet_pose = entity->getCanonicalizedLaneletPose()) { - return pose::isInLanelet( - canonicalized_lanelet_pose.value(), lanelet_id, tolerance, hdmap_utils_ptr_); + return pose::isInLanelet(canonicalized_lanelet_pose.value(), lanelet_id, tolerance); } } return false; diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index cb2f1169d70..8f6dc364914 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -15,6 +15,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -25,22 +28,21 @@ inline namespace distance { auto lateralDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - const traffic_simulator::RoutingConfiguration & routing_configuration, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional + const RoutingConfiguration & routing_configuration) -> std::optional { - return hdmap_utils_ptr->getLateralDistance( + return lanelet_wrapper::distance::lateralDistance( static_cast(from), static_cast(to), routing_configuration); } auto lateralDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - double matching_distance, const traffic_simulator::RoutingConfiguration & routing_configuration, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional + double matching_distance, const RoutingConfiguration & routing_configuration) + -> std::optional { if ( std::abs(static_cast(from).offset) <= matching_distance && std::abs(static_cast(to).offset) <= matching_distance) { - return lateralDistance(from, to, routing_configuration, hdmap_utils_ptr); + return lateralDistance(from, to, routing_configuration); } else { return std::nullopt; } @@ -59,24 +61,23 @@ auto countLaneChanges( /// @sa https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md auto longitudinalDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool include_adjacent_lanelet, bool include_opposite_direction, - const traffic_simulator::RoutingConfiguration & routing_configuration, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional + bool const include_adjacent_lanelet, bool const include_opposite_direction, + const RoutingConfiguration & routing_configuration) -> std::optional { if (!include_adjacent_lanelet) { auto to_canonicalized = static_cast(to); if (to.hasAlternativeLaneletPose()) { if ( const auto to_canonicalized_opt = to.getAlternativeLaneletPoseBaseOnShortestRouteFrom( - static_cast(from), hdmap_utils_ptr, routing_configuration)) { + static_cast(from), routing_configuration)) { to_canonicalized = to_canonicalized_opt.value(); } } - const auto forward_distance = hdmap_utils_ptr->getLongitudinalDistance( + const auto forward_distance = lanelet_wrapper::distance::longitudinalDistance( static_cast(from), to_canonicalized, routing_configuration); - const auto backward_distance = hdmap_utils_ptr->getLongitudinalDistance( + const auto backward_distance = lanelet_wrapper::distance::longitudinalDistance( to_canonicalized, static_cast(from), routing_configuration); if (forward_distance && backward_distance) { @@ -112,9 +113,9 @@ auto longitudinalDistance( for (const auto & from_pose : from_poses) { for (const auto & to_pose : to_poses) { if ( - const auto distance = longitudinalDistance( + const auto distance = distance::longitudinalDistance( CanonicalizedLaneletPose(from_pose), CanonicalizedLaneletPose(to_pose), false, - include_opposite_direction, routing_configuration, hdmap_utils_ptr)) { + include_opposite_direction, routing_configuration)) { distances.emplace_back(distance.value()); } } @@ -130,6 +131,7 @@ auto longitudinalDistance( } } +// BoundingBox auto boundingBoxDistance( const geometry_msgs::msg::Pose & from, const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, @@ -144,11 +146,9 @@ auto boundingBoxLaneLateralDistance( const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, - const traffic_simulator::RoutingConfiguration & routing_configuration, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional + const RoutingConfiguration & routing_configuration) -> std::optional { - if (const auto lateral_distance = - lateralDistance(from, to, routing_configuration, hdmap_utils_ptr); + if (const auto lateral_distance = lateralDistance(from, to, routing_configuration); lateral_distance) { const auto from_bounding_box_distances = math::geometry::getDistancesFromCenterToEdge(from_bounding_box); @@ -171,14 +171,12 @@ auto boundingBoxLaneLongitudinalDistance( 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_ptr) -> std::optional + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, + const bool include_adjacent_lanelet, const bool include_opposite_direction, + const RoutingConfiguration & routing_configuration) -> std::optional { - if (const auto longitudinal_distance = longitudinalDistance( - from, to, include_adjacent_lanelet, include_opposite_direction, routing_configuration, - hdmap_utils_ptr); + if (const auto longitudinal_distance = distance::longitudinalDistance( + from, to, include_adjacent_lanelet, include_opposite_direction, routing_configuration); longitudinal_distance) { const auto from_bounding_box_distances = math::geometry::getDistancesFromCenterToEdge(from_bounding_box); @@ -197,12 +195,13 @@ auto boundingBoxLaneLongitudinalDistance( return std::nullopt; } +// Bounds auto distanceToLeftLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, - const std::shared_ptr & hdmap_utils_ptr) -> double + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id) + -> double { - if (const auto bound = hdmap_utils_ptr->getLeftBound(lanelet_id); bound.empty()) { + if (const auto bound = lanelet_wrapper::lanelet_map::leftBound(lanelet_id); bound.empty()) { THROW_SEMANTIC_ERROR( "Failed to calculate left bounds of lanelet_id : ", lanelet_id, " please check lanelet map."); } else if (const auto polygon = @@ -216,26 +215,25 @@ auto distanceToLeftLaneBound( auto distanceToLeftLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, - const std::shared_ptr & hdmap_utils_ptr) -> double + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids) + -> double { if (lanelet_ids.empty()) { THROW_SEMANTIC_ERROR("Failing to calculate distanceToLeftLaneBound given an empty vector."); } std::vector distances; std::transform( - lanelet_ids.begin(), lanelet_ids.end(), std::back_inserter(distances), [&](auto lanelet_id) { - return distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr); - }); + lanelet_ids.begin(), lanelet_ids.end(), std::back_inserter(distances), + [&](auto lanelet_id) { return distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id); }); return *std::min_element(distances.begin(), distances.end()); } auto distanceToRightLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, - const std::shared_ptr & hdmap_utils_ptr) -> double + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id) + -> double { - if (const auto bound = hdmap_utils_ptr->getRightBound(lanelet_id); bound.empty()) { + if (const auto bound = lanelet_wrapper::lanelet_map::rightBound(lanelet_id); bound.empty()) { THROW_SEMANTIC_ERROR( "Failed to calculate right bounds of lanelet_id : ", lanelet_id, " please check lanelet map."); @@ -250,66 +248,78 @@ auto distanceToRightLaneBound( auto distanceToRightLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, - const std::shared_ptr & hdmap_utils_ptr) -> double + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids) + -> double { if (lanelet_ids.empty()) { THROW_SEMANTIC_ERROR("Failing to calculate distanceToRightLaneBound for given empty vector."); } std::vector distances; std::transform( - lanelet_ids.begin(), lanelet_ids.end(), std::back_inserter(distances), [&](auto lanelet_id) { - return distanceToRightLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr); - }); + lanelet_ids.begin(), lanelet_ids.end(), std::back_inserter(distances), + [&](auto lanelet_id) { return distanceToRightLaneBound(map_pose, bounding_box, lanelet_id); }); return *std::min_element(distances.begin(), distances.end()); } auto distanceToLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id, - const std::shared_ptr & hdmap_utils_ptr) -> double + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id) + -> double { return std::min( - distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr), - distanceToRightLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr)); + distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id), + distanceToRightLaneBound(map_pose, bounding_box, lanelet_id)); } auto distanceToLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, - const std::shared_ptr & hdmap_utils_ptr) -> double + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids) + -> double { return std::min( - distanceToLeftLaneBound(map_pose, bounding_box, lanelet_ids, hdmap_utils_ptr), - distanceToRightLaneBound(map_pose, bounding_box, lanelet_ids, hdmap_utils_ptr)); + distanceToLeftLaneBound(map_pose, bounding_box, lanelet_ids), + distanceToRightLaneBound(map_pose, bounding_box, lanelet_ids)); } -auto distanceToCrosswalk( - const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array, - const lanelet::Id target_crosswalk_id, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional +// Other objects +auto distanceToYieldStop( + const CanonicalizedLaneletPose & reference_pose, const lanelet::Ids & following_lanelets, + const std::vector & other_poses) -> std::optional { - if (waypoints_array.waypoints.empty()) { - return std::nullopt; - } else { - math::geometry::CatmullRomSpline spline(waypoints_array.waypoints); - auto polygon = hdmap_utils_ptr->getLaneletPolygon(target_crosswalk_id); - return spline.getCollisionPointIn2D(polygon); - } -} + auto getPosesOnLanelet = [&other_poses](const auto & lanelet_id) { + std::vector ret; + for (const auto & pose : other_poses) { + if (isSameLaneletId(pose, lanelet_id)) { + ret.emplace_back(pose); + } + } + return ret; + }; -auto distanceToStopLine( - const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array, - const lanelet::Id target_stop_line_id, - const std::shared_ptr & hdmap_utils_ptr) -> std::optional -{ - if (waypoints_array.waypoints.empty()) { - return std::nullopt; - } else { - const math::geometry::CatmullRomSpline spline(waypoints_array.waypoints); - const auto polygon = hdmap_utils_ptr->getStopLinePolygon(target_stop_line_id); - return spline.getCollisionPointIn2D(polygon); + std::set distances; + for (const auto & lanelet_id : following_lanelets) { + const auto right_of_way_ids = lanelet_wrapper::lanelet_map::rightOfWayLaneletIds(lanelet_id); + for (const auto right_of_way_id : right_of_way_ids) { + const auto other_poses = getPosesOnLanelet(right_of_way_id); + if (!other_poses.empty()) { + const auto distance_forward = lanelet_wrapper::distance::longitudinalDistance( + static_cast(reference_pose), + helper::constructLaneletPose(lanelet_id, 0.0, 0.0), RoutingConfiguration()); + const auto distance_backward = lanelet_wrapper::distance::longitudinalDistance( + helper::constructLaneletPose(lanelet_id, 0.0, 0.0), + static_cast(reference_pose), RoutingConfiguration()); + if (distance_forward) { + distances.insert(distance_forward.value()); + } else if (distance_backward) { + distances.insert(-distance_backward.value()); + } + } + } + if (distances.size() != 0) { + return *distances.begin(); + } } + return std::nullopt; } } // namespace distance } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index ace0a5a7b28..817e8526173 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -223,11 +223,9 @@ auto boundingBoxRelativePose( } // Relative LaneletPose -/// @todo HdMapUtils will be removed when lanelet_wrapper::distance is added auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - const RoutingConfiguration & routing_configuration, - const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose + const RoutingConfiguration & routing_configuration) -> LaneletPose { constexpr bool include_adjacent_lanelet{false}; constexpr bool include_opposite_direction{true}; @@ -236,27 +234,22 @@ auto relativeLaneletPose( // 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, routing_configuration, - hdmap_utils_ptr)) { + const auto longitudinal_distance = distance::longitudinalDistance( + from, to, include_adjacent_lanelet, include_opposite_direction, routing_configuration)) { position.s = longitudinal_distance.value(); } - if ( - const auto lateral_distance = - lateralDistance(from, to, routing_configuration, hdmap_utils_ptr)) { + if (const auto lateral_distance = distance::lateralDistance(from, to, routing_configuration)) { position.offset = lateral_distance.value(); } return position; } -/// @todo HdMapUtils will be removed when lanelet_wrapper::distance is added 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 RoutingConfiguration & routing_configuration, - const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose + const RoutingConfiguration & routing_configuration) -> LaneletPose { constexpr bool include_adjacent_lanelet{false}; constexpr bool include_opposite_direction{true}; @@ -265,23 +258,22 @@ auto boundingBoxRelativeLaneletPose( // 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( + const auto longitudinal_bounding_box_distance = distance::boundingBoxLaneLongitudinalDistance( from, from_bounding_box, to, to_bounding_box, include_adjacent_lanelet, - include_opposite_direction, routing_configuration, hdmap_utils_ptr)) { + include_opposite_direction, routing_configuration)) { position.s = longitudinal_bounding_box_distance.value(); } if ( - const auto lateral_bounding_box_distance = boundingBoxLaneLateralDistance( - from, from_bounding_box, to, to_bounding_box, routing_configuration, hdmap_utils_ptr)) { + const auto lateral_bounding_box_distance = distance::boundingBoxLaneLateralDistance( + from, from_bounding_box, to, to_bounding_box, routing_configuration)) { position.offset = lateral_bounding_box_distance.value(); } return position; } -/// @todo HdMapUtils will be removed when lanelet_wrapper::distance is added auto isInLanelet( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id lanelet_id, - const double tolerance, const std::shared_ptr & hdmap_utils_ptr) -> bool + const double tolerance) -> bool { constexpr bool include_adjacent_lanelet{false}; constexpr bool include_opposite_direction{false}; @@ -291,9 +283,9 @@ auto isInLanelet( return true; } else { const auto start_lanelet_pose = helper::constructCanonicalizedLaneletPose(lanelet_id, 0.0, 0.0); - if (const auto distance_to_start_lanelet_pose = longitudinalDistance( + if (const auto distance_to_start_lanelet_pose = distance::longitudinalDistance( start_lanelet_pose, canonicalized_lanelet_pose, include_adjacent_lanelet, - include_opposite_direction, routing_configuration, hdmap_utils_ptr); + include_opposite_direction, routing_configuration); distance_to_start_lanelet_pose and std::abs(distance_to_start_lanelet_pose.value()) <= tolerance) { return true; @@ -301,9 +293,9 @@ auto isInLanelet( const auto end_lanelet_pose = helper::constructCanonicalizedLaneletPose( lanelet_id, lanelet_wrapper::lanelet_map::laneletLength(lanelet_id), 0.0); - if (const auto distance_to_end_lanelet_pose = longitudinalDistance( + if (const auto distance_to_end_lanelet_pose = distance::longitudinalDistance( canonicalized_lanelet_pose, end_lanelet_pose, include_adjacent_lanelet, - include_opposite_direction, routing_configuration, hdmap_utils_ptr); + include_opposite_direction, routing_configuration); distance_to_end_lanelet_pose and std::abs(distance_to_end_lanelet_pose.value()) <= tolerance) { return true; @@ -317,7 +309,7 @@ auto isInLanelet(const geometry_msgs::msg::Point & point, const lanelet::Id lane return lanelet_wrapper::lanelet_map::isInLanelet(lanelet_id, point); } -/// @todo HdMapUtils will be removed when lanelet_wrapper::distance is added +/// @todo passing HdMapUtils will be removed when lanelet_wrapper::route::followingLanelets is added auto isAtEndOfLanelets( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const std::shared_ptr & hdmap_utils_ptr) -> bool From 5921ecf4d5381235155efa645cd94a2dba9d1157 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 5 Dec 2024 13:23:19 +0100 Subject: [PATCH 3/9] feat(traffic_simulator): adapt test for lanelet_wrapper --- ...t_distance_in_lane_coordinate_distance.cpp | 6 +- .../get_distance_to_lane_bound.cpp | 3 +- .../test/src/data_type/test_lanelet_pose.cpp | 12 +- .../test/src/utils/test_distance.cpp | 208 +++++++----------- .../test/src/utils/test_pose.cpp | 30 +-- 5 files changed, 103 insertions(+), 156 deletions(-) diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp index 49babeb764b..7f94114fea8 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp @@ -51,7 +51,7 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar return traffic_simulator::distance::lateralDistance( from_entity->getCanonicalizedLaneletPose().value(), to_entity->getCanonicalizedLaneletPose().value(), - traffic_simulator::RoutingConfiguration(), api_.getHdmapUtils()); + traffic_simulator::RoutingConfiguration()); } } return std::nullopt; @@ -68,7 +68,7 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar if (from_entity_lanelet_pose && to_entity_lanelet_pose) { return traffic_simulator::distance::lateralDistance( from_entity_lanelet_pose.value(), to_entity_lanelet_pose.value(), - traffic_simulator::RoutingConfiguration(), api_.getHdmapUtils()); + traffic_simulator::RoutingConfiguration()); } } } @@ -86,7 +86,7 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar return traffic_simulator::distance::longitudinalDistance( from_entity->getCanonicalizedLaneletPose().value(), to_entity->getCanonicalizedLaneletPose().value(), false, true, - traffic_simulator::RoutingConfiguration(), api_.getHdmapUtils()); + traffic_simulator::RoutingConfiguration()); } } return std::nullopt; diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp index b6731a63ae8..df01425afd9 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp @@ -46,8 +46,7 @@ class GetDistanceToLaneBoundScenario : public cpp_mock_scenarios::CppScenarioNod } if (auto ego_entity = api_.getEntity("ego")) { const auto distance = traffic_simulator::distance::distanceToLaneBound( - ego_entity->getMapPose(), ego_entity->getBoundingBox(), ego_entity->getRouteLanelets(), - api_.getHdmapUtils()); + ego_entity->getMapPose(), ego_entity->getBoundingBox(), ego_entity->getRouteLanelets()); // LCOV_EXCL_START if (distance <= 0.4 && distance >= 0.52) { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp b/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp index eb03e9970d9..7f3fc4fb15c 100644 --- a/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp +++ b/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp @@ -191,8 +191,8 @@ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRout const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); - const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils); - const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils); + const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1); + const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2); ASSERT_TRUE(result1.has_value()); ASSERT_TRUE(result2.has_value()); @@ -210,8 +210,8 @@ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRout const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); - const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils); - const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils); + const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1); + const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2); ASSERT_TRUE(result1.has_value()); ASSERT_TRUE(result2.has_value()); @@ -230,8 +230,8 @@ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRout const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); - const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils); - const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils); + const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1); + const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2); ASSERT_TRUE(result1.has_value()); ASSERT_TRUE(result2.has_value()); diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index 7b97ce02969..7c76473334c 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -34,52 +35,19 @@ int main(int argc, char ** argv) class distanceTest_FourTrackHighwayMap : public testing::Test { protected: - distanceTest_FourTrackHighwayMap() - : hdmap_utils_ptr(std::make_shared( - ament_index_cpp::get_package_share_directory("traffic_simulator") + - "/map/four_track_highway/lanelet2_map.osm", - geographic_msgs::build() - .latitude(35.22312494055522) - .longitude(138.8024583466017) - .altitude(0.0))) - { - activateLaneletWrapper("four_track_highway"); - } - std::shared_ptr hdmap_utils_ptr; + distanceTest_FourTrackHighwayMap() { activateLaneletWrapper("four_track_highway"); } }; class distanceTest_StandardMap : public testing::Test { protected: - distanceTest_StandardMap() - : hdmap_utils_ptr(std::make_shared( - ament_index_cpp::get_package_share_directory("traffic_simulator") + - "/map/standard_map/lanelet2_map.osm", - geographic_msgs::build() - .latitude(35.61836750154) - .longitude(139.78066608243) - .altitude(0.0))) - { - activateLaneletWrapper("standard_map"); - } - std::shared_ptr hdmap_utils_ptr; + distanceTest_StandardMap() { activateLaneletWrapper("standard_map"); } }; class distanceTest_IntersectionMap : public testing::Test { protected: - distanceTest_IntersectionMap() - : hdmap_utils_ptr(std::make_shared( - ament_index_cpp::get_package_share_directory("traffic_simulator") + - "/map/intersection/lanelet2_map.osm", - geographic_msgs::build() - .latitude(35.64200728302) - .longitude(139.74821144562) - .altitude(0.0))) - { - activateLaneletWrapper("intersection"); - } - std::shared_ptr hdmap_utils_ptr; + distanceTest_IntersectionMap() { activateLaneletWrapper("intersection"); } }; /** @@ -95,12 +63,12 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_noChange) { const auto result = traffic_simulator::distance::lateralDistance( pose_from, pose_to, std::numeric_limits::infinity(), - traffic_simulator::RoutingConfiguration(), hdmap_utils_ptr); + traffic_simulator::RoutingConfiguration()); EXPECT_FALSE(result.has_value()); } { const auto result = traffic_simulator::distance::lateralDistance( - pose_from, pose_to, traffic_simulator::RoutingConfiguration(), hdmap_utils_ptr); + pose_from, pose_to, traffic_simulator::RoutingConfiguration()); EXPECT_FALSE(result.has_value()); } } @@ -118,13 +86,13 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_noChange) { const auto result = traffic_simulator::distance::lateralDistance( pose_from, pose_to, std::numeric_limits::infinity(), - traffic_simulator::RoutingConfiguration(), hdmap_utils_ptr); + traffic_simulator::RoutingConfiguration()); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 0.0, std::numeric_limits::epsilon()); } { const auto result = traffic_simulator::distance::lateralDistance( - pose_from, pose_to, traffic_simulator::RoutingConfiguration(), hdmap_utils_ptr); + pose_from, pose_to, traffic_simulator::RoutingConfiguration()); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 0.0, std::numeric_limits::epsilon()); } @@ -146,14 +114,14 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_change) lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::lateralDistance( pose_from, pose_to, std::numeric_limits::infinity(), - lane_changeable_routing_configuration, hdmap_utils_ptr); + lane_changeable_routing_configuration); EXPECT_FALSE(result.has_value()); } { traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::lateralDistance( - pose_from, pose_to, lane_changeable_routing_configuration, hdmap_utils_ptr); + pose_from, pose_to, lane_changeable_routing_configuration); EXPECT_FALSE(result.has_value()); } } @@ -175,7 +143,7 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_change) lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::lateralDistance( pose_from, pose_to, std::numeric_limits::infinity(), - lane_changeable_routing_configuration, hdmap_utils_ptr); + lane_changeable_routing_configuration); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), approx_distance, tolerance); } @@ -183,7 +151,7 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_change) traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::lateralDistance( - pose_from, pose_to, lane_changeable_routing_configuration, hdmap_utils_ptr); + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), approx_distance, tolerance); } @@ -203,7 +171,7 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_matching) traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::lateralDistance( - pose_from, pose_to, 2.0, lane_changeable_routing_configuration, hdmap_utils_ptr); + pose_from, pose_to, 2.0, lane_changeable_routing_configuration); EXPECT_FALSE(result.has_value()); } } @@ -222,7 +190,7 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_matching) traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::lateralDistance( - pose_from, pose_to, 3.0, lane_changeable_routing_configuration, hdmap_utils_ptr); + pose_from, pose_to, 3.0, lane_changeable_routing_configuration); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), -3.0, 0.5); } @@ -244,8 +212,7 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, traffic_simulator::RoutingConfiguration(), - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, traffic_simulator::RoutingConfiguration()); EXPECT_FALSE(result.has_value()); } } @@ -266,8 +233,7 @@ TEST_F(distanceTest_StandardMap, longitudinalDistance_noAdjacent_noOpposite_noCh ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, traffic_simulator::RoutingConfiguration(), - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, traffic_simulator::RoutingConfiguration()); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 60.0, 1.0); } @@ -289,8 +255,7 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, traffic_simulator::RoutingConfiguration(), - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, traffic_simulator::RoutingConfiguration()); EXPECT_FALSE(result.has_value()); } } @@ -311,8 +276,7 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, traffic_simulator::RoutingConfiguration(), - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, traffic_simulator::RoutingConfiguration()); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 20.0, 1.0); } @@ -336,8 +300,7 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration); EXPECT_FALSE(result.has_value()); } { @@ -351,8 +314,7 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration); EXPECT_FALSE(result.has_value()); } } @@ -375,8 +337,7 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 145.0, 1.0); } @@ -391,8 +352,7 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 178.0, 1.0); } @@ -414,8 +374,7 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration); EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 118.0, 1.0)); } { @@ -429,8 +388,7 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration); EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 195.0, 1.0)); } { @@ -444,8 +402,7 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration); EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 257.0, 1.0)); } { @@ -459,8 +416,7 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration); EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 228.0, 1.0)); } } @@ -483,8 +439,7 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration, - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration); EXPECT_FALSE(result.has_value()); } { @@ -498,8 +453,7 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration, - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration); EXPECT_FALSE(result.has_value()); } } @@ -520,8 +474,7 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration, - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 103.0, 1.0); } @@ -536,8 +489,7 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration, - hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 131.0, 1.0); } @@ -593,57 +545,57 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_single) { const auto pose = makePose(3818.33, 73726.18, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 0.5, tolerance); } { const auto pose = makePose(3816.89, 73723.09, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 2.6, tolerance); } { const auto pose = makePose(3813.42, 73721.11, 30.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 2.7, tolerance); } { const auto pose = makePose(3813.42, 73721.11, 120.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 1.3, tolerance); } { const auto pose = makePose(3810.99, 73721.40, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 1.4, tolerance); } { const auto pose = makePose(3810.99, 73721.40, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 2.4, tolerance); } { const auto pose = makePose(3680.81, 73757.27, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, 34684L, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, 34684L); EXPECT_NEAR(result, 5.1, tolerance); } { const auto pose = makePose(3692.79, 73753.00, 30.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, 34684L, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, 34684L); EXPECT_NEAR(result, 7.2, tolerance); } } @@ -661,11 +613,10 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_multipleVector) lanelet_ids.cbegin(), lanelet_ids.cend(), std::numeric_limits::max(), [](const double lhs, const double rhs) { return std::min(lhs, rhs); }, [&pose, &bounding_box, this](const lanelet::Id lanelet_id) { - return traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + return traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); }); - const double result_distance = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_ids, hdmap_utils_ptr); + const double result_distance = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_ids); EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); EXPECT_NEAR(result_distance, 1.4, 0.1); } @@ -679,10 +630,10 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_singleVector) constexpr lanelet::Id lanelet_id = 34426L; const auto pose = makePose(3693.34, 73738.37, 300.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double actual_distance = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); - const double result_distance = traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, {lanelet_id}, hdmap_utils_ptr); + const double actual_distance = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet_id); + const double result_distance = + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, {lanelet_id}); EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); EXPECT_NEAR(result_distance, 1.8, 0.1); } @@ -695,8 +646,7 @@ TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_emptyVector) const auto pose = makePose(3825.87, 73773.08, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); EXPECT_THROW( - traffic_simulator::distance::distanceToLeftLaneBound( - pose, bounding_box, lanelet::Ids{}, hdmap_utils_ptr), + traffic_simulator::distance::distanceToLeftLaneBound(pose, bounding_box, lanelet::Ids{}), common::SemanticError); } @@ -710,57 +660,57 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_single) { const auto pose = makePose(86651.84, 44941.47, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 4.1, tolerance); } { const auto pose = makePose(86653.05, 44946.74, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 0.6, tolerance); } { const auto pose = makePose(86651.47, 44941.07, 120.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 4.3, tolerance); } { const auto pose = makePose(86651.47, 44941.07, 210.0); const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 3.1, tolerance); } { const auto pose = makePose(86644.10, 44951.86, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 2.0, tolerance); } { const auto pose = makePose(86644.10, 44951.86, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 1.1, tolerance); } { const auto pose = makePose(86644.11, 44941.21, 0.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 11.2, tolerance); } { const auto pose = makePose(86656.83, 44946.96, 0.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double result = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); EXPECT_NEAR(result, 2.6, tolerance); } } @@ -778,11 +728,10 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_multipleVector) lanelet_ids.cbegin(), lanelet_ids.cend(), std::numeric_limits::max(), [](const double lhs, const double rhs) { return std::min(lhs, rhs); }, [&pose, &bounding_box, this](const lanelet::Id lanelet_id) { - return traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); + return traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); }); - const double result_distance = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_ids, hdmap_utils_ptr); + const double result_distance = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_ids); EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); EXPECT_NEAR(result_distance, 2.7, 0.1); } @@ -796,10 +745,10 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_singleVector) constexpr lanelet::Id lanelet_id = 654L; const auto pose = makePose(86702.79, 44929.05, 150.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); - const double actual_distance = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet_id, hdmap_utils_ptr); - const double result_distance = traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, {lanelet_id}, hdmap_utils_ptr); + const double actual_distance = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet_id); + const double result_distance = + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, {lanelet_id}); EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); EXPECT_NEAR(result_distance, 2.4, 0.1); } @@ -812,7 +761,6 @@ TEST_F(distanceTest_IntersectionMap, distanceToRightLaneBound_emptyVector) const auto pose = makePose(3825.87, 73773.08, 135.0); const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); EXPECT_THROW( - traffic_simulator::distance::distanceToRightLaneBound( - pose, bounding_box, lanelet::Ids{}, hdmap_utils_ptr), + traffic_simulator::distance::distanceToRightLaneBound(pose, bounding_box, lanelet::Ids{}), common::SemanticError); } diff --git a/simulation/traffic_simulator/test/src/utils/test_pose.cpp b/simulation/traffic_simulator/test/src/utils/test_pose.cpp index 53c5c111ef5..4d2628a9351 100644 --- a/simulation/traffic_simulator/test/src/utils/test_pose.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_pose.cpp @@ -417,7 +417,7 @@ TEST_F(PoseTest, relativeLaneletPose_s_invalid) const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0); const auto relative = traffic_simulator::pose::relativeLaneletPose( - from, to, traffic_simulator::RoutingConfiguration(), hdmap_utils); + from, to, traffic_simulator::RoutingConfiguration()); EXPECT_TRUE(std::isnan(relative.s)); } @@ -431,7 +431,7 @@ TEST_F(PoseTest, relativeLaneletPose_s_valid) const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0); const auto relative = traffic_simulator::pose::relativeLaneletPose( - from, to, traffic_simulator::RoutingConfiguration(), hdmap_utils); + from, to, traffic_simulator::RoutingConfiguration()); EXPECT_NEAR(relative.s, 107.74, 0.001); } @@ -445,7 +445,7 @@ TEST_F(PoseTest, relativeLaneletPose_offset_invalid) const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0); const auto relative = traffic_simulator::pose::relativeLaneletPose( - from, to, traffic_simulator::RoutingConfiguration(), hdmap_utils); + from, to, traffic_simulator::RoutingConfiguration()); EXPECT_TRUE(std::isnan(relative.offset)); } @@ -460,8 +460,8 @@ TEST_F(PoseTest, relativeLaneletPose_offset_valid) traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; - const auto relative = traffic_simulator::pose::relativeLaneletPose( - from, to, lane_changeable_routing_configuration, hdmap_utils); + const auto relative = + traffic_simulator::pose::relativeLaneletPose(from, to, lane_changeable_routing_configuration); EXPECT_EQ(relative.offset, 1.0); } @@ -476,7 +476,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_invalid) const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration(), hdmap_utils); + from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration()); EXPECT_TRUE(std::isnan(relative.s)); } @@ -491,7 +491,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_valid) const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration(), hdmap_utils); + from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration()); EXPECT_NEAR(relative.s, 103.74, 0.01); } @@ -506,7 +506,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_invalid) const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration(), hdmap_utils); + from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration()); EXPECT_TRUE(std::isnan(relative.s)); } @@ -521,7 +521,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_valid) const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration(), hdmap_utils); + from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration()); EXPECT_EQ(relative.offset, 0.0); } @@ -533,8 +533,8 @@ TEST_F(PoseTest, isInLanelet_inside) { const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); - EXPECT_TRUE(traffic_simulator::pose::isInLanelet( - pose, 195, std::numeric_limits::epsilon(), hdmap_utils)); + EXPECT_TRUE( + traffic_simulator::pose::isInLanelet(pose, 195, std::numeric_limits::epsilon())); } /** @@ -545,7 +545,7 @@ TEST_F(PoseTest, isInLanelet_outsideFrontFar) const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -10.0, 0.0); - EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 3002163, 1.0, hdmap_utils)); + EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 3002163, 1.0)); } /** @@ -556,7 +556,7 @@ TEST_F(PoseTest, isInLanelet_outsideFrontClose) const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -1.0, 0.0); - EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 3002163, 2.0, hdmap_utils)); + EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 3002163, 2.0)); } /** @@ -566,7 +566,7 @@ TEST_F(PoseTest, isInLanelet_outsideBackFar) { const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 120.0, 0.0); - EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 195, 2, hdmap_utils)); + EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 195, 2)); } /** @@ -576,7 +576,7 @@ TEST_F(PoseTest, isInLanelet_outsideBackClose) { const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 110.0, 0.0); - EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 195, 10.0, hdmap_utils)); + EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 195, 10.0)); } /** From 91f6ad3a0a92c517ea60ef6de240979b5dde59ee Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 5 Dec 2024 13:24:13 +0100 Subject: [PATCH 4/9] reftraffic_simulator): use lanelet_wrapper in hdmap_utils, remove separated parts, adapt hdmap_utils tests --- .../hdmap_utils/hdmap_utils.hpp | 50 --- .../src/hdmap_utils/hdmap_utils.cpp | 331 ------------------ .../test/src/hdmap_utils/test_hdmap_utils.cpp | 292 +++++++-------- 3 files changed, 151 insertions(+), 522 deletions(-) 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 fddee73f040..afca76da216 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 @@ -106,30 +106,6 @@ class HdMapUtils traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - auto getDistanceToStopLine( - const lanelet::Ids & route_lanelets, - const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional; - - auto getDistanceToStopLine( - const lanelet::Ids & route_lanelets, - const std::vector & waypoints) const -> std::optional; - - auto getDistanceToTrafficLightStopLine( - const lanelet::Ids & route_lanelets, - const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional; - - auto getDistanceToTrafficLightStopLine( - const lanelet::Ids & route_lanelets, - const std::vector & waypoints) const -> std::optional; - - auto getDistanceToTrafficLightStopLine( - const math::geometry::CatmullRomSplineInterface & spline, - const lanelet::Id traffic_light_id) const -> std::optional; - - auto getDistanceToTrafficLightStopLine( - const std::vector & waypoints, - const lanelet::Id traffic_light_id) const -> std::optional; - auto getFollowingLanelets( const lanelet::Id current_lanelet_id, const lanelet::Ids & route, const double horizon = 100, const bool include_current_lanelet_id = true, @@ -173,20 +149,6 @@ class HdMapUtils auto getLanelets(const lanelet::Ids &) const -> lanelet::Lanelets; - auto getLateralDistance( - const traffic_simulator_msgs::msg::LaneletPose & from, - const traffic_simulator_msgs::msg::LaneletPose & to, - const traffic_simulator::RoutingConfiguration & routing_configuration = - traffic_simulator::RoutingConfiguration()) const -> std::optional; - - auto getLeftBound(const lanelet::Id) const -> std::vector; - - auto getLongitudinalDistance( - const traffic_simulator_msgs::msg::LaneletPose & from_pose, - const traffic_simulator_msgs::msg::LaneletPose & to_pose, - const traffic_simulator::RoutingConfiguration & routing_configuration = - traffic_simulator::RoutingConfiguration()) const -> std::optional; - auto getNearbyLaneletIds( const geometry_msgs::msg::Point &, const double distance_threshold, const bool include_crosswalk, const std::size_t search_count = 5) const -> lanelet::Ids; @@ -200,8 +162,6 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - auto getRightBound(const lanelet::Id) const -> std::vector; - auto getRightOfWayLaneletIds(const lanelet::Ids &) const -> std::unordered_map; @@ -217,12 +177,6 @@ class HdMapUtils traffic_simulator::RoutingConfiguration().routing_graph_type) const -> double; - auto getStopLineIds() const -> lanelet::Ids; - - auto getStopLineIdsOnPath(const lanelet::Ids & route_lanelets) const -> lanelet::Ids; - - auto getStopLinePolygon(const lanelet::Id) const -> std::vector; - auto getTangentVector(const lanelet::Id, const double s) const -> std::optional; @@ -352,10 +306,6 @@ class HdMapUtils const traffic_simulator::lane_change::TrajectoryShape, const double tangent_vector_size = 100) const -> math::geometry::HermiteCurve; - auto getStopLines() const -> lanelet::ConstLineStrings3d; - - auto getStopLinesOnPath(const lanelet::Ids &) const -> lanelet::ConstLineStrings3d; - auto getTrafficLightRegulatoryElementsOnPath(const lanelet::Ids &) const -> std::vector>; diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 01e24c889d7..b3682b607b5 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -740,18 +740,6 @@ auto HdMapUtils::getTrafficLightBulbPosition( return std::nullopt; } -auto HdMapUtils::getLeftBound(const lanelet::Id lanelet_id) const - -> std::vector -{ - return toPolygon(lanelet_map_ptr_->laneletLayer.get(lanelet_id).leftBound()); -} - -auto HdMapUtils::getRightBound(const lanelet::Id lanelet_id) const - -> std::vector -{ - return toPolygon(lanelet_map_ptr_->laneletLayer.get(lanelet_id).rightBound()); -} - auto HdMapUtils::getLaneChangeTrajectory( const traffic_simulator_msgs::msg::LaneletPose & from_pose, const traffic_simulator::lane_change::Parameter & lane_change_parameter) const @@ -929,125 +917,6 @@ auto HdMapUtils::canChangeLane( return routing_graphs_->traffic_rule(type)->canChangeLane(from_lanelet, to_lanelet); } -auto HdMapUtils::getLateralDistance( - const traffic_simulator_msgs::msg::LaneletPose & from, - const traffic_simulator_msgs::msg::LaneletPose & to, - const traffic_simulator::RoutingConfiguration & routing_configuration) const - -> std::optional -{ - const auto route = getRoute(from.lanelet_id, to.lanelet_id, routing_configuration); - if (route.empty()) { - return std::nullopt; - } - if (routing_configuration.allow_lane_change) { - double lateral_distance_by_lane_change = 0.0; - for (unsigned int i = 0; i < route.size() - 1; i++) { - auto next_lanelet_ids = - lanelet_map::nextLaneletIds(route[i], routing_configuration.routing_graph_type); - if (auto next_lanelet = std::find_if( - next_lanelet_ids.begin(), next_lanelet_ids.end(), - [&route, i](const lanelet::Id & id) { return id == route[i + 1]; }); - next_lanelet == next_lanelet_ids.end()) { - traffic_simulator_msgs::msg::LaneletPose next_lanelet_pose; - next_lanelet_pose.lanelet_id = route[i + 1]; - next_lanelet_pose.s = 0.0; - next_lanelet_pose.offset = 0.0; - - if ( - auto next_lanelet_origin_from_current_lanelet = - pose::toLaneletPose(pose::toMapPose(next_lanelet_pose).pose, route[i], 10.0)) { - lateral_distance_by_lane_change += next_lanelet_origin_from_current_lanelet->offset; - } else { - traffic_simulator_msgs::msg::LaneletPose current_lanelet_pose = next_lanelet_pose; - current_lanelet_pose.lanelet_id = route[i]; - if ( - auto current_lanelet_origin_from_next_lanelet = - pose::toLaneletPose(pose::toMapPose(current_lanelet_pose).pose, route[i + 1], 10.0)) { - lateral_distance_by_lane_change -= current_lanelet_origin_from_next_lanelet->offset; - } else { - return std::nullopt; - } - } - } - } - return to.offset - from.offset + lateral_distance_by_lane_change; - } else { - return to.offset - from.offset; - } -} - -auto HdMapUtils::getLongitudinalDistance( - const traffic_simulator_msgs::msg::LaneletPose & from_pose, - const traffic_simulator_msgs::msg::LaneletPose & to_pose, - const traffic_simulator::RoutingConfiguration & routing_configuration) const - -> std::optional -{ - const auto is_lane_change_required = [this, routing_configuration]( - const lanelet::Id current_lanelet, - const lanelet::Id next_lanelet) -> bool { - const auto next_lanelet_ids = - lanelet_map::nextLaneletIds(current_lanelet, routing_configuration.routing_graph_type); - return std::none_of( - next_lanelet_ids.cbegin(), next_lanelet_ids.cend(), - [next_lanelet](const lanelet::Id id) { return id == next_lanelet; }); - }; - - /// @sa https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/DistanceCalculation/ - if (const auto route = getRoute(from_pose.lanelet_id, to_pose.lanelet_id, routing_configuration); - not route.empty() || from_pose.lanelet_id == to_pose.lanelet_id) { - /// @note horizontal bar must intersect with the lanelet spline, so the distance was set arbitrarily to 10 meters. - static constexpr double matching_distance = 10.0; - double accumulated_distance = 0.0; - // accumulate lanelet lengths alongside the route, considering possible lane changes - for (std::size_t i = 1UL; i < route.size(); ++i) { - // if lane change is required, add the distance traveled during the lane change - // if lane change is not required, add the current lanelet length - if (is_lane_change_required(route[i - 1UL], route[i])) { - const auto current_lanelet_spline = getCenterPointsSpline(route[i - 1UL]); - const auto next_lanelet_spline = getCenterPointsSpline(route[i]); - - // first, lanelets are matched at the start (s = 0.0) of each lanelet; only if this fails, - // matching is performed at a larger s value, which should cover the rest of the cases. - static constexpr double lanelet_starting_s = 0.0; - const auto lanelet_min_middle_s = - std::min(current_lanelet_spline->getLength(), next_lanelet_spline->getLength()) * 0.5; - - const auto current_start_matching_s = current_lanelet_spline->getSValue( - next_lanelet_spline->getPose(lanelet_starting_s), matching_distance); - const auto next_start_matching_s = next_lanelet_spline->getSValue( - current_lanelet_spline->getPose(lanelet_starting_s), matching_distance); - const auto current_middle_matching_s = current_lanelet_spline->getSValue( - next_lanelet_spline->getPose(lanelet_min_middle_s), matching_distance); - const auto next_middle_matching_s = next_lanelet_spline->getSValue( - current_lanelet_spline->getPose(lanelet_min_middle_s), matching_distance); - - if (current_start_matching_s.has_value()) { - accumulated_distance += current_start_matching_s.value(); - } else if (next_start_matching_s.has_value()) { - accumulated_distance -= next_start_matching_s.value(); - } else if (current_middle_matching_s.has_value()) { - accumulated_distance += current_middle_matching_s.value() - lanelet_min_middle_s; - } else if (next_middle_matching_s.has_value()) { - accumulated_distance -= next_middle_matching_s.value() - lanelet_min_middle_s; - } else { - return std::nullopt; - } - } else { - accumulated_distance += lanelet_map::laneletLength(route[i - 1UL]); - } - } - // subtract the distance already traveled on the first lanelet: from_pose.s - // and add the distance that needs to be traveled on the last: to_pose.s. - if (const double distance = accumulated_distance - from_pose.s + to_pose.s; distance >= 0.0) { - return std::make_optional(distance); - } else { - return std::nullopt; - } - } else { - return std::nullopt; - } -} - auto HdMapUtils::toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin { std::stringstream ss; @@ -1236,51 +1105,6 @@ auto HdMapUtils::getTrafficLightRegulatoryElementsOnPath(const lanelet::Ids & la return ret; } -auto HdMapUtils::getStopLines() const -> lanelet::ConstLineStrings3d -{ - lanelet::ConstLineStrings3d ret; - for (const auto & traffic_sign : getTrafficSignRegulatoryElements()) { - if (traffic_sign->type() == "stop_sign") { - for (const auto & stop_line : traffic_sign->refLines()) { - ret.emplace_back(stop_line); - } - } - } - return ret; -} - -auto HdMapUtils::getStopLinesOnPath(const lanelet::Ids & lanelet_ids) const - -> lanelet::ConstLineStrings3d -{ - lanelet::ConstLineStrings3d ret; - for (const auto & traffic_sign : getTrafficSignRegulatoryElementsOnPath(lanelet_ids)) { - if (traffic_sign->type() == "stop_sign") { - for (const auto & stop_line : traffic_sign->refLines()) { - ret.emplace_back(stop_line); - } - } - } - return ret; -} - -auto HdMapUtils::getStopLineIds() const -> lanelet::Ids -{ - lanelet::Ids stop_line_ids; - for (const auto & ret : getStopLines()) { - stop_line_ids.push_back(ret.id()); - } - return stop_line_ids; -} - -auto HdMapUtils::getStopLineIdsOnPath(const lanelet::Ids & route_lanelets) const -> lanelet::Ids -{ - lanelet::Ids stop_line_ids; - for (const auto & ret : getStopLinesOnPath(route_lanelets)) { - stop_line_ids.push_back(ret.id()); - } - return stop_line_ids; -} - auto HdMapUtils::getTrafficLights(const lanelet::Id traffic_light_id) const -> std::vector { @@ -1337,21 +1161,6 @@ auto HdMapUtils::getTrafficLightStopLinesPoints(const lanelet::Id traffic_light_ return ret; } -auto HdMapUtils::getStopLinePolygon(const lanelet::Id lanelet_id) const - -> std::vector -{ - std::vector points; - const auto stop_line = lanelet_map_ptr_->lineStringLayer.get(lanelet_id); - for (const auto & point : stop_line) { - geometry_msgs::msg::Point p; - p.x = point.x(); - p.y = point.y(); - p.z = point.z(); - points.emplace_back(p); - } - return points; -} - auto HdMapUtils::getTrafficLightIdsOnPath(const lanelet::Ids & route_lanelets) const -> lanelet::Ids { lanelet::Ids ids; @@ -1367,145 +1176,6 @@ auto HdMapUtils::getTrafficLightIdsOnPath(const lanelet::Ids & route_lanelets) c return ids; } -auto HdMapUtils::getDistanceToTrafficLightStopLine( - const lanelet::Ids & route_lanelets, - const std::vector & waypoints) const -> std::optional -{ - auto traffic_light_ids = getTrafficLightIdsOnPath(route_lanelets); - if (traffic_light_ids.empty()) { - return std::nullopt; - } - std::set collision_points; - for (const auto id : traffic_light_ids) { - const auto collision_point = getDistanceToTrafficLightStopLine(waypoints, id); - if (collision_point) { - collision_points.insert(collision_point.value()); - } - } - if (collision_points.empty()) { - return std::nullopt; - } - return *collision_points.begin(); -} - -auto HdMapUtils::getDistanceToTrafficLightStopLine( - const lanelet::Ids & route_lanelets, - const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional -{ - auto traffic_light_ids = getTrafficLightIdsOnPath(route_lanelets); - if (traffic_light_ids.empty()) { - return std::nullopt; - } - std::set collision_points; - for (const auto id : traffic_light_ids) { - const auto collision_point = getDistanceToTrafficLightStopLine(spline, id); - if (collision_point) { - collision_points.insert(collision_point.value()); - } - } - if (collision_points.empty()) { - return std::nullopt; - } - return *collision_points.begin(); -} - -auto HdMapUtils::getDistanceToTrafficLightStopLine( - const std::vector & waypoints, - const lanelet::Id traffic_light_id) const -> std::optional -{ - if (waypoints.empty()) { - return std::nullopt; - } - math::geometry::CatmullRomSpline spline(waypoints); - const auto stop_lines = getTrafficLightStopLinesPoints(traffic_light_id); - for (const auto & stop_line : stop_lines) { - const auto collision_point = spline.getCollisionPointIn2D(stop_line); - if (collision_point) { - return collision_point; - } - } - return std::nullopt; -} - -auto HdMapUtils::getDistanceToTrafficLightStopLine( - const math::geometry::CatmullRomSplineInterface & spline, - const lanelet::Id traffic_light_id) const -> std::optional -{ - if (spline.getLength() <= 0) { - return std::nullopt; - } - const auto stop_lines = getTrafficLightStopLinesPoints(traffic_light_id); - for (const auto & stop_line : stop_lines) { - const auto collision_point = spline.getCollisionPointIn2D(stop_line); - if (collision_point) { - return collision_point; - } - } - return std::nullopt; -} - -auto HdMapUtils::getDistanceToStopLine( - const lanelet::Ids & route_lanelets, - const std::vector & waypoints) const -> std::optional -{ - if (waypoints.empty()) { - return std::nullopt; - } - std::set collision_points; - if (waypoints.empty()) { - return std::nullopt; - } - math::geometry::CatmullRomSpline spline(waypoints); - const auto stop_lines = getStopLinesOnPath({route_lanelets}); - for (const auto & stop_line : stop_lines) { - std::vector stop_line_points; - for (const auto & point : stop_line) { - geometry_msgs::msg::Point p; - p.x = point.x(); - p.y = point.y(); - p.z = point.z(); - stop_line_points.emplace_back(p); - } - const auto collision_point = spline.getCollisionPointIn2D(stop_line_points); - if (collision_point) { - collision_points.insert(collision_point.value()); - } - } - if (collision_points.empty()) { - return std::nullopt; - } - return *collision_points.begin(); -} - -auto HdMapUtils::getDistanceToStopLine( - const lanelet::Ids & route_lanelets, - const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional -{ - if (spline.getLength() <= 0) { - return std::nullopt; - } - std::set collision_points; - const auto stop_lines = getStopLinesOnPath({route_lanelets}); - for (const auto & stop_line : stop_lines) { - std::vector stop_line_points; - for (const auto & point : stop_line) { - geometry_msgs::msg::Point p; - p.x = point.x(); - p.y = point.y(); - p.z = point.z(); - stop_line_points.emplace_back(p); - } - const auto collision_point = spline.getCollisionPointIn2D(stop_line_points); - if (collision_point) { - collision_points.insert(collision_point.value()); - } - } - if (collision_points.empty()) { - return std::nullopt; - } - return *collision_points.begin(); -} - auto HdMapUtils::calculateSegmentDistances(const lanelet::ConstLineString3d & line_string) const -> std::vector { @@ -1775,5 +1445,4 @@ auto HdMapUtils::RoutingGraphs::getRoute( cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); return ids; } - } // namespace hdmap_utils diff --git a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp index 4b969db3b2c..0fc8f0b5456 100644 --- a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp +++ b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -1758,7 +1759,7 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_sameLane) { const auto from = traffic_simulator::helper::constructLaneletPose(3002185, 0.0, 0.5); const auto to = traffic_simulator::helper::constructLaneletPose(3002185, 10.0, 0.2); - const auto result = hdmap_utils.getLateralDistance(from, to); + const auto result = traffic_simulator::lanelet_wrapper::distance::lateralDistance(from, to); EXPECT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), to.offset - from.offset, 1e-3); @@ -1771,10 +1772,9 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_sameLane) */ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_parallelLanesCanNotChange) { - EXPECT_FALSE(hdmap_utils - .getLateralDistance( - traffic_simulator::helper::constructLaneletPose(3002185, 0.0, 0.5), - traffic_simulator::helper::constructLaneletPose(3002184, 10.0, 0.2)) + EXPECT_FALSE(traffic_simulator::lanelet_wrapper::distance::lateralDistance( + traffic_simulator::helper::constructLaneletPose(3002185, 0.0, 0.5), + traffic_simulator::helper::constructLaneletPose(3002184, 10.0, 0.2)) .has_value()); } @@ -1790,8 +1790,8 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_parallelLanesCanCh traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; - const auto result = - hdmap_utils.getLateralDistance(from, to, lane_changeable_routing_configuration); + const auto result = traffic_simulator::lanelet_wrapper::distance::lateralDistance( + from, to, lane_changeable_routing_configuration); EXPECT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 2.80373 / 2.0 + 3.03463 / 2.0 + to.offset - from.offset, 1e-3); @@ -1808,11 +1808,10 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_notConnected) { traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; - EXPECT_FALSE(hdmap_utils - .getLateralDistance( - traffic_simulator::helper::constructLaneletPose(3002185, 0.0, 0.5), - traffic_simulator::helper::constructLaneletPose(3002166, 10.0, 0.2), - lane_changeable_routing_configuration) + EXPECT_FALSE(traffic_simulator::lanelet_wrapper::distance::lateralDistance( + traffic_simulator::helper::constructLaneletPose(3002185, 0.0, 0.5), + traffic_simulator::helper::constructLaneletPose(3002166, 10.0, 0.2), + lane_changeable_routing_configuration) .has_value()); } @@ -2085,7 +2084,7 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLanelet) ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); - const auto result_distance = hdmap_utils.getLongitudinalDistance( + const auto result_distance = traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()); ASSERT_TRUE(result_distance.has_value()); @@ -2106,8 +2105,9 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLaneletBehind) ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); - const auto longitudinal_distance = hdmap_utils.getLongitudinalDistance( - pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()); + const auto longitudinal_distance = + traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()); EXPECT_FALSE(longitudinal_distance.has_value()); } @@ -2125,7 +2125,7 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_differentLanelet) ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); - const auto result_distance = hdmap_utils.getLongitudinalDistance( + const auto result_distance = traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()); ASSERT_TRUE(result_distance.has_value()); @@ -2146,9 +2146,8 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLongitudinalDistance_differentLane ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); - EXPECT_FALSE(hdmap_utils - .getLongitudinalDistance( - pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()) + EXPECT_FALSE(traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()) .has_value()); } @@ -2163,7 +2162,8 @@ TEST_F(HdMapUtilsTest_KashiwanohaMap, getLongitudinalDistance_PullRequest1348) traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; EXPECT_NO_THROW(EXPECT_DOUBLE_EQ( - hdmap_utils.getLongitudinalDistance(pose_from, pose_to, lane_changeable_routing_configuration) + traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration) .value(), 54.18867466433655977198213804513216018676757812500000)); } @@ -2184,11 +2184,13 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) const auto pose_to = traffic_simulator::helper::constructLaneletPose(659L, 5.0); const auto without_lane_change = - hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); + traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( + pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance( - pose_from, pose_to, lane_changeable_routing_configuration); + const auto with_lane_change = + traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 157.0, 1.0); } @@ -2197,11 +2199,13 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) const auto pose_to = traffic_simulator::helper::constructLaneletPose(658L, 5.0); const auto without_lane_change = - hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); + traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( + pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance( - pose_from, pose_to, lane_changeable_routing_configuration); + const auto with_lane_change = + traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 161.0, 1.0); } @@ -2210,11 +2214,13 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) const auto pose_to = traffic_simulator::helper::constructLaneletPose(657L, 5.0); const auto without_lane_change = - hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); + traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( + pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance( - pose_from, pose_to, lane_changeable_routing_configuration); + const auto with_lane_change = + traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 161.0, 1.0); } @@ -2223,11 +2229,13 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) const auto pose_to = traffic_simulator::helper::constructLaneletPose(666L, 5.0); const auto without_lane_change = - hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); + traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( + pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance( - pose_from, pose_to, lane_changeable_routing_configuration); + const auto with_lane_change = + traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 250.0, 1.0); } @@ -2236,11 +2244,13 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) const auto pose_to = traffic_simulator::helper::constructLaneletPose(665L, 5.0); const auto without_lane_change = - hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); + traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( + pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance( - pose_from, pose_to, lane_changeable_routing_configuration); + const auto with_lane_change = + traffic_simulator::lanelet_wrapper::distance::longitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 253.0, 1.0); } @@ -2253,7 +2263,9 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) TEST_F(HdMapUtilsTest_StandardMap, getStopLineIdsOnPath_noStopLines) { EXPECT_EQ( - hdmap_utils.getStopLineIdsOnPath({34507, 34795, 34606, 34672}).size(), static_cast(0)); + traffic_simulator::lanelet_wrapper::lanelet_map::stopLineIdsOnPath({34507, 34795, 34606, 34672}) + .size(), + static_cast(0)); } /** @@ -2263,7 +2275,8 @@ TEST_F(HdMapUtilsTest_StandardMap, getStopLineIdsOnPath_noStopLines) TEST_F(HdMapUtilsTest_StandardMap, getStopLineIdsOnPath_someStopLines) { EXPECT_EQ( - hdmap_utils.getStopLineIdsOnPath({34408, 34633, 34579, 34780, 34675, 34744, 34690}), + traffic_simulator::lanelet_wrapper::lanelet_map::stopLineIdsOnPath( + {34408, 34633, 34579, 34780, 34675, 34744, 34690}), (lanelet::Ids{120635})); } @@ -2272,7 +2285,9 @@ TEST_F(HdMapUtilsTest_StandardMap, getStopLineIdsOnPath_someStopLines) */ TEST_F(HdMapUtilsTest_StandardMap, getStopLineIdsOnPath_empty) { - EXPECT_EQ(hdmap_utils.getStopLineIdsOnPath(lanelet::Ids{}).size(), static_cast(0)); + EXPECT_EQ( + traffic_simulator::lanelet_wrapper::lanelet_map::stopLineIdsOnPath(lanelet::Ids{}).size(), + static_cast(0)); } /** @@ -2401,7 +2416,8 @@ TEST_F( */ TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getStopLinePolygon_stopLine) { - const auto result_stoplines_points = hdmap_utils.getStopLinePolygon(lanelet::Id{120663}); + const auto result_stoplines_points = + traffic_simulator::lanelet_wrapper::lanelet_map::stopLinePolygon(lanelet::Id{120663}); const auto actual_stoplines_points = std::vector{ makePoint(3768.5, 73737.5, -0.5), makePoint(3765.5, 73735.5, -0.5)}; @@ -2416,7 +2432,8 @@ TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getStopLinePolygon_stopLine) */ TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getStopLinePolygon_invalidLaneletId) { - EXPECT_THROW(hdmap_utils.getStopLinePolygon(1000039), std::runtime_error); + EXPECT_THROW( + traffic_simulator::lanelet_wrapper::lanelet_map::stopLinePolygon(1000039), std::runtime_error); } /** @@ -2428,10 +2445,11 @@ TEST_F( HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToTrafficLightStopLine_trafficLightOnSpline) { const auto start_waypoint = makePoint(3771.06, 73728.35); - const auto result_distance = hdmap_utils.getDistanceToTrafficLightStopLine( - math::geometry::CatmullRomSpline(std::vector{ - start_waypoint, makePoint(3756.30, 73755.87), makePoint(3746.90, 73774.44)}), - lanelet::Id{34836}); + const auto result_distance = + traffic_simulator::lanelet_wrapper::distance::distanceToTrafficLightStopLine( + math::geometry::CatmullRomSpline(std::vector{ + start_waypoint, makePoint(3756.30, 73755.87), makePoint(3746.90, 73774.44)}), + lanelet::Id{34836}); EXPECT_TRUE(result_distance.has_value()); const auto stopline_midpoint = makePoint(3767.00, 73736.47); @@ -2450,13 +2468,12 @@ TEST_F( HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToTrafficLightStopLine_noTrafficLightOnSpline) { - EXPECT_FALSE(hdmap_utils - .getDistanceToTrafficLightStopLine( - math::geometry::CatmullRomSpline(std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), - makePoint(3773.19, 73723.27)}), - lanelet::Id{34836}) - .has_value()); + EXPECT_FALSE( + traffic_simulator::lanelet_wrapper::distance::distanceToTrafficLightStopLine( + math::geometry::CatmullRomSpline(std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}), + lanelet::Id{34836}) + .has_value()); } /** @@ -2469,10 +2486,11 @@ TEST_F( getDistanceToTrafficLightStopLine_trafficLightOnWaypoints) { const auto start_waypoint = makePoint(3771.06, 73728.35); - const auto result_distance = hdmap_utils.getDistanceToTrafficLightStopLine( - std::vector{ - start_waypoint, makePoint(3756.30, 73755.87), makePoint(3746.90, 73774.44)}, - lanelet::Id{34836}); + const auto result_distance = + traffic_simulator::lanelet_wrapper::distance::distanceToTrafficLightStopLine( + std::vector{ + start_waypoint, makePoint(3756.30, 73755.87), makePoint(3746.90, 73774.44)}, + lanelet::Id{34836}); EXPECT_TRUE(result_distance.has_value()); const auto stopline_midpoint = makePoint(3767.00, 73736.47); @@ -2492,11 +2510,11 @@ TEST_F( getDistanceToTrafficLightStopLine_noTrafficLightOnWaypoints) { EXPECT_FALSE( - hdmap_utils - .getDistanceToTrafficLightStopLine( - std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}, - lanelet::Id{34836}) + + traffic_simulator::lanelet_wrapper::distance::distanceToTrafficLightStopLine( + std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}, + lanelet::Id{34836}) .has_value()); } @@ -2507,9 +2525,8 @@ TEST_F( HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToTrafficLightStopLine_emptyVector_waypoints) { - EXPECT_FALSE(hdmap_utils - .getDistanceToTrafficLightStopLine( - std::vector{}, lanelet::Id{34836}) + EXPECT_FALSE(traffic_simulator::lanelet_wrapper::distance::distanceToTrafficLightStopLine( + std::vector{}, lanelet::Id{34836}) .has_value()); } @@ -2524,10 +2541,11 @@ TEST_F( { const auto start_waypoint = makePoint(3771.06, 73728.35); - const auto result_distance = hdmap_utils.getDistanceToTrafficLightStopLine( - lanelet::Ids{34576, 34570, 34564}, - math::geometry::CatmullRomSpline(std::vector{ - start_waypoint, makePoint(3756.30, 73755.87), makePoint(3746.90, 73774.44)})); + const auto result_distance = + traffic_simulator::lanelet_wrapper::distance::distanceToTrafficLightStopLine( + lanelet::Ids{34576, 34570, 34564}, + math::geometry::CatmullRomSpline(std::vector{ + start_waypoint, makePoint(3756.30, 73755.87), makePoint(3746.90, 73774.44)})); EXPECT_TRUE(result_distance.has_value()); const auto stopline_midpoint = makePoint(3767.00, 73736.47); @@ -2546,13 +2564,12 @@ TEST_F( HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToTrafficLightStopLine_routeWithNoTrafficLightsOnSplineCongruent) { - EXPECT_FALSE(hdmap_utils - .getDistanceToTrafficLightStopLine( - lanelet::Ids{34690, 34759, 34576}, - math::geometry::CatmullRomSpline(std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), - makePoint(3773.19, 73723.27)})) - .has_value()); + EXPECT_FALSE( + traffic_simulator::lanelet_wrapper::distance::distanceToTrafficLightStopLine( + lanelet::Ids{34690, 34759, 34576}, + math::geometry::CatmullRomSpline(std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)})) + .has_value()); } /** @@ -2566,13 +2583,12 @@ TEST_F( HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToTrafficLightStopLine_routeWithTrafficLightsNotOnSplineIncongruent) { - EXPECT_FALSE(hdmap_utils - .getDistanceToTrafficLightStopLine( - lanelet::Ids{34576, 34570, 34564}, - math::geometry::CatmullRomSpline(std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), - makePoint(3773.19, 73723.27)})) - .has_value()); + EXPECT_FALSE( + traffic_simulator::lanelet_wrapper::distance::distanceToTrafficLightStopLine( + lanelet::Ids{34576, 34570, 34564}, + math::geometry::CatmullRomSpline(std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)})) + .has_value()); } /** @@ -2583,11 +2599,10 @@ TEST_F( getDistanceToTrafficLightStopLine_emptyVector_splineRoute) { EXPECT_FALSE( - hdmap_utils - .getDistanceToTrafficLightStopLine( - lanelet::Ids{}, math::geometry::CatmullRomSpline(std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), - makePoint(3773.19, 73723.27)})) + traffic_simulator::lanelet_wrapper::distance::distanceToTrafficLightStopLine( + lanelet::Ids{}, + math::geometry::CatmullRomSpline(std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)})) .has_value()); } @@ -2602,10 +2617,11 @@ TEST_F( { const auto start_waypoint = makePoint(3771.06, 73728.35); - const auto result_distance = hdmap_utils.getDistanceToTrafficLightStopLine( - lanelet::Ids{34576, 34570, 34564}, - std::vector{ - start_waypoint, makePoint(3756.30, 73755.87), makePoint(3746.90, 73774.44)}); + const auto result_distance = + traffic_simulator::lanelet_wrapper::distance::distanceToTrafficLightStopLine( + lanelet::Ids{34576, 34570, 34564}, + std::vector{ + start_waypoint, makePoint(3756.30, 73755.87), makePoint(3746.90, 73774.44)}); EXPECT_TRUE(result_distance.has_value()); const auto stopline_midpoint = makePoint(3767.00, 73736.47); @@ -2627,11 +2643,11 @@ TEST_F( getDistanceToTrafficLightStopLine_routeWithNoTrafficLightsOnWaypointsIncongruent) { EXPECT_FALSE( - hdmap_utils - .getDistanceToTrafficLightStopLine( - lanelet::Ids{34690, 34759, 34576}, - std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) + + traffic_simulator::lanelet_wrapper::distance::distanceToTrafficLightStopLine( + lanelet::Ids{34690, 34759, 34576}, + std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) .has_value()); } @@ -2645,11 +2661,11 @@ TEST_F( getDistanceToTrafficLightStopLine_routeWithTrafficLightsNotOnWaypointsCongruent) { EXPECT_FALSE( - hdmap_utils - .getDistanceToTrafficLightStopLine( - lanelet::Ids{34576, 34570, 34564}, - std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) + + traffic_simulator::lanelet_wrapper::distance::distanceToTrafficLightStopLine( + lanelet::Ids{34576, 34570, 34564}, + std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) .has_value()); } @@ -2661,11 +2677,11 @@ TEST_F( getDistanceToTrafficLightStopLine_emptyVector_waypointsRoute) { EXPECT_FALSE( - hdmap_utils - .getDistanceToTrafficLightStopLine( - lanelet::Ids{}, - std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) + + traffic_simulator::lanelet_wrapper::distance::distanceToTrafficLightStopLine( + lanelet::Ids{}, + std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) .has_value()); } @@ -2677,7 +2693,7 @@ TEST_F( TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_stopLineOnSpline) { const auto start_waypoint = makePoint(3821.86, 73777.20); - const auto result_distance = hdmap_utils.getDistanceToStopLine( + const auto result_distance = traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( lanelet::Ids{34780, 34675, 34744}, math::geometry::CatmullRomSpline(std::vector{ start_waypoint, makePoint(3837.28, 73762.67), makePoint(3846.10, 73741.38)})); @@ -2696,13 +2712,12 @@ TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_stopLine */ TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_noStopLineOnSplineCongruent) { - EXPECT_FALSE(hdmap_utils - .getDistanceToStopLine( - lanelet::Ids{34690, 34759, 34576}, - math::geometry::CatmullRomSpline(std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), - makePoint(3773.19, 73723.27)})) - .has_value()); + EXPECT_FALSE( + traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( + lanelet::Ids{34690, 34759, 34576}, + math::geometry::CatmullRomSpline(std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)})) + .has_value()); } /** @@ -2715,13 +2730,12 @@ TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_noStopLi TEST_F( HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_noStopLineOnSplineIncongruent) { - EXPECT_FALSE(hdmap_utils - .getDistanceToStopLine( - lanelet::Ids{34576, 34570, 34564}, - math::geometry::CatmullRomSpline(std::vector{ - makePoint(3821.86, 73777.20), makePoint(3837.28, 73762.67), - makePoint(3846.10, 73741.38)})) - .has_value()); + EXPECT_FALSE( + traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( + lanelet::Ids{34576, 34570, 34564}, + math::geometry::CatmullRomSpline(std::vector{ + makePoint(3821.86, 73777.20), makePoint(3837.28, 73762.67), makePoint(3846.10, 73741.38)})) + .has_value()); } /** @@ -2730,11 +2744,10 @@ TEST_F( TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_emptyVector_spline) { EXPECT_FALSE( - hdmap_utils - .getDistanceToStopLine( - lanelet::Ids{}, math::geometry::CatmullRomSpline(std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), - makePoint(3773.19, 73723.27)})) + traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( + lanelet::Ids{}, + math::geometry::CatmullRomSpline(std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)})) .has_value()); } @@ -2746,7 +2759,7 @@ TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_emptyVec TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_stopLineOnWaypoints) { const auto start_waypoint = makePoint(3821.86, 73777.20); - const auto result_distance = hdmap_utils.getDistanceToStopLine( + const auto result_distance = traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( lanelet::Ids{34780, 34675, 34744}, std::vector{ start_waypoint, makePoint(3837.28, 73762.67), makePoint(3846.10, 73741.38)}); @@ -2767,11 +2780,10 @@ TEST_F( HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_noStopLineOnWaypointsCongruent) { EXPECT_FALSE( - hdmap_utils - .getDistanceToStopLine( - lanelet::Ids{34690, 34759, 34576}, - std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) + traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( + lanelet::Ids{34690, 34759, 34576}, + std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) .has_value()); } @@ -2786,11 +2798,10 @@ TEST_F( HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_noStopLineOnWaypointsIncongruent) { EXPECT_FALSE( - hdmap_utils - .getDistanceToStopLine( - lanelet::Ids{34576, 34570, 34564}, - std::vector{ - makePoint(3821.86, 73777.20), makePoint(3837.28, 73762.67), makePoint(3846.10, 73741.38)}) + traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( + lanelet::Ids{34576, 34570, 34564}, + std::vector{ + makePoint(3821.86, 73777.20), makePoint(3837.28, 73762.67), makePoint(3846.10, 73741.38)}) .has_value()); } @@ -2800,11 +2811,10 @@ TEST_F( TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getDistanceToStopLine_emptyVector_waypoints) { EXPECT_FALSE( - hdmap_utils - .getDistanceToStopLine( - lanelet::Ids{}, - std::vector{ - makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) + traffic_simulator::lanelet_wrapper::distance::distanceToStopLine( + lanelet::Ids{}, + std::vector{ + makePoint(3807.63, 73715.99), makePoint(3785.76, 73707.70), makePoint(3773.19, 73723.27)}) .has_value()); } From 4b3a45c31ee2a57fd528fa0aa3bfaf38f3ac9d26 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 5 Dec 2024 13:41:47 +0100 Subject: [PATCH 5/9] ref(traffic_simulator): little lanelet_wrapper refactor --- .../behavior_tree_plugin/src/action_node.cpp | 16 ++++++++-------- .../include/simulation_interface/conversions.hpp | 15 ++++++--------- .../behavior/behavior_plugin_base.hpp | 14 +++++++------- .../lanelet_wrapper/distance.hpp | 9 +-------- .../lanelet_wrapper/lanelet_wrapper.hpp | 2 ++ .../traffic_simulator/lanelet_wrapper/route.hpp | 2 +- .../lanelet_wrapper/traffic_lights.hpp | 1 - .../include/traffic_simulator/utils/distance.hpp | 3 ++- .../src/lanelet_wrapper/distance.cpp | 5 ++--- .../src/lanelet_wrapper/pose.cpp | 1 - .../src/lanelet_wrapper/route.cpp | 1 - 11 files changed, 29 insertions(+), 40 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index f51f59d88c3..ea989e79af0 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -93,13 +93,13 @@ auto ActionNode::getBlackBoardValues() -> void auto ActionNode::getOtherEntitiesCanonicalizedLaneletPoses() const -> std::vector { - std::vector other_poses; - for (const auto & [entity_name, entity_status] : other_entity_status) { - if (auto const canonicalized_lanelet_pose = entity_status.getCanonicalizedLaneletPose()) { - other_poses.push_back(canonicalized_lanelet_pose.value()); + std::vector other_canonicalized_lanelet_poses; + for (const auto & [name, status] : other_entity_status) { + if (auto const canonicalized_lanelet_pose = status.getCanonicalizedLaneletPose()) { + other_canonicalized_lanelet_poses.push_back(canonicalized_lanelet_pose.value()); } } - return other_poses; + return other_canonicalized_lanelet_poses; } auto ActionNode::getHorizon() const -> double @@ -127,10 +127,10 @@ auto ActionNode::getYieldStopDistance(const lanelet::Ids & following_lanelets) c if ( const auto canonicalized_lanelet_pose = canonicalized_entity_status->getCanonicalizedLaneletPose()) { - if (const auto other_poses = getOtherEntitiesCanonicalizedLaneletPoses(); - !other_poses.empty()) { + if (const auto other_canonicalized_lanelet_poses = getOtherEntitiesCanonicalizedLaneletPoses(); + !other_canonicalized_lanelet_poses.empty()) { traffic_simulator::distance::distanceToYieldStop( - canonicalized_lanelet_pose.value(), following_lanelets, other_poses); + canonicalized_lanelet_pose.value(), following_lanelets, other_canonicalized_lanelet_poses); } } return std::nullopt; diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 4ec8320616b..aca2245de55 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -168,9 +168,9 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time) void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto); void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); -#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ - auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ - auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void +#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ + auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &) -> void; \ + auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &) -> void DEFINE_CONVERSION(autoware_auto_control_msgs, AckermannLateralCommand); DEFINE_CONVERSION(autoware_auto_control_msgs, LongitudinalCommand); @@ -192,8 +192,7 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr - { + auto convert_color = [](auto color) constexpr { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -208,8 +207,7 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr - { + auto convert_shape = [](auto shape) constexpr { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -237,8 +235,7 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr - { + auto convert_status = [](auto status) constexpr { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index f4162f3e9b0..c4dac740c60 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -44,13 +44,13 @@ class BehaviorPluginBase virtual auto update(const double current_time, const double step_time) -> void = 0; virtual const std::string & getCurrentAction() const = 0; -#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const->const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const -> const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp index 04705251ee3..3aa2ce01f6e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp @@ -18,9 +18,7 @@ #include -#include -#include -#include +#include namespace traffic_simulator { @@ -28,11 +26,6 @@ namespace lanelet_wrapper { namespace distance { -using Point = geometry_msgs::msg::Point; -using Spline = math::geometry::CatmullRomSpline; -using SplineInterface = math::geometry::CatmullRomSplineInterface; -using LaneletPose = traffic_simulator_msgs::msg::LaneletPose; - auto lateralDistance( const LaneletPose & from, const LaneletPose & to, const RoutingConfiguration & routing_configuration = RoutingConfiguration()) diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp index 765e73079eb..b75a61fef86 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -65,6 +66,7 @@ using Point = geometry_msgs::msg::Point; using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; using Spline = math::geometry::CatmullRomSpline; +using SplineInterface = math::geometry::CatmullRomSplineInterface; using Vector3 = geometry_msgs::msg::Vector3; class RouteCache diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp index b8ac35388c5..0d4a91be7de 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp @@ -17,7 +17,7 @@ #include -#include +#include namespace traffic_simulator { diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_lights.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_lights.hpp index 3eb5f603ef2..cb4d91ca08a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_lights.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_lights.hpp @@ -18,7 +18,6 @@ #define TRAFFIC_SIMULATOR__LANELET_WRAPPER_TRAFFIC_LIGHTS_HPP_ #include -#include #include #include diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index d810952721f..a6feeafa9de 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -74,7 +74,8 @@ auto boundingBoxLaneLongitudinalDistance( // Bounds auto distanceToLaneBound( const geometry_msgs::msg::Pose & map_pose, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id) -> double; + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id) + -> double; auto distanceToLaneBound( const geometry_msgs::msg::Pose & map_pose, diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp index 631ca1bf734..dddd02c8334 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp @@ -55,8 +55,7 @@ auto lateralDistance( pose::toMapPose(current_lanelet_pose).pose, route[i + 1], matching_distance)) { lateral_distance_by_lane_change -= current_lanelet_origin_from_next_lanelet->offset; } else { - /// @todo maybe an exception should be thrown here? since the route exists but is - /// incorrect? + /// @todo maybe an exception should be thrown here? since the route exists but is incorrect? return std::nullopt; } } @@ -242,7 +241,7 @@ auto distanceToTrafficLightStopLine( return distanceToTrafficLightStopLine(route_lanelets, Spline{route_waypoints}); } -// crosswalk +// Crosswalk auto distanceToCrosswalk(const std::vector & route_waypoints, const lanelet::Id crosswalk_id) -> std::optional { diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 4ff31179246..51e2961df3c 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -22,7 +22,6 @@ #include #include #include -#include namespace traffic_simulator { diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp index 2d812b62b40..0b08feae26a 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp @@ -23,7 +23,6 @@ #include #include #include -#include namespace traffic_simulator { From b418a3a4d3ad8b976ab884a24a6ebc5e2e9a8a69 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Thu, 5 Dec 2024 20:59:30 +0100 Subject: [PATCH 6/9] ref(clang): revert undesired changes --- .../include/simulation_interface/conversions.hpp | 15 +++++++++------ .../behavior/behavior_plugin_base.hpp | 14 +++++++------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index aca2245de55..4ec8320616b 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -168,9 +168,9 @@ void toMsg(const rosgraph_msgs::Clock & proto, rosgraph_msgs::msg::Clock & time) void toProto(const std_msgs::msg::Header & header, std_msgs::Header & proto); void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); -#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ - auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &) -> void; \ - auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &) -> void +#define DEFINE_CONVERSION(PACKAGE, TYPENAME) \ + auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ + auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void DEFINE_CONVERSION(autoware_auto_control_msgs, AckermannLateralCommand); DEFINE_CONVERSION(autoware_auto_control_msgs, LongitudinalCommand); @@ -192,7 +192,8 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr { + auto convert_color = [](auto color) constexpr + { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -207,7 +208,8 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr { + auto convert_shape = [](auto shape) constexpr + { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -235,7 +237,8 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr { + auto convert_status = [](auto status) constexpr + { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index c4dac740c60..f4162f3e9b0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -44,13 +44,13 @@ class BehaviorPluginBase virtual auto update(const double current_time, const double step_time) -> void = 0; virtual const std::string & getCurrentAction() const = 0; -#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ - virtual TYPE get##NAME() = 0; \ - virtual void set##NAME(const TYPE & value) = 0; \ - auto get##NAME##Key() const -> const std::string & \ - { \ - static const std::string key = KEY; \ - return key; \ +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + virtual TYPE get##NAME() = 0; \ + virtual void set##NAME(const TYPE & value) = 0; \ + auto get##NAME##Key() const->const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ } // clang-format off From 44d489619e7cb2714ab2481666243a2e2b3a928f Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Mon, 9 Dec 2024 13:27:17 +0100 Subject: [PATCH 7/9] Trigger CI pipeline From 768b84e27a7b658af1a8a15deb385499c3577175 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 10 Dec 2024 15:47:27 +0100 Subject: [PATCH 8/9] ref(traffic_simulator): improve hdmaputils::countLaneChanges --- .../src/hdmap_utils/hdmap_utils.cpp | 27 +++++++++---------- 1 file changed, 12 insertions(+), 15 deletions(-) diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 02b11bed002..f11a2ddfaa3 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -86,7 +86,6 @@ HdMapUtils::HdMapUtils( // If route is not specified, the lanelet_id with the lowest array index is used as a candidate for // canonicalize destination. - auto HdMapUtils::countLaneChanges( const traffic_simulator_msgs::msg::LaneletPose & from, const traffic_simulator_msgs::msg::LaneletPose & to, @@ -102,20 +101,18 @@ auto HdMapUtils::countLaneChanges( for (std::size_t i = 1; i < route.size(); ++i) { const auto & previous = route[i - 1]; const auto & current = route[i]; - - if (auto followings = - lanelet_map::nextLaneletIds(previous, routing_configuration.routing_graph_type); - std::find(followings.begin(), followings.end(), current) == followings.end()) { - if (auto lefts = pose::leftLaneletIds( - previous, routing_configuration.routing_graph_type, include_opposite_direction); - std::find(lefts.begin(), lefts.end(), current) != lefts.end()) { - lane_changes.first++; - } else if (auto rights = pose::rightLaneletIds( - previous, routing_configuration.routing_graph_type, - include_opposite_direction); - std::find(rights.begin(), rights.end(), current) != rights.end()) { - lane_changes.second++; - } + if (const auto followings = lanelet_map::nextLaneletIds( + previous, routing_configuration.routing_graph_type, include_opposite_direction); + std::find(followings.begin(), followings.end(), current) != followings.end()) { + continue; + } else if (const auto lefts = pose::leftLaneletIds( + previous, routing_configuration.routing_graph_type, include_opposite_direction); + std::find(lefts.begin(), lefts.end(), current) != lefts.end()) { + lane_changes.first++; + } else if (const auto rights = pose::rightLaneletIds( + previous, routing_configuration.routing_graph_type, include_opposite_direction); + std::find(rights.begin(), rights.end(), current) != rights.end()) { + lane_changes.second++; } } return lane_changes; From 0394908e3e2632142588fe5623bc4d24b62eb499 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 18 Dec 2024 09:11:28 +0100 Subject: [PATCH 9/9] fix(traffic_simulator): fix after merge --- .../traffic_simulator/data_type/routing_configuration.hpp | 4 ++++ simulation/traffic_simulator/src/utils/pose.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_configuration.hpp index 4408d9d750f..bdd89d71d98 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_configuration.hpp @@ -22,6 +22,10 @@ namespace traffic_simulator { struct RoutingConfiguration { + RoutingConfiguration() = default; + explicit RoutingConfiguration(const bool allow_lane_change) + : allow_lane_change(allow_lane_change){}; + bool allow_lane_change = false; traffic_simulator::RoutingGraphType routing_graph_type = traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER; diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 991e00ea15d..13ba0fe74cc 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -225,7 +225,7 @@ auto relativeLaneletPose( const RoutingConfiguration & routing_configuration) -> LaneletPose { constexpr bool include_adjacent_lanelet{false}; - constexpr bool include_opposite_direction{true}; + constexpr bool include_opposite_direction{false}; LaneletPose position = quietNaNLaneletPose(); // here the s and offset are intentionally assigned independently, even if