From 5e825534f763cae0e977cc3f3905e4b788be941c Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 3 Dec 2024 15:53:00 +0100 Subject: [PATCH 01/62] feat(traffic_simulator): add lanelet_wrapper as a replacement for hdmap_utils, at this point mainly for pose calculations --- simulation/traffic_simulator/CMakeLists.txt | 21 +- .../lanelet_wrapper/distance.hpp | 83 ++++ .../lanelet_wrapper/lane_change.hpp | 74 +++ .../lanelet_wrapper/lanelet_map.hpp | 127 +++++ .../lanelet_wrapper/lanelet_wrapper.hpp | 296 ++++++++++++ .../lanelet_wrapper/pose.hpp | 106 ++++ .../lanelet_wrapper/route.hpp | 53 ++ .../lanelet_wrapper/traffic_lights.hpp | 61 +++ .../lanelet_wrapper/traffic_rules.hpp | 74 +++ .../traffic_simulator/utils/lanelet_map.hpp | 52 ++ .../src/lanelet_wrapper/distance.cpp | 318 ++++++++++++ .../src/lanelet_wrapper/lane_change.cpp | 245 ++++++++++ .../src/lanelet_wrapper/lanelet_map.cpp | 345 +++++++++++++ .../src/lanelet_wrapper/lanelet_wrapper.cpp | 257 ++++++++++ .../src/lanelet_wrapper/pose.cpp | 453 ++++++++++++++++++ .../src/lanelet_wrapper/route.cpp | 256 ++++++++++ .../src/lanelet_wrapper/traffic_lights.cpp | 187 ++++++++ .../src/lanelet_wrapper/traffic_rules.cpp | 27 ++ .../src/utils/lanelet_map.cpp | 121 +++++ 19 files changed, 3150 insertions(+), 6 deletions(-) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp create mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lane_change.hpp create mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp create mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp create mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.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/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp create mode 100644 simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/lane_change.cpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/route.cpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/traffic_lights.cpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp create mode 100644 simulation/traffic_simulator/src/utils/lanelet_map.cpp diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index 9394ec74af8..abe46dd36a3 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -47,20 +47,29 @@ ament_auto_add_library(traffic_simulator SHARED src/hdmap_utils/hdmap_utils.cpp src/hdmap_utils/traffic_rules.cpp src/helper/helper.cpp - src/job/job_list.cpp src/job/job.cpp + src/job/job_list.cpp + # src/lanelet_wrapper/distance.cpp + # src/lanelet_wrapper/lane_change.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 + src/traffic/traffic_sink.cpp + src/traffic/traffic_source.cpp src/traffic_lights/configurable_rate_updater.cpp + src/traffic_lights/traffic_light.cpp src/traffic_lights/traffic_light_marker_publisher.cpp src/traffic_lights/traffic_light_publisher.cpp - src/traffic_lights/traffic_light.cpp - src/traffic_lights/traffic_lights_base.cpp src/traffic_lights/traffic_lights.cpp - src/traffic/traffic_controller.cpp - src/traffic/traffic_sink.cpp - src/traffic/traffic_source.cpp + src/traffic_lights/traffic_lights_base.cpp src/utils/distance.cpp src/utils/pose.cpp + src/utils/lanelet_map.cpp ) ament_auto_add_library(visualization_component SHARED 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..a755d7c5e5b --- /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 +// #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/lane_change.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lane_change.hpp new file mode 100644 index 00000000000..8b7d0972fea --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lane_change.hpp @@ -0,0 +1,74 @@ +// 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_LANE_CHANGE_HPP_ +// #define TRAFFIC_SIMULATOR__LANELET_WRAPPER_LANE_CHANGE_HPP_ + +// #include + +// #include +// #include +// #include + +// namespace traffic_simulator +// { +// namespace lanelet_wrapper +// { +// namespace lane_change +// { +// using Pose = geometry_msgs::msg::Pose; +// using Vector3 = geometry_msgs::msg::Vector3; +// using Curve = math::geometry::HermiteCurve; + +// using Parameter = traffic_simulator::lane_change::Parameter; +// using Direction = traffic_simulator::lane_change::Direction; +// using Constraint = traffic_simulator::lane_change::Constraint; +// using TrajectoryShape = traffic_simulator::lane_change::TrajectoryShape; + +// auto canChangeLane( +// const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, +// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> bool; + +// auto laneChangeableLaneletId( +// const lanelet::Id lanelet_id, const Direction & direction, +// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) +// -> std::optional; + +// auto laneChangeableLaneletId( +// const lanelet::Id lanelet_id, const Direction & direction, const std::uint8_t shift, +// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) +// -> std::optional; + +// auto countLaneChanges( +// const lanelet::Id & from_lanelet_id, const lanelet::Id & to_lanelet_id, +// const RoutingConfiguration & routing_configuration = RoutingConfiguration()) +// -> std::optional>; + +// // Trajectory +// auto laneChangeTrajectory( +// const Pose & from_pose, const LaneletPose & to_lanelet_pose, +// const TrajectoryShape & trajectory_shape, const double tangent_vector_size) -> Curve; + +// auto laneChangeTrajectory( +// const LaneletPose & from_lanelet_pose, const Parameter & lane_change_parameter) +// -> std::optional>; + +// auto laneChangeTrajectory( +// const Pose & from_pose, const Parameter & lane_change_parameter, +// const double maximum_curvature_threshold, const double target_trajectory_length, +// const double forward_distance_threshold) -> std::optional>; +// } // namespace lane_change +// } // namespace lanelet_wrapper +// } // namespace traffic_simulator +// #endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_LANE_CHANGE_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 new file mode 100644 index 00000000000..7f0901c6d11 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp @@ -0,0 +1,127 @@ +// 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_LANELET_MAP_HPP_ +#define TRAFFIC_SIMULATOR__LANELET_WRAPPER_LANELET_MAP_HPP_ + +#include +#include + +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace lanelet_map +{ +using Point = geometry_msgs::msg::Point; +using Spline = math::geometry::CatmullRomSpline; + +auto isInLanelet(const lanelet::Id lanelet_id, const double lanelet_pose_s) -> bool; + +auto isInLanelet(const lanelet::Id lanelet_id, const Point point) -> bool; + +auto laneletLength(const lanelet::Id lanelet_id) -> double; + +// auto laneletYaw(const lanelet::Id lanelet_id, const Point & point) +// -> std::tuple; + +template +auto laneletIds(const std::vector & lanelets) -> lanelet::Ids +{ + lanelet::Ids ids; + std::transform( + lanelets.begin(), lanelets.end(), std::back_inserter(ids), + [](const auto & lanelet) { return lanelet.id(); }); + return ids; +} + +auto laneletIds() -> lanelet::Ids; + +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; + +auto nextLaneletIds( + const lanelet::Ids & lanelet_ids, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +auto nextLaneletIds( + const lanelet::Id lanelet_id, const std::string & turn_direction, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +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; + +auto previousLaneletIds( + const lanelet::Ids & lanelet_ids, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +auto previousLaneletIds( + const lanelet::Id lanelet_id, const std::string & turn_direction, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +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; + +// auto conflictingCrosswalkIds(const lanelet::Ids & lanelet_ids) -> lanelet::Ids; + +// auto conflictingLaneIds( +// const lanelet::Ids & lanelet_ids, +// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; +} // namespace lanelet_map +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_LANELET_MAP_HPP_ 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 new file mode 100644 index 00000000000..801120f590f --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp @@ -0,0 +1,296 @@ +// 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_HPP_ +#define TRAFFIC_SIMULATOR__LANELET_WRAPPER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace std +{ +template <> +struct hash> +{ +public: + size_t operator()(const std::tuple & data) const + { + std::hash lanelet_id_hash; + size_t seed = 0; + // hash combine like boost library + seed ^= lanelet_id_hash(std::get<0>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + seed ^= lanelet_id_hash(std::get<1>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + seed ^= std::hash{}(std::get<2>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + return seed; + } +}; +} // namespace std + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +using Point = geometry_msgs::msg::Point; +using Spline = math::geometry::CatmullRomSpline; + +class RouteCache +{ +public: + auto getRoute( + const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, + const lanelet::LaneletMapPtr & lanelet_map, + const traffic_simulator::RoutingConfiguration & routing_configuration, + const lanelet::routing::RoutingGraphConstPtr & routing_graph) -> lanelet::Ids + { + if (!exists(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change)) { + constexpr int routing_cost_id = 0; + const auto & from_lanelet = lanelet_map->laneletLayer.get(from_lanelet_id); + const auto & to_lanelet = lanelet_map->laneletLayer.get(to_lanelet_id); + if (const auto route = routing_graph->getRoute( + from_lanelet, to_lanelet, routing_cost_id, routing_configuration.allow_lane_change); + !route || route->shortestPath().empty()) { + appendData( + from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, lanelet::Ids()); + } else { + lanelet::Ids shortest_path_ids; + for (const auto & lanelet : route->shortestPath()) { + shortest_path_ids.push_back(lanelet.id()); + } + appendData( + from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, + shortest_path_ids); + } + } + std::lock_guard lock(mutex_); + return data_.at({from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change}); + } + + auto getRoute(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) + -> decltype(auto) + { + if (!exists(from, to, allow_lane_change)) { + THROW_SIMULATION_ERROR( + "route from : ", from, " to : ", to, (allow_lane_change ? " with" : " without"), + " lane change does not exists on route cache."); + } else { + std::lock_guard lock(mutex_); + return data_.at({from, to, allow_lane_change}); + } + } + + std::unordered_map, lanelet::Ids> data_; + std::mutex mutex_; + + auto exists(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) -> bool + { + std::lock_guard lock(mutex_); + std::tuple key = {from, to, allow_lane_change}; + return data_.find(key) != data_.end(); + } + + auto appendData( + const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change, + const lanelet::Ids & route) -> void + { + std::lock_guard lock(mutex_); + data_[{from, to, allow_lane_change}] = route; + } +}; + +class CenterPointsCache +{ +public: + auto centerPoints(lanelet::Id lanelet_id) -> decltype(auto) + { + if (!exists(lanelet_id)) { + THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); + } + std::lock_guard lock(mutex_); + return data_.at(lanelet_id); + } + + auto centerPointsSpline(lanelet::Id lanelet_id) -> decltype(auto) + { + if (!exists(lanelet_id)) { + THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); + } + std::lock_guard lock(mutex_); + return splines_[lanelet_id]; + } + + auto getCenterPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) + -> std::vector + { + if (!exists(lanelet_id)) { + appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); + } + std::lock_guard lock(mutex_); + return data_.at(lanelet_id); + } + + auto getCenterPointsSpline( + const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) + -> std::shared_ptr + { + if (!exists(lanelet_id)) { + appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); + } + std::lock_guard lock(mutex_); + return splines_[lanelet_id]; + } + + std::unordered_map> data_; + std::unordered_map> splines_; + std::mutex mutex_; + + auto exists(const lanelet::Id lanelet_id) -> bool + { + std::lock_guard lock(mutex_); + return data_.find(lanelet_id) != data_.end(); + } + + auto appendData(const lanelet::Id lanelet_id, const std::vector & route) -> void + { + std::lock_guard lock(mutex_); + data_[lanelet_id] = route; + splines_[lanelet_id] = std::make_shared(route); + } + + auto centerPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) + -> std::vector + { + std::vector center_points; + for (const auto & point : lanelet_map->laneletLayer.get(lanelet_id).centerline()) { + center_points.push_back(geometry_msgs::build().x(point.x()).y(point.y()).z(point.z())); + } + if (center_points.size() == 2) { + const auto p0 = center_points[0]; + const auto p2 = center_points[1]; + const auto p1 = geometry_msgs::build() + .x((p0.x + p2.x) * 0.5) + .y((p0.y + p2.y) * 0.5) + .z((p0.z + p2.z) * 0.5); + center_points.clear(); + center_points.push_back(p0); + center_points.push_back(p1); + center_points.push_back(p2); + } + return center_points; + } +}; + +class LaneletLengthCache +{ +public: + auto getLength(lanelet::Id lanelet_id) + { + if (!exists(lanelet_id)) { + THROW_SIMULATION_ERROR("length of : ", lanelet_id, " does not exists on route cache."); + } + std::lock_guard lock(mutex_); + return data_[lanelet_id]; + } + + auto getLength(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) -> double + { + if (!exists(lanelet_id)) { + appendData( + lanelet_id, lanelet::utils::getLaneletLength2d(lanelet_map->laneletLayer.get(lanelet_id))); + } + std::lock_guard lock(mutex_); + return data_[lanelet_id]; + } + + std::unordered_map data_; + std::mutex mutex_; + + auto exists(const lanelet::Id lanelet_id) -> bool + { + std::lock_guard lock(mutex_); + return data_.find(lanelet_id) != data_.end(); + } + + auto appendData(const lanelet::Id lanelet_id, double length) -> void + { + std::lock_guard lock(mutex_); + data_[lanelet_id] = length; + } +}; + +struct TrafficRulesWithRoutingGraph +{ + lanelet::traffic_rules::TrafficRulesPtr rules; + lanelet::routing::RoutingGraphConstPtr graph; + mutable RouteCache route_cache; +}; + +class LaneletWrapper +{ +public: + static auto activate(const std::string & lanelet_map_path) -> void; + + [[nodiscard]] static auto map() -> const lanelet::LaneletMapPtr; + [[nodiscard]] static auto routingGraph(const RoutingGraphType type) + -> const lanelet::routing::RoutingGraphConstPtr; + [[nodiscard]] static auto trafficRules(const RoutingGraphType type) + -> const lanelet::traffic_rules::TrafficRulesPtr; + + [[nodiscard]] static auto routeCache(const RoutingGraphType type) -> RouteCache &; + [[nodiscard]] static auto centerPointsCache() -> CenterPointsCache &; + [[nodiscard]] static auto laneletLengthCache() -> LaneletLengthCache &; + +private: + LaneletWrapper(const std::filesystem::path & lanelet_map_path); + static LaneletWrapper & getInstance(); + + auto calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) + -> std::vector; + + auto resamplePoints( + const lanelet::ConstLineString3d & line_string, const std::int32_t num_segments) + -> lanelet::BasicPoints3d; + + auto overwriteLaneletsCenterline() -> void; + + inline static std::unique_ptr instance{nullptr}; + inline static std::string lanelet_map_path_{""}; + inline static std::mutex mutex_; + + lanelet::projection::MGRSProjector mgrs_projector_; + lanelet::ErrorMessages lanelet_errors_; + + const lanelet::LaneletMapPtr lanelet_map_ptr_; + /// @todo It is worth trying to add const to each attribute of type TrafficRulesWithRoutingGraph + TrafficRulesWithRoutingGraph vehicle_; + TrafficRulesWithRoutingGraph vehicle_with_road_shoulder_; + TrafficRulesWithRoutingGraph pedestrian_; + + mutable CenterPointsCache center_points_cache_; + mutable LaneletLengthCache lanelet_length_cache_; +}; +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp new file mode 100644 index 00000000000..0dbbdb6a12f --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp @@ -0,0 +1,106 @@ +// 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_POSE_HPP_ +#define TRAFFIC_SIMULATOR__LANELET_WRAPPER_POSE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace pose +{ +using Point = geometry_msgs::msg::Point; +using Vector3 = geometry_msgs::msg::Vector3; +using Pose = geometry_msgs::msg::Pose; +using PoseStamped = geometry_msgs::msg::PoseStamped; +using BoundingBox = traffic_simulator_msgs::msg::BoundingBox; +using LaneletPose = traffic_simulator_msgs::msg::LaneletPose; +using EntityType = traffic_simulator_msgs::msg::EntityType; + +/// @note This value was determined experimentally by @hakuturu583 and not theoretically. +/// @sa https://github.com/tier4/scenario_simulator_v2/commit/4c8e9f496b061b00bec799159d59c33f2ba46b3a +constexpr static double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; + +auto toMapPose(const LaneletPose & lanelet_pose, const bool fill_pitch = true) -> PoseStamped; + +auto toLaneletPose( + const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance) + -> std::optional; + +auto toLaneletPose( + const Pose & map_pose, const lanelet::Ids & lanelet_ids, const double matching_distance) + -> std::optional; + +auto toLaneletPose( + const Pose & map_pose, const bool include_crosswalk, const double matching_distance) + -> std::optional; + +auto toLaneletPose( + const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk, + const double matching_distance, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) + -> std::optional; + +auto toLaneletPoses( + const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance, + const bool include_opposite_direction, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) + -> std::vector; + +auto alternativeLaneletPoses(const LaneletPose & lanelet_pose) -> std::vector; + +auto alongLaneletPose( + const LaneletPose & from_pose, const lanelet::Ids & route_lanelets, const double distance) + -> LaneletPose; + +auto alongLaneletPose( + const LaneletPose & from_pose, const double distance, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> LaneletPose; + +auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose) + -> std::tuple, std::optional>; + +auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose, const lanelet::Ids & route_lanelets) + -> std::tuple, std::optional>; + +// used only by this namespace +auto matchToLane( + const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk, + const double matching_distance, + const double reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) + -> std::optional; + +auto leftLaneletIds( + const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) + -> lanelet::Ids; + +auto rightLaneletIds( + const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) + -> lanelet::Ids; +} // namespace pose +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_POSE_HPP_ 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..2ed3fc4c056 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp @@ -0,0 +1,53 @@ +// 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 isInRoute(const lanelet::Id lanelet_id, const lanelet::Ids & route_lanelets_ids) -> bool; + +// auto speedLimit( +// const lanelet::Ids & lanelet_ids, +// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> double; + +// auto route( +// const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, +// const RoutingConfiguration & routing_configuration = RoutingConfiguration()) -> lanelet::Ids; + +// auto followingLanelets( +// const lanelet::Id lanelet_id, const lanelet::Ids & candidate_lanelet_ids, +// const double distance = 100, const bool include_self = true, +// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +// auto followingLanelets( +// const lanelet::Id lanelet_id, const double distance = 100, const bool include_self = true, +// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +// auto previousLanelets( +// const lanelet::Id current_lanelet_id, const double backward_horizon = 100, +// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> 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..b357e6de97c --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_lights.hpp @@ -0,0 +1,61 @@ +// 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 +// { +// using Point = geometry_msgs::msg::Point; + +// auto isTrafficLight(const lanelet::Id lanelet_id) -> bool; + +// auto isTrafficLightRegulatoryElement(const lanelet::Id lanelet_id) -> bool; + +// auto toTrafficLightRegulatoryElement(const lanelet::Id traffic_light_regulatory_element_id) +// -> lanelet::TrafficLight::Ptr; + +// auto toAutowareTrafficLights(const lanelet::Id traffic_light_id) +// -> std::vector; + +// auto trafficLightBulbPosition(const lanelet::Id traffic_light_id, const std::string & color_name) +// -> std::optional; + +// auto trafficLightStopLinesPoints(const lanelet::Id traffic_light_id) +// -> std::vector>; + +// auto trafficLightRegulatoryElementIDsFromTrafficLight(const lanelet::Id traffic_light_way_id) +// -> lanelet::Ids; + +// // On path +// auto autowareTrafficLightsOnPath(const lanelet::Ids & lanelet_ids) +// -> std::vector; + +// auto trafficLightIdsOnPath(const lanelet::Ids & route_lanelets) -> lanelet::Ids; + +// auto trafficSignsOnPath(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/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp new file mode 100644 index 00000000000..41743932542 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp @@ -0,0 +1,74 @@ +// Copyright 2015 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__HDMAP_UTILS__TRAFFIC_RULES_HPP_ +#define TRAFFIC_SIMULATOR__HDMAP_UTILS__TRAFFIC_RULES_HPP_ + +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +struct Locations +{ + /// @note DIRTY HACK!! Originally, a location code should be an ISO country code. + /// @sa https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/507033b82d9915f086f2539d56c3b62e71802438/lanelet2_traffic_rules/include/lanelet2_traffic_rules/TrafficRules.h#L97 + static constexpr char RoadShoulderPassableGermany[] = "de_road_shoulder_passable"; +}; + +class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanVehicle +{ +public: + using lanelet::traffic_rules::GermanVehicle::GermanVehicle; + +protected: + /// @note this function overrides and adds road shoulder handling to GenericTrafficRules::canPass + lanelet::Optional canPass( + const std::string & type, const std::string & /*location*/) const override + { + using lanelet::AttributeValueString; + using lanelet::Participants; + const static std::map> participants_map{ + // clang-format off + {"", {Participants::Vehicle}}, + {AttributeValueString::Road, {Participants::Vehicle, Participants::Bicycle}}, + {"road_shoulder", {Participants::Vehicle, Participants::Bicycle}}, // add road_shoulder + {AttributeValueString::Highway, {Participants::Vehicle}}, + {AttributeValueString::BicycleLane, {Participants::Bicycle}}, + {AttributeValueString::PlayStreet, {Participants::Pedestrian, Participants::Bicycle, Participants::Vehicle}}, + {AttributeValueString::EmergencyLane, {Participants::VehicleEmergency}}, + {AttributeValueString::Exit, {Participants::Pedestrian, Participants::Bicycle, Participants::Vehicle}}, + {AttributeValueString::Walkway, {Participants::Pedestrian}}, + {AttributeValueString::Crosswalk, {Participants::Pedestrian}}, + {AttributeValueString::Stairs, {Participants::Pedestrian}}, + {AttributeValueString::SharedWalkway, {Participants::Pedestrian, Participants::Bicycle}} + // clang-format on + }; + auto participants = participants_map.find(type); + if (participants == participants_map.end()) { + return {}; + } + + auto startsWith = [](const std::string & str, const std::string & substr) { + return str.compare(0, substr.size(), substr) == 0; + }; + return lanelet::utils::anyOf(participants->second, [this, startsWith](auto & participant) { + return startsWith(this->participant(), participant); + }); + } +}; +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__HDMAP_UTILS__TRAFFIC_RULES_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp new file mode 100644 index 00000000000..d940f4b4655 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp @@ -0,0 +1,52 @@ +// 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__LANELET_MAP_HPP_ +#define TRAFFIC_SIMULATOR__UTILS__LANELET_MAP_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +inline namespace lanelet_map +{ +using Point = geometry_msgs::msg::Point; +using Pose = geometry_msgs::msg::Pose; + +template +inline auto activate(Ts &&... xs) +{ + return lanelet_wrapper::LaneletWrapper::activate(std::forward(xs)...); +} + +auto laneletLength(const lanelet::Id lanelet_id) -> double; + +// auto laneletYaw(const Point & point, const lanelet::Id lanelet_id) +// -> std::tuple; + +// auto nearbyLaneletIds( +// const Pose & pose, const double distance_threshold, const bool include_crosswalk, +// const std::size_t search_count) -> lanelet::Ids; + +// auto borderlinePoses() -> std::vector>; + +// auto visualizationMarker() -> visualization_msgs::msg::MarkerArray; +} // namespace lanelet_map +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__UTILS__LANELET_MAP_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..24129103fb2 --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp @@ -0,0 +1,318 @@ +// 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; +// } +// 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 lateralDistance( +// // const LaneletPose & from, const LaneletPose & to, +// // const RoutingConfiguration & routing_configuration) -> std::optional +// // { +// // // route must exist for lateral distance to be calculated at all +// // if (const auto route = route::route(from.lanelet_id, to.lanelet_id, routing_configuration); +// // 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) { +// // // if there is a lane change +// // if (lane_change::canChangeLane(route[i], route[i + 1])) { +// // 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 { +// auto stopLinesOnPath = [](const lanelet::Ids & lanelet_ids) -> lanelet::ConstLineStrings3d { +// lanelet::ConstLineStrings3d stop_lines; +// for (const auto & traffic_sign : traffic_lights::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; +// }; + +// std::vector collision_points; +// // fill in collision_points using stop_lines +// const auto stop_lines = 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 = 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/lane_change.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lane_change.cpp new file mode 100644 index 00000000000..f8cc66478db --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lane_change.cpp @@ -0,0 +1,245 @@ +// 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 + +// namespace traffic_simulator +// { +// namespace lanelet_wrapper +// { +// namespace lane_change +// { +// auto canChangeLane( +// const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, const RoutingGraphType type) +// -> bool +// { +// const auto from_lanelet = LaneletWrapper::map()->laneletLayer.get(from_lanelet_id); +// const auto to_lanelet = LaneletWrapper::map()->laneletLayer.get(to_lanelet_id); +// return LaneletWrapper::trafficRules(type)->canChangeLane(from_lanelet, to_lanelet); +// } + +// auto laneChangeableLaneletId( +// const lanelet::Id lanelet_id, const Direction & direction, const RoutingGraphType type) +// -> std::optional +// { +// const auto lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); +// if (direction == Direction::STRAIGHT) { +// return lanelet.id(); +// } else if (direction == Direction::LEFT && LaneletWrapper::routingGraph(type)->left(lanelet)) { +// return LaneletWrapper::routingGraph(type)->left(lanelet)->id(); +// } else if (direction == Direction::RIGHT && LaneletWrapper::routingGraph(type)->right(lanelet)) { +// return LaneletWrapper::routingGraph(type)->right(lanelet)->id(); +// } else { +// return std::nullopt; +// } +// } + +// auto laneChangeableLaneletId( +// const lanelet::Id lanelet_id, const Direction & direction, const std::uint8_t shift, +// const RoutingGraphType type) -> std::optional +// { +// if (shift == 0) { +// return laneChangeableLaneletId(lanelet_id, Direction::STRAIGHT, type); +// } else { +// auto reference_id = lanelet_id; +// for (std::size_t i = 0; i < shift; ++i) { +// if (const auto id_opt = laneChangeableLaneletId(reference_id, direction, type); !id_opt) { +// return std::nullopt; +// } else { +// reference_id = id_opt.value(); +// } +// } +// return reference_id; +// } +// } + +// auto countLaneChanges( +// const lanelet::Id & from_lanelet_id, const lanelet::Id & to_lanelet_id, +// const RoutingConfiguration & routing_configuration) -> std::optional> +// { +// constexpr bool include_opposite_direction{true}; +// const auto traveled_route = route::route(from_lanelet_id, to_lanelet_id, routing_configuration); +// if (traveled_route.empty()) { +// return std::nullopt; +// } else { +// std::pair lane_changes{0, 0}; +// for (std::size_t i = 1; i < traveled_route.size(); ++i) { +// const auto & previous = traveled_route[i - 1]; +// const auto & current = traveled_route[i]; + +// if (auto followings = +// lanelet_map::nextLaneletIds(previous, routing_configuration.routing_graph_type); +// std::find(followings.begin(), followings.end(), current) == followings.end()) { +// traffic_simulator_msgs::msg::EntityType type; +// type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; +// 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++; +// } +// } +// } +// return lane_changes; +// } +// } + +// // Trajectory +// auto laneChangeTrajectory( +// const Pose & from_pose, const LaneletPose & to_lanelet_pose, +// const TrajectoryShape & trajectory_shape, const double tangent_vector_size) -> Curve +// { +// const auto to_pose = pose::toMapPose(to_lanelet_pose).pose; + +// auto vectorFromPose = [](const Pose & pose, const double magnitude) -> Vector3 { +// const auto direction = math::geometry::convertQuaternionToEulerAngle(pose.orientation); +// return geometry_msgs::build() +// .x(magnitude * std::cos(direction.z)) +// .y(magnitude * std::sin(direction.z)) +// .z(0); +// }; + +// auto tangentVector = [](const lanelet::Id lanelet_id, const double s) -> std::optional { +// return lanelet_map::centerPointsSpline(lanelet_id)->getTangentVector(s); +// }; + +// double tangent_vector_size_in_curve{0.0}; +// Vector3 from_vector; +// Vector3 to_vector; +// switch (trajectory_shape) { +// case TrajectoryShape::CUBIC: +// tangent_vector_size_in_curve = tangent_vector_size; +// from_vector = vectorFromPose(from_pose, tangent_vector_size); +// if ( +// const auto tangent_vector = tangentVector(to_lanelet_pose.lanelet_id, to_lanelet_pose.s)) { +// to_vector = tangent_vector.value(); +// } else { +// THROW_SIMULATION_ERROR( +// "Failed to calculate tangent vector at lanelet_id : ", to_lanelet_pose.lanelet_id, +// " s : ", to_lanelet_pose.s); +// } +// break; +// case TrajectoryShape::LINEAR: +// tangent_vector_size_in_curve = 1; +// from_vector.x = (to_pose.position.x - from_pose.position.x); +// from_vector.y = (to_pose.position.y - from_pose.position.y); +// from_vector.z = (to_pose.position.z - from_pose.position.z); +// to_vector = from_vector; +// break; +// default: +// throw std::invalid_argument("Unknown trajectory_shape constraint type"); +// } + +// return Curve( +// from_pose, to_pose, from_vector, +// geometry_msgs::build() +// .x(to_vector.x * tangent_vector_size_in_curve) +// .y(to_vector.y * tangent_vector_size_in_curve) +// .z(to_vector.z * tangent_vector_size_in_curve)); +// } + +// auto laneChangeTrajectory( +// const LaneletPose & from_lanelet_pose, const Parameter & lane_change_parameter) +// -> std::optional> +// { +// double longitudinal_distance = [&]() { +// switch (lane_change_parameter.constraint.type) { +// case Constraint::Type::LONGITUDINAL_DISTANCE: +// return lane_change_parameter.constraint.value; +// default: +// return Parameter::default_lanechange_distance; +// } +// }(); + +// const auto along_lanelet_pose = pose::alongLaneletPose(from_lanelet_pose, longitudinal_distance); +// auto left_boundary_lanelet_pose = along_lanelet_pose; +// left_boundary_lanelet_pose.offset += 5.0; +// auto right_boundary_lanelet_pose = along_lanelet_pose; +// right_boundary_lanelet_pose.offset -= 5.0; + +// const auto left_boundary_point = pose::toMapPose(left_boundary_lanelet_pose).pose.position; +// const auto right_boundary_point = pose::toMapPose(right_boundary_lanelet_pose).pose.position; +// if (const auto to_lanelet_pose_s = +// lanelet_map::centerPointsSpline(lane_change_parameter.target.lanelet_id) +// ->getCollisionPointIn2D(left_boundary_point, right_boundary_point); +// !to_lanelet_pose_s) { +// return std::nullopt; +// } else { +// const auto to_lanelet_pose = helper::constructLaneletPose( +// lane_change_parameter.target.lanelet_id, to_lanelet_pose_s.value(), +// lane_change_parameter.target.offset); +// const auto from_pose = pose::toMapPose(from_lanelet_pose).pose; +// const auto to_pose = pose::toMapPose(to_lanelet_pose).pose; +// const auto euclidean_distance = math::geometry::hypot(from_pose.position, to_pose.position); +// const auto lane_change_trajectory = laneChangeTrajectory( +// from_pose, to_lanelet_pose, lane_change_parameter.trajectory_shape, euclidean_distance * 0.5); +// return std::make_pair(lane_change_trajectory, to_lanelet_pose_s.value()); +// } +// } + +// auto laneChangeTrajectory( +// const Pose & from_pose, const Parameter & lane_change_parameter, +// const double maximum_curvature_threshold, const double target_trajectory_length, +// const double forward_distance_threshold) -> std::optional> +// { +// std::vector candidates_evaluation, candidates_s; +// std::vector candidates_curves; + +// const auto lanelet_length = lanelet_map::laneletLength(lane_change_parameter.target.lanelet_id); +// for (double to_lanelet_pose_s = 0; to_lanelet_pose_s < lanelet_length; to_lanelet_pose_s += 1.0) { +// const auto to_lanelet_pose = +// helper::constructLaneletPose(lane_change_parameter.target.lanelet_id, to_lanelet_pose_s, 0.0); +// // skip those poses that are too close +// if (const auto to_pose = pose::toMapPose(to_lanelet_pose).pose; +// math::geometry::getRelativePose(from_pose, to_pose).position.x <= +// forward_distance_threshold) { +// continue; +// } else { +// const auto euclidean_distance = math::geometry::hypot(from_pose.position, to_pose.position); +// if (const auto lane_change_trajectory = laneChangeTrajectory( +// from_pose, to_lanelet_pose, lane_change_parameter.trajectory_shape, +// euclidean_distance * 0.5); +// lane_change_trajectory.getMaximum2DCurvature() < maximum_curvature_threshold) { +// candidates_evaluation.push_back( +// std::fabs(target_trajectory_length - lane_change_trajectory.getLength())); +// candidates_curves.push_back(lane_change_trajectory); +// candidates_s.push_back(to_lanelet_pose_s); +// } +// } +// } + +// if (candidates_evaluation.empty()) { +// return std::nullopt; +// } else { +// const auto min_iterator = +// std::min_element(candidates_evaluation.begin(), candidates_evaluation.end()); +// const auto min_index = std::distance(candidates_evaluation.begin(), min_iterator); +// return std::make_pair(candidates_curves[min_index], candidates_s[min_index]); +// } +// } +// } // namespace lane_change +// } // 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 new file mode 100644 index 00000000000..770a3fada72 --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -0,0 +1,345 @@ +// 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 + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace lanelet_map +{ +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); +} + +auto isInLanelet(const lanelet::Id lanelet_id, const Point point) -> bool +{ + return lanelet::geometry::inside( + LaneletWrapper::map()->laneletLayer.get(lanelet_id), lanelet::BasicPoint2d(point.x, point.y)); +} + +auto laneletLength(const lanelet::Id lanelet_id) -> double +{ + return LaneletWrapper::laneletLengthCache().getLength(lanelet_id, LaneletWrapper::map()); +} + +// auto laneletYaw(const lanelet::Id lanelet_id, const Point & point) +// -> std::tuple +// { +// if (const auto centerline_points = lanelet_wrapper::lanelet_map::centerPoints(lanelet_id); +// centerline_points.empty()) { +// THROW_SIMULATION_ERROR( +// "There is no center points for lanelet with id: " + std::to_string(lanelet_id)); +// } else { +// auto findNearestPointIndex = [](const std::vector & points, const Point & point) { +// return std::distance( +// points.begin(), +// std::min_element(points.begin(), points.end(), [&](const Point & p1, const Point & p2) { +// return math::geometry::hypot(p1, point) < math::geometry::hypot(p2, point); +// })); +// }; +// const size_t nearest_point_index = findNearestPointIndex(centerline_points, point); +// const auto & nearest_point = centerline_points.at(nearest_point_index); +// const auto & next_point = centerline_points.at(nearest_point_index + 1); +// const auto yaw = std::atan2(next_point.y - nearest_point.y, next_point.x - nearest_point.x); +// return std::make_tuple(yaw, nearest_point, next_point); +// } +// } + +auto laneletIds() -> lanelet::Ids +{ + lanelet::Ids ids; + for (const auto & lanelet : LaneletWrapper::map()->laneletLayer) { + ids.push_back(lanelet.id()); + } + return ids; +} + +auto nearbyLaneletIds( + const Point & point, const double distance_thresh, const bool include_crosswalk, + const std::size_t search_count) -> lanelet::Ids +{ + auto excludeSubtypeLanelets = + []( + const std::vector> & pair_distance_lanelet, + const char subtype[]) -> std::vector> { + std::vector> filtered_lanelets; + for (const auto & pair : pair_distance_lanelet) { + if (pair.second.hasAttribute(lanelet::AttributeName::Subtype)) { + if (pair.second.attribute(lanelet::AttributeName::Subtype).value() != subtype) { + filtered_lanelets.push_back(pair); + } + } + } + return filtered_lanelets; + }; + + auto nearest_lanelets = lanelet::geometry::findNearest( + LaneletWrapper::map()->laneletLayer, lanelet::BasicPoint2d(point.x, point.y), search_count); + + // check for current content, if not empty then optionally apply filter + if (nearest_lanelets.empty() || nearest_lanelets.front().first > distance_thresh) { + return {}; + } else if (!include_crosswalk) { + nearest_lanelets = + excludeSubtypeLanelets(nearest_lanelets, lanelet::AttributeValueString::Crosswalk); + } + + // check again + if (nearest_lanelets.empty() || nearest_lanelets.front().first > distance_thresh) { + return {}; + } else { + lanelet::Ids target_lanelet_ids; + for (const auto & lanelet : nearest_lanelets) { + if (lanelet.first <= distance_thresh) { + target_lanelet_ids.emplace_back(lanelet.second.id()); + } + } + return target_lanelet_ids; + } +} + +// Center points +auto centerPoints(const lanelet::Ids & lanelet_ids) -> std::vector +{ + if (lanelet_ids.empty()) { + return {}; + } else { + std::vector center_points; + for (const auto & lanelet_id : lanelet_ids) { + auto points = centerPoints(lanelet_id); + center_points.insert(center_points.end(), points.begin(), points.end()); + } + center_points.erase( + std::unique(center_points.begin(), center_points.end()), center_points.end()); + return center_points; + } +} + +auto centerPoints(const lanelet::Id lanelet_id) -> std::vector +{ + return LaneletWrapper::centerPointsCache().getCenterPoints(lanelet_id, LaneletWrapper::map()); +} + +auto centerPointsSpline(const lanelet::Id lanelet_id) -> std::shared_ptr +{ + return LaneletWrapper::centerPointsCache().getCenterPointsSpline( + lanelet_id, LaneletWrapper::map()); +} + +// Next lanelet +auto nextLaneletIds(const lanelet::Id lanelet_id, const RoutingGraphType type) -> lanelet::Ids +{ + lanelet::Ids next_lanelet_ids; + const auto lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); + for (const auto & following_lanelet : LaneletWrapper::routingGraph(type)->following(lanelet)) { + next_lanelet_ids.push_back(following_lanelet.id()); + } + return next_lanelet_ids; +} + +auto nextLaneletIds(const lanelet::Ids & lanelet_ids, const RoutingGraphType type) -> lanelet::Ids +{ + std::set next_lanelet_ids_set; + for (const auto & lanelet_id : lanelet_ids) { + auto next_lanelet_ids = nextLaneletIds(lanelet_id, type); + next_lanelet_ids_set.insert(next_lanelet_ids.begin(), next_lanelet_ids.end()); + } + return lanelet::Ids(next_lanelet_ids_set.begin(), next_lanelet_ids_set.end()); +} + +auto nextLaneletIds( + const lanelet::Id lanelet_id, const std::string & turn_direction, const RoutingGraphType type) + -> lanelet::Ids +{ + lanelet::Ids next_lanelet_ids; + const auto & reference_lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); + for (const auto & following_lanelet : + LaneletWrapper::routingGraph(type)->following(reference_lanelet)) { + if (following_lanelet.attributeOr("turn_direction", "else") == turn_direction) { + next_lanelet_ids.push_back(following_lanelet.id()); + } + } + return next_lanelet_ids; +} + +auto nextLaneletIds( + const lanelet::Ids & lanelet_ids, const std::string & turn_direction, const RoutingGraphType type) + -> lanelet::Ids +{ + std::set next_lanelet_ids_set; + for (const auto & lanelet_id : lanelet_ids) { + auto next_lanelet_ids = nextLaneletIds(lanelet_id, turn_direction, type); + next_lanelet_ids_set.insert(next_lanelet_ids.begin(), next_lanelet_ids.end()); + } + return lanelet::Ids(next_lanelet_ids_set.begin(), next_lanelet_ids_set.end()); +} + +// Previus lanelet +auto previousLaneletIds(const lanelet::Id lanelet_id, const RoutingGraphType type) -> lanelet::Ids +{ + lanelet::Ids previous_lanelet_ids; + const auto lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); + for (const auto & previous_lanelet : LaneletWrapper::routingGraph(type)->previous(lanelet)) { + previous_lanelet_ids.push_back(previous_lanelet.id()); + } + return previous_lanelet_ids; +} + +auto previousLaneletIds(const lanelet::Ids & lanelet_ids, const RoutingGraphType type) + -> lanelet::Ids +{ + std::set previous_lanelet_ids_set; + for (const auto & lanelet_id : lanelet_ids) { + auto previous_lanelet_ids = previousLaneletIds(lanelet_id, type); + previous_lanelet_ids_set.insert(previous_lanelet_ids.begin(), previous_lanelet_ids.end()); + } + return lanelet::Ids(previous_lanelet_ids_set.begin(), previous_lanelet_ids_set.end()); +} + +auto previousLaneletIds( + const lanelet::Id lanelet_id, const std::string & turn_direction, const RoutingGraphType type) + -> lanelet::Ids +{ + lanelet::Ids previous_lanelet_ids; + const auto reference_lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); + for (const auto & previous_lanelet : + LaneletWrapper::routingGraph(type)->previous(reference_lanelet)) { + if (previous_lanelet.attributeOr("turn_direction", "else") == turn_direction) { + previous_lanelet_ids.push_back(previous_lanelet.id()); + } + } + return previous_lanelet_ids; +} + +auto previousLaneletIds( + const lanelet::Ids & lanelet_ids, const std::string & turn_direction, const RoutingGraphType type) + -> lanelet::Ids +{ + std::set previous_lanelet_ids_set; + for (const auto & lanelet_id : lanelet_ids) { + auto previous_lanelet_ids = previousLaneletIds(lanelet_id, turn_direction, type); + previous_lanelet_ids_set.insert(previous_lanelet_ids.begin(), previous_lanelet_ids.end()); + } + 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; +// } + +// auto conflictingCrosswalkIds(const lanelet::Ids & lanelet_ids) -> lanelet::Ids +// { +// constexpr size_t routing_graph_id{1}; +// constexpr double height_clearance{4}; +// /// @note it is not clear if the distinction for crosswalks only is implemented here +// lanelet::Ids conflicting_crosswalk_ids; +// lanelet::routing::RoutingGraphContainer graphs_container( +// {LaneletWrapper::routingGraph(RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER), +// LaneletWrapper::routingGraph(RoutingGraphType::PEDESTRIAN)}); +// for (const auto & lanelet_id : lanelet_ids) { +// const auto & conflicting_crosswalks = graphs_container.conflictingInGraph( +// LaneletWrapper::map()->laneletLayer.get(lanelet_id), routing_graph_id, height_clearance); +// for (const auto & conflicting_crosswalk : conflicting_crosswalks) { +// conflicting_crosswalk_ids.emplace_back(conflicting_crosswalk.id()); +// } +// } +// return conflicting_crosswalk_ids; +// } + +// auto conflictingLaneIds(const lanelet::Ids & lanelet_ids, const RoutingGraphType type) +// -> lanelet::Ids +// { +// lanelet::Ids conflicting_lanes_ids; +// for (const auto & lanelet_id : lanelet_ids) { +// const auto & conflicting_lanelets = lanelet::utils::getConflictingLanelets( +// LaneletWrapper::routingGraph(type), LaneletWrapper::map()->laneletLayer.get(lanelet_id)); +// for (const auto & conflicting_lanelet : conflicting_lanelets) { +// conflicting_lanes_ids.emplace_back(conflicting_lanelet.id()); +// } +// } +// return conflicting_lanes_ids; +// } +} // namespace lanelet_map +} // namespace lanelet_wrapper +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp new file mode 100644 index 00000000000..9356b902e50 --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp @@ -0,0 +1,257 @@ +// 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 + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +auto LaneletWrapper::activate(const std::string & lanelet_map_path) -> void +{ + lanelet_map_path_ = lanelet_map_path; + if (instance) { + std::lock_guard lock(mutex_); + instance.reset(); + } +} + +LaneletWrapper & LaneletWrapper::getInstance() +{ + std::lock_guard lock(mutex_); + if (!instance) { + if (!lanelet_map_path_.empty()) { + instance.reset(new LaneletWrapper(lanelet_map_path_)); + } else { + THROW_SIMULATION_ERROR( + "lanelet_map_path is empty! Please call lanelet_map::activate() first."); + } + } + return *instance; +} + +auto LaneletWrapper::map() -> const lanelet::LaneletMapPtr +{ + return getInstance().lanelet_map_ptr_; +} + +auto LaneletWrapper::routingGraph(const RoutingGraphType type) + -> const lanelet::routing::RoutingGraphConstPtr +{ + switch (type) { + case RoutingGraphType::VEHICLE: + return getInstance().vehicle_.graph; + case RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER: + return getInstance().vehicle_with_road_shoulder_.graph; + case RoutingGraphType::PEDESTRIAN: + return getInstance().pedestrian_.graph; + default: + std::stringstream what; + what << "Invalid routing graph type: " << static_cast(type); + throw common::Error(what.str()); + } +} + +auto LaneletWrapper::trafficRules(const RoutingGraphType type) + -> const lanelet::traffic_rules::TrafficRulesPtr +{ + switch (type) { + case RoutingGraphType::VEHICLE: + return getInstance().vehicle_.rules; + case RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER: + return getInstance().vehicle_with_road_shoulder_.rules; + case RoutingGraphType::PEDESTRIAN: + return getInstance().pedestrian_.rules; + default: + std::stringstream what; + what << "Invalid routing graph type: " << static_cast(type); + throw common::Error(what.str()); + } +} + +auto LaneletWrapper::routeCache(const RoutingGraphType type) -> RouteCache & +{ + switch (type) { + case RoutingGraphType::VEHICLE: + return getInstance().vehicle_.route_cache; + case RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER: + return getInstance().vehicle_with_road_shoulder_.route_cache; + case RoutingGraphType::PEDESTRIAN: + return getInstance().pedestrian_.route_cache; + default: + std::stringstream what; + what << "Invalid routing graph type: " << static_cast(type); + throw common::Error(what.str()); + } +} + +auto LaneletWrapper::centerPointsCache() -> CenterPointsCache & +{ + return getInstance().center_points_cache_; +} + +auto LaneletWrapper::laneletLengthCache() -> LaneletLengthCache & +{ + return getInstance().lanelet_length_cache_; +} + +LaneletWrapper::LaneletWrapper(const std::filesystem::path & lanelet_map_path) +: lanelet_map_ptr_(lanelet::load(lanelet_map_path.string(), mgrs_projector_, &lanelet_errors_)) +{ + if (!lanelet_errors_.empty()) { + std::stringstream ss; + ss << "Failed to load lanelet map, errors:\n"; + for (const auto & error : lanelet_errors_) { + ss << " - " << error << "\n"; + } + THROW_SIMULATION_ERROR(ss.str()); + } + + if (lanelet_map_ptr_->laneletLayer.empty()) { + THROW_SIMULATION_ERROR("Lanelet layer is empty!"); + } + + overwriteLaneletsCenterline(); + vehicle_.rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + vehicle_.graph = lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *vehicle_.rules); + + vehicle_with_road_shoulder_.rules = lanelet::traffic_rules::TrafficRulesFactory::create( + Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle); + vehicle_with_road_shoulder_.graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *vehicle_with_road_shoulder_.rules); + + pedestrian_.rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Pedestrian); + pedestrian_.graph = lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_.rules); +} + +auto LaneletWrapper::calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) + -> std::vector +{ + auto calculateSegmentDistances = + [](const lanelet::ConstLineString3d & line_string) -> std::vector { + std::vector segment_distances; + segment_distances.reserve(line_string.size() - 1); + for (size_t i = 1; i < line_string.size(); ++i) { + segment_distances.push_back(lanelet::geometry::distance(line_string[i], line_string[i - 1])); + } + return segment_distances; + }; + + const auto segment_distances = calculateSegmentDistances(line_string); + std::vector accumulated_lengths{0}; + accumulated_lengths.reserve(segment_distances.size() + 1); + std::partial_sum( + std::begin(segment_distances), std::end(segment_distances), + std::back_inserter(accumulated_lengths)); + return accumulated_lengths; +} + +auto LaneletWrapper::resamplePoints( + const lanelet::ConstLineString3d & line_string, const std::int32_t num_segments) + -> lanelet::BasicPoints3d +{ + const auto accumulated_lengths = calculateAccumulatedLengths(line_string); + + auto findNearestIndexPair = + [&](const double target_length) -> std::pair { + const auto N = accumulated_lengths.size(); + if (target_length < accumulated_lengths.at(1)) { // Front + return std::make_pair(0, 1); + } else if (target_length > accumulated_lengths.at(N - 2)) { // Back + return std::make_pair(N - 2, N - 1); + } else // Middle + { + for (size_t i = 1; i < N; ++i) { + if ( + accumulated_lengths.at(i - 1) <= target_length && + target_length <= accumulated_lengths.at(i)) { + return std::make_pair(i - 1, i); + } + } + } + THROW_SEMANTIC_ERROR("findNearestIndexPair(): No nearest point found."); + }; + + // Create each segment + lanelet::BasicPoints3d resampled_points; + for (auto i = 0; i <= num_segments; ++i) { + // Find two nearest points + const double target_length = (static_cast(i) / num_segments) * + static_cast(lanelet::geometry::length(line_string)); + const auto index_pair = findNearestIndexPair(target_length); + + // Apply linear interpolation + const lanelet::BasicPoint3d back_point = line_string[index_pair.first]; + const lanelet::BasicPoint3d front_point = line_string[index_pair.second]; + const auto direction_vector = (front_point - back_point); + + const auto back_length = accumulated_lengths.at(index_pair.first); + const auto front_length = accumulated_lengths.at(index_pair.second); + const auto segment_length = front_length - back_length; + const auto target_point = + back_point + (direction_vector * (target_length - back_length) / segment_length); + + // Add to list + resampled_points.push_back(target_point); + } + return resampled_points; +} + +auto LaneletWrapper::overwriteLaneletsCenterline() -> void +{ + constexpr double resolution{2.0}; + + auto generateFineCenterline = + [&](const lanelet::ConstLanelet & lanelet_obj) -> lanelet::LineString3d { + // Get length of longer border + const double left_length = + static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); + const double right_length = + static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); + const double longer_distance = (left_length > right_length) ? left_length : right_length; + const int32_t num_segments = + std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d centerline(lanelet::utils::getId()); + for (size_t i = 0; i < static_cast(num_segments + 1); i++) { + // Add ID for the average point of left and right + const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2.0; + const lanelet::Point3d center_point( + lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(), + center_basic_point.z()); + centerline.push_back(center_point); + } + return centerline; + }; + + for (auto & lanelet_obj : lanelet_map_ptr_->laneletLayer) { + if (!lanelet_obj.hasCustomCenterline()) { + lanelet_obj.setCenterline(generateFineCenterline(lanelet_obj)); + } + } +} +} // namespace lanelet_wrapper +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp new file mode 100644 index 00000000000..d3dbca38ffd --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -0,0 +1,453 @@ +// 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 +#include +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace pose +{ +auto toMapPose(const LaneletPose & lanelet_pose, const bool fill_pitch) -> PoseStamped +{ + using math::geometry::operator*; + using math::geometry::operator+=; + if ( + const auto canonicalized_lanelet_pose = + std::get>(pose::canonicalizeLaneletPose(lanelet_pose))) { + PoseStamped pose_stamped; + pose_stamped.header.frame_id = "map"; + const auto lanelet_spline = + lanelet_map::centerPointsSpline(canonicalized_lanelet_pose->lanelet_id); + // map position + const auto normal_vector = lanelet_spline->getNormalVector(canonicalized_lanelet_pose->s); + const auto offset_transition_vector = + math::geometry::normalize(normal_vector) * canonicalized_lanelet_pose->offset; + pose_stamped.pose = lanelet_spline->getPose(canonicalized_lanelet_pose->s); + pose_stamped.pose.position += offset_transition_vector; + // map orientation + const auto tangent_vector = lanelet_spline->getTangentVector(canonicalized_lanelet_pose->s); + const auto lanelet_rpy = + geometry_msgs::build() + .x(0.0) + .y( + fill_pitch ? std::atan2(-tangent_vector.z, std::hypot(tangent_vector.x, tangent_vector.y)) + : 0.0) + .z(std::atan2(tangent_vector.y, tangent_vector.x)); + pose_stamped.pose.orientation = + math::geometry::convertEulerAngleToQuaternion(lanelet_rpy) * + math::geometry::convertEulerAngleToQuaternion(canonicalized_lanelet_pose->rpy); + return pose_stamped; + } else { + THROW_SEMANTIC_ERROR( + "Lanelet pose (id=", lanelet_pose.lanelet_id, ",s=", lanelet_pose.s, + ",offset=", lanelet_pose.offset, ",rpy.x=", lanelet_pose.rpy.x, ",rpy.y=", lanelet_pose.rpy.y, + ",rpy.z=", lanelet_pose.rpy.z, ") is invalid, please check lanelet length and connection."); + } +} + +auto toLaneletPose( + const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance) + -> std::optional +{ + constexpr double yaw_threshold = 0.25; + const auto lanelet_spline = lanelet_map::centerPointsSpline(lanelet_id); + if (const auto lanelet_pose_s = lanelet_spline->getSValue(map_pose, matching_distance); + !lanelet_pose_s) { + return std::nullopt; + } else { + const auto lanelet_quaternion = lanelet_spline->getPose(lanelet_pose_s.value()).orientation; + if (const auto lanelet_pose_rpy = math::geometry::convertQuaternionToEulerAngle( + math::geometry::getRotation(lanelet_quaternion, map_pose.orientation)); + std::fabs(lanelet_pose_rpy.z) > M_PI * yaw_threshold && + std::fabs(lanelet_pose_rpy.z) < M_PI * (1 - yaw_threshold)) { + return std::nullopt; + } else { + double lanelet_pose_offset = std::sqrt( + lanelet_spline->getSquaredDistanceIn2D(map_pose.position, lanelet_pose_s.value())); + if (const double inner_product = math::geometry::innerProduct( + lanelet_spline->getNormalVector(lanelet_pose_s.value()), + lanelet_spline->getSquaredDistanceVector(map_pose.position, lanelet_pose_s.value())); + inner_product < 0) { + lanelet_pose_offset = lanelet_pose_offset * -1; + } + return traffic_simulator_msgs::build() + .lanelet_id(lanelet_id) + .s(lanelet_pose_s.value()) + .offset(lanelet_pose_offset) + .rpy(lanelet_pose_rpy); + } + } +} + +auto toLaneletPose( + const Pose & map_pose, const lanelet::Ids & lanelet_ids, const double matching_distance) + -> std::optional +{ + for (const auto & lanelet_id : lanelet_ids) { + if (const auto lanelet_pose = toLaneletPose(map_pose, lanelet_id, matching_distance); + lanelet_pose) { + return lanelet_pose.value(); + } + } + return std::nullopt; +} + +auto toLaneletPose( + const Pose & map_pose, const bool include_crosswalk, const double matching_distance) + -> std::optional +{ + constexpr double distance_threshold{0.1}; + constexpr std::size_t search_count{5}; + const auto nearby_lanelet_ids = lanelet_map::nearbyLaneletIds( + map_pose.position, distance_threshold, include_crosswalk, search_count); + return toLaneletPose(map_pose, nearby_lanelet_ids, matching_distance); +} + +auto toLaneletPose( + const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk, + const double matching_distance, const RoutingGraphType type) -> std::optional +{ + if ( + const auto lanelet_id_using_bounding_box = matchToLane( + map_pose, bounding_box, include_crosswalk, matching_distance, + DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, type)) { + if ( + const auto lanelet_pose_using_bounding_box = + toLaneletPose(map_pose, lanelet_id_using_bounding_box.value(), matching_distance)) { + return lanelet_pose_using_bounding_box; + } else { + for (const auto & previous_lanelet_id : + lanelet_map::previousLaneletIds(lanelet_id_using_bounding_box.value(), type)) { + if ( + const auto lanelet_pose_in_previous_lanelet = + toLaneletPose(map_pose, previous_lanelet_id, matching_distance)) { + return lanelet_pose_in_previous_lanelet; + } + } + for (const auto & next_lanelet_id : + lanelet_map::nextLaneletIds(lanelet_id_using_bounding_box.value(), type)) { + if ( + const auto lanelet_pose_in_next_lanelet = + toLaneletPose(map_pose, next_lanelet_id, matching_distance)) { + return lanelet_pose_in_next_lanelet; + } + } + } + } + return toLaneletPose(map_pose, include_crosswalk, matching_distance); +} + +auto toLaneletPoses( + const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance, + const bool include_opposite_direction, const traffic_simulator::RoutingGraphType type) + -> std::vector +{ + std::vector lanelet_poses; + std::set lanelet_ids_set{lanelet_id}; + auto insertIds = [&](const auto & new_ids) { + lanelet_ids_set.insert(new_ids.begin(), new_ids.end()); + }; + insertIds(leftLaneletIds(lanelet_id, type, include_opposite_direction)); + insertIds(rightLaneletIds(lanelet_id, type, include_opposite_direction)); + insertIds(lanelet_map::previousLaneletIds({lanelet_id}, type)); + insertIds(lanelet_map::nextLaneletIds({lanelet_id}, type)); + for (const auto & lanelet_id : lanelet_ids_set) { + if (const auto & lanelet_pose = toLaneletPose(map_pose, lanelet_id, matching_distance)) { + lanelet_poses.emplace_back(lanelet_pose.value()); + } + } + return lanelet_poses; +} + +auto alternativeLaneletPoses(const LaneletPose & lanelet_pose) -> std::vector +{ + const auto alternativesInPreviousLanelet = + [](const auto & lanelet_pose) -> std::vector { + std::vector lanelet_poses_in_previous_lanelet; + if (const auto previous_lanelet_ids = lanelet_map::previousLaneletIds(lanelet_pose.lanelet_id); + !previous_lanelet_ids.empty()) { + for (const auto & previous_lanelet_id : previous_lanelet_ids) { + const auto lanelet_pose_in_previous_lanelet = helper::constructLaneletPose( + previous_lanelet_id, lanelet_pose.s + lanelet_map::laneletLength(previous_lanelet_id), + lanelet_pose.offset); + if (const auto recurency_alternative_poses = + alternativeLaneletPoses(lanelet_pose_in_previous_lanelet); + recurency_alternative_poses.empty()) { + lanelet_poses_in_previous_lanelet.emplace_back(lanelet_pose_in_previous_lanelet); + } else { + lanelet_poses_in_previous_lanelet.insert( + lanelet_poses_in_previous_lanelet.end(), recurency_alternative_poses.begin(), + recurency_alternative_poses.end()); + } + } + } + return lanelet_poses_in_previous_lanelet; + }; + + const auto alternativesInNextLanelet = [](const auto & lanelet_pose) -> std::vector { + std::vector lanelet_poses_in_next_lanelet; + if (const auto next_lanelet_ids = lanelet_map::nextLaneletIds(lanelet_pose.lanelet_id); + !next_lanelet_ids.empty()) { + for (const auto & next_lanelet_id : next_lanelet_ids) { + const auto lanelet_pose_in_next_lanelet = helper::constructLaneletPose( + next_lanelet_id, lanelet_pose.s - lanelet_map::laneletLength(lanelet_pose.lanelet_id), + lanelet_pose.offset); + if (const auto recurency_alternative_poses = + alternativeLaneletPoses(lanelet_pose_in_next_lanelet); + recurency_alternative_poses.empty()) { + lanelet_poses_in_next_lanelet.emplace_back(lanelet_pose_in_next_lanelet); + } else { + lanelet_poses_in_next_lanelet.insert( + lanelet_poses_in_next_lanelet.end(), recurency_alternative_poses.begin(), + recurency_alternative_poses.end()); + } + } + } + return lanelet_poses_in_next_lanelet; + }; + + /// @note If s value under 0, it means this pose is on the previous lanelet. + if (lanelet_pose.s < 0) { + return alternativesInPreviousLanelet(lanelet_pose); + } + /// @note If s value overs it's lanelet length, it means this pose is on the next lanelet. + else if (lanelet_pose.s > (lanelet_map::laneletLength(lanelet_pose.lanelet_id))) { + return alternativesInNextLanelet(lanelet_pose); + } + /// @note If s value is in range [0,length_of_the_lanelet], return lanelet_pose. + else { + return {lanelet_pose}; + } +} + +auto alongLaneletPose( + const LaneletPose & from_pose, const double distance, const RoutingGraphType type) -> LaneletPose +{ + auto lanelet_pose = from_pose; + lanelet_pose.s += distance; + if (lanelet_pose.s >= 0) { + while (lanelet_pose.s >= lanelet_map::laneletLength(lanelet_pose.lanelet_id)) { + auto next_lanelet_ids = + lanelet_map::nextLaneletIds(lanelet_pose.lanelet_id, "straight", type); + if (next_lanelet_ids.empty()) { + /// @note if empty try to use other than "straight", but the first found + next_lanelet_ids = lanelet_map::nextLaneletIds(lanelet_pose.lanelet_id, type); + } + if (next_lanelet_ids.empty()) { + THROW_SEMANTIC_ERROR( + "failed to calculate along pose (id,s) = (", from_pose.lanelet_id, ",", + from_pose.s + distance, "), next lanelet of id = ", lanelet_pose.lanelet_id, "is empty."); + } + lanelet_pose.s = lanelet_pose.s - lanelet_map::laneletLength(lanelet_pose.lanelet_id); + lanelet_pose.lanelet_id = next_lanelet_ids[0]; + } + } else { + while (lanelet_pose.s < 0) { + auto previous_lanelet_ids = + lanelet_map::previousLaneletIds(lanelet_pose.lanelet_id, "straight", type); + if (previous_lanelet_ids.empty()) { + /// @note if empty try to use other than "straight", but the first found + previous_lanelet_ids = lanelet_map::previousLaneletIds(lanelet_pose.lanelet_id, type); + } + if (previous_lanelet_ids.empty()) { + THROW_SEMANTIC_ERROR( + "failed to calculate along pose (id,s) = (", from_pose.lanelet_id, ",", + from_pose.s + distance, "), next lanelet of id = ", lanelet_pose.lanelet_id, "is empty."); + } + lanelet_pose.s = lanelet_pose.s + lanelet_map::laneletLength(previous_lanelet_ids[0]); + lanelet_pose.lanelet_id = previous_lanelet_ids[0]; + } + } + return lanelet_pose; +} + +auto alongLaneletPose( + const LaneletPose & from_pose, const lanelet::Ids & route_lanelets, const double distance) + -> LaneletPose +{ + auto lanelet_pose = from_pose; + lanelet_pose.s += distance; + const auto canonicalized = canonicalizeLaneletPose(lanelet_pose, route_lanelets); + if (const auto canonicalized_lanelet_pose = std::get>(canonicalized)) { + // If canonicalize succeed, just return canonicalized pose + return canonicalized_lanelet_pose.value(); + } else { + // If canonicalize failed, return lanelet pose as end of road + if (const auto end_of_road_lanelet_id = std::get>(canonicalized)) { + return traffic_simulator_msgs::build() + .lanelet_id(end_of_road_lanelet_id.value()) + .s(lanelet_pose.s <= 0 ? 0 : lanelet_map::laneletLength(end_of_road_lanelet_id.value())) + .offset(lanelet_pose.offset) + .rpy(lanelet_pose.rpy); + } else { + THROW_SIMULATION_ERROR("Failed to find trailing lanelet_id."); + } + } +} + +// If route is not specified, the lanelet_id with the lowest array index is used as a candidate for +// canonicalize destination. +auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose) + -> std::tuple, std::optional> +{ + auto canonicalized_lanelet_pose = lanelet_pose; + while (canonicalized_lanelet_pose.s < 0) { + if (const auto previous_lanelet_ids = + lanelet_map::previousLaneletIds(canonicalized_lanelet_pose.lanelet_id); + previous_lanelet_ids.empty()) { + return {std::nullopt, canonicalized_lanelet_pose.lanelet_id}; + } else { + canonicalized_lanelet_pose.s += lanelet_map::laneletLength(previous_lanelet_ids[0]); + canonicalized_lanelet_pose.lanelet_id = previous_lanelet_ids[0]; + } + } + while (canonicalized_lanelet_pose.s > + lanelet_map::laneletLength(canonicalized_lanelet_pose.lanelet_id)) { + if (const auto next_lanelet_ids = + lanelet_map::nextLaneletIds(canonicalized_lanelet_pose.lanelet_id); + next_lanelet_ids.empty()) { + return {std::nullopt, canonicalized_lanelet_pose.lanelet_id}; + } else { + canonicalized_lanelet_pose.s -= + lanelet_map::laneletLength(canonicalized_lanelet_pose.lanelet_id); + canonicalized_lanelet_pose.lanelet_id = next_lanelet_ids[0]; + } + } + return {canonicalized_lanelet_pose, std::nullopt}; +} + +auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose, const lanelet::Ids & route_lanelets) + -> std::tuple, std::optional> +{ + if (lanelet_pose.s < 0) { + // When canonicalizing to backward lanelet_id, do not consider route + return canonicalizeLaneletPose(lanelet_pose); + } + auto canonicalized_lanelet_pose = lanelet_pose; + while (canonicalized_lanelet_pose.s > + lanelet_map::laneletLength(canonicalized_lanelet_pose.lanelet_id)) { + // When canonicalizing to forward lanelet_id, consider route + bool found_next_lanelet_in_route = false; + for (const auto & next_lanelet_id : + lanelet_map::nextLaneletIds(canonicalized_lanelet_pose.lanelet_id)) { + if ( + std::find(route_lanelets.begin(), route_lanelets.end(), next_lanelet_id) != + route_lanelets.end()) { + found_next_lanelet_in_route = true; + canonicalized_lanelet_pose.s -= + lanelet_map::laneletLength(canonicalized_lanelet_pose.lanelet_id); + canonicalized_lanelet_pose.lanelet_id = next_lanelet_id; + } + } + if (!found_next_lanelet_in_route) { + return {std::nullopt, canonicalized_lanelet_pose.lanelet_id}; + } + } + return {canonicalized_lanelet_pose, std::nullopt}; +} + +// used only by this namespace +auto matchToLane( + const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk, + const double matching_distance, const double reduction_ratio, const RoutingGraphType type) + -> std::optional +{ + const auto absoluteHullPolygon = + [&reduction_ratio](const auto & bounding_box, const auto & pose) -> lanelet::BasicPolygon2d { + auto relative_hull = lanelet::matching::Hull2d{ + lanelet::BasicPoint2d{ + bounding_box.center.x + bounding_box.dimensions.x * 0.5 * reduction_ratio, + bounding_box.center.y + bounding_box.dimensions.y * 0.5 * reduction_ratio}, + lanelet::BasicPoint2d{ + bounding_box.center.x - bounding_box.dimensions.x * 0.5 * reduction_ratio, + bounding_box.center.y - bounding_box.dimensions.y * 0.5 * reduction_ratio}}; + lanelet::BasicPolygon2d absolute_hull_polygon; + absolute_hull_polygon.reserve(relative_hull.size()); + for (const auto & relative_hull_point : relative_hull) { + absolute_hull_polygon.push_back(pose * relative_hull_point); + } + return absolute_hull_polygon; + }; + // prepere object for matching + const auto yaw = math::geometry::convertQuaternionToEulerAngle(map_pose.orientation).z; + lanelet::matching::Object2d bounding_box_object; + bounding_box_object.pose.translation() = + lanelet::BasicPoint2d(map_pose.position.x, map_pose.position.y); + bounding_box_object.pose.linear() = Eigen::Rotation2D(yaw).matrix(); + bounding_box_object.absoluteHull = absoluteHullPolygon(bounding_box, bounding_box_object.pose); + // find matches and optionally filter + auto matches = lanelet::matching::getDeterministicMatches( + *LaneletWrapper::map(), bounding_box_object, matching_distance); + if (!include_crosswalk) { + matches = + lanelet::matching::removeNonRuleCompliantMatches(matches, LaneletWrapper::trafficRules(type)); + } + // find best match (minimalize offset) + if (matches.empty()) { + return std::nullopt; + } else { + std::optional> min_pair_id_offset; + for (const auto & match : matches) { + if ( + const auto lanelet_pose = + pose::toLaneletPose(map_pose, match.lanelet.id(), matching_distance)) { + if (!min_pair_id_offset || lanelet_pose->offset < min_pair_id_offset->second) { + min_pair_id_offset = std::make_pair(lanelet_pose->lanelet_id, lanelet_pose->offset); + } + } + } + return min_pair_id_offset ? std::optional(min_pair_id_offset->first) : std::nullopt; + } +} + +auto leftLaneletIds( + const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type, + const bool include_opposite_direction) -> lanelet::Ids +{ + if (include_opposite_direction) { + return lanelet_map::laneletIds(LaneletWrapper::routingGraph(type)->lefts( + LaneletWrapper::map()->laneletLayer.get(lanelet_id))); + } else { + return lanelet_map::laneletIds(LaneletWrapper::routingGraph(type)->adjacentLefts( + LaneletWrapper::map()->laneletLayer.get(lanelet_id))); + } +} + +auto rightLaneletIds( + const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) + -> lanelet::Ids +{ + if (include_opposite_direction) { + return lanelet_map::laneletIds(LaneletWrapper::routingGraph(type)->rights( + LaneletWrapper::map()->laneletLayer.get(lanelet_id))); + } else { + return lanelet_map::laneletIds(LaneletWrapper::routingGraph(type)->adjacentRights( + LaneletWrapper::map()->laneletLayer.get(lanelet_id))); + } +} +} // namespace pose +} // 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..db58570e9d4 --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp @@ -0,0 +1,256 @@ +// 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 isInRoute(const lanelet::Id lanelet_id, const lanelet::Ids & route_lanelets_ids) -> bool +// { +// return std::find(route_lanelets_ids.begin(), route_lanelets_ids.end(), lanelet_id) != +// route_lanelets_ids.end(); +// } + +// // it returns the speed limit in meters per second +// auto speedLimit(const lanelet::Ids & lanelet_ids, const RoutingGraphType type) -> double +// { +// if (lanelet_ids.empty()) { +// THROW_SEMANTIC_ERROR("size of the vector lanelet ids should be more than 1"); +// } else { +// std::vector limits; +// limits.reserve(lanelet_ids.size()); +// for (const auto & lanelet_id : lanelet_ids) { +// const auto & lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); +// const auto & limit = LaneletWrapper::trafficRules(type)->speedLimit(lanelet); +// limits.push_back(lanelet::units::KmHQuantity(limit.speedLimit).value() / 3.6); +// } +// return *std::min_element(limits.begin(), limits.end()); +// } +// } + +// auto route( +// const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, +// const RoutingConfiguration & routing_configuration) -> lanelet::Ids +// { +// 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)); +// } + +// // auto route( +// // const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, +// // const RoutingConfiguration & routing_configuration) -> lanelet::Ids +// // { +// // auto & cache = LaneletWrapper::routeCache(routing_configuration.routing_graph_type); +// // std::cout << " ########## A ##########" << std::endl; +// // if (cache.exists(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change)) { +// // return cache.getRoute(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change); +// // } +// // std::cout << " ########## B ##########" << std::endl; + +// // lanelet::Ids ids; +// // const auto lanelet = LaneletWrapper::map()->laneletLayer.get(from_lanelet_id); +// // const auto to_lanelet = LaneletWrapper::map()->laneletLayer.get(to_lanelet_id); +// // std::cout << " ########## B + ##########" << std::endl; +// // const auto graph = LaneletWrapper::routingGraph(routing_configuration.routing_graph_type); +// // std::cout << " ########## B ++ ##########" << std::endl; +// // lanelet::Optional route = +// // graph->getRoute(lanelet, to_lanelet, 0, routing_configuration.allow_lane_change); +// // std::cout << " ########## C ##########" << std::endl; + +// // if (!route) { +// // cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); +// // return ids; +// // } +// // lanelet::routing::LaneletPath shortest_path = route->shortestPath(); +// // std::cout << " ########## D ##########" << std::endl; + +// // if (shortest_path.empty()) { +// // cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); +// // return ids; +// // } +// // std::cout << " ########## E ##########" << std::endl; + +// // for (auto lane_itr = shortest_path.begin(); lane_itr != shortest_path.end(); lane_itr++) { +// // ids.push_back(lane_itr->id()); +// // } +// // cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); +// // std::cout << " ########## G ##########" << std::endl; + +// // return ids; +// // } + +// auto followingLanelets( +// const lanelet::Id current_lanelet_id, const lanelet::Ids & route, const double horizon, +// const bool include_current_lanelet_id, const RoutingGraphType type) -> lanelet::Ids +// { +// const auto is_following_lanelet = +// [&type](const auto & current_lanelet, const auto & candidate_lanelet) { +// const auto next_ids = lanelet_map::nextLaneletIds(current_lanelet, type); +// return std::find(next_ids.cbegin(), next_ids.cend(), candidate_lanelet) != next_ids.cend(); +// }; + +// lanelet::Ids following_lanelets_ids; + +// if (route.empty()) { +// return following_lanelets_ids; +// } + +// double total_distance = 0.0; +// bool found_starting_lanelet = false; + +// for (const auto & candidate_lanelet_id : route) { +// if (found_starting_lanelet) { +// if (const auto previous_lanelet = +// following_lanelets_ids.empty() ? current_lanelet_id : following_lanelets_ids.back(); +// not is_following_lanelet(previous_lanelet, candidate_lanelet_id)) { +// THROW_SEMANTIC_ERROR( +// candidate_lanelet_id + " is not the follower of lanelet " + previous_lanelet); +// } +// following_lanelets_ids.push_back(candidate_lanelet_id); +// total_distance += lanelet_map::laneletLength(candidate_lanelet_id); +// if (total_distance > horizon) { +// break; +// } +// } else if (candidate_lanelet_id == current_lanelet_id) { +// found_starting_lanelet = true; +// if (include_current_lanelet_id) { +// following_lanelets_ids.push_back(candidate_lanelet_id); +// } +// } +// } +// if (following_lanelets_ids.empty()) { +// THROW_SEMANTIC_ERROR("lanelet id does not match"); +// } else if (total_distance <= horizon) { +// const auto remaining_lanelets = +// followingLanelets(route.back(), horizon - total_distance, false, type); +// following_lanelets_ids.insert( +// following_lanelets_ids.end(), remaining_lanelets.begin(), remaining_lanelets.end()); +// } + +// return following_lanelets_ids; +// } + +// // auto followingLanelets( +// // const lanelet::Id lanelet_id, const lanelet::Ids & candidate_lanelet_ids, const double +// // distance, const bool include_self) -> lanelet::Ids +// // { +// // if (candidate_lanelet_ids.empty()) { +// // return {}; +// // } else { +// // lanelet::Ids following_lanelets_ids; +// // double total_distance = 0.0; +// // bool found_reference_lanelet_id = false; +// // for (const auto & candidate_lanelet_id : candidate_lanelet_ids) { +// // if (found_reference_lanelet_id) { +// // following_lanelets_ids.push_back(candidate_lanelet_id); +// // total_distance += lanelet_map::laneletLength(candidate_lanelet_id); +// // if (total_distance > distance) { +// // return following_lanelets_ids; +// // } +// // } +// // if (!found_reference_lanelet_id && candidate_lanelet_id == lanelet_id) { +// // found_reference_lanelet_id = true; +// // if (include_self) { +// // following_lanelets_ids.push_back(candidate_lanelet_id); +// // } +// // } +// // } + +// // if (!found_reference_lanelet_id) { +// // THROW_SEMANTIC_ERROR("lanelet id does not match"); +// // } else if (total_distance > distance) { +// // return following_lanelets_ids; +// // } else { +// // const auto remaining_lanelets = +// // followingLanelets(candidate_lanelet_ids.back(), distance - total_distance, false); +// // following_lanelets_ids.insert( +// // following_lanelets_ids.end(), remaining_lanelets.begin(), remaining_lanelets.end()); +// // return following_lanelets_ids; +// // } +// // } +// // } + +// auto followingLanelets( +// const lanelet::Id lanelet_id, const double distance, const bool include_self, +// const RoutingGraphType type) -> lanelet::Ids +// { +// lanelet::Ids following_lanelets_ids; +// if (include_self) { +// following_lanelets_ids.push_back(lanelet_id); +// } +// double total_distance = 0.0; +// auto reference_lanelet_id = lanelet_id; +// while (total_distance < distance) { +// if (const auto straight_lanelet_ids = +// lanelet_map::nextLaneletIds(reference_lanelet_id, "straight", type); +// !straight_lanelet_ids.empty()) { +// total_distance = total_distance + lanelet_map::laneletLength(straight_lanelet_ids[0]); +// following_lanelets_ids.push_back(straight_lanelet_ids[0]); +// } else if (const auto non_straight_lanelet_ids = +// lanelet_map::nextLaneletIds(reference_lanelet_id, type); +// !non_straight_lanelet_ids.empty()) { +// total_distance = total_distance + lanelet_map::laneletLength(non_straight_lanelet_ids[0]); +// following_lanelets_ids.push_back(non_straight_lanelet_ids[0]); +// } else { +// break; +// } +// reference_lanelet_id = following_lanelets_ids.back(); +// } +// return following_lanelets_ids; +// } + +// auto previousLanelets( +// const lanelet::Id current_lanelet_id, const double backward_horizon, const RoutingGraphType type) +// -> lanelet::Ids +// { +// lanelet::Ids previous_lanelets_ids; +// double total_distance = 0.0; +// previous_lanelets_ids.push_back(current_lanelet_id); +// while (total_distance < backward_horizon) { +// const auto & reference_lanelet_id = previous_lanelets_ids.back(); +// if (const auto straight_lanelet_ids = +// lanelet_map::previousLaneletIds(reference_lanelet_id, "straight", type); +// not straight_lanelet_ids.empty()) { +// total_distance = total_distance + lanelet_map::laneletLength(straight_lanelet_ids[0]); +// previous_lanelets_ids.push_back(straight_lanelet_ids[0]); +// } else if (auto non_straight_lanelet_ids = +// lanelet_map::previousLaneletIds(reference_lanelet_id, type); +// not non_straight_lanelet_ids.empty()) { +// total_distance = total_distance + lanelet_map::laneletLength(non_straight_lanelet_ids[0]); +// previous_lanelets_ids.push_back(non_straight_lanelet_ids[0]); +// } else { +// break; +// } +// } +// return previous_lanelets_ids; +// } +// } // 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..66e29432b1f --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/traffic_lights.cpp @@ -0,0 +1,187 @@ +// 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 isTrafficLight(const lanelet::Id lanelet_id) -> bool +// { +// if (LaneletWrapper::map()->lineStringLayer.exists(lanelet_id)) { +// if (const auto && linestring = LaneletWrapper::map()->lineStringLayer.get(lanelet_id); +// linestring.hasAttribute(lanelet::AttributeName::Type)) { +// return linestring.attribute(lanelet::AttributeName::Type).value() == "traffic_light"; +// } +// } +// return false; +// } + +// auto isTrafficLightRegulatoryElement(const lanelet::Id lanelet_id) -> bool +// { +// return LaneletWrapper::map()->regulatoryElementLayer.exists(lanelet_id) && +// std::dynamic_pointer_cast( +// LaneletWrapper::map()->regulatoryElementLayer.get(lanelet_id)); +// } + +// auto toTrafficLightRegulatoryElement(const lanelet::Id traffic_light_regulatory_element_id) +// -> lanelet::TrafficLight::Ptr +// { +// if (isTrafficLightRegulatoryElement(traffic_light_regulatory_element_id)) { +// return std::dynamic_pointer_cast( +// LaneletWrapper::map()->regulatoryElementLayer.get(traffic_light_regulatory_element_id)); +// } else { +// THROW_SEMANTIC_ERROR( +// traffic_light_regulatory_element_id, " is not traffic light regulatory element!"); +// } +// } + +// 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 trafficLightBulbPosition(const lanelet::Id traffic_light_id, const std::string & color_name) +// -> std::optional +// { +// auto isBulbOfExpectedColor = [&color_name](auto bulb) -> bool { +// return bulb.hasAttribute("color") and !bulb.hasAttribute("arrow") and +// bulb.attribute("color").value().compare(color_name) == 0; +// }; + +// for (const auto & autoware_traffic_light : toAutowareTrafficLights(traffic_light_id)) { +// for (auto three_light_bulbs : autoware_traffic_light->lightBulbs()) { +// for (auto bulb : static_cast(three_light_bulbs)) { +// if (isBulbOfExpectedColor(bulb)) { +// return geometry_msgs::build().x(bulb.x()).y(bulb.y()).z(bulb.z()); +// } +// } +// } +// } +// return std::nullopt; +// } + +// 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 trafficLightRegulatoryElementIDsFromTrafficLight(const lanelet::Id traffic_light_id) +// -> lanelet::Ids +// { +// if (isTrafficLight(traffic_light_id)) { +// lanelet::Ids traffic_light_regulatory_element_ids; +// for (const auto & regulatory_element : LaneletWrapper::map()->regulatoryElementLayer) { +// if ( +// regulatory_element->attribute(lanelet::AttributeName::Subtype).value() == "traffic_light") { +// for (const auto & reference_traffic_light : +// regulatory_element->getParameters("refers")) { +// if (reference_traffic_light.id() == traffic_light_id) { +// traffic_light_regulatory_element_ids.push_back(regulatory_element->id()); +// } +// } +// } +// } +// return traffic_light_regulatory_element_ids; +// } else { +// THROW_SEMANTIC_ERROR(traffic_light_id, " is not traffic light!"); +// } +// } + +// // On path +// 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; +// } + +// 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 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; +// } +// } // namespace traffic_lights +// } // namespace lanelet_wrapper +// } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp new file mode 100644 index 00000000000..13124229b4a --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp @@ -0,0 +1,27 @@ +// Copyright 2015 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 + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +lanelet::traffic_rules::RegisterTrafficRules + germanRoadShoulderPassableVehicleRules( + Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle); +} +} // namespace traffic_simulator \ No newline at end of file diff --git a/simulation/traffic_simulator/src/utils/lanelet_map.cpp b/simulation/traffic_simulator/src/utils/lanelet_map.cpp new file mode 100644 index 00000000000..b2655372ccc --- /dev/null +++ b/simulation/traffic_simulator/src/utils/lanelet_map.cpp @@ -0,0 +1,121 @@ +// 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 +{ +inline namespace lanelet_map +{ +auto laneletLength(const lanelet::Id lanelet_id) -> double +{ + return lanelet_wrapper::lanelet_map::laneletLength(lanelet_id); +} + +// auto laneletYaw(const Point & point, const lanelet::Id lanelet_id) +// -> std::tuple +// { +// return lanelet_wrapper::lanelet_map::laneletYaw(lanelet_id, point); +// } + +// auto nearbyLaneletIds( +// const Pose & pose, const double distance_thresh, const bool include_crosswalk, +// const std::size_t search_count) -> lanelet::Ids +// { +// return lanelet_wrapper::lanelet_map::nearbyLaneletIds( +// pose.position, distance_thresh, include_crosswalk, search_count); +// } + +// auto borderlinePoses() -> std::vector> +// { +// std::vector> borderline_poses; +// for (const auto & lanelet_id : lanelet_wrapper::lanelet_map::laneletIds()) { +// if (lanelet_wrapper::lanelet_map::nextLaneletIds(lanelet_id).empty()) { +// LaneletPose lanelet_pose; +// lanelet_pose.lanelet_id = lanelet_id; +// lanelet_pose.s = lanelet_map::laneletLength(lanelet_id); +// borderline_poses.push_back({lanelet_id, pose::toMapPose(lanelet_pose)}); +// } +// } +// return borderline_poses; +// } + +// auto visualizationMarker() -> visualization_msgs::msg::MarkerArray +// { +// visualization_msgs::msg::MarkerArray markers; +// auto insertMarkerArray = []( +// visualization_msgs::msg::MarkerArray & a1, +// const visualization_msgs::msg::MarkerArray & a2) -> void { +// a1.markers.insert(a1.markers.end(), a2.markers.begin(), a2.markers.end()); +// }; + +// lanelet::ConstLanelets all_lanelets = +// lanelet::utils::query::laneletLayer(lanelet_wrapper::LaneletWrapper::map()); +// lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); +// lanelet::ConstLanelets crosswalk_lanelets = +// lanelet::utils::query::crosswalkLanelets(all_lanelets); +// lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); +// lanelet::ConstLineStrings3d stop_lines = lanelet::utils::query::stopLinesLanelets(road_lanelets); +// std::vector aw_tl_reg_elems = +// lanelet::utils::query::autowareTrafficLights(all_lanelets); +// std::vector da_reg_elems = +// lanelet::utils::query::detectionAreas(all_lanelets); +// lanelet::ConstLineStrings3d parking_spaces = +// lanelet::utils::query::getAllParkingSpaces(lanelet_wrapper::LaneletWrapper::map()); +// lanelet::ConstPolygons3d parking_lots = +// lanelet::utils::query::getAllParkingLots(lanelet_wrapper::LaneletWrapper::map()); + +// auto cl_ll_borders = color_utils::fromRgba(1.0, 1.0, 1.0, 0.999); +// auto cl_road = color_utils::fromRgba(0.2, 0.7, 0.7, 0.3); +// auto cl_cross = color_utils::fromRgba(0.2, 0.7, 0.2, 0.3); +// auto cl_stoplines = color_utils::fromRgba(1.0, 0.0, 0.0, 0.5); +// auto cl_trafficlights = color_utils::fromRgba(0.7, 0.7, 0.7, 0.8); +// auto cl_detection_areas = color_utils::fromRgba(0.7, 0.7, 0.7, 0.3); +// auto cl_parking_lots = color_utils::fromRgba(0.7, 0.7, 0.0, 0.3); +// auto cl_parking_spaces = color_utils::fromRgba(1.0, 0.647, 0.0, 0.6); +// auto cl_lanelet_id = color_utils::fromRgba(0.8, 0.2, 0.2, 0.999); + +// insertMarkerArray( +// markers, +// lanelet::visualization::laneletsBoundaryAsMarkerArray(road_lanelets, cl_ll_borders, true)); +// insertMarkerArray( +// markers, +// lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road)); +// insertMarkerArray( +// markers, lanelet::visualization::laneletsAsTriangleMarkerArray( +// "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); +// insertMarkerArray( +// markers, lanelet::visualization::laneletsAsTriangleMarkerArray( +// "walkway_lanelets", walkway_lanelets, cl_cross)); +// insertMarkerArray(markers, lanelet::visualization::laneletDirectionAsMarkerArray(road_lanelets)); +// insertMarkerArray( +// markers, +// lanelet::visualization::lineStringsAsMarkerArray(stop_lines, "stop_lines", cl_stoplines, 0.1)); +// insertMarkerArray( +// markers, +// lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); +// insertMarkerArray( +// markers, lanelet::visualization::detectionAreasAsMarkerArray(da_reg_elems, cl_detection_areas)); +// insertMarkerArray( +// markers, lanelet::visualization::parkingLotsAsMarkerArray(parking_lots, cl_parking_lots)); +// insertMarkerArray( +// markers, lanelet::visualization::parkingSpacesAsMarkerArray(parking_spaces, cl_parking_spaces)); +// insertMarkerArray( +// markers, lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id)); +// insertMarkerArray( +// markers, lanelet::visualization::generateLaneletIdMarker(crosswalk_lanelets, cl_lanelet_id)); +// return markers; +// } +} // namespace lanelet_map +} // namespace traffic_simulator From b5cf5e53e0e0eb54c5b2b43a14f50fdab9f5ddcf Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 3 Dec 2024 15:56:15 +0100 Subject: [PATCH 02/62] feat(traffic_simulator, openscenario_interpreter, behavior_tree_plugin, simple_sensor_simulator): use pose:: from lanelet_wrapper instead of hdmap_utils methods --- .../simulator_core.hpp | 6 +- .../src/openscenario_interpreter.cpp | 23 +- .../behavior_tree_plugin/src/action_node.cpp | 2 +- .../src/pedestrian/pedestrian_action_node.cpp | 2 +- simulation/do_nothing_plugin/src/plugin.cpp | 4 +- .../src/simple_sensor_simulator.cpp | 2 + .../ego_entity_simulation.cpp | 3 +- .../test_ego_entity_simulation.cpp | 8 +- .../traffic_simulator/api/configuration.hpp | 44 +++- .../data_type/entity_status.hpp | 8 +- .../data_type/lanelet_pose.hpp | 32 +-- .../entity/entity_manager.hpp | 2 +- .../traffic_simulator/hdmap_utils/cache.hpp | 20 +- .../traffic_simulator/helper/helper.hpp | 15 +- .../include/traffic_simulator/utils/pose.hpp | 60 +++-- simulation/traffic_simulator/src/api/api.cpp | 4 +- .../src/data_type/entity_status.cpp | 16 +- .../src/data_type/lanelet_pose.cpp | 93 +++---- .../src/entity/ego_entity.cpp | 2 +- .../src/entity/entity_base.cpp | 11 +- .../src/entity/pedestrian_entity.cpp | 4 +- .../src/entity/vehicle_entity.cpp | 6 +- .../traffic_simulator/src/helper/helper.cpp | 21 +- .../src/traffic/traffic_controller.cpp | 5 +- .../src/traffic/traffic_source.cpp | 27 +- .../traffic_simulator/src/utils/distance.cpp | 5 +- .../traffic_simulator/src/utils/pose.cpp | 238 ++++++++++++++---- 27 files changed, 389 insertions(+), 274 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 685737ef982..5fe8749dabc 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -74,7 +74,7 @@ class SimulatorCore static auto canonicalize(const traffic_simulator::LaneletPose & non_canonicalized) -> NativeLanePosition { - return NativeLanePosition(non_canonicalized, core->getHdmapUtils()); + return NativeLanePosition(non_canonicalized); } template , int> = 0> @@ -82,8 +82,8 @@ class SimulatorCore { constexpr bool include_crosswalk{false}; if ( - const auto result = traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, include_crosswalk, core->getHdmapUtils())) { + const auto result = + traffic_simulator::pose::toCanonicalizedLaneletPose(pose, include_crosswalk)) { return result.value(); } else { throw Error( diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index d2f7c910cc0..e65f6677705 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -65,21 +65,18 @@ auto Interpreter::currentScenarioDefinition() const -> const std::shared_ptr traffic_simulator::Configuration { + constexpr bool auto_sink{false}; const auto logic_file = currentScenarioDefinition()->road_network.logic_file; - - auto configuration = traffic_simulator::Configuration( - logic_file.isDirectory() ? logic_file : logic_file.filepath.parent_path()); - { - configuration.auto_sink = false; - configuration.scenario_path = osc_path; - - // XXX DIRTY HACK!!! - if (not logic_file.isDirectory() and logic_file.filepath.extension() == ".osm") { - configuration.lanelet2_map_file = logic_file.filepath.filename().string(); - } + // XXX DIRTY HACK!!! + if (not logic_file.isDirectory() and logic_file.filepath.extension() == ".osm") { + return traffic_simulator::Configuration( + logic_file.isDirectory() ? logic_file : logic_file.filepath.parent_path(), + logic_file.filepath.filename().string(), osc_path, auto_sink); + } else { + return traffic_simulator::Configuration( + logic_file.isDirectory() ? logic_file : logic_file.filepath.parent_path(), osc_path, + auto_sink); } - - return configuration; } auto Interpreter::on_configure(const rclcpp_lifecycle::State &) -> Result diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index cbe86b1c649..bf405333f5f 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -105,7 +105,7 @@ auto ActionNode::setCanonicalizedEntityStatus(const traffic_simulator::EntitySta -> void { canonicalized_entity_status->set( - entity_status, default_matching_distance_for_lanelet_pose_calculation, hdmap_utils); + entity_status, default_matching_distance_for_lanelet_pose_calculation); } auto ActionNode::getOtherEntityStatus(lanelet::Id lanelet_id) const diff --git a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp index cbf2f304c22..c7f8a35a3a4 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp @@ -58,7 +58,7 @@ auto PedestrianActionNode::calculateUpdatedEntityStatusInWorldFrame(double targe traffic_simulator::pose::pedestrian::transformToCanonicalizedLaneletPose( entity_status_updated.pose, canonicalized_entity_status->getBoundingBox(), canonicalized_entity_status->getLaneletIds(), true, - default_matching_distance_for_lanelet_pose_calculation, hdmap_utils)) { + default_matching_distance_for_lanelet_pose_calculation)) { entity_status_updated.lanelet_pose_valid = true; entity_status_updated.lanelet_pose = static_cast(canonicalized_lanelet_pose.value()); diff --git a/simulation/do_nothing_plugin/src/plugin.cpp b/simulation/do_nothing_plugin/src/plugin.cpp index af44aebae02..9a24230969b 100644 --- a/simulation/do_nothing_plugin/src/plugin.cpp +++ b/simulation/do_nothing_plugin/src/plugin.cpp @@ -181,7 +181,7 @@ void DoNothingBehavior::update(double current_time, double step_time) if (getRequest() == traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY) { canonicalized_entity_status_->set( interpolate_entity_status_on_polyline_trajectory(), - getDefaultMatchingDistanceForLaneletPoseCalculation(), getHdMapUtils()); + getDefaultMatchingDistanceForLaneletPoseCalculation()); if ( getCurrentTime() + getStepTime() >= do_nothing_behavior::follow_trajectory::getLastVertexTimestamp(getPolylineTrajectory())) { @@ -190,7 +190,7 @@ void DoNothingBehavior::update(double current_time, double step_time) } else { canonicalized_entity_status_->set( static_cast(*canonicalized_entity_status_), - getDefaultMatchingDistanceForLaneletPoseCalculation(), getHdMapUtils()); + getDefaultMatchingDistanceForLaneletPoseCalculation()); } } diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index a4b64bfd044..de523d83b17 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -83,6 +84,7 @@ auto ScenarioSimulator::initialize(const simulation_api_schema::InitializeReques step_time_ = req.step_time(); current_simulation_time_ = req.initialize_time(); current_scenario_time_ = std::numeric_limits::quiet_NaN(); + traffic_simulator::lanelet_map::activate(req.lanelet2_map_path()); builtin_interfaces::msg::Time t; simulation_interface::toMsg(req.initialize_ros_time(), t); current_ros_time_ = t; diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp index f28111b6a1c..534477d0b4b 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp @@ -428,8 +428,7 @@ auto EgoEntitySimulation::setStatus(const traffic_simulator_msgs::msg::EntitySta /// value from EntityStatus, therefore canonicalization has to be done in advance, /// not inside CanonicalizedEntityStatus const auto canonicalized_lanelet_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( - status.pose, status.bounding_box, unique_route_lanelets, false, matching_distance, - hdmap_utils_ptr_); + status.pose, status.bounding_box, unique_route_lanelets, false, matching_distance); status_.set(traffic_simulator::CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); setAutowareStatus(); } diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp index 00c86c4f71b..f5b522890fd 100644 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include using namespace vehicle_simulation; @@ -36,6 +37,10 @@ TEST(EgoEntitySimulation, calculateAccelerationBySlope) .longitude(-1.20294718763) .altitude(0.0)); + const auto lanelet_path = ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/slope/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(lanelet_path); + constexpr double gravity_acceleration = -9.81; // expected value in the lanelet(id:7) // first 25m: 1m up @@ -51,8 +56,7 @@ TEST(EgoEntitySimulation, calculateAccelerationBySlope) initial_status.name = "ego"; initial_status.lanelet_pose_valid = true; initial_status.lanelet_pose = lanelet_pose; - initial_status.pose = - traffic_simulator::pose::toMapPose(initial_status.lanelet_pose, hdmap_utils); + initial_status.pose = traffic_simulator::pose::toMapPose(initial_status.lanelet_pose); EgoEntitySimulation ego_entity_simulation( initial_status, traffic_simulator_msgs::msg::VehicleParameters(), 1.f / 30.f, hdmap_utils, diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index 155fab255b9..e62833cbd90 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace traffic_simulator { @@ -31,17 +32,17 @@ struct Configuration using Pathname = boost::filesystem::path; - bool auto_sink = true; + const bool auto_sink = true; bool verbose = false; - bool standalone_mode = false; + const bool standalone_mode = false; std::string simulator_host = "localhost"; - double conventional_traffic_light_publish_rate = 30.0; + const double conventional_traffic_light_publish_rate = 30.0; - double v2i_traffic_light_publish_rate = 10.0; + const double v2i_traffic_light_publish_rate = 10.0; /* ---- NOTE ----------------------------------------------------------------- * @@ -58,17 +59,33 @@ struct Configuration * ------------------------------------------------------------------------ */ const Pathname map_path; - Filename lanelet2_map_file; + const Filename lanelet2_map_file; - Filename pointcloud_map_file; + const Filename pointcloud_map_file; - Pathname scenario_path = ""; + const Pathname scenario_path; - explicit Configuration(const Pathname & map_path) // - : map_path(assertMapPath(map_path)), + explicit Configuration( + const Pathname & map_path, const Pathname & scenario_path = "", const bool auto_sink = true) + : auto_sink(auto_sink), + map_path(assertMapPath(map_path)), lanelet2_map_file(findLexicographicallyFirstFilenameOf(map_path, ".osm")), - pointcloud_map_file(findLexicographicallyFirstFilenameOf(map_path, ".pcd")) + pointcloud_map_file(findLexicographicallyFirstFilenameOf(map_path, ".pcd")), + scenario_path(scenario_path) { + traffic_simulator::lanelet_map::activate(lanelet2_map_path().string()); + } + + explicit Configuration( + const Pathname & map_path, const Filename & lanelet2_map_file, + const Pathname & scenario_path = "", const bool auto_sink = true) + : auto_sink(auto_sink), + map_path(assertMapPath(map_path)), + lanelet2_map_file(lanelet2_map_file), + pointcloud_map_file(findLexicographicallyFirstFilenameOf(map_path, ".pcd")), + scenario_path(scenario_path) + { + traffic_simulator::lanelet_map::activate(lanelet2_map_path().string()); } auto assertMapPath(const Pathname & map_path) const -> const Pathname & @@ -130,9 +147,12 @@ struct Configuration } } - auto lanelet2_map_path() const { return map_path / lanelet2_map_file; } + auto lanelet2_map_path() const -> boost::filesystem::path { return map_path / lanelet2_map_file; } - auto pointcloud_map_path() const { return map_path / pointcloud_map_file; } + auto pointcloud_map_path() const -> boost::filesystem::path + { + return map_path / pointcloud_map_file; + } }; } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp index 68a781808a3..ea611063199 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp @@ -39,11 +39,9 @@ class CanonicalizedEntityStatus auto set(const CanonicalizedEntityStatus & status) -> void; auto set( - const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance, - const std::shared_ptr & hdmap_utils_ptr) -> void; - auto set( - const EntityStatus & status, const double matching_distance, - const std::shared_ptr & hdmap_utils_ptr) -> void; + const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance) + -> void; + auto set(const EntityStatus & status, const double matching_distance) -> void; auto setAction(const std::string & action) -> void; auto getActionStatus() const noexcept -> const traffic_simulator_msgs::msg::ActionStatus &; 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 34d75b4e1f3..11fca6a9dc4 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 @@ -15,7 +15,15 @@ #ifndef TRAFFIC_SIMULATOR__DATA_TYPE__LANELET_POSE_HPP_ #define TRAFFIC_SIMULATOR__DATA_TYPE__LANELET_POSE_HPP_ +#include + +#include +#include +#include +#include +#include #include +#include namespace traffic_simulator { @@ -26,23 +34,24 @@ inline namespace lanelet_pose class CanonicalizedLaneletPose { public: + explicit CanonicalizedLaneletPose(const LaneletPose & non_canonicalized_lanelet_pose); explicit CanonicalizedLaneletPose( - const LaneletPose & maybe_non_canonicalized_lanelet_pose, - const std::shared_ptr & hdmap_utils); - explicit CanonicalizedLaneletPose( - const LaneletPose & maybe_non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets, - const std::shared_ptr & hdmap_utils); + const LaneletPose & non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets); CanonicalizedLaneletPose(const CanonicalizedLaneletPose & other); CanonicalizedLaneletPose(CanonicalizedLaneletPose && other) noexcept; CanonicalizedLaneletPose & operator=(const CanonicalizedLaneletPose & obj); explicit operator LaneletPose() const noexcept { return lanelet_pose_; } explicit operator geometry_msgs::msg::Pose() const noexcept { return map_pose_; } + auto getLaneletPose() const -> const LaneletPose & { return lanelet_pose_; } + auto getLaneletId() const noexcept -> lanelet::Id { return lanelet_pose_.lanelet_id; } + auto alignOrientationToLanelet() -> void; auto hasAlternativeLaneletPose() const -> bool { return lanelet_poses_.size() > 1; } auto getAlternativeLaneletPoseBaseOnShortestRouteFrom( LaneletPose from, const std::shared_ptr & hdmap_utils, - const traffic_simulator::RoutingConfiguration & routing_configuration = - traffic_simulator::RoutingConfiguration()) const -> std::optional; + const RoutingConfiguration & routing_configuration = RoutingConfiguration()) const + -> std::optional; + static auto setConsiderPoseByRoadSlope(bool consider_pose_by_road_slope) -> void { consider_pose_by_road_slope_ = consider_pose_by_road_slope; @@ -72,14 +81,7 @@ class CanonicalizedLaneletPose #undef DEFINE_COMPARISON_OPERATOR private: - auto adjustOrientationAndOzPosition(const std::shared_ptr & hdmap_utils) - -> void; - auto canonicalize( - const LaneletPose & may_non_canonicalized_lanelet_pose, - const std::shared_ptr & hdmap_utils) -> LaneletPose; - auto canonicalize( - const LaneletPose & may_non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets, - const std::shared_ptr & hdmap_utils) -> LaneletPose; + auto adjustOrientationAndOzPosition() -> void; LaneletPose lanelet_pose_; std::vector lanelet_poses_; geometry_msgs::msg::Pose map_pose_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 89c2bbec9d3..778b1d61459 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -373,7 +373,7 @@ class EntityManager } else if constexpr (std::is_same_v, geometry_msgs::msg::Pose>) { entity_status.pose = pose; const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( - pose, parameters.bounding_box, include_crosswalk, matching_distance, hdmap_utils_ptr_); + pose, parameters.bounding_box, include_crosswalk, matching_distance); return CanonicalizedEntityStatus(entity_status, canonicalized_lanelet_pose); } }; diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp index 01efef8059d..f8895a0a3c2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp @@ -20,28 +20,10 @@ #include #include #include +#include #include #include -namespace std -{ -template <> -struct hash> -{ -public: - size_t operator()(const std::tuple & data) const - { - std::hash lanelet_id_hash; - size_t seed = 0; - // hash combine like boost library - seed ^= lanelet_id_hash(std::get<0>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); - seed ^= lanelet_id_hash(std::get<1>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); - seed ^= std::hash{}(std::get<2>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); - return seed; - } -}; -} // namespace std - namespace hdmap_utils { class RouteCache diff --git a/simulation/traffic_simulator/include/traffic_simulator/helper/helper.hpp b/simulation/traffic_simulator/include/traffic_simulator/helper/helper.hpp index e10656fc00a..c86e3723c3c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/helper/helper.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/helper/helper.hpp @@ -26,7 +26,6 @@ #include #include #include -#include #include #include @@ -67,12 +66,10 @@ LaneletPose constructLaneletPose( * @param lanelet_id lanelet id * @param s s value in lane coordinate * @param offset offset value in lane coordinate - * @param hdmap_utils_ptr pointer to HdmapUtils * @return LaneletPose */ -auto constructCanonicalizedLaneletPose( - lanelet::Id lanelet_id, double s, double offset, - const std::shared_ptr & hdmap_utils_ptr) -> CanonicalizedLaneletPose; +auto constructCanonicalizedLaneletPose(lanelet::Id lanelet_id, double s, double offset) + -> CanonicalizedLaneletPose; /** * @brief helper function for constructing canonicalized lanelet pose @@ -83,12 +80,11 @@ auto constructCanonicalizedLaneletPose( * @param roll roll value in the lane coordinate * @param pitch pitch value in the lane coordinate * @param yaw yaw value in the lane coordinate - * @param hdmap_utils_ptr pointer to HdmapUtils * @return LaneletPose */ auto constructCanonicalizedLaneletPose( - lanelet::Id lanelet_id, double s, double offset, double roll, double pitch, double yaw, - const std::shared_ptr & hdmap_utils_ptr) -> CanonicalizedLaneletPose; + lanelet::Id lanelet_id, double s, double offset, double roll, double pitch, double yaw) + -> CanonicalizedLaneletPose; /** * @brief helper function for constructing rpy @@ -157,7 +153,8 @@ const simulation_api_schema::DetectionSensorConfiguration constructDetectionSens } // namespace helper } // namespace traffic_simulator -std::ostream & operator<<(std::ostream & os, const traffic_simulator::LaneletPose & ll_pose); +std::ostream & operator<<( + std::ostream & os, const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose); std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Point & point); diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 9dc3b8053f8..235b6dffa51 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -16,51 +16,56 @@ #define TRAFFIC_SIMULATOR__UTILS__POSE_HPP_ #include +#include namespace traffic_simulator { inline namespace pose { -// Useful constructors auto quietNaNPose() -> geometry_msgs::msg::Pose; auto quietNaNLaneletPose() -> LaneletPose; // Conversions -auto canonicalize( - const LaneletPose & lanelet_pose, - const std::shared_ptr & hdmap_utils_ptr) - -> std::optional; +auto canonicalize(const LaneletPose & lanelet_pose) -> LaneletPose; + +auto canonicalize(const LaneletPose & lanelet_pose, const lanelet::Ids & route_lanelets) + -> LaneletPose; auto toMapPose(const CanonicalizedLaneletPose & lanelet_pose) -> geometry_msgs::msg::Pose; -auto toMapPose( - const LaneletPose & lanelet_pose, - const std::shared_ptr & hdmap_utils_ptr) -> geometry_msgs::msg::Pose; +auto toMapPose(const LaneletPose & lanelet_pose) -> geometry_msgs::msg::Pose; + +auto alternativeLaneletPoses(const LaneletPose & lanelet_pose) -> std::vector; + +auto toCanonicalizedLaneletPose(const LaneletPose & lanelet_pose) + -> std::optional; auto toCanonicalizedLaneletPose( - const geometry_msgs::msg::Pose & map_pose, const bool include_crosswalk, - const std::shared_ptr & hdmap_utils_ptr) + const geometry_msgs::msg::Pose & map_pose, const bool include_crosswalk) -> std::optional; +auto toCanonicalizedLaneletPose( + const geometry_msgs::msg::Point & map_point, + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk, + const double matching_distance) -> std::optional; + auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk, - const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional; + const double matching_distance) -> std::optional; auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & unique_route_lanelets, const bool include_crosswalk, - const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional; + const double matching_distance) -> std::optional; auto transformRelativePoseToGlobal( const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose) -> geometry_msgs::msg::Pose; -// Relative msg::Pose +// Relative msg::geometry_msgs::msg::Pose auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) -> std::optional; @@ -83,6 +88,10 @@ auto relativeLaneletPose( const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose; +// auto relativeLaneletPose( +// const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, +// const RoutingConfiguration & routing_configuration) -> LaneletPose; + auto boundingBoxRelativeLaneletPose( const CanonicalizedLaneletPose & from, const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, @@ -91,19 +100,29 @@ auto boundingBoxRelativeLaneletPose( const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> 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) -> 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; +// auto isInLanelet( +// const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id lanelet_id, +// const double tolerance) -> bool; + +auto isInLanelet(const geometry_msgs::msg::Point & point, const lanelet::Id lanelet_id) -> bool; + auto isAtEndOfLanelets( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const std::shared_ptr & hdmap_utils_ptr) -> bool; -// it will be moved to "lanelet" namespace -auto laneletLength( - const lanelet::Id lanelet_id, const std::shared_ptr & hdmap_utils_ptr) - -> double; +// auto isAtEndOfLanelets(const CanonicalizedLaneletPose & canonicalized_lanelet_pose) -> bool; namespace pedestrian { @@ -111,8 +130,7 @@ auto transformToCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & unique_route_lanelets, const bool include_crosswalk, - const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional; + const double matching_distance) -> std::optional; } // namespace pedestrian } // namespace pose } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 1f24e201b3c..1056c17257c 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -151,9 +151,7 @@ auto API::setEntityStatus( const std::string & name, const LaneletPose & lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void { - if ( - const auto canonicalized_lanelet_pose = - pose::canonicalize(lanelet_pose, entity_manager_ptr_->getHdmapUtils())) { + if (const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(lanelet_pose)) { setEntityStatus(name, canonicalized_lanelet_pose.value(), action_status); } else { std::stringstream ss; diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index b2bfb78432e..391d8c05cfe 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -64,8 +64,8 @@ auto CanonicalizedEntityStatus::set(const CanonicalizedEntityStatus & status) -> } auto CanonicalizedEntityStatus::set( - const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance, - const std::shared_ptr & hdmap_utils_ptr) -> void + const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance) + -> void { const auto include_crosswalk = getType().type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN || @@ -73,21 +73,19 @@ auto CanonicalizedEntityStatus::set( std::optional canonicalized_lanelet_pose; if (status.lanelet_pose_valid) { - canonicalized_lanelet_pose = pose::canonicalize(status.lanelet_pose, hdmap_utils_ptr); + canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(status.lanelet_pose); } else { // prefer the current lanelet canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( - status.pose, getBoundingBox(), lanelet_ids, include_crosswalk, matching_distance, - hdmap_utils_ptr); + status.pose, getBoundingBox(), lanelet_ids, include_crosswalk, matching_distance); } set(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); } -auto CanonicalizedEntityStatus::set( - const EntityStatus & status, const double matching_distance, - const std::shared_ptr & hdmap_utils_ptr) -> void +auto CanonicalizedEntityStatus::set(const EntityStatus & status, const double matching_distance) + -> void { - set(status, getLaneletIds(), matching_distance, hdmap_utils_ptr); + set(status, getLaneletIds(), matching_distance); } auto CanonicalizedEntityStatus::setAction(const std::string & action) -> void diff --git a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp index b4dfb4dc0d8..975129d65c9 100644 --- a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp +++ b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include namespace traffic_simulator @@ -25,25 +26,21 @@ namespace traffic_simulator inline namespace lanelet_pose { CanonicalizedLaneletPose::CanonicalizedLaneletPose( - const LaneletPose & maybe_non_canonicalized_lanelet_pose, - const std::shared_ptr & hdmap_utils) -: lanelet_pose_(canonicalize(maybe_non_canonicalized_lanelet_pose, hdmap_utils)), - lanelet_poses_( - hdmap_utils->getAllCanonicalizedLaneletPoses(maybe_non_canonicalized_lanelet_pose)), - map_pose_(pose::toMapPose(lanelet_pose_, hdmap_utils)) + const LaneletPose & non_canonicalized_lanelet_pose) +: lanelet_pose_(pose::canonicalize(non_canonicalized_lanelet_pose)), + lanelet_poses_(pose::alternativeLaneletPoses(non_canonicalized_lanelet_pose)), + map_pose_(pose::toMapPose(lanelet_pose_)) { - adjustOrientationAndOzPosition(hdmap_utils); + adjustOrientationAndOzPosition(); } CanonicalizedLaneletPose::CanonicalizedLaneletPose( - const LaneletPose & maybe_non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets, - const std::shared_ptr & hdmap_utils) -: lanelet_pose_(canonicalize(maybe_non_canonicalized_lanelet_pose, route_lanelets, hdmap_utils)), - lanelet_poses_( - hdmap_utils->getAllCanonicalizedLaneletPoses(maybe_non_canonicalized_lanelet_pose)), - map_pose_(pose::toMapPose(lanelet_pose_, hdmap_utils)) + const LaneletPose & non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets) +: lanelet_pose_(pose::canonicalize(non_canonicalized_lanelet_pose, route_lanelets)), + lanelet_poses_(pose::alternativeLaneletPoses(non_canonicalized_lanelet_pose)), + map_pose_(pose::toMapPose(lanelet_pose_)) { - adjustOrientationAndOzPosition(hdmap_utils); + adjustOrientationAndOzPosition(); } CanonicalizedLaneletPose::CanonicalizedLaneletPose(const CanonicalizedLaneletPose & other) @@ -69,50 +66,9 @@ auto CanonicalizedLaneletPose::operator=(const CanonicalizedLaneletPose & other) return *this; } -auto CanonicalizedLaneletPose::canonicalize( - const LaneletPose & may_non_canonicalized_lanelet_pose, - const std::shared_ptr & hdmap_utils) -> LaneletPose -{ - if ( - const auto canonicalized = std::get>( - hdmap_utils->canonicalizeLaneletPose(may_non_canonicalized_lanelet_pose))) { - return canonicalized.value(); - } else { - THROW_SEMANTIC_ERROR( - "Lanelet pose (id=", may_non_canonicalized_lanelet_pose.lanelet_id, - ",s=", may_non_canonicalized_lanelet_pose.s, - ",offset=", may_non_canonicalized_lanelet_pose.offset, - ",rpy.x=", may_non_canonicalized_lanelet_pose.rpy.x, - ",rpy.y=", may_non_canonicalized_lanelet_pose.rpy.y, - ",rpy.z=", may_non_canonicalized_lanelet_pose.rpy.z, - ") is invalid, please check lanelet length and connection."); - } -} - -auto CanonicalizedLaneletPose::canonicalize( - const LaneletPose & may_non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets, - const std::shared_ptr & hdmap_utils) -> LaneletPose -{ - if ( - const auto canonicalized = std::get>( - hdmap_utils->canonicalizeLaneletPose(may_non_canonicalized_lanelet_pose, route_lanelets))) { - return canonicalized.value(); - } else { - THROW_SEMANTIC_ERROR( - "Lanelet pose (id=", may_non_canonicalized_lanelet_pose.lanelet_id, - ",s=", may_non_canonicalized_lanelet_pose.s, - ",offset=", may_non_canonicalized_lanelet_pose.offset, - ",rpy.x=", may_non_canonicalized_lanelet_pose.rpy.x, - ",rpy.y=", may_non_canonicalized_lanelet_pose.rpy.y, - ",rpy.z=", may_non_canonicalized_lanelet_pose.rpy.z, - ") is invalid, please check lanelet length, connection and entity route."); - } -} - auto CanonicalizedLaneletPose::getAlternativeLaneletPoseBaseOnShortestRouteFrom( LaneletPose from, const std::shared_ptr & hdmap_utils, - const traffic_simulator::RoutingConfiguration & routing_configuration) const - -> std::optional + const RoutingConfiguration & routing_configuration) const -> std::optional { if (lanelet_poses_.empty()) { return std::nullopt; @@ -131,14 +87,31 @@ auto CanonicalizedLaneletPose::getAlternativeLaneletPoseBaseOnShortestRouteFrom( return alternative_lanelet_pose; } -auto CanonicalizedLaneletPose::adjustOrientationAndOzPosition( - const std::shared_ptr & hdmap_utils) -> void +auto CanonicalizedLaneletPose::alignOrientationToLanelet() -> void +{ + using math::geometry::convertEulerAngleToQuaternion; + using math::geometry::convertQuaternionToEulerAngle; + /// @todo it will be changed to route::toSpline(...) + const auto spline = math::geometry::CatmullRomSpline( + lanelet_wrapper::lanelet_map::centerPoints({lanelet_pose_.lanelet_id})); + const auto lanelet_quaternion = spline.getPose(lanelet_pose_.s, true).orientation; + const auto lanelet_rpy = convertQuaternionToEulerAngle(lanelet_quaternion); + map_pose_.orientation = + convertEulerAngleToQuaternion(geometry_msgs::build() + .x(lanelet_rpy.x) + .y(lanelet_rpy.y) + .z(lanelet_rpy.z)); + lanelet_pose_.rpy = geometry_msgs::build().x(0.0).y(0.0).z(0.0); +} + +auto CanonicalizedLaneletPose::adjustOrientationAndOzPosition() -> void { using math::geometry::convertEulerAngleToQuaternion; using math::geometry::convertQuaternionToEulerAngle; using math::geometry::getRotation; - const math::geometry::CatmullRomSpline spline( - hdmap_utils->getCenterPoints(lanelet_pose_.lanelet_id)); + /// @todo it will be changed to route::toSpline(...) + const auto spline = math::geometry::CatmullRomSpline( + lanelet_wrapper::lanelet_map::centerPoints({lanelet_pose_.lanelet_id})); // adjust Oz position if (const auto s_value = spline.getSValue(map_pose_)) { map_pose_.position.z = spline.getPoint(s_value.value()).z; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 215ea627064..84b8412555c 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -303,7 +303,7 @@ auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void // prefer current lanelet on Autoware side status_->set( entity_status, helper::getUniqueValues(getRouteLanelets()), - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); + getDefaultMatchingDistanceForLaneletPoseCalculation()); } } // namespace entity } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 4a283710e8d..44f55312b13 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -99,7 +99,7 @@ auto EntityBase::getCanonicalizedLaneletPose(const double matching_distance) con // prefer the current lanelet return pose::toCanonicalizedLaneletPose( status_->getMapPose(), status_->getBoundingBox(), status_->getLaneletIds(), include_crosswalk, - matching_distance, hdmap_utils_ptr_); + matching_distance); } auto EntityBase::getDefaultMatchingDistanceForLaneletPoseCalculation() const -> double @@ -545,13 +545,12 @@ void EntityBase::setOtherStatus( auto EntityBase::setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void { - status_->set( - status, lanelet_ids, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); + status_->set(status, lanelet_ids, getDefaultMatchingDistanceForLaneletPoseCalculation()); } auto EntityBase::setStatus(const EntityStatus & status) -> void { - status_->set(status, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); + status_->set(status, getDefaultMatchingDistanceForLaneletPoseCalculation()); } auto EntityBase::setCanonicalizedStatus(const CanonicalizedEntityStatus & status) -> void @@ -734,8 +733,8 @@ auto EntityBase::requestSynchronize( : other_status_.find(target_name)->second.getLaneletPose(); const auto target_entity_distance = longitudinalDistance( - CanonicalizedLaneletPose(target_entity_lanelet_pose, hdmap_utils_ptr_), target_sync_pose, - true, true, lane_changeable_routing_configuration, hdmap_utils_ptr_); + CanonicalizedLaneletPose(target_entity_lanelet_pose), target_sync_pose, true, true, + lane_changeable_routing_configuration, hdmap_utils_ptr_); 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/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp index da7ec5198e4..292c918e86d 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp @@ -72,7 +72,7 @@ void PedestrianEntity::requestAssignRoute(const std::vectorgetBoundingBox(), true, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { + getDefaultMatchingDistanceForLaneletPoseCalculation())) { route.emplace_back(canonicalized_lanelet_pose.value()); } else { THROW_SEMANTIC_ERROR("Waypoint of pedestrian entity should be on lane."); @@ -148,7 +148,7 @@ void PedestrianEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & m if ( const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( map_pose, status_->getBoundingBox(), true, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { + getDefaultMatchingDistanceForLaneletPoseCalculation())) { requestAcquirePosition(canonicalized_lanelet_pose.value()); } else { THROW_SEMANTIC_ERROR("Goal of the pedestrian entity should be on lane."); diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index e67fccac006..59df55b01c2 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -174,7 +174,7 @@ void VehicleEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & map_ if ( const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( map_pose, status_->getBoundingBox(), false, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { + getDefaultMatchingDistanceForLaneletPoseCalculation())) { requestAcquirePosition(canonicalized_lanelet_pose.value()); } else { THROW_SEMANTIC_ERROR("Goal of the vehicle entity should be on lane."); @@ -202,7 +202,7 @@ void VehicleEntity::requestAssignRoute(const std::vectorgetBoundingBox(), false, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { + getDefaultMatchingDistanceForLaneletPoseCalculation())) { route.emplace_back(canonicalized_lanelet_pose.value()); } else { THROW_SEMANTIC_ERROR("Waypoint of vehicle entity should be on lane."); @@ -221,7 +221,7 @@ auto VehicleEntity::requestFollowTrajectory( if ( const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( vertex.position, status_->getBoundingBox(), false, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { + getDefaultMatchingDistanceForLaneletPoseCalculation())) { waypoints.emplace_back(canonicalized_lanelet_pose.value()); } else { /// @todo such a protection most likely makes sense, but test scenario diff --git a/simulation/traffic_simulator/src/helper/helper.cpp b/simulation/traffic_simulator/src/helper/helper.cpp index ccd672cdb94..b36e3aacfb1 100644 --- a/simulation/traffic_simulator/src/helper/helper.cpp +++ b/simulation/traffic_simulator/src/helper/helper.cpp @@ -51,13 +51,12 @@ LaneletPose constructLaneletPose( } auto constructCanonicalizedLaneletPose( - lanelet::Id lanelet_id, double s, double offset, double roll, double pitch, double yaw, - const std::shared_ptr & hdmap_utils_ptr) -> CanonicalizedLaneletPose + lanelet::Id lanelet_id, double s, double offset, double roll, double pitch, double yaw) + -> CanonicalizedLaneletPose { if ( - auto canonicalized_lanelet_pose = pose::canonicalize( - traffic_simulator::helper::constructLaneletPose(lanelet_id, s, offset, roll, pitch, yaw), - hdmap_utils_ptr)) { + auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + traffic_simulator::helper::constructLaneletPose(lanelet_id, s, offset, roll, pitch, yaw))) { return canonicalized_lanelet_pose.value(); } else { THROW_SEMANTIC_ERROR( @@ -67,11 +66,10 @@ auto constructCanonicalizedLaneletPose( } } -auto constructCanonicalizedLaneletPose( - lanelet::Id lanelet_id, double s, double offset, - const std::shared_ptr & hdmap_utils_ptr) -> CanonicalizedLaneletPose +auto constructCanonicalizedLaneletPose(lanelet::Id lanelet_id, double s, double offset) + -> CanonicalizedLaneletPose { - return constructCanonicalizedLaneletPose(lanelet_id, s, offset, 0, 0, 0, hdmap_utils_ptr); + return constructCanonicalizedLaneletPose(lanelet_id, s, offset, 0, 0, 0); } geometry_msgs::msg::Vector3 constructRPY(double roll, double pitch, double yaw) @@ -189,9 +187,10 @@ const simulation_api_schema::LidarConfiguration constructLidarConfiguration( } // namespace helper } // namespace traffic_simulator -std::ostream & operator<<(std::ostream & os, const traffic_simulator::LaneletPose & ll_pose) +std::ostream & operator<<( + std::ostream & os, const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) { - os << "lanelet id : " << ll_pose.lanelet_id << "\ns : " << ll_pose.s; + os << "lanelet id : " << lanelet_pose.lanelet_id << "\ns : " << lanelet_pose.s; return os; } diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index cd8ab6d5a37..92ddb0c8349 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -58,8 +59,8 @@ void TrafficController::autoSink() if (hdmap_utils_->getNextLaneletIds(lanelet_id).empty()) { LaneletPose lanelet_pose; lanelet_pose.lanelet_id = lanelet_id; - lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_); - const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_); + lanelet_pose.s = lanelet_map::laneletLength(lanelet_id); + const auto pose = pose::toMapPose(lanelet_pose); addModule( lanelet_id, 1, pose.position, get_entity_names_function, get_entity_pose_function, despawn_function); diff --git a/simulation/traffic_simulator/src/traffic/traffic_source.cpp b/simulation/traffic_simulator/src/traffic/traffic_source.cpp index 350920a42d1..903bd61ebce 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_source.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_source.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include namespace traffic_simulator @@ -172,18 +173,14 @@ auto TrafficSource::isPoseValid( return {true, std::nullopt}; } - if (const auto lanelet_pose = - hdmap_utils_->toLaneletPose(pose, std::holds_alternative(parameter)); - lanelet_pose) { + if ( + auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + pose, std::holds_alternative(parameter))) { /// @note reset orientation - to align the entity with lane - auto corrected_pose = lanelet_pose.value(); - corrected_pose.rpy.z = 0.0; - - auto out_pose = std::make_optional(corrected_pose, hdmap_utils_); - + canonicalized_lanelet_pose->alignOrientationToLanelet(); /// @note Step 3: check whether the bounding box can be outside lanelet if (not configuration_.require_footprint_fitting) { - return std::make_pair(true, out_pose); + return std::make_pair(true, canonicalized_lanelet_pose.value()); } /// @note Step 4: check whether the bounding box fits inside the lanelet @@ -192,17 +189,17 @@ auto TrafficSource::isPoseValid( not configuration_.require_footprint_fitting or validate_considering_crosswalk( math::geometry::transformPoints( - hdmap_utils_->toMapPose(corrected_pose).pose, bbox_corners), - corrected_pose.lanelet_id), - out_pose); + pose::toMapPose(canonicalized_lanelet_pose.value()), bbox_corners), + canonicalized_lanelet_pose->getLaneletId()), + canonicalized_lanelet_pose.value()); } else { return std::make_pair( not configuration_.require_footprint_fitting or validate( math::geometry::transformPoints( - hdmap_utils_->toMapPose(corrected_pose).pose, bbox_corners), - corrected_pose.lanelet_id), - out_pose); + pose::toMapPose(canonicalized_lanelet_pose.value()), bbox_corners), + canonicalized_lanelet_pose->getLaneletId()), + canonicalized_lanelet_pose.value()); } } else { return {false, std::nullopt}; diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 98bac102b88..d4b579a6042 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -112,9 +112,8 @@ auto longitudinalDistance( for (const auto & to_pose : to_poses) { if ( const auto distance = longitudinalDistance( - CanonicalizedLaneletPose(from_pose, hdmap_utils_ptr), - CanonicalizedLaneletPose(to_pose, hdmap_utils_ptr), false, include_opposite_direction, - routing_configuration, hdmap_utils_ptr)) { + CanonicalizedLaneletPose(from_pose), CanonicalizedLaneletPose(to_pose), false, + include_opposite_direction, routing_configuration, hdmap_utils_ptr)) { distances.emplace_back(distance.value()); } } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index b354ea0ea0f..0a2596a5782 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -13,7 +13,11 @@ // limitations under the License. #include +#include #include +#include +#include +#include #include #include #include @@ -44,15 +48,34 @@ auto quietNaNLaneletPose() -> LaneletPose .z(std::numeric_limits::quiet_NaN())); } -auto canonicalize( - const LaneletPose & lanelet_pose, - const std::shared_ptr & hdmap_utils_ptr) - -> std::optional +// Conversions +auto canonicalize(const LaneletPose & lanelet_pose) -> LaneletPose { - if (lanelet_pose == LaneletPose()) { - return std::nullopt; + if ( + const auto canonicalized = std::get>( + lanelet_wrapper::pose::canonicalizeLaneletPose(lanelet_pose))) { + return canonicalized.value(); } else { - return CanonicalizedLaneletPose(lanelet_pose, hdmap_utils_ptr); + THROW_SEMANTIC_ERROR( + "Lanelet pose (id=", lanelet_pose.lanelet_id, ",s=", lanelet_pose.s, + ",offset=", lanelet_pose.offset, ",rpy.x=", lanelet_pose.rpy.x, ",rpy.y=", lanelet_pose.rpy.y, + ",rpy.z=", lanelet_pose.rpy.z, ") is invalid, please check lanelet length and connection."); + } +} + +auto canonicalize(const LaneletPose & lanelet_pose, const lanelet::Ids & route_lanelets) + -> LaneletPose +{ + if ( + const auto canonicalized = std::get>( + lanelet_wrapper::pose::canonicalizeLaneletPose(lanelet_pose, route_lanelets))) { + return canonicalized.value(); + } else { + THROW_SEMANTIC_ERROR( + "Lanelet pose (id=", lanelet_pose.lanelet_id, ",s=", lanelet_pose.s, + ",offset=", lanelet_pose.offset, ",rpy.x=", lanelet_pose.rpy.x, ",rpy.y=", lanelet_pose.rpy.y, + ",rpy.z=", lanelet_pose.rpy.z, + ") is invalid, please check lanelet length, connection and entity route."); } } @@ -61,23 +84,38 @@ auto toMapPose(const CanonicalizedLaneletPose & lanelet_pose) -> geometry_msgs:: return static_cast(lanelet_pose); } -auto toMapPose( - const LaneletPose & lanelet_pose, - const std::shared_ptr & hdmap_utils_ptr) -> geometry_msgs::msg::Pose +auto toMapPose(const LaneletPose & lanelet_pose) -> geometry_msgs::msg::Pose { - return hdmap_utils_ptr - ->toMapPose(lanelet_pose, CanonicalizedLaneletPose::getConsiderPoseByRoadSlope()) + return lanelet_wrapper::pose::toMapPose( + lanelet_pose, CanonicalizedLaneletPose::getConsiderPoseByRoadSlope()) .pose; } +auto alternativeLaneletPoses(const LaneletPose & lanelet_pose) -> std::vector +{ + return lanelet_wrapper::pose::alternativeLaneletPoses(lanelet_pose); +} + +auto toCanonicalizedLaneletPose(const LaneletPose & lanelet_pose) + -> std::optional +{ + if (lanelet_pose == LaneletPose()) { + return std::nullopt; + } else { + return CanonicalizedLaneletPose(lanelet_pose); + } +} + auto toCanonicalizedLaneletPose( - const geometry_msgs::msg::Pose & map_pose, const bool include_crosswalk, - const std::shared_ptr & hdmap_utils_ptr) + const geometry_msgs::msg::Pose & map_pose, const bool include_crosswalk) -> std::optional { /// @todo here matching_distance should be passed - if (const auto pose = hdmap_utils_ptr->toLaneletPose(map_pose, include_crosswalk)) { - return canonicalize(pose.value(), hdmap_utils_ptr); + constexpr double matching_distance{1.0}; + if ( + const auto pose = + lanelet_wrapper::pose::toLaneletPose(map_pose, include_crosswalk, matching_distance)) { + return toCanonicalizedLaneletPose(pose.value()); } else { return std::nullopt; } @@ -86,36 +124,45 @@ auto toCanonicalizedLaneletPose( auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk, - const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional + const double matching_distance) -> std::optional { if ( - const auto pose = hdmap_utils_ptr->toLaneletPose( + const auto pose = lanelet_wrapper::pose::toLaneletPose( map_pose, bounding_box, include_crosswalk, matching_distance)) { - return canonicalize(pose.value(), hdmap_utils_ptr); + return toCanonicalizedLaneletPose(pose.value()); } else { return std::nullopt; } } +auto toCanonicalizedLaneletPose( + const geometry_msgs::msg::Point & map_point, + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk, + const double matching_distance) -> std::optional +{ + return toCanonicalizedLaneletPose( + geometry_msgs::build().position(map_point).orientation( + geometry_msgs::build().x(0).y(0).z(0).w(1)), + bounding_box, include_crosswalk, matching_distance); +} + auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & unique_route_lanelets, const bool include_crosswalk, - const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional + const double matching_distance) -> std::optional { std::optional lanelet_pose; if (!unique_route_lanelets.empty()) { lanelet_pose = - hdmap_utils_ptr->toLaneletPose(map_pose, unique_route_lanelets, matching_distance); + lanelet_wrapper::pose::toLaneletPose(map_pose, unique_route_lanelets, matching_distance); } if (!lanelet_pose) { - lanelet_pose = - hdmap_utils_ptr->toLaneletPose(map_pose, bounding_box, include_crosswalk, matching_distance); + lanelet_pose = lanelet_wrapper::pose::toLaneletPose( + map_pose, bounding_box, include_crosswalk, matching_distance); } if (lanelet_pose) { - return canonicalize(lanelet_pose.value(), hdmap_utils_ptr); + return toCanonicalizedLaneletPose(lanelet_pose.value()); } else { return std::nullopt; } @@ -133,6 +180,7 @@ auto transformRelativePoseToGlobal( return ret; } +// Relative msg::Pose auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) -> std::optional { @@ -175,6 +223,7 @@ auto boundingBoxRelativePose( return std::nullopt; } +// Relative LaneletPose auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, const traffic_simulator::RoutingConfiguration & routing_configuration, @@ -200,6 +249,27 @@ auto relativeLaneletPose( return position; } +// auto relativeLaneletPose( +// const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, +// const RoutingConfiguration & routing_configuration) -> LaneletPose +// { +// constexpr bool include_adjacent_lanelet{false}; +// constexpr bool include_opposite_direction{true}; + +// LaneletPose position = quietNaNLaneletPose(); +// // 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 = distance::longitudinalDistance( +// from, to, include_adjacent_lanelet, include_opposite_direction, routing_configuration)) { +// position.s = longitudinal_distance.value(); +// } +// if (const auto lateral_distance = distance::lateralDistance(from, to, routing_configuration)) { +// position.offset = lateral_distance.value(); +// } +// return position; +// } + auto boundingBoxRelativeLaneletPose( const CanonicalizedLaneletPose & from, const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, @@ -228,6 +298,33 @@ auto boundingBoxRelativeLaneletPose( return position; } +// 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) -> LaneletPose +// { +// constexpr bool include_adjacent_lanelet{false}; +// constexpr bool include_opposite_direction{true}; + +// LaneletPose position = quietNaNLaneletPose(); +// // 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 = distance::boundingBoxLaneLongitudinalDistance( +// from, from_bounding_box, to, to_bounding_box, include_adjacent_lanelet, +// include_opposite_direction, routing_configuration)) { +// position.s = longitudinal_bounding_box_distance.value(); +// } +// if ( +// 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; +// } + auto isInLanelet( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id lanelet_id, const double tolerance, const std::shared_ptr & hdmap_utils_ptr) -> bool @@ -239,8 +336,7 @@ auto isInLanelet( if (isSameLaneletId(canonicalized_lanelet_pose, lanelet_id)) { return true; } else { - const auto start_lanelet_pose = - helper::constructCanonicalizedLaneletPose(lanelet_id, 0.0, 0.0, hdmap_utils_ptr); + const auto start_lanelet_pose = helper::constructCanonicalizedLaneletPose(lanelet_id, 0.0, 0.0); if (const auto distance_to_start_lanelet_pose = longitudinalDistance( start_lanelet_pose, canonicalized_lanelet_pose, include_adjacent_lanelet, include_opposite_direction, routing_configuration, hdmap_utils_ptr); @@ -250,7 +346,7 @@ auto isInLanelet( } const auto end_lanelet_pose = helper::constructCanonicalizedLaneletPose( - lanelet_id, hdmap_utils_ptr->getLaneletLength(lanelet_id), 0.0, hdmap_utils_ptr); + lanelet_id, lanelet_wrapper::lanelet_map::laneletLength(lanelet_id), 0.0); if (const auto distance_to_end_lanelet_pose = longitudinalDistance( canonicalized_lanelet_pose, end_lanelet_pose, include_adjacent_lanelet, include_opposite_direction, routing_configuration, hdmap_utils_ptr); @@ -262,21 +358,59 @@ auto isInLanelet( return false; } +// auto isInLanelet( +// const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id lanelet_id, +// const double tolerance) -> bool +// { +// constexpr bool include_adjacent_lanelet{false}; +// constexpr bool include_opposite_direction{false}; +// constexpr RoutingConfiguration routing_configuration; + +// if (isSameLaneletId(canonicalized_lanelet_pose, lanelet_id)) { +// return true; +// } else { +// const auto start_lanelet_pose = helper::constructCanonicalizedLaneletPose(lanelet_id, 0.0, 0.0); +// if (const auto distance_to_start_lanelet_pose = distance::longitudinalDistance( +// start_lanelet_pose, canonicalized_lanelet_pose, include_adjacent_lanelet, +// include_opposite_direction, routing_configuration); +// distance_to_start_lanelet_pose and +// std::abs(distance_to_start_lanelet_pose.value()) <= tolerance) { +// return true; +// } + +// 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 = distance::longitudinalDistance( +// canonicalized_lanelet_pose, end_lanelet_pose, include_adjacent_lanelet, +// include_opposite_direction, routing_configuration); +// distance_to_end_lanelet_pose and +// std::abs(distance_to_end_lanelet_pose.value()) <= tolerance) { +// return true; +// } +// } +// return false; +// } + +auto isInLanelet(const geometry_msgs::msg::Point & point, const lanelet::Id lanelet_id) -> bool +{ + return lanelet_wrapper::lanelet_map::isInLanelet(lanelet_id, point); +} + auto isAtEndOfLanelets( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const std::shared_ptr & hdmap_utils_ptr) -> bool { const auto lanelet_pose = static_cast(canonicalized_lanelet_pose); return hdmap_utils_ptr->getFollowingLanelets(lanelet_pose.lanelet_id).size() == 1 && - hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) <= lanelet_pose.s; + lanelet_wrapper::lanelet_map::laneletLength(lanelet_pose.lanelet_id) <= lanelet_pose.s; } -auto laneletLength( - const lanelet::Id lanelet_id, const std::shared_ptr & hdmap_utils_ptr) - -> double -{ - return hdmap_utils_ptr->getLaneletLength(lanelet_id); -} +// auto isAtEndOfLanelets(const CanonicalizedLaneletPose & canonicalized_lanelet_pose) -> bool +// { +// const auto lanelet_pose = static_cast(canonicalized_lanelet_pose); +// return lanelet_wrapper::route::followingLanelets(lanelet_pose.lanelet_id).size() == 1 && +// lanelet_wrapper::lanelet_map::laneletLength(lanelet_pose.lanelet_id) <= lanelet_pose.s; +// } namespace pedestrian { @@ -289,46 +423,44 @@ auto transformToCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & unique_route_lanelets, const bool include_crosswalk, - const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional + const double matching_distance) -> std::optional { if ( const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - map_pose, bounding_box, unique_route_lanelets, include_crosswalk, matching_distance, - hdmap_utils_ptr)) { + map_pose, bounding_box, unique_route_lanelets, include_crosswalk, matching_distance)) { return canonicalized_lanelet_pose; } /** * @note Hard coded parameter. 2.0 is a matching threshold for lanelet. * In this branch, the algorithm only consider entity pose. */ - if (const auto lanelet_pose = hdmap_utils_ptr->toLaneletPose(map_pose, include_crosswalk, 2.0)) { - const auto canonicalized_tuple = hdmap_utils_ptr->canonicalizeLaneletPose(lanelet_pose.value()); + if ( + const auto lanelet_pose = + lanelet_wrapper::pose::toLaneletPose(map_pose, include_crosswalk, 2.0)) { + const auto canonicalized_tuple = + lanelet_wrapper::pose::canonicalizeLaneletPose(lanelet_pose.value()); if ( const auto canonicalized_lanelet_pose = std::get>(canonicalized_tuple)) { - return canonicalize(lanelet_pose.value(), hdmap_utils_ptr); + return toCanonicalizedLaneletPose(lanelet_pose.value()); } else { /// @note If canonicalize failed, set end of road lanelet pose. if ( const auto end_of_road_lanelet_id = std::get>(canonicalized_tuple)) { if (lanelet_pose.value().s < 0) { - return CanonicalizedLaneletPose( - traffic_simulator_msgs::build() - .lanelet_id(end_of_road_lanelet_id.value()) - .s(0.0) - .offset(lanelet_pose.value().offset) - .rpy(lanelet_pose.value().rpy), - hdmap_utils_ptr); + return CanonicalizedLaneletPose(traffic_simulator_msgs::build() + .lanelet_id(end_of_road_lanelet_id.value()) + .s(0.0) + .offset(lanelet_pose.value().offset) + .rpy(lanelet_pose.value().rpy)); } else { return CanonicalizedLaneletPose( traffic_simulator_msgs::build() .lanelet_id(end_of_road_lanelet_id.value()) - .s(hdmap_utils_ptr->getLaneletLength(end_of_road_lanelet_id.value())) + .s(lanelet_wrapper::lanelet_map::laneletLength(end_of_road_lanelet_id.value())) .offset(lanelet_pose.value().offset) - .rpy(lanelet_pose.value().rpy), - hdmap_utils_ptr); + .rpy(lanelet_pose.value().rpy)); } } else { THROW_SIMULATION_ERROR("Failed to find trailing lanelet_id for LaneletPose estimation."); From 8e2f7a08c2ddb217df1b6b70d196cdcfb63ea36e Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 3 Dec 2024 15:57:04 +0100 Subject: [PATCH 03/62] feat(cpp_mock_scenarios): adapt cpp screnarios for using pose:: from lanelet_wrapper --- .../cpp_mock_scenarios/cpp_scenario_node.hpp | 10 ++-- .../load_do_nothing_plugin.cpp | 14 ++--- .../src/collision/crashing_npc.cpp | 8 +-- .../src/collision/spawn_with_offset.cpp | 8 +-- .../src/crosswalk/stop_at_crosswalk.cpp | 14 ++--- .../accelerate_and_follow.cpp | 8 +-- .../decelerate_and_follow.cpp | 8 +-- .../acquire_position_in_world_frame.cpp | 11 ++-- .../assign_route_in_world_frame.cpp | 10 ++-- .../src/follow_lane/cancel_request.cpp | 11 ++-- .../src/follow_lane/follow_with_offset.cpp | 4 +- .../src/lane_change/lanechange_left.cpp | 4 +- .../lane_change/lanechange_left_with_id.cpp | 4 +- .../src/lane_change/lanechange_linear.cpp | 4 +- .../lanechange_linear_lateral_velocity.cpp | 4 +- .../lane_change/lanechange_linear_time.cpp | 4 +- .../lanechange_longitudinal_distance.cpp | 4 +- .../src/lane_change/lanechange_right.cpp | 4 +- .../lane_change/lanechange_right_with_id.cpp | 4 +- .../src/lane_change/lanechange_time.cpp | 4 +- ...t_distance_in_lane_coordinate_distance.cpp | 12 ++--- .../get_distance_to_lane_bound.cpp | 4 +- .../src/merge/merge_left.cpp | 8 +-- .../src/metrics/traveled_distance.cpp | 4 +- .../src/move_backward/move_backward.cpp | 4 +- .../src/pedestrian/walk_straight.cpp | 14 ++--- .../src/random_scenario/random001.cpp | 30 +++++------ .../src/respawn_ego/respawn_ego.cpp | 8 ++- .../src/spawn/spawn_in_map_frame.cpp | 6 +-- .../speed_planning/request_speed_change.cpp | 8 +-- .../request_speed_change_continuous_false.cpp | 4 +- .../request_speed_change_relative.cpp | 8 +-- .../request_speed_change_step.cpp | 8 +-- .../request_speed_change_with_limit.cpp | 4 +- ...uest_speed_change_with_time_constraint.cpp | 8 +-- ...eed_change_with_time_constraint_linear.cpp | 4 +- ...d_change_with_time_constraint_relative.cpp | 8 +-- .../synchronized_action.cpp | 22 ++++---- .../synchronized_action_with_speed.cpp | 22 ++++---- .../src/traffic_simulation_demo.cpp | 54 ++++++------------- .../define_traffic_source_delay.cpp | 4 +- .../define_traffic_source_high_rate.cpp | 4 +- .../define_traffic_source_large.cpp | 4 +- .../define_traffic_source_mixed.cpp | 4 +- .../define_traffic_source_multiple.cpp | 4 +- .../define_traffic_source_outside_lane.cpp | 4 +- .../define_traffic_source_pedestrian.cpp | 4 +- .../define_traffic_source_vehicle.cpp | 4 +- 48 files changed, 122 insertions(+), 292 deletions(-) diff --git a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp index 4d1141c8fc7..630c6d91c2f 100644 --- a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp +++ b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp @@ -73,13 +73,9 @@ class CppScenarioNode : public rclcpp::Node const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose) -> traffic_simulator::Configuration { - auto configuration = traffic_simulator::Configuration(map_path); - { - configuration.lanelet2_map_file = lanelet2_map_file; - // configuration.lanelet2_map_file = "lanelet2_map_with_private_road_and_walkway_ele_fix.osm"; - configuration.scenario_path = scenario_filename; - configuration.verbose = verbose; - } + auto configuration = + traffic_simulator::Configuration(map_path, lanelet2_map_file, scenario_filename); + configuration.verbose = verbose; checkConfiguration(configuration); return configuration; } diff --git a/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp index d961cde5e32..80dff74fea3 100644 --- a/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp +++ b/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp @@ -67,27 +67,21 @@ class LoadDoNothingPluginScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters(), traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); api_.spawn( - "pedestrian", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 3.0, 0.0, api_.getHdmapUtils()), + "pedestrian", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 3.0, 0.0), getPedestrianParameters(), traffic_simulator::entity::PedestrianEntity::BuiltinBehavior::doNothing()); api_.spawn( "vehicle_spawn_with_behavior_tree", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 2.0, 0.0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 2.0, 0.0), getVehicleParameters(), traffic_simulator::entity::VehicleEntity::BuiltinBehavior::behaviorTree()); api_.spawn( "pedestrian_spawn_with_behavior_tree", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 3.0, 0.0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 3.0, 0.0), getPedestrianParameters(), traffic_simulator::entity::PedestrianEntity::BuiltinBehavior::behaviorTree()); } diff --git a/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp b/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp index 3ebe51a9925..4596322662f 100644 --- a/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp +++ b/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp @@ -52,9 +52,7 @@ class CrashingNpcScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 15, true); @@ -63,9 +61,7 @@ class CrashingNpcScenario : public cpp_mock_scenarios::CppScenarioNode api_.setBehaviorParameter("ego", behavior_parameter); api_.spawn( - "npc", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc", 0.0); api_.requestSpeedChange("npc", 5, true); diff --git a/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp b/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp index 75f19793f70..522145b3de7 100644 --- a/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp +++ b/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp @@ -50,17 +50,13 @@ class SpawnWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.2, 1.3, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.2, 1.3), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.requestSpeedChange("ego", 0, true); api_.spawn( - "bob", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, -0.874, api_.getHdmapUtils()), + "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, -0.874), getPedestrianParameters()); api_.setLinearVelocity("bob", 0); api_.requestSpeedChange("bob", 0, true); diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index 74dbf25598b..5ac253751f7 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -74,23 +74,17 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 8, true); api_.requestAssignRoute( "ego", std::vector{ - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils()), - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34690, 0.0, 0.0, api_.getHdmapUtils())}); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34675, 0.0, 0.0), + traffic_simulator::helper::constructCanonicalizedLaneletPose(34690, 0.0, 0.0)}); api_.spawn( - "bob", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34378, 0.0, 0.0, api_.getHdmapUtils()), + "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose(34378, 0.0, 0.0), getPedestrianParameters()); api_.setLinearVelocity("bob", 0); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp index 266080f9961..4ee77de7bc8 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp @@ -59,16 +59,12 @@ class AccelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 3); api_.spawn( - "npc", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc", 10); api_.requestSpeedChange("npc", 10, true); diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp index e8bbcda8abe..c6b833663d4 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp @@ -59,16 +59,12 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 15); api_.spawn( - "npc", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 15.0, 0.0, api_.getHdmapUtils()), + "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 15.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc", 10); api_.requestSpeedChange("npc", 10, true); diff --git a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp index 450de807a29..01463611f9e 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp @@ -47,19 +47,14 @@ class AcquirePositionInWorldFrameScenario : public cpp_mock_scenarios::CppScenar void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setEntityStatus( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 0.0), traffic_simulator::helper::constructActionStatus(10)); api_.requestSpeedChange("ego", 10, true); const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34408, 1.0, 0.0, api_.getHdmapUtils())); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34408, 1.0, 0.0)); api_.requestAcquirePosition("ego", goal_pose); } }; diff --git a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp index 9794dc48934..8df5d2906e4 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp @@ -47,19 +47,15 @@ class AcquireRouteInWorldFrameScenario : public cpp_mock_scenarios::CppScenarioN void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); std::vector goal_poses; goal_poses.emplace_back(traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34408, 1.0, 0.0, api_.getHdmapUtils()))); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34408, 1.0, 0.0))); goal_poses.emplace_back(traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34408, 10, 0.0, api_.getHdmapUtils()))); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34408, 10, 0.0))); api_.requestAssignRoute("ego", goal_poses); } }; diff --git a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp index 8db02b87da9..cc09337e277 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp @@ -40,10 +40,7 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { if (api_.reachPosition( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 30, 0, api_.getHdmapUtils()), - 3.0)) { + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 30, 0), 3.0)) { api_.cancelRequest("ego"); canceled = true; } @@ -54,14 +51,12 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 7); api_.requestSpeedChange("ego", 7, true); const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructLaneletPose(34408, 0.0, 0.0), api_.getHdmapUtils()); + traffic_simulator::helper::constructLaneletPose(34408, 0.0, 0.0)); api_.requestAcquirePosition("ego", goal_pose); } }; diff --git a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp index c3f2c16306d..0c165176e9d 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp @@ -55,9 +55,7 @@ class FollowLaneWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, 3.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 3.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp index 9dfe60ac151..0a0a8ee6bd4 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp @@ -51,9 +51,7 @@ class LaneChangeLeftScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp index c04184811e8..5745bd5eb82 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp @@ -51,9 +51,7 @@ class LaneChangeLeftWithIdScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp index 6cabbd683a8..face1ea094a 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp @@ -55,9 +55,7 @@ class LaneChangeLinearScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp index f6bf51b7c80..669f38f0640 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp @@ -59,9 +59,7 @@ class LaneChangeLinearLateralVelocityScenario : public cpp_mock_scenarios::CppSc void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp index bc00c1b7640..91eb620c7db 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp @@ -60,9 +60,7 @@ class LaneChangeLinearTimeScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp index e04070e2380..119c6672db2 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp @@ -47,9 +47,7 @@ class LaneChangeLongitudinalDistanceScenario : public cpp_mock_scenarios::CppSce void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 1); api_.requestSpeedChange("ego", 1, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp index 0210fc3c154..d0d5c98edc4 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp @@ -51,9 +51,7 @@ class LaneChangeRightScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp index eb3e2690d9e..a5566f47f44 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp @@ -51,9 +51,7 @@ class LaneChangeRightWithIdScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp index 88a2c446d61..1f07591bf70 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp @@ -60,9 +60,7 @@ class LaneChangeTimeScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); 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 eed467535ce..49babeb764b 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 @@ -144,25 +144,19 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 5.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 5.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 3, true); api_.spawn( - "front", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 10.0, 1.0, api_.getHdmapUtils()), + "front", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 10.0, 1.0), getVehicleParameters()); api_.setLinearVelocity("front", 10); api_.requestSpeedChange("front", 3, true); api_.spawn( - "behind", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, -1.0, api_.getHdmapUtils()), + "behind", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, -1.0), getVehicleParameters()); api_.setLinearVelocity("behind", 10); api_.requestSpeedChange("behind", 3, true); 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 4003c78f731..b6731a63ae8 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 @@ -58,9 +58,7 @@ class GetDistanceToLaneBoundScenario : public cpp_mock_scenarios::CppScenarioNod void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 5.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 5.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 3, true); diff --git a/mock/cpp_mock_scenarios/src/merge/merge_left.cpp b/mock/cpp_mock_scenarios/src/merge/merge_left.cpp index 44978b272e3..cf20ad99b84 100644 --- a/mock/cpp_mock_scenarios/src/merge/merge_left.cpp +++ b/mock/cpp_mock_scenarios/src/merge/merge_left.cpp @@ -54,18 +54,14 @@ class MergeLeftScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 15.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 15.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 5); api_.requestSpeedChange("ego", 5, true); api_.requestLaneChange("ego", 34513); api_.spawn( - "npc", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, 0.0, api_.getHdmapUtils()), + "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc", 10); } diff --git a/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp b/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp index ee4786cc94d..9e842315754 100644 --- a/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp +++ b/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp @@ -68,9 +68,7 @@ class TraveledDistanceScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 3); api_.requestSpeedChange("ego", 3, true); diff --git a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp index 08cb36d12f6..eee09676089 100644 --- a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp +++ b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp @@ -52,9 +52,7 @@ class MoveBackwardScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", -3); api_.requestSpeedChange("ego", -3, true); diff --git a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp index 6f57fb4e666..82a53ad57ec 100644 --- a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp +++ b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp @@ -70,22 +70,16 @@ class WalkStraightScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 8, true); api_.requestAssignRoute( "ego", std::vector{ - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils()), - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34690, 0.0, 0.0, api_.getHdmapUtils())}); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34675, 0.0, 0.0), + traffic_simulator::helper::constructCanonicalizedLaneletPose(34690, 0.0, 0.0)}); api_.spawn( - "bob", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34378, 0.0, 0.0, api_.getHdmapUtils()), + "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose(34378, 0.0, 0.0), getPedestrianParameters()); api_.setLinearVelocity("bob", 0); api_.requestWalkStraight("bob"); diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 0a6a6e4b11e..9e5019df4f7 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -70,9 +71,9 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( spawn_lanelet_id, static_cast(entity_index) / static_cast(number_of_vehicles) * - traffic_simulator::pose::laneletLength(spawn_lanelet_id, api_.getHdmapUtils()) + + traffic_simulator::lanelet_map::laneletLength(spawn_lanelet_id) + normal_dist(engine_), - offset, api_.getHdmapUtils()), + offset), getVehicleParameters( get_entity_subtype(params_.random_parameters.road_parking_vehicle.entity_type))); api_.requestSpeedChange(entity_name, 0, true); @@ -122,8 +123,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode if (!api_.entityExists(entity_name)) { api_.spawn( entity_name, - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, spawn_s_value, 0.0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, spawn_s_value, 0.0), getVehicleParameters()); std::uniform_real_distribution<> speed_distribution( params_.random_parameters.lane_following_vehicle.min_speed, @@ -132,7 +132,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode api_.requestSpeedChange(entity_name, speed, true); api_.setLinearVelocity(entity_name, speed); std::uniform_real_distribution<> lane_change_position_distribution( - 0.0, traffic_simulator::pose::laneletLength(34684, api_.getHdmapUtils())); + 0.0, traffic_simulator::lanelet_map::laneletLength(34684)); lane_change_position = lane_change_position_distribution(engine_); lane_change_requested = false; } @@ -157,9 +157,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode if ( !api_.entityExists(entity_name) && !api_.reachPosition( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34576, 25.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34576, 25.0, 0.0), 5.0)) { std::normal_distribution<> offset_distribution( 0.0, params_.random_parameters.crossing_pedestrian.offset_variance); @@ -169,7 +167,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode api_.spawn( entity_name, traffic_simulator::helper::constructCanonicalizedLaneletPose( - lanelet_id, 0.0, offset_distribution(engine_), api_.getHdmapUtils()), + lanelet_id, 0.0, offset_distribution(engine_)), getPedestrianParameters()); const auto speed = speed_distribution(engine_); api_.requestSpeedChange(entity_name, speed, true); @@ -184,10 +182,10 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode } { - const auto trigger_position = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34621, 10, 0.0, api_.getHdmapUtils()); - const auto ego_goal_position = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34606, 0.0, 0.0, api_.getHdmapUtils()); + const auto trigger_position = + traffic_simulator::helper::constructCanonicalizedLaneletPose(34621, 10, 0.0); + const auto ego_goal_position = + traffic_simulator::helper::constructCanonicalizedLaneletPose(34606, 0.0, 0.0); const auto entity_name = "spawn_nearby_ego"; if (const auto ego = api_.getEntity("ego")) { @@ -223,10 +221,8 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode spawnRoadParkingVehicles(); spawnEgoEntity( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34621, 10.0, 0.0, api_.getHdmapUtils()), - {traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34606, 0.0, 0.0, api_.getHdmapUtils())}, + traffic_simulator::helper::constructCanonicalizedLaneletPose(34621, 10.0, 0.0), + {traffic_simulator::helper::constructCanonicalizedLaneletPose(34606, 0.0, 0.0)}, getVehicleParameters()); if (const auto ego = api_.getEntity("ego")) { api_.spawn( diff --git a/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp index f1db7fd4e06..7e309765df4 100644 --- a/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp +++ b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp @@ -30,8 +30,7 @@ class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode : cpp_mock_scenarios::CppScenarioNode( "respawn_ego", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", "lanelet2_map.osm", __FILE__, false, option), - goal_pose{traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34606, 0, 0, api_.getHdmapUtils())}, + goal_pose{traffic_simulator::helper::constructCanonicalizedLaneletPose(34606, 0, 0)}, new_position_subscriber{create_subscription( "/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).reliable(), [this](const geometry_msgs::msg::PoseWithCovarianceStamped & message) { @@ -55,9 +54,8 @@ class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { spawnEgoEntity( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34621, 10.0, 0.0, api_.getHdmapUtils()), - {goal_pose}, getVehicleParameters()); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34621, 10.0, 0.0), {goal_pose}, + getVehicleParameters()); } private: diff --git a/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp b/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp index 56d48813a89..e31df21eef0 100644 --- a/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp +++ b/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp @@ -39,8 +39,7 @@ class SpawnInMapFrameScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { const auto map_pose = traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils())); + traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0.0, 0.0)); if (api_.reachPosition("ego", map_pose, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { @@ -53,8 +52,7 @@ class SpawnInMapFrameScenario : public cpp_mock_scenarios::CppScenarioNode api_.spawn( "ego", traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils())), + traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0.0, 0.0)), getVehicleParameters()); } }; diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp index 548794166b8..9f00065436a 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp @@ -55,9 +55,7 @@ class RequestSpeedChangeScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.requestSpeedChange( @@ -67,9 +65,7 @@ class RequestSpeedChangeScenario : public cpp_mock_scenarios::CppScenarioNode true); api_.spawn( - "front", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "front", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("front", 0); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp index 372ba2c0658..a83a987e2f8 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp @@ -62,9 +62,7 @@ class RequestSpeedChangeContinuousFalseScenario : public cpp_mock_scenarios::Cpp { speed_reached = false; api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp index 7d277e44b5c..f65f1f28e83 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp @@ -76,17 +76,13 @@ class RequestSpeedChangeRelativeScenario : public cpp_mock_scenarios::CppScenari void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 3); api_.requestSpeedChange("ego", 3.0, true); api_.spawn( - "front", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "front", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("front", 3); } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp index b3c672a6627..a9d6755a600 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp @@ -52,9 +52,7 @@ class RequestSpeedChangeStepScenario : public cpp_mock_scenarios::CppScenarioNod void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.requestSpeedChange( @@ -64,9 +62,7 @@ class RequestSpeedChangeStepScenario : public cpp_mock_scenarios::CppScenarioNod true); api_.spawn( - "front", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "front", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("front", 0); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp index 25afc545654..d63a10f746f 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp @@ -52,9 +52,7 @@ class RequestSpeedChangeWithLimitScenario : public cpp_mock_scenarios::CppScenar void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.setVelocityLimit("ego", 5.0); diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp index 7c0b4d11b48..c3b47049b38 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp @@ -56,9 +56,7 @@ class RequestSpeedChangeWithTimeConstraintScenario : public cpp_mock_scenarios:: void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.requestSpeedChange( @@ -68,9 +66,7 @@ class RequestSpeedChangeWithTimeConstraintScenario : public cpp_mock_scenarios:: false); api_.spawn( - "front", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "front", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("front", 10); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp index 735ab50a3bc..a077508ba0f 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp @@ -52,9 +52,7 @@ class RequestSpeedChangeWithTimeConstraintLinearScenario void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp index 17992f4d687..ba8e3eceee9 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp @@ -57,9 +57,7 @@ class RequestSpeedChangeWithTimeConstraintRelativeScenario void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 1.0); api_.requestSpeedChange( @@ -72,9 +70,7 @@ class RequestSpeedChangeWithTimeConstraintRelativeScenario false); api_.spawn( - "front", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "front", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("front", 10); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp index 42140bbee4c..f55fb1a6ae5 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp @@ -38,9 +38,9 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode private: const traffic_simulator::CanonicalizedLaneletPose ego_target = - traffic_simulator::helper::constructCanonicalizedLaneletPose(34585, 0, 0, api_.getHdmapUtils()); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34585, 0, 0); const traffic_simulator::CanonicalizedLaneletPose npc_target = - traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0, 0, api_.getHdmapUtils()); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0, 0); void onUpdate() override { @@ -63,26 +63,22 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34976, 20, 0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34976, 20, 0), getVehicleParameters()); api_.setLinearVelocity("ego", 3); api_.requestSpeedChange("ego", 3, true); std::vector goal_poses; - goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34579, 20, 0, api_.getHdmapUtils())); + goal_poses.emplace_back( + traffic_simulator::helper::constructCanonicalizedLaneletPose(34579, 20, 0)); api_.requestAssignRoute("ego", goal_poses); api_.spawn( - "npc", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34576, 0, 0, api_.getHdmapUtils()), + "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34576, 0, 0), getVehicleParameters()); std::vector npc_goal_poses; - npc_goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34564, 20, 0, api_.getHdmapUtils())); + npc_goal_poses.emplace_back( + traffic_simulator::helper::constructCanonicalizedLaneletPose(34564, 20, 0)); api_.requestAssignRoute("npc", npc_goal_poses); api_.setLinearVelocity("npc", 6); } @@ -90,7 +86,7 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) -> std::optional { - return traffic_simulator::pose::canonicalize(lanelet_pose, api_.getHdmapUtils()); + return traffic_simulator::pose::toCanonicalizedLaneletPose(lanelet_pose); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp index 2aa571173fa..30c62fadda5 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp @@ -38,9 +38,9 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode private: const traffic_simulator::CanonicalizedLaneletPose ego_target = - traffic_simulator::helper::constructCanonicalizedLaneletPose(34585, 0, 0, api_.getHdmapUtils()); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34585, 0, 0); const traffic_simulator::CanonicalizedLaneletPose npc_target = - traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0, 0, api_.getHdmapUtils()); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0, 0); void onUpdate() override { @@ -63,26 +63,22 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34976, 20, 0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34976, 20, 0), getVehicleParameters()); api_.setLinearVelocity("ego", 3); api_.requestSpeedChange("ego", 3, true); std::vector goal_poses; - goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34579, 20, 0, api_.getHdmapUtils())); + goal_poses.emplace_back( + traffic_simulator::helper::constructCanonicalizedLaneletPose(34579, 20, 0)); api_.requestAssignRoute("ego", goal_poses); api_.spawn( - "npc", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34576, 0, 0, api_.getHdmapUtils()), + "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34576, 0, 0), getVehicleParameters()); std::vector npc_goal_poses; - npc_goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34564, 20, 0, api_.getHdmapUtils())); + npc_goal_poses.emplace_back( + traffic_simulator::helper::constructCanonicalizedLaneletPose(34564, 20, 0)); api_.requestAssignRoute("npc", npc_goal_poses); api_.setLinearVelocity("npc", 6); } @@ -90,7 +86,7 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) -> std::optional { - return traffic_simulator::pose::canonicalize(lanelet_pose, api_.getHdmapUtils()); + return traffic_simulator::pose::toCanonicalizedLaneletPose(lanelet_pose); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index 6845404af94..ce6ed50fe33 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -47,46 +47,35 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode } if (api_.getCurrentTime() >= 4 && api_.entityExists("obstacle")) { api_.setEntityStatus( - "obstacle", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils()), + "obstacle", traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0.0, 0.0), traffic_simulator::helper::constructActionStatus(10)); } if (api_.getCurrentTime() >= 6 && api_.entityExists("obstacle")) { api_.despawn("obstacle"); } if (api_.reachPosition( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34615, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34615, 10.0, 0.0), 5)) { api_.requestAcquirePosition( - "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose( - 35026, 0.0, 0.0, api_.getHdmapUtils())); + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(35026, 0.0, 0.0)); if (api_.entityExists("npc2")) { api_.requestSpeedChange("npc2", 13, true); } } if (api_.reachPosition( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34579, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34579, 0.0, 0.0), 5)) { api_.requestAcquirePosition( - "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils())); + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34675, 0.0, 0.0)); if (api_.entityExists("npc2")) { api_.requestSpeedChange("npc2", 3, true); } } if (api_.reachPosition( - "npc2", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, 0.0, api_.getHdmapUtils()), + "npc2", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 0.0), 5)) { api_.requestAcquirePosition( - "npc2", traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34630, 0.0, 0.0, api_.getHdmapUtils())); + "npc2", traffic_simulator::helper::constructCanonicalizedLaneletPose(34630, 0.0, 0.0)); api_.requestSpeedChange("npc2", 13, true); } if (api_.getCurrentTime() > 10.0 && api_.entityExists("bob")) { @@ -99,18 +88,14 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode lanechange_executed_ = false; api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 8, true); api_.requestAssignRoute( "ego", std::vector{ - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils()), - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34690, 0.0, 0.0, api_.getHdmapUtils())}); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34675, 0.0, 0.0), + traffic_simulator::helper::constructCanonicalizedLaneletPose(34690, 0.0, 0.0)}); api_.spawn( "tom", traffic_simulator::helper::constructPose(10, 3, 0, 0, 0, -1.57), @@ -122,36 +107,27 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode api_.requestSpeedChange("tom", 3, true); api_.spawn( - "bob", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34378, 0.0, 0.0, api_.getHdmapUtils()), + "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose(34378, 0.0, 0.0), getPedestrianParameters()); api_.setLinearVelocity("bob", 1.0); api_.requestSpeedChange("bob", 1, true); api_.spawn( - "npc1", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34579, 20.0, 0.0, api_.getHdmapUtils()), + "npc1", traffic_simulator::helper::constructCanonicalizedLaneletPose(34579, 20.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc1", 5.0); api_.requestSpeedChange("npc1", 5, true); api_.requestAcquirePosition( - "npc1", traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils())); + "npc1", traffic_simulator::helper::constructCanonicalizedLaneletPose(34675, 0.0, 0.0)); api_.spawn( - "npc2", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34606, 20.0, 0.0, api_.getHdmapUtils()), + "npc2", traffic_simulator::helper::constructCanonicalizedLaneletPose(34606, 20.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc2", 5); api_.requestSpeedChange("npc2", 0, true); api_.spawn( - "npc3", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34468, 0.0, 0.0, api_.getHdmapUtils()), + "npc3", traffic_simulator::helper::constructCanonicalizedLaneletPose(34468, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc3", 10); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp index 46532152560..739e39b2c45 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp @@ -92,9 +92,7 @@ class DefineTrafficSourceDelay : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp index 7a5c8d97a83..7b5e818196c 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp @@ -82,9 +82,7 @@ class DefineTrafficSourceHighRate : public cpp_mock_scenarios::CppScenarioNode false, true, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp index 6d7976cfa05..31b1b884f3d 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp @@ -78,9 +78,7 @@ class DefineTrafficSourceLarge : public cpp_mock_scenarios::CppScenarioNode false, true, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp index 4f9141d9d92..8eb2927d3bd 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp @@ -93,9 +93,7 @@ class DefineTrafficSourceMixed : public cpp_mock_scenarios::CppScenarioNode false, true, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp index f4ba78518ff..685637fbac0 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp @@ -115,9 +115,7 @@ class DefineTrafficSourceMultiple : public cpp_mock_scenarios::CppScenarioNode false, true, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp index 02db2b0bd26..d7d924387a1 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp @@ -79,9 +79,7 @@ class DefineTrafficSourceOutsideLane : public cpp_mock_scenarios::CppScenarioNod true, false, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp index 02c46bbd755..7f05f3d80a9 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp @@ -82,9 +82,7 @@ class DefineTrafficSourcePedestrian : public cpp_mock_scenarios::CppScenarioNode false, true, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp index d38b251004f..72c2e146c37 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp @@ -82,9 +82,7 @@ class DefineTrafficSourceVehicle : public cpp_mock_scenarios::CppScenarioNode false, true, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); From cfd6d6afb4ea736ff1d45a3521a81fec7e223cac Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 3 Dec 2024 15:57:32 +0100 Subject: [PATCH 04/62] feat(traffic_simulator, random_test_runner): adapt tests for using pose:: from lanelet_wrapper --- .../test/src/behavior/test_route_planner.cpp | 29 ++- .../test/src/data_type/test_lanelet_pose.cpp | 84 +++++---- .../src/entity/test_misc_object_entity.cpp | 78 ++++---- .../test/src/helper_functions.hpp | 34 ++-- .../traffic_lights/common_test_fixtures.hpp | 5 + .../src/traffic_lights/test_traffic_light.cpp | 4 + .../traffic_lights/test_traffic_lights.cpp | 8 +- .../test/src/utils/test_distance.cpp | 171 +++++++++--------- .../test/src/utils/test_pose.cpp | 129 ++++++------- .../random_test_runner/test_executor.hpp | 9 +- .../random_test_runner/src/lanelet_utils.cpp | 2 + .../test/test_test_executor.cpp | 8 + 12 files changed, 275 insertions(+), 286 deletions(-) diff --git a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp index f4f949deac1..3158d8124e8 100644 --- a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp +++ b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp @@ -30,12 +30,12 @@ class RoutePlannerTest : public testing::Test { protected: RoutePlannerTest() - : hdmap_utils_ptr(makeHdMapUtilsSharedPointer()), - planner(traffic_simulator::RoutingConfiguration().routing_graph_type, hdmap_utils_ptr) + : planner( + traffic_simulator::RoutingConfiguration().routing_graph_type, makeHdMapUtilsSharedPointer()) { + activateLaneletWrapper("standard_map"); } - std::shared_ptr hdmap_utils_ptr; traffic_simulator::RoutePlanner planner; }; @@ -46,9 +46,8 @@ class RoutePlannerTest : public testing::Test TEST_F(RoutePlannerTest, getGoalPoses) { const auto in_poses = std::vector{ - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660), - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 34468)}; + makeCanonicalizedLaneletPose(120659), makeCanonicalizedLaneletPose(120660), + makeCanonicalizedLaneletPose(34468)}; planner.setWaypoints(in_poses); @@ -70,9 +69,8 @@ TEST_F(RoutePlannerTest, getGoalPoses) TEST_F(RoutePlannerTest, getGoalPosesInWorldFrame) { const auto in_poses = std::vector{ - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660), - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 34468)}; + makeCanonicalizedLaneletPose(120659), makeCanonicalizedLaneletPose(120660), + makeCanonicalizedLaneletPose(34468)}; planner.setWaypoints(in_poses); @@ -92,9 +90,8 @@ TEST_F(RoutePlannerTest, getRouteLanelets_horizon) { const lanelet::Id id_target = 34579; - planner.setWaypoints({makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target)}); - auto route = - planner.getRouteLanelets(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), 1000.0); + planner.setWaypoints({makeCanonicalizedLaneletPose(id_target)}); + auto route = planner.getRouteLanelets(makeCanonicalizedLaneletPose(120659), 1000.0); EXPECT_TRUE(std::find(route.begin(), route.end(), id_target) != route.end()); } @@ -107,9 +104,8 @@ TEST_F(RoutePlannerTest, getRouteLanelets_noHorizon) { lanelet::Id id_target = 34579; - planner.setWaypoints({makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target)}); - const auto route = - planner.getRouteLanelets(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), 100.0); + planner.setWaypoints({makeCanonicalizedLaneletPose(id_target)}); + const auto route = planner.getRouteLanelets(makeCanonicalizedLaneletPose(120659), 100.0); EXPECT_FALSE(std::find(route.begin(), route.end(), id_target) != route.end()); } @@ -124,8 +120,7 @@ TEST_F(RoutePlannerTest, getRouteLanelets_empty) const lanelet::Ids following_ids({120659, 120660, 34468, 34465, 34462}); planner.setWaypoints({}); - const auto route = - planner.getRouteLanelets(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), 100.0); + const auto route = planner.getRouteLanelets(makeCanonicalizedLaneletPose(120659), 100.0); EXPECT_EQ(route.size(), following_ids.size()); for (size_t i = 0; i < route.size(); i++) { 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 24d74f063ce..eb03e9970d9 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 @@ -28,8 +28,10 @@ int main(int argc, char ** argv) class CanonicalizedLaneletPoseTest : public testing::Test { protected: - CanonicalizedLaneletPoseTest() : hdmap_utils(makeHdMapUtilsSharedPointer()) {} - + CanonicalizedLaneletPoseTest() : hdmap_utils(makeHdMapUtilsSharedPointer()) + { + activateLaneletWrapper("standard_map"); + } std::shared_ptr hdmap_utils; }; @@ -40,8 +42,7 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withRoute_invalid) { EXPECT_THROW( CanonicalizedLaneletPose( - traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0), lanelet::Ids{}, - hdmap_utils), + traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0), lanelet::Ids{}), std::runtime_error); } @@ -53,8 +54,7 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withRoute) std::shared_ptr pose; EXPECT_NO_THROW( pose = std::make_shared( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), lanelet::Ids{120659}, - hdmap_utils)); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), lanelet::Ids{120659})); EXPECT_LANELET_POSE_EQ( static_cast(*pose), traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); @@ -67,7 +67,7 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withoutRoute_inval { EXPECT_THROW( CanonicalizedLaneletPose( - traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0), hdmap_utils), + traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0)), std::runtime_error); } @@ -79,7 +79,7 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withoutRoute) std::shared_ptr pose; EXPECT_NO_THROW( pose = std::make_shared( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils)); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0))); EXPECT_LANELET_POSE_EQ( static_cast(*pose), traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); @@ -91,7 +91,7 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withoutRoute) TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_copyConstructor) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose_copy(pose); EXPECT_LANELET_POSE_EQ( static_cast(pose), @@ -104,7 +104,7 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_copyConstructor) TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_moveConstructor) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose2(pose); const CanonicalizedLaneletPose pose_move = std::move(pose2); EXPECT_LANELET_POSE_EQ( @@ -118,9 +118,9 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_moveConstructor) TEST_F(CanonicalizedLaneletPoseTest, copyAssignment) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); CanonicalizedLaneletPose pose_assign( - traffic_simulator::helper::constructLaneletPose(34468, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(34468, 0.0, 0.0)); pose_assign = pose; @@ -135,7 +135,7 @@ TEST_F(CanonicalizedLaneletPoseTest, copyAssignment) TEST_F(CanonicalizedLaneletPoseTest, conversionLaneletPose) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const traffic_simulator::LaneletPose pose_casted = static_cast(pose); @@ -151,7 +151,7 @@ TEST_F(CanonicalizedLaneletPoseTest, conversionLaneletPose) TEST_F(CanonicalizedLaneletPoseTest, conversionPose) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const geometry_msgs::msg::Pose pose1 = makePose(makePoint(3822.3815, 73784.9618, -1.761), makeQuaternionFromYaw(2.060578777273)); @@ -165,7 +165,7 @@ TEST_F(CanonicalizedLaneletPoseTest, conversionPose) TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_true) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, -10.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, -10.0, 0.0)); EXPECT_TRUE(pose.hasAlternativeLaneletPose()); } @@ -176,7 +176,7 @@ TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_true) TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_false) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 10.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 10.0, 0.0)); EXPECT_FALSE(pose.hasAlternativeLaneletPose()); } @@ -187,7 +187,7 @@ TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_false) TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_empty) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 20.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 20.0, 0.0)); const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); @@ -206,8 +206,7 @@ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRout */ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_single) { - CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(34666, -20.0, 0.0), hdmap_utils); + CanonicalizedLaneletPose pose(traffic_simulator::helper::constructLaneletPose(34666, -20.0, 0.0)); const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); @@ -227,7 +226,7 @@ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRout TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_multiple) { CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, -20.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, -20.0, 0.0)); const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); @@ -265,13 +264,13 @@ TEST(CanonicalizedLaneletPose, setConsiderPoseByRoadSlope) TEST_F(CanonicalizedLaneletPoseTest, operatorLessEqual) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_equal( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_less( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose_greater( - traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0)); EXPECT_TRUE(pose_less <= pose); EXPECT_TRUE(pose_equal <= pose); @@ -284,13 +283,13 @@ TEST_F(CanonicalizedLaneletPoseTest, operatorLessEqual) TEST_F(CanonicalizedLaneletPoseTest, operatorLess) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_equal( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_less( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose_greater( - traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0)); EXPECT_TRUE(pose_less < pose); EXPECT_FALSE(pose_equal < pose); @@ -303,13 +302,13 @@ TEST_F(CanonicalizedLaneletPoseTest, operatorLess) TEST_F(CanonicalizedLaneletPoseTest, operatorGreaterEqual) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_equal( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_less( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose_greater( - traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0)); EXPECT_FALSE(pose_less >= pose); EXPECT_TRUE(pose_equal >= pose); @@ -322,13 +321,13 @@ TEST_F(CanonicalizedLaneletPoseTest, operatorGreaterEqual) TEST_F(CanonicalizedLaneletPoseTest, operatorGreater) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_equal( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_less( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose_greater( - traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0)); EXPECT_FALSE(pose_less > pose); EXPECT_FALSE(pose_equal > pose); @@ -341,9 +340,9 @@ TEST_F(CanonicalizedLaneletPoseTest, operatorGreater) TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_same) { const CanonicalizedLaneletPose pose1( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose2( - traffic_simulator::helper::constructLaneletPose(120659, 1.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 1.0, 0.0)); EXPECT_TRUE(traffic_simulator::isSameLaneletId(pose1, pose2)); } @@ -354,9 +353,8 @@ TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_same) TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_different) { const CanonicalizedLaneletPose pose1( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); - CanonicalizedLaneletPose pose2( - traffic_simulator::helper::constructLaneletPose(34606, 1.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); + CanonicalizedLaneletPose pose2(traffic_simulator::helper::constructLaneletPose(34606, 1.0, 0.0)); EXPECT_FALSE(traffic_simulator::isSameLaneletId(pose1, pose2)); } @@ -367,7 +365,7 @@ TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_different) TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withLanelet_same) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); EXPECT_TRUE(traffic_simulator::isSameLaneletId(pose, 120659)); } @@ -378,7 +376,7 @@ TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withLanelet_same) TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withLanelet_different) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); EXPECT_FALSE(traffic_simulator::isSameLaneletId(pose, 34606)); } diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp index 987b8220f3e..8ba79375d15 100644 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -36,8 +36,8 @@ class MiscObjectEntityTest_HdMapUtils : public testing::Test MiscObjectEntityTest_HdMapUtils() : hdmap_utils_ptr(makeHdMapUtilsSharedPointer()), entity_name("misc_object_entity") { + activateLaneletWrapper("standard_map"); } - std::shared_ptr hdmap_utils_ptr; const std::string entity_name; }; @@ -47,9 +47,9 @@ class MiscObjectEntityTest_FullObject : public MiscObjectEntityTest_HdMapUtils protected: MiscObjectEntityTest_FullObject() : id(120659), - pose(makeCanonicalizedLaneletPose(hdmap_utils_ptr, id)), + pose(makeCanonicalizedLaneletPose(id)), bbox(makeBoundingBox()), - status(makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, 0.0, entity_name)), + status(makeCanonicalizedEntityStatus(pose, bbox, 0.0, entity_name)), misc_object( entity_name, status, hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}), entity_base(&misc_object) @@ -70,21 +70,22 @@ class MiscObjectEntityTest_FullObject : public MiscObjectEntityTest_HdMapUtils TEST_F(MiscObjectEntityTest_HdMapUtils, getCurrentAction_npcNotStarted) { auto non_canonicalized_status = makeEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), 0.0, - entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT); + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT); non_canonicalized_status.action_status.current_action = "current_action_name"; const auto blob = traffic_simulator::entity::MiscObjectEntity( entity_name, traffic_simulator::entity_status::CanonicalizedEntityStatus( - non_canonicalized_status, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659)), + non_canonicalized_status, makeCanonicalizedLaneletPose(120659)), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); EXPECT_EQ(blob.getCurrentAction(), "current_action_name"); } /** - * @note Test function behavior when absolute speed change is requested - the goal is to test throwing error. + * @note Test function behavior when absolute speed change is requested - the goal is to test + * throwing error. */ TEST_F(MiscObjectEntityTest_HdMapUtils, requestSpeedChange_absolute) { @@ -92,34 +93,34 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, requestSpeedChange_absolute) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestSpeedChange(10.0, false), common::SemanticError); } /** - * @note Test function behavior when relative speed change is requested - the goal is to test throwing error. + * @note Test function behavior when relative speed change is requested - the goal is to test + * throwing error. */ TEST_F(MiscObjectEntityTest_HdMapUtils, requestSpeedChange_relative) { - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659); + auto pose = makeCanonicalizedLaneletPose(120659); auto bbox = makeBoundingBox(); auto blob = traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, pose, bbox, 0.0, entity_name, - traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + pose, bbox, 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); std::unordered_map others; others.emplace( - "other_entity", makeCanonicalizedEntityStatus( - hdmap_utils_ptr, pose, bbox, 17.0, "other_entity_name", - traffic_simulator_msgs::msg::EntityType::MISC_OBJECT)); + "other_entity", + makeCanonicalizedEntityStatus( + pose, bbox, 17.0, "other_entity_name", traffic_simulator_msgs::msg::EntityType::MISC_OBJECT)); blob.setOtherStatus(others); EXPECT_THROW( @@ -140,8 +141,8 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, requestSpeedChange_absoluteTransition) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestSpeedChange( 10.0, traffic_simulator::speed_change::Transition::AUTO, @@ -161,11 +162,11 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, requestAssignRoute_laneletPose) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestAssignRoute(std::vector{ - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660)}), + makeCanonicalizedLaneletPose(120660)}), common::SemanticError); } @@ -179,8 +180,8 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, requestAssignRoute_pose) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestAssignRoute( std::vector{makePose(makePoint(3759.34, 73791.38))}), @@ -197,10 +198,10 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, requestAcquirePosition_laneletPose) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) - .requestAcquirePosition(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660)), + .requestAcquirePosition(makeCanonicalizedLaneletPose(120660)), common::SemanticError); } @@ -214,8 +215,8 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, requestAcquirePosition_pose) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestAcquirePosition(makePose(makePoint(3759.34, 73791.38))), common::SemanticError); @@ -230,8 +231,8 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, getRouteLanelets) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getRouteLanelets(100.0), common::SemanticError); @@ -350,8 +351,7 @@ TEST_F(MiscObjectEntityTest_FullObject, requestLaneChange_relativeTargetLaneletP auto other_status = std::unordered_map{}; other_status.emplace( - target_name, - makeCanonicalizedEntityStatus(hdmap_utils_ptr, makePose(makePoint(3810.0, 73745.0)), bbox)); + target_name, makeCanonicalizedEntityStatus(makePose(makePoint(3810.0, 73745.0)), bbox)); entity_base->setOtherStatus(other_status); @@ -375,9 +375,7 @@ TEST_F(MiscObjectEntityTest_FullObject, requestLaneChange_relativeTargetName) auto other_status = std::unordered_map{}; other_status.emplace( - target_name, - makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 34468, 5.0), bbox)); + target_name, makeCanonicalizedEntityStatus(makeCanonicalizedLaneletPose(34468, 5.0), bbox)); entity_base->setOtherStatus(other_status); EXPECT_THROW( @@ -402,9 +400,7 @@ TEST_F(MiscObjectEntityTest_FullObject, requestLaneChange_relativeTargetInvalid) auto other_status = std::unordered_map{}; other_status.emplace( - target_name, - makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 34468, 5.0), bbox)); + target_name, makeCanonicalizedEntityStatus(makeCanonicalizedLaneletPose(34468, 5.0), bbox)); entity_base->setOtherStatus(other_status); EXPECT_THROW( @@ -463,8 +459,8 @@ TEST_F( EXPECT_FALSE(traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 1.0, - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 1.0, 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getCanonicalizedLaneletPose(5.0) .has_value()); @@ -481,7 +477,6 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, getCanonicalizedLaneletPose_onRoadAndCro traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makePose(makePoint(3766.1, 73738.2), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), makeBoundingBox(), 1.0, 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), @@ -502,7 +497,6 @@ TEST_F( traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makePose(makePoint(3764.5, 73737.5), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), makeBoundingBox(), 1.0, 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index a12b5e33328..840b566c101 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -95,17 +96,22 @@ auto makeHdMapUtilsSharedPointer() -> std::shared_ptr .altitude(0.0)); } +auto activateLaneletWrapper(const std::string map_name) -> void +{ + const auto lanelet_path = ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/" + map_name + "/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(lanelet_path); +} + auto makeCanonicalizedLaneletPose( - std::shared_ptr hdmap_utils, const lanelet::Id id = 120659, - const double s = 0.0, const double offset = 0.0) + const lanelet::Id id = 120659, const double s = 0.0, const double offset = 0.0) -> traffic_simulator::lanelet_pose::CanonicalizedLaneletPose { return traffic_simulator::lanelet_pose::CanonicalizedLaneletPose( - traffic_simulator::helper::constructLaneletPose(id, s, offset), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(id, s, offset)); } auto makeEntityStatus( - std::shared_ptr hdmap_utils, traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, const std::string name = "default_entity_name", @@ -120,15 +126,14 @@ auto makeEntityStatus( .name(name) .bounding_box(bbox) .action_status(traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0)) - .pose(hdmap_utils->toMapPose(static_cast(pose)).pose) + .pose(traffic_simulator::pose::toMapPose(pose)) .lanelet_pose(static_cast(pose)) .lanelet_pose_valid(true); } auto makeEntityStatus( - std::shared_ptr /* hdmap_utils */, geometry_msgs::msg::Pose pose, - traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "default_entity_name", + geometry_msgs::msg::Pose pose, traffic_simulator_msgs::msg::BoundingBox bbox, + const double speed = 0.0, const std::string name = "default_entity_name", const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::EntityStatus { @@ -146,7 +151,6 @@ auto makeEntityStatus( } auto makeCanonicalizedEntityStatus( - std::shared_ptr hdmap_utils, traffic_simulator::lanelet_pose::CanonicalizedLaneletPose canonicalized_lanelet_pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, const std::string name = "default_entity_name", @@ -154,14 +158,14 @@ auto makeCanonicalizedEntityStatus( -> traffic_simulator::entity_status::CanonicalizedEntityStatus { return traffic_simulator::entity_status::CanonicalizedEntityStatus( - makeEntityStatus(hdmap_utils, canonicalized_lanelet_pose, bbox, speed, name, type), + makeEntityStatus(canonicalized_lanelet_pose, bbox, speed, name, type), canonicalized_lanelet_pose); } auto makeCanonicalizedEntityStatus( - std::shared_ptr hdmap_utils, geometry_msgs::msg::Pose pose, - traffic_simulator_msgs::msg::BoundingBox bbox, const double matching_distance = 1.0, - const double speed = 0.0, const std::string name = "default_entity_name", + geometry_msgs::msg::Pose pose, traffic_simulator_msgs::msg::BoundingBox bbox, + const double matching_distance = 1.0, const double speed = 0.0, + const std::string name = "default_entity_name", const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::entity_status::CanonicalizedEntityStatus { @@ -169,9 +173,9 @@ auto makeCanonicalizedEntityStatus( (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == type || traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == type); const auto canonicalized_lanelet_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, bbox, include_crosswalk, matching_distance, hdmap_utils); + pose, bbox, include_crosswalk, matching_distance); return traffic_simulator::entity_status::CanonicalizedEntityStatus( - makeEntityStatus(hdmap_utils, pose, bbox, speed, name, type), canonicalized_lanelet_pose); + makeEntityStatus(pose, bbox, speed, name, type), canonicalized_lanelet_pose); } #endif // TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ diff --git a/simulation/traffic_simulator/test/src/traffic_lights/common_test_fixtures.hpp b/simulation/traffic_simulator/test/src/traffic_lights/common_test_fixtures.hpp index 0f95927fc1e..6d39e8f1e90 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/common_test_fixtures.hpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/common_test_fixtures.hpp @@ -19,6 +19,7 @@ #include #include +#include constexpr char architecture_old[] = "awf/universe/20230906"; constexpr char architecture_new[] = "awf/universe/20240605"; @@ -40,6 +41,10 @@ class TrafficLightsInternalTestArchitectureDependent : public testing::Test std::is_same_v or std::is_same_v, "Given TrafficLights type is not supported"); + + const auto lanelet_path = ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(lanelet_path); } const lanelet::Id id{34836}; diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp index 36ff6baf51a..e2522ac5428 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp @@ -19,6 +19,7 @@ #include #include #include +#include using TrafficLight = traffic_simulator::TrafficLight; using Color = TrafficLight::Color; @@ -724,6 +725,9 @@ class TrafficLightTest : public testing::Test .longitude(139.78066608243) .altitude(0.0)) { + const auto lanelet_path = ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(lanelet_path); } hdmap_utils::HdMapUtils map_manager; }; diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp index edc0c7283ef..ea549b6d785 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "../expect_eq_macros.hpp" #include "helper.hpp" @@ -31,7 +32,12 @@ using namespace std::chrono_literals; class TrafficLightsTest : public testing::Test { public: - TrafficLightsTest() = default; + TrafficLightsTest() + { + const auto lanelet_path = ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(lanelet_path); + } const lanelet::Id id{34836}; diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index ebe1558fb98..7b97ce02969 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -43,6 +43,7 @@ class distanceTest_FourTrackHighwayMap : public testing::Test .longitude(138.8024583466017) .altitude(0.0))) { + activateLaneletWrapper("four_track_highway"); } std::shared_ptr hdmap_utils_ptr; }; @@ -59,6 +60,7 @@ class distanceTest_StandardMap : public testing::Test .longitude(139.78066608243) .altitude(0.0))) { + activateLaneletWrapper("standard_map"); } std::shared_ptr hdmap_utils_ptr; }; @@ -75,6 +77,7 @@ class distanceTest_IntersectionMap : public testing::Test .longitude(139.74821144562) .altitude(0.0))) { + activateLaneletWrapper("intersection"); } std::shared_ptr hdmap_utils_ptr; }; @@ -86,10 +89,9 @@ class distanceTest_IntersectionMap : public testing::Test */ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_noChange) { - const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 3002184L, 0.0, 0.0, hdmap_utils_ptr); - const auto pose_to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002184L, 0.0, 0.0); + const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0); { const auto result = traffic_simulator::distance::lateralDistance( pose_from, pose_to, std::numeric_limits::infinity(), @@ -110,10 +112,9 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_noChange) */ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_noChange) { - const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 3002184L, 0.0, 0.0, hdmap_utils_ptr); - const auto pose_to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(201L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002184L, 0.0, 0.0); + const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose(201L, 0.0, 0.0); { const auto result = traffic_simulator::distance::lateralDistance( pose_from, pose_to, std::numeric_limits::infinity(), @@ -137,9 +138,9 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_noChange) TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_change) { const auto pose_from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); - const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 3002184L, 0.0, 0.0, hdmap_utils_ptr); + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0); + const auto pose_to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002184L, 0.0, 0.0); { traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; @@ -164,10 +165,9 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_change) */ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_change) { - const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 3002184L, 0.0, 0.0, hdmap_utils_ptr); - const auto pose_to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002184L, 0.0, 0.0); + const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0); constexpr double approx_distance = -3.0; constexpr double tolerance = 0.5; { @@ -196,9 +196,9 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_change) TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_matching) { const auto pose_from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); - const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 3002184L, 0.0, 0.0, hdmap_utils_ptr); + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0); + const auto pose_to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002184L, 0.0, 0.0); { traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; @@ -214,10 +214,9 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_matching) */ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_matching) { - const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 3002184L, 0.0, 0.0, hdmap_utils_ptr); - const auto pose_to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002184L, 0.0, 0.0); + const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0); { traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -237,11 +236,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_matching) TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOpposite_noChange_false) { { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81595.44, 50006.09, 100.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81595.44, 50006.09, 100.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81584.48, 50084.76, 100.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81584.48, 50084.76, 100.0), false); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -259,11 +258,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos TEST_F(distanceTest_StandardMap, longitudinalDistance_noAdjacent_noOpposite_noChange) { { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(3800.05, 73715.77, 30.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(3800.05, 73715.77, 30.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(3841.26, 73748.80, 110.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(3841.26, 73748.80, 110.0), false); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -282,11 +281,11 @@ TEST_F(distanceTest_StandardMap, longitudinalDistance_noAdjacent_noOpposite_noCh TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposite_noChange_false) { { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81585.79, 50042.62, 100.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81585.79, 50042.62, 100.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81588.34, 50083.23, 100.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81588.34, 50083.23, 100.0), false); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -304,11 +303,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposite_noChange) { { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.02, 50065.76, 280.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81599.02, 50065.76, 280.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.61, 50045.16, 280.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81599.61, 50045.16, 280.0), false); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -327,11 +326,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOpposite_change_false) { { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81595.47, 49982.80, 100.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81595.47, 49982.80, 100.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.34, 50022.34, 100.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81599.34, 50022.34, 100.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -342,11 +341,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos EXPECT_FALSE(result.has_value()); } { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81612.35, 50015.63, 280.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81612.35, 50015.63, 280.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81612.95, 49991.30, 280.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81612.95, 49991.30, 280.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -366,11 +365,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOpposite_change_case0) { { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81592.96, 49997.94, 100.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81592.96, 49997.94, 100.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81570.56, 50141.75, 100.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81570.56, 50141.75, 100.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -382,11 +381,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos EXPECT_NEAR(result.value(), 145.0, 1.0); } { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81587.31, 50165.57, 100.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81587.31, 50165.57, 100.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81610.25, 49988.59, 100.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(81610.25, 49988.59, 100.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -405,11 +404,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_change_case1) { { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86627.71, 44972.06, 340.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86627.71, 44972.06, 340.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86647.23, 44882.51, 240.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -420,11 +419,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 118.0, 1.0)); } { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86555.38, 45000.88, 340.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86555.38, 45000.88, 340.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86647.23, 44882.51, 240.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -435,11 +434,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 195.0, 1.0)); } { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86788.82, 44993.77, 210.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86553.48, 44990.56, 150.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86553.48, 44990.56, 150.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -450,11 +449,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 257.0, 1.0)); } { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86788.82, 44993.77, 210.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86788.82, 44993.77, 210.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86579.91, 44979.00, 150.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86579.91, 44979.00, 150.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -474,11 +473,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_change_false) { { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86736.13, 44969.63, 210.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86736.13, 44969.63, 210.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86642.95, 44958.78, 340.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86642.95, 44958.78, 340.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -489,11 +488,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch EXPECT_FALSE(result.has_value()); } { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86732.06, 44976.58, 210.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86732.06, 44976.58, 210.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86704.59, 44927.32, 340.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86704.59, 44927.32, 340.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -513,10 +512,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_change) { { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86637.19, 44967.35, 340.0), false, hdmap_utils_ptr); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86648.82, 44886.19, 240.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86637.19, 44967.35, 340.0), false); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86648.82, 44886.19, 240.0), false); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; @@ -527,11 +526,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch EXPECT_NEAR(result.value(), 103.0, 1.0); } { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86719.94, 44957.20, 210.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86719.94, 44957.20, 210.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86599.32, 44975.01, 180.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(86599.32, 44975.01, 180.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; diff --git a/simulation/traffic_simulator/test/src/utils/test_pose.cpp b/simulation/traffic_simulator/test/src/utils/test_pose.cpp index 78c4c107294..53c5c111ef5 100644 --- a/simulation/traffic_simulator/test/src/utils/test_pose.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_pose.cpp @@ -36,6 +36,7 @@ class PoseTest : public testing::Test .longitude(138.8024583466017) .altitude(0.0))) { + activateLaneletWrapper("four_track_highway"); } std::shared_ptr hdmap_utils; @@ -79,7 +80,7 @@ TEST(pose, quietNaNLaneletPose) TEST_F(PoseTest, canonicalize_default) { const auto pose = - traffic_simulator::pose::canonicalize(traffic_simulator_msgs::msg::LaneletPose(), hdmap_utils); + traffic_simulator::pose::toCanonicalizedLaneletPose(traffic_simulator_msgs::msg::LaneletPose()); EXPECT_FALSE(pose.has_value()); } @@ -90,8 +91,8 @@ TEST_F(PoseTest, canonicalize_default) TEST_F(PoseTest, canonicalize_invalid) { EXPECT_THROW( - traffic_simulator::pose::canonicalize( - traffic_simulator::pose::quietNaNLaneletPose(), hdmap_utils), + traffic_simulator::pose::toCanonicalizedLaneletPose( + traffic_simulator::pose::quietNaNLaneletPose()), std::runtime_error); } @@ -103,7 +104,7 @@ TEST_F(PoseTest, canonicalize_valid) const auto pose = traffic_simulator::helper::constructLaneletPose(195, 0.0, 0.0); std::optional canonicalized_pose; - EXPECT_NO_THROW(canonicalized_pose = traffic_simulator::pose::canonicalize(pose, hdmap_utils)); + EXPECT_NO_THROW(canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose(pose)); ASSERT_TRUE(canonicalized_pose.has_value()); EXPECT_LANELET_POSE_EQ( static_cast(canonicalized_pose.value()), pose); @@ -115,7 +116,7 @@ TEST_F(PoseTest, canonicalize_valid) TEST_F(PoseTest, toMapPose_CanonicalizedLaneletPose) { const traffic_simulator::lanelet_pose::CanonicalizedLaneletPose canonicalized_pose( - traffic_simulator::helper::constructLaneletPose(195, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(195, 0.0, 0.0)); const geometry_msgs::msg::Pose pose = makePose( makePoint(81585.1622, 50176.9202, 34.2595), @@ -135,7 +136,7 @@ TEST_F(PoseTest, toMapPose_LaneletPose) makePoint(81585.1622, 50176.9202, 34.2595), geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); - EXPECT_POSE_NEAR(traffic_simulator::pose::toMapPose(lanelet_pose, hdmap_utils), pose, 0.01); + EXPECT_POSE_NEAR(traffic_simulator::pose::toMapPose(lanelet_pose), pose, 0.01); } /** @@ -144,12 +145,11 @@ TEST_F(PoseTest, toMapPose_LaneletPose) TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_valid) { const auto lanelet_pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); - const auto canonicalized_pose = - traffic_simulator::pose::toCanonicalizedLaneletPose(pose, true, hdmap_utils); + const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose(pose, true); ASSERT_TRUE(canonicalized_pose.has_value()); EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); @@ -165,8 +165,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_invalid) { const geometry_msgs::msg::Pose pose = makePose(makePoint(0.0, 0.0, 0.0)); - EXPECT_EQ( - traffic_simulator::pose::toCanonicalizedLaneletPose(pose, true, hdmap_utils), std::nullopt); + EXPECT_EQ(traffic_simulator::pose::toCanonicalizedLaneletPose(pose, true), std::nullopt); } /** @@ -175,12 +174,12 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_invalid) TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_valid) { const auto lanelet_pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); - const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeBoundingBox(), true, 1.0, hdmap_utils); + const auto canonicalized_pose = + traffic_simulator::pose::toCanonicalizedLaneletPose(pose, makeBoundingBox(), true, 1.0); ASSERT_TRUE(canonicalized_pose.has_value()); EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); @@ -195,13 +194,12 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_valid) TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_invalid) { const auto lanelet_pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 10.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 10.0, 0.0); const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); EXPECT_EQ( - traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeSmallBoundingBox(), true, 0.0, hdmap_utils), + traffic_simulator::pose::toCanonicalizedLaneletPose(pose, makeSmallBoundingBox(), true, 0.0), std::nullopt); } @@ -214,7 +212,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyInvalid) EXPECT_EQ( traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeBoundingBox(), lanelet::Ids{}, true, 1.0, hdmap_utils), + pose, makeBoundingBox(), lanelet::Ids{}, true, 1.0), std::nullopt); } @@ -224,12 +222,12 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyInvalid) TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyValid) { const auto lanelet_pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeBoundingBox(), lanelet::Ids{}, true, 1.0, hdmap_utils); + pose, makeBoundingBox(), lanelet::Ids{}, true, 1.0); ASSERT_TRUE(canonicalized_pose.has_value()); EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); @@ -246,8 +244,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyInvalid) const geometry_msgs::msg::Pose pose = makePose(makePoint(0.0, 0.0, 0.0)); EXPECT_EQ( - traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeBoundingBox(), {195}, true, 1.0, hdmap_utils), + traffic_simulator::pose::toCanonicalizedLaneletPose(pose, makeBoundingBox(), {195}, true, 1.0), std::nullopt); } @@ -257,12 +254,12 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyInvalid) TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyValid) { const auto lanelet_pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeBoundingBox(), lanelet::Ids{195}, true, 1.0, hdmap_utils); + pose, makeBoundingBox(), lanelet::Ids{195}, true, 1.0); ASSERT_TRUE(canonicalized_pose.has_value()); EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); @@ -338,8 +335,7 @@ TEST_F(PoseTest, relativePose_canonicalized_end_position) const auto from = makePose( makePoint(81585.1622, 50176.9202, 34.2595), geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 10.0, 0.0, hdmap_utils); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 10.0, 0.0); const auto relative = traffic_simulator::pose::relativePose(from, to); @@ -355,8 +351,7 @@ TEST_F(PoseTest, relativePose_canonicalized_start_position) const auto pose_relative = makePose( makePoint(145244.7916, 786706.3326, 0.0127), geometry_msgs::build().x(0).y(0).z(0).w(1)); - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); const auto to = makePose( makePoint(881586.9767, 50167.0862, 34.2722), geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); @@ -418,10 +413,8 @@ TEST_F(PoseTest, boundingBoxRelativePose_intersect) */ TEST_F(PoseTest, relativeLaneletPose_s_invalid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + 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); @@ -434,10 +427,8 @@ TEST_F(PoseTest, relativeLaneletPose_s_invalid) */ TEST_F(PoseTest, relativeLaneletPose_s_valid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + 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); @@ -450,10 +441,8 @@ TEST_F(PoseTest, relativeLaneletPose_s_valid) */ TEST_F(PoseTest, relativeLaneletPose_offset_invalid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + 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); @@ -466,10 +455,8 @@ TEST_F(PoseTest, relativeLaneletPose_offset_invalid) */ TEST_F(PoseTest, relativeLaneletPose_offset_valid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; @@ -484,10 +471,8 @@ TEST_F(PoseTest, relativeLaneletPose_offset_valid) */ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_invalid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0); const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( @@ -501,10 +486,8 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_invalid) */ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_valid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0); const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( @@ -518,10 +501,8 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_valid) */ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_invalid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0); const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( @@ -535,10 +516,8 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_invalid) */ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_valid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, -1.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, -1.0); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0); const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( @@ -552,8 +531,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_valid) */ TEST_F(PoseTest, isInLanelet_inside) { - const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + 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)); @@ -565,7 +543,7 @@ TEST_F(PoseTest, isInLanelet_inside) TEST_F(PoseTest, isInLanelet_outsideFrontFar) { const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -10.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -10.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 3002163, 1.0, hdmap_utils)); } @@ -576,7 +554,7 @@ TEST_F(PoseTest, isInLanelet_outsideFrontFar) TEST_F(PoseTest, isInLanelet_outsideFrontClose) { const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -1.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -1.0, 0.0); EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 3002163, 2.0, hdmap_utils)); } @@ -586,8 +564,7 @@ TEST_F(PoseTest, isInLanelet_outsideFrontClose) */ TEST_F(PoseTest, isInLanelet_outsideBackFar) { - const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 120.0, 0.0, hdmap_utils); + const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 120.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 195, 2, hdmap_utils)); } @@ -597,8 +574,7 @@ TEST_F(PoseTest, isInLanelet_outsideBackFar) */ TEST_F(PoseTest, isInLanelet_outsideBackClose) { - const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 110.0, 0.0, hdmap_utils); + const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 110.0, 0.0); EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 195, 10.0, hdmap_utils)); } @@ -609,7 +585,7 @@ TEST_F(PoseTest, isInLanelet_outsideBackClose) TEST_F(PoseTest, isAtEndOfLanelets_noFollowing_within) { const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002171, 31.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002171, 31.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); } @@ -619,8 +595,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_noFollowing_within) */ TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_within) { - const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002167, 5.0, 0.0, hdmap_utils); + const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002167, 5.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); } @@ -631,7 +606,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_within) TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_outside) { const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002167, 20.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002167, 20.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); } @@ -641,8 +616,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_outside) */ TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_within) { - const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 5.0, 0.0, hdmap_utils); + const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 5.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); } @@ -652,8 +626,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_within) */ TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_outside) { - const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 120.0, 0.0, hdmap_utils); + const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 120.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); } @@ -664,7 +637,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_outside) TEST_F(PoseTest, laneletLength_invalid) { EXPECT_THROW( - traffic_simulator::pose::laneletLength(10000000000000000, hdmap_utils), std::runtime_error); + traffic_simulator::lanelet_map::laneletLength(10000000000000000), std::runtime_error); } /** @@ -672,5 +645,5 @@ TEST_F(PoseTest, laneletLength_invalid) */ TEST_F(PoseTest, laneletLength_valid) { - EXPECT_NEAR(traffic_simulator::pose::laneletLength(195, hdmap_utils), 107.74, 0.01); + EXPECT_NEAR(traffic_simulator::lanelet_map::laneletLength(195), 107.74, 0.01); } diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index a33b62fb37c..fa426092b46 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -78,8 +78,9 @@ class TestExecutor RCLCPP_INFO_STREAM(logger_, message); scenario_completed_ = false; - if (const auto ego_start_canonicalized_lanelet_pose = traffic_simulator::pose::canonicalize( - test_description_.ego_start_position, api_->getHdmapUtils()); + if (const auto ego_start_canonicalized_lanelet_pose = + traffic_simulator::pose::toCanonicalizedLaneletPose( + test_description_.ego_start_position); !ego_start_canonicalized_lanelet_pose) { throw std::runtime_error( "Can not canonicalize ego start lanelet pose: id: " + @@ -135,8 +136,8 @@ class TestExecutor for (size_t i = 0; i < test_description_.npcs_descriptions.size(); i++) { const auto & npc_descr = test_description_.npcs_descriptions[i]; - if (const auto npc_start_canonicalized_lanelet_pose = traffic_simulator::pose::canonicalize( - npc_descr.start_position, api_->getHdmapUtils()); + if (const auto npc_start_canonicalized_lanelet_pose = + traffic_simulator::pose::toCanonicalizedLaneletPose(npc_descr.start_position); !npc_start_canonicalized_lanelet_pose) { throw std::runtime_error( "Can not canonicalize npc start lanelet pose: id: " + diff --git a/test_runner/random_test_runner/src/lanelet_utils.cpp b/test_runner/random_test_runner/src/lanelet_utils.cpp index dd462c9ff00..80de0a2e6ad 100644 --- a/test_runner/random_test_runner/src/lanelet_utils.cpp +++ b/test_runner/random_test_runner/src/lanelet_utils.cpp @@ -27,6 +27,7 @@ #include #include #include +#include LaneletUtils::LaneletUtils(const boost::filesystem::path & filename) { @@ -45,6 +46,7 @@ LaneletUtils::LaneletUtils(const boost::filesystem::path & filename) hdmap_utils_ptr_ = std::make_shared(filename, geographic_msgs::msg::GeoPoint()); + traffic_simulator::lanelet_map::activate(filename.string()); } std::vector LaneletUtils::getLaneletIds() { return hdmap_utils_ptr_->getLaneletIds(); } diff --git a/test_runner/random_test_runner/test/test_test_executor.cpp b/test_runner/random_test_runner/test/test_test_executor.cpp index 04fcacd9ac4..6ec14ed5f82 100644 --- a/test_runner/random_test_runner/test/test_test_executor.cpp +++ b/test_runner/random_test_runner/test/test_test_executor.cpp @@ -17,6 +17,7 @@ #include #include +#include class MockFieldOperatorApplication { @@ -66,6 +67,13 @@ auto getTestDescription() -> TestDescription class MockTrafficSimulatorAPI { public: + MockTrafficSimulatorAPI() + { + const std::string path = + ament_index_cpp::get_package_share_directory("random_test_runner") + "/map/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(path); + } + traffic_simulator::EntityStatus entity_status_; std::shared_ptr<::testing::StrictMock> field_operator_application_mock = From 6c6b931eef24742332a019cab3df1fb03349fdd5 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 3 Dec 2024 16:18:35 +0100 Subject: [PATCH 05/62] ref(traffic_simulator): remove unused parts lanelet_wrapper --- simulation/traffic_simulator/CMakeLists.txt | 4 - .../lanelet_wrapper/distance.hpp | 83 ----- .../lanelet_wrapper/lane_change.hpp | 74 ---- .../lanelet_wrapper/lanelet_map.hpp | 27 -- .../lanelet_wrapper/route.hpp | 53 --- .../lanelet_wrapper/traffic_lights.hpp | 61 ---- .../traffic_simulator/utils/lanelet_map.hpp | 11 - .../include/traffic_simulator/utils/pose.hpp | 15 - .../src/lanelet_wrapper/distance.cpp | 318 ------------------ .../src/lanelet_wrapper/lane_change.cpp | 245 -------------- .../src/lanelet_wrapper/lanelet_map.cpp | 23 -- .../src/lanelet_wrapper/route.cpp | 256 -------------- .../src/lanelet_wrapper/traffic_lights.cpp | 187 ---------- .../src/utils/lanelet_map.cpp | 95 ------ .../traffic_simulator/src/utils/pose.cpp | 93 +---- 15 files changed, 4 insertions(+), 1541 deletions(-) delete mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp delete mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lane_change.hpp delete mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp delete mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_lights.hpp delete mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp delete mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/lane_change.cpp delete mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/route.cpp delete 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 abe46dd36a3..9eecfc78100 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -49,13 +49,9 @@ 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/lane_change.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 deleted file mode 100644 index a755d7c5e5b..00000000000 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp +++ /dev/null @@ -1,83 +0,0 @@ -// 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 -// #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/lane_change.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lane_change.hpp deleted file mode 100644 index 8b7d0972fea..00000000000 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lane_change.hpp +++ /dev/null @@ -1,74 +0,0 @@ -// 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_LANE_CHANGE_HPP_ -// #define TRAFFIC_SIMULATOR__LANELET_WRAPPER_LANE_CHANGE_HPP_ - -// #include - -// #include -// #include -// #include - -// namespace traffic_simulator -// { -// namespace lanelet_wrapper -// { -// namespace lane_change -// { -// using Pose = geometry_msgs::msg::Pose; -// using Vector3 = geometry_msgs::msg::Vector3; -// using Curve = math::geometry::HermiteCurve; - -// using Parameter = traffic_simulator::lane_change::Parameter; -// using Direction = traffic_simulator::lane_change::Direction; -// using Constraint = traffic_simulator::lane_change::Constraint; -// using TrajectoryShape = traffic_simulator::lane_change::TrajectoryShape; - -// auto canChangeLane( -// const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, -// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> bool; - -// auto laneChangeableLaneletId( -// const lanelet::Id lanelet_id, const Direction & direction, -// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -// -> std::optional; - -// auto laneChangeableLaneletId( -// const lanelet::Id lanelet_id, const Direction & direction, const std::uint8_t shift, -// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -// -> std::optional; - -// auto countLaneChanges( -// const lanelet::Id & from_lanelet_id, const lanelet::Id & to_lanelet_id, -// const RoutingConfiguration & routing_configuration = RoutingConfiguration()) -// -> std::optional>; - -// // Trajectory -// auto laneChangeTrajectory( -// const Pose & from_pose, const LaneletPose & to_lanelet_pose, -// const TrajectoryShape & trajectory_shape, const double tangent_vector_size) -> Curve; - -// auto laneChangeTrajectory( -// const LaneletPose & from_lanelet_pose, const Parameter & lane_change_parameter) -// -> std::optional>; - -// auto laneChangeTrajectory( -// const Pose & from_pose, const Parameter & lane_change_parameter, -// const double maximum_curvature_threshold, const double target_trajectory_length, -// const double forward_distance_threshold) -> std::optional>; -// } // namespace lane_change -// } // namespace lanelet_wrapper -// } // namespace traffic_simulator -// #endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_LANE_CHANGE_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 7f0901c6d11..654e731e652 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 @@ -38,9 +38,6 @@ auto isInLanelet(const lanelet::Id lanelet_id, const Point point) -> bool; auto laneletLength(const lanelet::Id lanelet_id) -> double; -// auto laneletYaw(const lanelet::Id lanelet_id, const Point & point) -// -> std::tuple; - template auto laneletIds(const std::vector & lanelets) -> lanelet::Ids { @@ -97,30 +94,6 @@ 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; - -// auto conflictingCrosswalkIds(const lanelet::Ids & lanelet_ids) -> lanelet::Ids; - -// auto conflictingLaneIds( -// const lanelet::Ids & lanelet_ids, -// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> 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 deleted file mode 100644 index 2ed3fc4c056..00000000000 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// 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 isInRoute(const lanelet::Id lanelet_id, const lanelet::Ids & route_lanelets_ids) -> bool; - -// auto speedLimit( -// const lanelet::Ids & lanelet_ids, -// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> double; - -// auto route( -// const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, -// const RoutingConfiguration & routing_configuration = RoutingConfiguration()) -> lanelet::Ids; - -// auto followingLanelets( -// const lanelet::Id lanelet_id, const lanelet::Ids & candidate_lanelet_ids, -// const double distance = 100, const bool include_self = true, -// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; - -// auto followingLanelets( -// const lanelet::Id lanelet_id, const double distance = 100, const bool include_self = true, -// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; - -// auto previousLanelets( -// const lanelet::Id current_lanelet_id, const double backward_horizon = 100, -// const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> 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 deleted file mode 100644 index b357e6de97c..00000000000 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_lights.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// 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 -// { -// using Point = geometry_msgs::msg::Point; - -// auto isTrafficLight(const lanelet::Id lanelet_id) -> bool; - -// auto isTrafficLightRegulatoryElement(const lanelet::Id lanelet_id) -> bool; - -// auto toTrafficLightRegulatoryElement(const lanelet::Id traffic_light_regulatory_element_id) -// -> lanelet::TrafficLight::Ptr; - -// auto toAutowareTrafficLights(const lanelet::Id traffic_light_id) -// -> std::vector; - -// auto trafficLightBulbPosition(const lanelet::Id traffic_light_id, const std::string & color_name) -// -> std::optional; - -// auto trafficLightStopLinesPoints(const lanelet::Id traffic_light_id) -// -> std::vector>; - -// auto trafficLightRegulatoryElementIDsFromTrafficLight(const lanelet::Id traffic_light_way_id) -// -> lanelet::Ids; - -// // On path -// auto autowareTrafficLightsOnPath(const lanelet::Ids & lanelet_ids) -// -> std::vector; - -// auto trafficLightIdsOnPath(const lanelet::Ids & route_lanelets) -> lanelet::Ids; - -// auto trafficSignsOnPath(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/include/traffic_simulator/utils/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp index d940f4b4655..76cdc500f1e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp @@ -36,17 +36,6 @@ inline auto activate(Ts &&... xs) } auto laneletLength(const lanelet::Id lanelet_id) -> double; - -// auto laneletYaw(const Point & point, const lanelet::Id lanelet_id) -// -> std::tuple; - -// auto nearbyLaneletIds( -// const Pose & pose, const double distance_threshold, const bool include_crosswalk, -// const std::size_t search_count) -> lanelet::Ids; - -// auto borderlinePoses() -> std::vector>; - -// auto visualizationMarker() -> visualization_msgs::msg::MarkerArray; } // namespace lanelet_map } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__UTILS__LANELET_MAP_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 235b6dffa51..a42fe6622b1 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -88,10 +88,6 @@ auto relativeLaneletPose( const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose; -// auto relativeLaneletPose( -// const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, -// const RoutingConfiguration & routing_configuration) -> LaneletPose; - auto boundingBoxRelativeLaneletPose( const CanonicalizedLaneletPose & from, const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, @@ -100,22 +96,11 @@ auto boundingBoxRelativeLaneletPose( const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> 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) -> 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; -// auto isInLanelet( -// const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id lanelet_id, -// const double tolerance) -> bool; - auto isInLanelet(const geometry_msgs::msg::Point & point, const lanelet::Id lanelet_id) -> bool; auto isAtEndOfLanelets( diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp deleted file mode 100644 index 24129103fb2..00000000000 --- a/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp +++ /dev/null @@ -1,318 +0,0 @@ -// 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; -// } -// 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 lateralDistance( -// // const LaneletPose & from, const LaneletPose & to, -// // const RoutingConfiguration & routing_configuration) -> std::optional -// // { -// // // route must exist for lateral distance to be calculated at all -// // if (const auto route = route::route(from.lanelet_id, to.lanelet_id, routing_configuration); -// // 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) { -// // // if there is a lane change -// // if (lane_change::canChangeLane(route[i], route[i + 1])) { -// // 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 { -// auto stopLinesOnPath = [](const lanelet::Ids & lanelet_ids) -> lanelet::ConstLineStrings3d { -// lanelet::ConstLineStrings3d stop_lines; -// for (const auto & traffic_sign : traffic_lights::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; -// }; - -// std::vector collision_points; -// // fill in collision_points using stop_lines -// const auto stop_lines = 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 = 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/lane_change.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lane_change.cpp deleted file mode 100644 index f8cc66478db..00000000000 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lane_change.cpp +++ /dev/null @@ -1,245 +0,0 @@ -// 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 - -// namespace traffic_simulator -// { -// namespace lanelet_wrapper -// { -// namespace lane_change -// { -// auto canChangeLane( -// const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, const RoutingGraphType type) -// -> bool -// { -// const auto from_lanelet = LaneletWrapper::map()->laneletLayer.get(from_lanelet_id); -// const auto to_lanelet = LaneletWrapper::map()->laneletLayer.get(to_lanelet_id); -// return LaneletWrapper::trafficRules(type)->canChangeLane(from_lanelet, to_lanelet); -// } - -// auto laneChangeableLaneletId( -// const lanelet::Id lanelet_id, const Direction & direction, const RoutingGraphType type) -// -> std::optional -// { -// const auto lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); -// if (direction == Direction::STRAIGHT) { -// return lanelet.id(); -// } else if (direction == Direction::LEFT && LaneletWrapper::routingGraph(type)->left(lanelet)) { -// return LaneletWrapper::routingGraph(type)->left(lanelet)->id(); -// } else if (direction == Direction::RIGHT && LaneletWrapper::routingGraph(type)->right(lanelet)) { -// return LaneletWrapper::routingGraph(type)->right(lanelet)->id(); -// } else { -// return std::nullopt; -// } -// } - -// auto laneChangeableLaneletId( -// const lanelet::Id lanelet_id, const Direction & direction, const std::uint8_t shift, -// const RoutingGraphType type) -> std::optional -// { -// if (shift == 0) { -// return laneChangeableLaneletId(lanelet_id, Direction::STRAIGHT, type); -// } else { -// auto reference_id = lanelet_id; -// for (std::size_t i = 0; i < shift; ++i) { -// if (const auto id_opt = laneChangeableLaneletId(reference_id, direction, type); !id_opt) { -// return std::nullopt; -// } else { -// reference_id = id_opt.value(); -// } -// } -// return reference_id; -// } -// } - -// auto countLaneChanges( -// const lanelet::Id & from_lanelet_id, const lanelet::Id & to_lanelet_id, -// const RoutingConfiguration & routing_configuration) -> std::optional> -// { -// constexpr bool include_opposite_direction{true}; -// const auto traveled_route = route::route(from_lanelet_id, to_lanelet_id, routing_configuration); -// if (traveled_route.empty()) { -// return std::nullopt; -// } else { -// std::pair lane_changes{0, 0}; -// for (std::size_t i = 1; i < traveled_route.size(); ++i) { -// const auto & previous = traveled_route[i - 1]; -// const auto & current = traveled_route[i]; - -// if (auto followings = -// lanelet_map::nextLaneletIds(previous, routing_configuration.routing_graph_type); -// std::find(followings.begin(), followings.end(), current) == followings.end()) { -// traffic_simulator_msgs::msg::EntityType type; -// type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; -// 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++; -// } -// } -// } -// return lane_changes; -// } -// } - -// // Trajectory -// auto laneChangeTrajectory( -// const Pose & from_pose, const LaneletPose & to_lanelet_pose, -// const TrajectoryShape & trajectory_shape, const double tangent_vector_size) -> Curve -// { -// const auto to_pose = pose::toMapPose(to_lanelet_pose).pose; - -// auto vectorFromPose = [](const Pose & pose, const double magnitude) -> Vector3 { -// const auto direction = math::geometry::convertQuaternionToEulerAngle(pose.orientation); -// return geometry_msgs::build() -// .x(magnitude * std::cos(direction.z)) -// .y(magnitude * std::sin(direction.z)) -// .z(0); -// }; - -// auto tangentVector = [](const lanelet::Id lanelet_id, const double s) -> std::optional { -// return lanelet_map::centerPointsSpline(lanelet_id)->getTangentVector(s); -// }; - -// double tangent_vector_size_in_curve{0.0}; -// Vector3 from_vector; -// Vector3 to_vector; -// switch (trajectory_shape) { -// case TrajectoryShape::CUBIC: -// tangent_vector_size_in_curve = tangent_vector_size; -// from_vector = vectorFromPose(from_pose, tangent_vector_size); -// if ( -// const auto tangent_vector = tangentVector(to_lanelet_pose.lanelet_id, to_lanelet_pose.s)) { -// to_vector = tangent_vector.value(); -// } else { -// THROW_SIMULATION_ERROR( -// "Failed to calculate tangent vector at lanelet_id : ", to_lanelet_pose.lanelet_id, -// " s : ", to_lanelet_pose.s); -// } -// break; -// case TrajectoryShape::LINEAR: -// tangent_vector_size_in_curve = 1; -// from_vector.x = (to_pose.position.x - from_pose.position.x); -// from_vector.y = (to_pose.position.y - from_pose.position.y); -// from_vector.z = (to_pose.position.z - from_pose.position.z); -// to_vector = from_vector; -// break; -// default: -// throw std::invalid_argument("Unknown trajectory_shape constraint type"); -// } - -// return Curve( -// from_pose, to_pose, from_vector, -// geometry_msgs::build() -// .x(to_vector.x * tangent_vector_size_in_curve) -// .y(to_vector.y * tangent_vector_size_in_curve) -// .z(to_vector.z * tangent_vector_size_in_curve)); -// } - -// auto laneChangeTrajectory( -// const LaneletPose & from_lanelet_pose, const Parameter & lane_change_parameter) -// -> std::optional> -// { -// double longitudinal_distance = [&]() { -// switch (lane_change_parameter.constraint.type) { -// case Constraint::Type::LONGITUDINAL_DISTANCE: -// return lane_change_parameter.constraint.value; -// default: -// return Parameter::default_lanechange_distance; -// } -// }(); - -// const auto along_lanelet_pose = pose::alongLaneletPose(from_lanelet_pose, longitudinal_distance); -// auto left_boundary_lanelet_pose = along_lanelet_pose; -// left_boundary_lanelet_pose.offset += 5.0; -// auto right_boundary_lanelet_pose = along_lanelet_pose; -// right_boundary_lanelet_pose.offset -= 5.0; - -// const auto left_boundary_point = pose::toMapPose(left_boundary_lanelet_pose).pose.position; -// const auto right_boundary_point = pose::toMapPose(right_boundary_lanelet_pose).pose.position; -// if (const auto to_lanelet_pose_s = -// lanelet_map::centerPointsSpline(lane_change_parameter.target.lanelet_id) -// ->getCollisionPointIn2D(left_boundary_point, right_boundary_point); -// !to_lanelet_pose_s) { -// return std::nullopt; -// } else { -// const auto to_lanelet_pose = helper::constructLaneletPose( -// lane_change_parameter.target.lanelet_id, to_lanelet_pose_s.value(), -// lane_change_parameter.target.offset); -// const auto from_pose = pose::toMapPose(from_lanelet_pose).pose; -// const auto to_pose = pose::toMapPose(to_lanelet_pose).pose; -// const auto euclidean_distance = math::geometry::hypot(from_pose.position, to_pose.position); -// const auto lane_change_trajectory = laneChangeTrajectory( -// from_pose, to_lanelet_pose, lane_change_parameter.trajectory_shape, euclidean_distance * 0.5); -// return std::make_pair(lane_change_trajectory, to_lanelet_pose_s.value()); -// } -// } - -// auto laneChangeTrajectory( -// const Pose & from_pose, const Parameter & lane_change_parameter, -// const double maximum_curvature_threshold, const double target_trajectory_length, -// const double forward_distance_threshold) -> std::optional> -// { -// std::vector candidates_evaluation, candidates_s; -// std::vector candidates_curves; - -// const auto lanelet_length = lanelet_map::laneletLength(lane_change_parameter.target.lanelet_id); -// for (double to_lanelet_pose_s = 0; to_lanelet_pose_s < lanelet_length; to_lanelet_pose_s += 1.0) { -// const auto to_lanelet_pose = -// helper::constructLaneletPose(lane_change_parameter.target.lanelet_id, to_lanelet_pose_s, 0.0); -// // skip those poses that are too close -// if (const auto to_pose = pose::toMapPose(to_lanelet_pose).pose; -// math::geometry::getRelativePose(from_pose, to_pose).position.x <= -// forward_distance_threshold) { -// continue; -// } else { -// const auto euclidean_distance = math::geometry::hypot(from_pose.position, to_pose.position); -// if (const auto lane_change_trajectory = laneChangeTrajectory( -// from_pose, to_lanelet_pose, lane_change_parameter.trajectory_shape, -// euclidean_distance * 0.5); -// lane_change_trajectory.getMaximum2DCurvature() < maximum_curvature_threshold) { -// candidates_evaluation.push_back( -// std::fabs(target_trajectory_length - lane_change_trajectory.getLength())); -// candidates_curves.push_back(lane_change_trajectory); -// candidates_s.push_back(to_lanelet_pose_s); -// } -// } -// } - -// if (candidates_evaluation.empty()) { -// return std::nullopt; -// } else { -// const auto min_iterator = -// std::min_element(candidates_evaluation.begin(), candidates_evaluation.end()); -// const auto min_index = std::distance(candidates_evaluation.begin(), min_iterator); -// return std::make_pair(candidates_curves[min_index], candidates_s[min_index]); -// } -// } -// } // namespace lane_change -// } // 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 770a3fada72..25c31ee957a 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -44,29 +44,6 @@ auto laneletLength(const lanelet::Id lanelet_id) -> double return LaneletWrapper::laneletLengthCache().getLength(lanelet_id, LaneletWrapper::map()); } -// auto laneletYaw(const lanelet::Id lanelet_id, const Point & point) -// -> std::tuple -// { -// if (const auto centerline_points = lanelet_wrapper::lanelet_map::centerPoints(lanelet_id); -// centerline_points.empty()) { -// THROW_SIMULATION_ERROR( -// "There is no center points for lanelet with id: " + std::to_string(lanelet_id)); -// } else { -// auto findNearestPointIndex = [](const std::vector & points, const Point & point) { -// return std::distance( -// points.begin(), -// std::min_element(points.begin(), points.end(), [&](const Point & p1, const Point & p2) { -// return math::geometry::hypot(p1, point) < math::geometry::hypot(p2, point); -// })); -// }; -// const size_t nearest_point_index = findNearestPointIndex(centerline_points, point); -// const auto & nearest_point = centerline_points.at(nearest_point_index); -// const auto & next_point = centerline_points.at(nearest_point_index + 1); -// const auto yaw = std::atan2(next_point.y - nearest_point.y, next_point.x - nearest_point.x); -// return std::make_tuple(yaw, nearest_point, next_point); -// } -// } - auto laneletIds() -> lanelet::Ids { lanelet::Ids ids; diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp deleted file mode 100644 index db58570e9d4..00000000000 --- a/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp +++ /dev/null @@ -1,256 +0,0 @@ -// 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 isInRoute(const lanelet::Id lanelet_id, const lanelet::Ids & route_lanelets_ids) -> bool -// { -// return std::find(route_lanelets_ids.begin(), route_lanelets_ids.end(), lanelet_id) != -// route_lanelets_ids.end(); -// } - -// // it returns the speed limit in meters per second -// auto speedLimit(const lanelet::Ids & lanelet_ids, const RoutingGraphType type) -> double -// { -// if (lanelet_ids.empty()) { -// THROW_SEMANTIC_ERROR("size of the vector lanelet ids should be more than 1"); -// } else { -// std::vector limits; -// limits.reserve(lanelet_ids.size()); -// for (const auto & lanelet_id : lanelet_ids) { -// const auto & lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); -// const auto & limit = LaneletWrapper::trafficRules(type)->speedLimit(lanelet); -// limits.push_back(lanelet::units::KmHQuantity(limit.speedLimit).value() / 3.6); -// } -// return *std::min_element(limits.begin(), limits.end()); -// } -// } - -// auto route( -// const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, -// const RoutingConfiguration & routing_configuration) -> lanelet::Ids -// { -// 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)); -// } - -// // auto route( -// // const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, -// // const RoutingConfiguration & routing_configuration) -> lanelet::Ids -// // { -// // auto & cache = LaneletWrapper::routeCache(routing_configuration.routing_graph_type); -// // std::cout << " ########## A ##########" << std::endl; -// // if (cache.exists(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change)) { -// // return cache.getRoute(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change); -// // } -// // std::cout << " ########## B ##########" << std::endl; - -// // lanelet::Ids ids; -// // const auto lanelet = LaneletWrapper::map()->laneletLayer.get(from_lanelet_id); -// // const auto to_lanelet = LaneletWrapper::map()->laneletLayer.get(to_lanelet_id); -// // std::cout << " ########## B + ##########" << std::endl; -// // const auto graph = LaneletWrapper::routingGraph(routing_configuration.routing_graph_type); -// // std::cout << " ########## B ++ ##########" << std::endl; -// // lanelet::Optional route = -// // graph->getRoute(lanelet, to_lanelet, 0, routing_configuration.allow_lane_change); -// // std::cout << " ########## C ##########" << std::endl; - -// // if (!route) { -// // cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); -// // return ids; -// // } -// // lanelet::routing::LaneletPath shortest_path = route->shortestPath(); -// // std::cout << " ########## D ##########" << std::endl; - -// // if (shortest_path.empty()) { -// // cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); -// // return ids; -// // } -// // std::cout << " ########## E ##########" << std::endl; - -// // for (auto lane_itr = shortest_path.begin(); lane_itr != shortest_path.end(); lane_itr++) { -// // ids.push_back(lane_itr->id()); -// // } -// // cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); -// // std::cout << " ########## G ##########" << std::endl; - -// // return ids; -// // } - -// auto followingLanelets( -// const lanelet::Id current_lanelet_id, const lanelet::Ids & route, const double horizon, -// const bool include_current_lanelet_id, const RoutingGraphType type) -> lanelet::Ids -// { -// const auto is_following_lanelet = -// [&type](const auto & current_lanelet, const auto & candidate_lanelet) { -// const auto next_ids = lanelet_map::nextLaneletIds(current_lanelet, type); -// return std::find(next_ids.cbegin(), next_ids.cend(), candidate_lanelet) != next_ids.cend(); -// }; - -// lanelet::Ids following_lanelets_ids; - -// if (route.empty()) { -// return following_lanelets_ids; -// } - -// double total_distance = 0.0; -// bool found_starting_lanelet = false; - -// for (const auto & candidate_lanelet_id : route) { -// if (found_starting_lanelet) { -// if (const auto previous_lanelet = -// following_lanelets_ids.empty() ? current_lanelet_id : following_lanelets_ids.back(); -// not is_following_lanelet(previous_lanelet, candidate_lanelet_id)) { -// THROW_SEMANTIC_ERROR( -// candidate_lanelet_id + " is not the follower of lanelet " + previous_lanelet); -// } -// following_lanelets_ids.push_back(candidate_lanelet_id); -// total_distance += lanelet_map::laneletLength(candidate_lanelet_id); -// if (total_distance > horizon) { -// break; -// } -// } else if (candidate_lanelet_id == current_lanelet_id) { -// found_starting_lanelet = true; -// if (include_current_lanelet_id) { -// following_lanelets_ids.push_back(candidate_lanelet_id); -// } -// } -// } -// if (following_lanelets_ids.empty()) { -// THROW_SEMANTIC_ERROR("lanelet id does not match"); -// } else if (total_distance <= horizon) { -// const auto remaining_lanelets = -// followingLanelets(route.back(), horizon - total_distance, false, type); -// following_lanelets_ids.insert( -// following_lanelets_ids.end(), remaining_lanelets.begin(), remaining_lanelets.end()); -// } - -// return following_lanelets_ids; -// } - -// // auto followingLanelets( -// // const lanelet::Id lanelet_id, const lanelet::Ids & candidate_lanelet_ids, const double -// // distance, const bool include_self) -> lanelet::Ids -// // { -// // if (candidate_lanelet_ids.empty()) { -// // return {}; -// // } else { -// // lanelet::Ids following_lanelets_ids; -// // double total_distance = 0.0; -// // bool found_reference_lanelet_id = false; -// // for (const auto & candidate_lanelet_id : candidate_lanelet_ids) { -// // if (found_reference_lanelet_id) { -// // following_lanelets_ids.push_back(candidate_lanelet_id); -// // total_distance += lanelet_map::laneletLength(candidate_lanelet_id); -// // if (total_distance > distance) { -// // return following_lanelets_ids; -// // } -// // } -// // if (!found_reference_lanelet_id && candidate_lanelet_id == lanelet_id) { -// // found_reference_lanelet_id = true; -// // if (include_self) { -// // following_lanelets_ids.push_back(candidate_lanelet_id); -// // } -// // } -// // } - -// // if (!found_reference_lanelet_id) { -// // THROW_SEMANTIC_ERROR("lanelet id does not match"); -// // } else if (total_distance > distance) { -// // return following_lanelets_ids; -// // } else { -// // const auto remaining_lanelets = -// // followingLanelets(candidate_lanelet_ids.back(), distance - total_distance, false); -// // following_lanelets_ids.insert( -// // following_lanelets_ids.end(), remaining_lanelets.begin(), remaining_lanelets.end()); -// // return following_lanelets_ids; -// // } -// // } -// // } - -// auto followingLanelets( -// const lanelet::Id lanelet_id, const double distance, const bool include_self, -// const RoutingGraphType type) -> lanelet::Ids -// { -// lanelet::Ids following_lanelets_ids; -// if (include_self) { -// following_lanelets_ids.push_back(lanelet_id); -// } -// double total_distance = 0.0; -// auto reference_lanelet_id = lanelet_id; -// while (total_distance < distance) { -// if (const auto straight_lanelet_ids = -// lanelet_map::nextLaneletIds(reference_lanelet_id, "straight", type); -// !straight_lanelet_ids.empty()) { -// total_distance = total_distance + lanelet_map::laneletLength(straight_lanelet_ids[0]); -// following_lanelets_ids.push_back(straight_lanelet_ids[0]); -// } else if (const auto non_straight_lanelet_ids = -// lanelet_map::nextLaneletIds(reference_lanelet_id, type); -// !non_straight_lanelet_ids.empty()) { -// total_distance = total_distance + lanelet_map::laneletLength(non_straight_lanelet_ids[0]); -// following_lanelets_ids.push_back(non_straight_lanelet_ids[0]); -// } else { -// break; -// } -// reference_lanelet_id = following_lanelets_ids.back(); -// } -// return following_lanelets_ids; -// } - -// auto previousLanelets( -// const lanelet::Id current_lanelet_id, const double backward_horizon, const RoutingGraphType type) -// -> lanelet::Ids -// { -// lanelet::Ids previous_lanelets_ids; -// double total_distance = 0.0; -// previous_lanelets_ids.push_back(current_lanelet_id); -// while (total_distance < backward_horizon) { -// const auto & reference_lanelet_id = previous_lanelets_ids.back(); -// if (const auto straight_lanelet_ids = -// lanelet_map::previousLaneletIds(reference_lanelet_id, "straight", type); -// not straight_lanelet_ids.empty()) { -// total_distance = total_distance + lanelet_map::laneletLength(straight_lanelet_ids[0]); -// previous_lanelets_ids.push_back(straight_lanelet_ids[0]); -// } else if (auto non_straight_lanelet_ids = -// lanelet_map::previousLaneletIds(reference_lanelet_id, type); -// not non_straight_lanelet_ids.empty()) { -// total_distance = total_distance + lanelet_map::laneletLength(non_straight_lanelet_ids[0]); -// previous_lanelets_ids.push_back(non_straight_lanelet_ids[0]); -// } else { -// break; -// } -// } -// return previous_lanelets_ids; -// } -// } // 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 deleted file mode 100644 index 66e29432b1f..00000000000 --- a/simulation/traffic_simulator/src/lanelet_wrapper/traffic_lights.cpp +++ /dev/null @@ -1,187 +0,0 @@ -// 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 isTrafficLight(const lanelet::Id lanelet_id) -> bool -// { -// if (LaneletWrapper::map()->lineStringLayer.exists(lanelet_id)) { -// if (const auto && linestring = LaneletWrapper::map()->lineStringLayer.get(lanelet_id); -// linestring.hasAttribute(lanelet::AttributeName::Type)) { -// return linestring.attribute(lanelet::AttributeName::Type).value() == "traffic_light"; -// } -// } -// return false; -// } - -// auto isTrafficLightRegulatoryElement(const lanelet::Id lanelet_id) -> bool -// { -// return LaneletWrapper::map()->regulatoryElementLayer.exists(lanelet_id) && -// std::dynamic_pointer_cast( -// LaneletWrapper::map()->regulatoryElementLayer.get(lanelet_id)); -// } - -// auto toTrafficLightRegulatoryElement(const lanelet::Id traffic_light_regulatory_element_id) -// -> lanelet::TrafficLight::Ptr -// { -// if (isTrafficLightRegulatoryElement(traffic_light_regulatory_element_id)) { -// return std::dynamic_pointer_cast( -// LaneletWrapper::map()->regulatoryElementLayer.get(traffic_light_regulatory_element_id)); -// } else { -// THROW_SEMANTIC_ERROR( -// traffic_light_regulatory_element_id, " is not traffic light regulatory element!"); -// } -// } - -// 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 trafficLightBulbPosition(const lanelet::Id traffic_light_id, const std::string & color_name) -// -> std::optional -// { -// auto isBulbOfExpectedColor = [&color_name](auto bulb) -> bool { -// return bulb.hasAttribute("color") and !bulb.hasAttribute("arrow") and -// bulb.attribute("color").value().compare(color_name) == 0; -// }; - -// for (const auto & autoware_traffic_light : toAutowareTrafficLights(traffic_light_id)) { -// for (auto three_light_bulbs : autoware_traffic_light->lightBulbs()) { -// for (auto bulb : static_cast(three_light_bulbs)) { -// if (isBulbOfExpectedColor(bulb)) { -// return geometry_msgs::build().x(bulb.x()).y(bulb.y()).z(bulb.z()); -// } -// } -// } -// } -// return std::nullopt; -// } - -// 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 trafficLightRegulatoryElementIDsFromTrafficLight(const lanelet::Id traffic_light_id) -// -> lanelet::Ids -// { -// if (isTrafficLight(traffic_light_id)) { -// lanelet::Ids traffic_light_regulatory_element_ids; -// for (const auto & regulatory_element : LaneletWrapper::map()->regulatoryElementLayer) { -// if ( -// regulatory_element->attribute(lanelet::AttributeName::Subtype).value() == "traffic_light") { -// for (const auto & reference_traffic_light : -// regulatory_element->getParameters("refers")) { -// if (reference_traffic_light.id() == traffic_light_id) { -// traffic_light_regulatory_element_ids.push_back(regulatory_element->id()); -// } -// } -// } -// } -// return traffic_light_regulatory_element_ids; -// } else { -// THROW_SEMANTIC_ERROR(traffic_light_id, " is not traffic light!"); -// } -// } - -// // On path -// 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; -// } - -// 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 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; -// } -// } // namespace traffic_lights -// } // namespace lanelet_wrapper -// } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/utils/lanelet_map.cpp b/simulation/traffic_simulator/src/utils/lanelet_map.cpp index b2655372ccc..e04f122d4a1 100644 --- a/simulation/traffic_simulator/src/utils/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/utils/lanelet_map.cpp @@ -22,100 +22,5 @@ auto laneletLength(const lanelet::Id lanelet_id) -> double { return lanelet_wrapper::lanelet_map::laneletLength(lanelet_id); } - -// auto laneletYaw(const Point & point, const lanelet::Id lanelet_id) -// -> std::tuple -// { -// return lanelet_wrapper::lanelet_map::laneletYaw(lanelet_id, point); -// } - -// auto nearbyLaneletIds( -// const Pose & pose, const double distance_thresh, const bool include_crosswalk, -// const std::size_t search_count) -> lanelet::Ids -// { -// return lanelet_wrapper::lanelet_map::nearbyLaneletIds( -// pose.position, distance_thresh, include_crosswalk, search_count); -// } - -// auto borderlinePoses() -> std::vector> -// { -// std::vector> borderline_poses; -// for (const auto & lanelet_id : lanelet_wrapper::lanelet_map::laneletIds()) { -// if (lanelet_wrapper::lanelet_map::nextLaneletIds(lanelet_id).empty()) { -// LaneletPose lanelet_pose; -// lanelet_pose.lanelet_id = lanelet_id; -// lanelet_pose.s = lanelet_map::laneletLength(lanelet_id); -// borderline_poses.push_back({lanelet_id, pose::toMapPose(lanelet_pose)}); -// } -// } -// return borderline_poses; -// } - -// auto visualizationMarker() -> visualization_msgs::msg::MarkerArray -// { -// visualization_msgs::msg::MarkerArray markers; -// auto insertMarkerArray = []( -// visualization_msgs::msg::MarkerArray & a1, -// const visualization_msgs::msg::MarkerArray & a2) -> void { -// a1.markers.insert(a1.markers.end(), a2.markers.begin(), a2.markers.end()); -// }; - -// lanelet::ConstLanelets all_lanelets = -// lanelet::utils::query::laneletLayer(lanelet_wrapper::LaneletWrapper::map()); -// lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); -// lanelet::ConstLanelets crosswalk_lanelets = -// lanelet::utils::query::crosswalkLanelets(all_lanelets); -// lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); -// lanelet::ConstLineStrings3d stop_lines = lanelet::utils::query::stopLinesLanelets(road_lanelets); -// std::vector aw_tl_reg_elems = -// lanelet::utils::query::autowareTrafficLights(all_lanelets); -// std::vector da_reg_elems = -// lanelet::utils::query::detectionAreas(all_lanelets); -// lanelet::ConstLineStrings3d parking_spaces = -// lanelet::utils::query::getAllParkingSpaces(lanelet_wrapper::LaneletWrapper::map()); -// lanelet::ConstPolygons3d parking_lots = -// lanelet::utils::query::getAllParkingLots(lanelet_wrapper::LaneletWrapper::map()); - -// auto cl_ll_borders = color_utils::fromRgba(1.0, 1.0, 1.0, 0.999); -// auto cl_road = color_utils::fromRgba(0.2, 0.7, 0.7, 0.3); -// auto cl_cross = color_utils::fromRgba(0.2, 0.7, 0.2, 0.3); -// auto cl_stoplines = color_utils::fromRgba(1.0, 0.0, 0.0, 0.5); -// auto cl_trafficlights = color_utils::fromRgba(0.7, 0.7, 0.7, 0.8); -// auto cl_detection_areas = color_utils::fromRgba(0.7, 0.7, 0.7, 0.3); -// auto cl_parking_lots = color_utils::fromRgba(0.7, 0.7, 0.0, 0.3); -// auto cl_parking_spaces = color_utils::fromRgba(1.0, 0.647, 0.0, 0.6); -// auto cl_lanelet_id = color_utils::fromRgba(0.8, 0.2, 0.2, 0.999); - -// insertMarkerArray( -// markers, -// lanelet::visualization::laneletsBoundaryAsMarkerArray(road_lanelets, cl_ll_borders, true)); -// insertMarkerArray( -// markers, -// lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road)); -// insertMarkerArray( -// markers, lanelet::visualization::laneletsAsTriangleMarkerArray( -// "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); -// insertMarkerArray( -// markers, lanelet::visualization::laneletsAsTriangleMarkerArray( -// "walkway_lanelets", walkway_lanelets, cl_cross)); -// insertMarkerArray(markers, lanelet::visualization::laneletDirectionAsMarkerArray(road_lanelets)); -// insertMarkerArray( -// markers, -// lanelet::visualization::lineStringsAsMarkerArray(stop_lines, "stop_lines", cl_stoplines, 0.1)); -// insertMarkerArray( -// markers, -// lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); -// insertMarkerArray( -// markers, lanelet::visualization::detectionAreasAsMarkerArray(da_reg_elems, cl_detection_areas)); -// insertMarkerArray( -// markers, lanelet::visualization::parkingLotsAsMarkerArray(parking_lots, cl_parking_lots)); -// insertMarkerArray( -// markers, lanelet::visualization::parkingSpacesAsMarkerArray(parking_spaces, cl_parking_spaces)); -// insertMarkerArray( -// markers, lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id)); -// insertMarkerArray( -// markers, lanelet::visualization::generateLaneletIdMarker(crosswalk_lanelets, cl_lanelet_id)); -// return markers; -// } } // namespace lanelet_map } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 0a2596a5782..8d73628c447 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include @@ -224,6 +223,7 @@ auto boundingBoxRelativePose( } // Relative LaneletPose +/// @todo HdMapUtils will be removed when lanelet_wrapper::distance is added auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, const traffic_simulator::RoutingConfiguration & routing_configuration, @@ -249,27 +249,7 @@ auto relativeLaneletPose( return position; } -// auto relativeLaneletPose( -// const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, -// const RoutingConfiguration & routing_configuration) -> LaneletPose -// { -// constexpr bool include_adjacent_lanelet{false}; -// constexpr bool include_opposite_direction{true}; - -// LaneletPose position = quietNaNLaneletPose(); -// // 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 = distance::longitudinalDistance( -// from, to, include_adjacent_lanelet, include_opposite_direction, routing_configuration)) { -// position.s = longitudinal_distance.value(); -// } -// 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, @@ -298,33 +278,7 @@ auto boundingBoxRelativeLaneletPose( return position; } -// 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) -> LaneletPose -// { -// constexpr bool include_adjacent_lanelet{false}; -// constexpr bool include_opposite_direction{true}; - -// LaneletPose position = quietNaNLaneletPose(); -// // 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 = distance::boundingBoxLaneLongitudinalDistance( -// from, from_bounding_box, to, to_bounding_box, include_adjacent_lanelet, -// include_opposite_direction, routing_configuration)) { -// position.s = longitudinal_bounding_box_distance.value(); -// } -// if ( -// 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 @@ -358,44 +312,12 @@ auto isInLanelet( return false; } -// auto isInLanelet( -// const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id lanelet_id, -// const double tolerance) -> bool -// { -// constexpr bool include_adjacent_lanelet{false}; -// constexpr bool include_opposite_direction{false}; -// constexpr RoutingConfiguration routing_configuration; - -// if (isSameLaneletId(canonicalized_lanelet_pose, lanelet_id)) { -// return true; -// } else { -// const auto start_lanelet_pose = helper::constructCanonicalizedLaneletPose(lanelet_id, 0.0, 0.0); -// if (const auto distance_to_start_lanelet_pose = distance::longitudinalDistance( -// start_lanelet_pose, canonicalized_lanelet_pose, include_adjacent_lanelet, -// include_opposite_direction, routing_configuration); -// distance_to_start_lanelet_pose and -// std::abs(distance_to_start_lanelet_pose.value()) <= tolerance) { -// return true; -// } - -// 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 = distance::longitudinalDistance( -// canonicalized_lanelet_pose, end_lanelet_pose, include_adjacent_lanelet, -// include_opposite_direction, routing_configuration); -// distance_to_end_lanelet_pose and -// std::abs(distance_to_end_lanelet_pose.value()) <= tolerance) { -// return true; -// } -// } -// return false; -// } - auto isInLanelet(const geometry_msgs::msg::Point & point, const lanelet::Id lanelet_id) -> bool { return lanelet_wrapper::lanelet_map::isInLanelet(lanelet_id, point); } +/// @todo HdMapUtils will be removed when lanelet_wrapper::distance is added auto isAtEndOfLanelets( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const std::shared_ptr & hdmap_utils_ptr) -> bool @@ -405,13 +327,6 @@ auto isAtEndOfLanelets( lanelet_wrapper::lanelet_map::laneletLength(lanelet_pose.lanelet_id) <= lanelet_pose.s; } -// auto isAtEndOfLanelets(const CanonicalizedLaneletPose & canonicalized_lanelet_pose) -> bool -// { -// const auto lanelet_pose = static_cast(canonicalized_lanelet_pose); -// return lanelet_wrapper::route::followingLanelets(lanelet_pose.lanelet_id).size() == 1 && -// lanelet_wrapper::lanelet_map::laneletLength(lanelet_pose.lanelet_id) <= lanelet_pose.s; -// } - namespace pedestrian { /* From cefcf214a795f5b283b86af894f1f52c5fe7f652 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 3 Dec 2024 18:35:29 +0100 Subject: [PATCH 06/62] feat(traffic_simulator, behavior_tree_plugin): use lanelet_wrapper::pose in parts previously overlooked --- .../behavior_tree_plugin/src/action_node.cpp | 101 +++++------------- .../src/vehicle/lane_change_action.cpp | 24 +++-- .../lanelet_wrapper/pose.hpp | 23 ++-- .../src/behavior/follow_trajectory.cpp | 11 +- .../traffic_simulator/src/utils/distance.cpp | 5 +- .../random_test_runner/src/lanelet_utils.cpp | 11 +- .../src/test_randomizer.cpp | 2 +- 7 files changed, 66 insertions(+), 111 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index bf405333f5f..2d5e9f48418 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -407,7 +408,8 @@ auto ActionNode::foundConflictingEntity(const lanelet::Ids & following_lanelets) } auto ActionNode::calculateUpdatedEntityStatus( - double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const + const double target_speed, + const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const -> traffic_simulator::EntityStatus { const auto speed_planner = @@ -420,82 +422,29 @@ auto ActionNode::calculateUpdatedEntityStatus( const double linear_jerk_new = std::get<2>(dynamics); const geometry_msgs::msg::Accel accel_new = std::get<1>(dynamics); const geometry_msgs::msg::Twist twist_new = std::get<0>(dynamics); - - if (!canonicalized_entity_status->laneMatchingSucceed()) { - THROW_SIMULATION_ERROR( - "Entity ", canonicalized_entity_status->getName(), " is not matched to the lanelet."); - } else { - auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); - lanelet_pose.s = - lanelet_pose.s + + if ( + const auto canonicalized_lanelet_pose = + canonicalized_entity_status->getCanonicalizedLaneletPose()) { + const auto distance = (twist_new.linear.x + canonicalized_entity_status->getTwist().linear.x) / 2.0 * step_time; - const auto canonicalized = hdmap_utils->canonicalizeLaneletPose(lanelet_pose, route_lanelets); - if ( - const auto canonicalized_lanelet_pose = - std::get>(canonicalized)) { - // If canonicalize succeed, set canonicalized pose and set other values. - auto entity_status_updated = - static_cast(*canonicalized_entity_status); - { - entity_status_updated.time = current_time + step_time; - entity_status_updated.lanelet_pose = canonicalized_lanelet_pose.value(); - entity_status_updated.lanelet_pose_valid = true; - entity_status_updated.action_status.twist = twist_new; - entity_status_updated.action_status.accel = accel_new; - entity_status_updated.action_status.linear_jerk = linear_jerk_new; - entity_status_updated.pose = - hdmap_utils->toMapPose(canonicalized_lanelet_pose.value()).pose; - } - return entity_status_updated; - } else { - // If canonicalize failed, set end of road lanelet pose. - if (const auto end_of_road_lanelet_id = std::get>(canonicalized)) { - if (lanelet_pose.s < 0) { - traffic_simulator::LaneletPose end_of_road_lanelet_pose; - { - end_of_road_lanelet_pose.lanelet_id = end_of_road_lanelet_id.value(); - end_of_road_lanelet_pose.s = 0; - end_of_road_lanelet_pose.offset = lanelet_pose.offset; - end_of_road_lanelet_pose.rpy = lanelet_pose.rpy; - } - auto entity_status_updated = - static_cast(*canonicalized_entity_status); - { - entity_status_updated.time = current_time + step_time; - entity_status_updated.lanelet_pose = end_of_road_lanelet_pose; - entity_status_updated.lanelet_pose_valid = true; - entity_status_updated.action_status.twist = twist_new; - entity_status_updated.action_status.accel = accel_new; - entity_status_updated.action_status.linear_jerk = linear_jerk_new; - entity_status_updated.pose = hdmap_utils->toMapPose(end_of_road_lanelet_pose).pose; - } - return entity_status_updated; - } else { - traffic_simulator::LaneletPose end_of_road_lanelet_pose; - { - end_of_road_lanelet_pose.lanelet_id = end_of_road_lanelet_id.value(); - end_of_road_lanelet_pose.s = - hdmap_utils->getLaneletLength(end_of_road_lanelet_id.value()); - end_of_road_lanelet_pose.offset = lanelet_pose.offset; - end_of_road_lanelet_pose.rpy = lanelet_pose.rpy; - } - auto entity_status_updated = - static_cast(*canonicalized_entity_status); - { - entity_status_updated.time = current_time + step_time; - entity_status_updated.lanelet_pose = end_of_road_lanelet_pose; - entity_status_updated.lanelet_pose_valid = true; - entity_status_updated.action_status.twist = twist_new; - entity_status_updated.action_status.accel = accel_new; - entity_status_updated.action_status.linear_jerk = linear_jerk_new; - entity_status_updated.pose = hdmap_utils->toMapPose(end_of_road_lanelet_pose).pose; - } - return entity_status_updated; - } - } else { - THROW_SIMULATION_ERROR("Failed to find trailing lanelet_id."); - } - } + auto entity_status_updated = + static_cast(*canonicalized_entity_status); + entity_status_updated.time = current_time + step_time; + entity_status_updated.action_status.twist = twist_new; + entity_status_updated.action_status.accel = accel_new; + entity_status_updated.action_status.linear_jerk = linear_jerk_new; + /// @todo it will be moved to route::moveAlongLaneletPose(...) + entity_status_updated.lanelet_pose = traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + static_cast(canonicalized_lanelet_pose.value()), + route_lanelets, distance); + entity_status_updated.lanelet_pose_valid = true; + entity_status_updated.pose = + traffic_simulator::pose::toMapPose(entity_status_updated.lanelet_pose); + return entity_status_updated; + } else { + THROW_SIMULATION_ERROR( + "Cannot move along lanelet - entity ", std::quoted(canonicalized_entity_status->getName()), + " has invalid lanelet pose."); } } diff --git a/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp index 593915b94b0..0e6cc886761 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include namespace entity_behavior @@ -112,6 +113,7 @@ BT::NodeStatus LaneChangeAction::tick() } std::optional> traj_with_goal; traffic_simulator::LaneletPose along_pose, goal_pose; + /// @todo traffic_simulator::lanelet_wrapper::pose::alongLaneletPose(...) will be moved to traffic_simulator::route::moveAlongLaneletPose(...) switch (lane_change_parameters_->constraint.type) { case traffic_simulator::lane_change::Constraint::Type::NONE: /** @@ -121,27 +123,27 @@ BT::NodeStatus LaneChangeAction::tick() 1.0 is a forward_distance_threshold (If the goal x position in the cartesian coordinate was under 1.0, the goal was rejected.) */ traj_with_goal = hdmap_utils->getLaneChangeTrajectory( - hdmap_utils->toMapPose(lanelet_pose).pose, lane_change_parameters_.value(), 10.0, 20.0, - 1.0); - along_pose = hdmap_utils->getAlongLaneletPose( + traffic_simulator::pose::toMapPose(lanelet_pose), lane_change_parameters_.value(), 10.0, + 20.0, 1.0); + along_pose = traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( lanelet_pose, traffic_simulator::lane_change::Parameter::default_lanechange_distance); break; case traffic_simulator::lane_change::Constraint::Type::LATERAL_VELOCITY: traj_with_goal = hdmap_utils->getLaneChangeTrajectory(lanelet_pose, lane_change_parameters_.value()); - along_pose = hdmap_utils->getAlongLaneletPose( + along_pose = traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( lanelet_pose, traffic_simulator::lane_change::Parameter::default_lanechange_distance); break; case traffic_simulator::lane_change::Constraint::Type::LONGITUDINAL_DISTANCE: traj_with_goal = hdmap_utils->getLaneChangeTrajectory(lanelet_pose, lane_change_parameters_.value()); - along_pose = hdmap_utils->getAlongLaneletPose( + along_pose = traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( lanelet_pose, lane_change_parameters_->constraint.value); break; case traffic_simulator::lane_change::Constraint::Type::TIME: traj_with_goal = hdmap_utils->getLaneChangeTrajectory(lanelet_pose, lane_change_parameters_.value()); - along_pose = hdmap_utils->getAlongLaneletPose( + along_pose = traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( lanelet_pose, lane_change_parameters_->constraint.value); break; } @@ -150,10 +152,10 @@ BT::NodeStatus LaneChangeAction::tick() target_s_ = traj_with_goal->second; goal_pose.lanelet_id = lane_change_parameters_->target.lanelet_id; goal_pose.s = traj_with_goal->second; - double offset = std::fabs( - math::geometry::getRelativePose( - hdmap_utils->toMapPose(along_pose).pose, hdmap_utils->toMapPose(goal_pose).pose) - .position.y); + double offset = std::fabs(math::geometry::getRelativePose( + traffic_simulator::pose::toMapPose(along_pose), + traffic_simulator::pose::toMapPose(goal_pose)) + .position.y); switch (lane_change_parameters_->constraint.type) { case traffic_simulator::lane_change::Constraint::Type::NONE: lane_change_velocity_ = canonicalized_entity_status->getTwist().linear.x; @@ -254,7 +256,7 @@ BT::NodeStatus LaneChangeAction::tick() lanelet_pose.offset = 0; entity_status_updated.lanelet_pose = lanelet_pose; entity_status_updated.lanelet_pose_valid = true; - entity_status_updated.pose = hdmap_utils->toMapPose(lanelet_pose).pose; + entity_status_updated.pose = traffic_simulator::pose::toMapPose(lanelet_pose); entity_status_updated.action_status = canonicalized_entity_status->getActionStatus(); setCanonicalizedEntityStatus(entity_status_updated); return BT::NodeStatus::SUCCESS; diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp index 0dbbdb6a12f..9a8069f4df2 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp @@ -24,6 +24,7 @@ #include #include #include +#include namespace traffic_simulator { @@ -46,26 +47,26 @@ constexpr static double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; auto toMapPose(const LaneletPose & lanelet_pose, const bool fill_pitch = true) -> PoseStamped; auto toLaneletPose( - const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance) + const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance = 1.0) -> std::optional; auto toLaneletPose( - const Pose & map_pose, const lanelet::Ids & lanelet_ids, const double matching_distance) + const Pose & map_pose, const lanelet::Ids & lanelet_ids, const double matching_distance = 1.0) -> std::optional; auto toLaneletPose( - const Pose & map_pose, const bool include_crosswalk, const double matching_distance) + const Pose & map_pose, const bool include_crosswalk, const double matching_distance = 1.0) -> std::optional; auto toLaneletPose( const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk, - const double matching_distance, + const double matching_distance = 1.0, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> std::optional; auto toLaneletPoses( - const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance, - const bool include_opposite_direction, + const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance = 5.0, + const bool include_opposite_direction = true, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> std::vector; @@ -88,18 +89,18 @@ auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose, const lanelet::Id // used only by this namespace auto matchToLane( const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk, - const double matching_distance, + const double matching_distance = 1.0, const double reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> std::optional; auto leftLaneletIds( - const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) - -> lanelet::Ids; + const lanelet::Id lanelet_id, const RoutingGraphType type, + const bool include_opposite_direction = true) -> lanelet::Ids; auto rightLaneletIds( - const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) - -> lanelet::Ids; + const lanelet::Id lanelet_id, const RoutingGraphType type, + const bool include_opposite_direction = true) -> lanelet::Ids; } // namespace pose } // namespace lanelet_wrapper } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index da2ced4710c..92e1ad14b46 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -68,14 +68,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 = - hdmap_utils->toLaneletPose(from, entity_status.bounding_box, false, matching_distance); + if (const auto from_lanelet_pose = pose::toCanonicalizedLaneletPose( + from, entity_status.bounding_box, false, matching_distance); from_lanelet_pose) { - if (const auto to_lanelet_pose = - hdmap_utils->toLaneletPose(to, entity_status.bounding_box, false, matching_distance); + if (const auto to_lanelet_pose = pose::toCanonicalizedLaneletPose( + to, entity_status.bounding_box, false, matching_distance); to_lanelet_pose) { if (const auto distance = hdmap_utils->getLongitudinalDistance( - from_lanelet_pose.value(), to_lanelet_pose.value()); + static_cast(from_lanelet_pose.value()), + static_cast(to_lanelet_pose.value())); distance) { return distance.value(); } diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index d4b579a6042..cb2f1169d70 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -97,12 +98,12 @@ auto longitudinalDistance( */ constexpr double matching_distance = 5.0; - auto from_poses = hdmap_utils_ptr->toLaneletPoses( + auto from_poses = lanelet_wrapper::pose::toLaneletPoses( static_cast(from), static_cast(from).lanelet_id, matching_distance, include_opposite_direction, routing_configuration.routing_graph_type); from_poses.emplace_back(from); - auto to_poses = hdmap_utils_ptr->toLaneletPoses( + auto to_poses = lanelet_wrapper::pose::toLaneletPoses( static_cast(to), static_cast(to).lanelet_id, matching_distance, include_opposite_direction, routing_configuration.routing_graph_type); to_poses.emplace_back(to); diff --git a/test_runner/random_test_runner/src/lanelet_utils.cpp b/test_runner/random_test_runner/src/lanelet_utils.cpp index 80de0a2e6ad..04b189be7e3 100644 --- a/test_runner/random_test_runner/src/lanelet_utils.cpp +++ b/test_runner/random_test_runner/src/lanelet_utils.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include LaneletUtils::LaneletUtils(const boost::filesystem::path & filename) @@ -54,7 +55,7 @@ std::vector LaneletUtils::getLaneletIds() { return hdmap_utils_ptr_->ge geometry_msgs::msg::PoseStamped LaneletUtils::toMapPose( const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose, const bool fill_pitch) { - return hdmap_utils_ptr_->toMapPose(lanelet_pose, fill_pitch); + return traffic_simulator::lanelet_wrapper::pose::toMapPose(lanelet_pose, fill_pitch); } std::vector LaneletUtils::getRoute(int64_t from_lanelet_id, int64_t to_lanelet_id) @@ -64,15 +65,15 @@ std::vector LaneletUtils::getRoute(int64_t from_lanelet_id, int64_t to_ double LaneletUtils::getLaneletLength(int64_t lanelet_id) { - return hdmap_utils_ptr_->getLaneletLength(lanelet_id); + return traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(lanelet_id); } double LaneletUtils::computeDistance( const traffic_simulator_msgs::msg::LaneletPose & p1, const traffic_simulator_msgs::msg::LaneletPose & p2) { - auto p1_g = hdmap_utils_ptr_->toMapPose(p1).pose.position; - auto p2_g = hdmap_utils_ptr_->toMapPose(p2).pose.position; + auto p1_g = traffic_simulator::lanelet_wrapper::pose::toMapPose(p1).pose.position; + auto p2_g = traffic_simulator::lanelet_wrapper::pose::toMapPose(p2).pose.position; geometry_msgs::msg::Point d; d.x = p1_g.x - p2_g.x; d.y = p1_g.y - p2_g.y; @@ -138,7 +139,7 @@ std::optional LaneletUtils::getOpposit global_pose.position.y = opposite_lane_global_position.y; global_pose.position.z = opposite_lane_global_position.z; - return hdmap_utils_ptr_->toLaneletPose(global_pose, false); + return traffic_simulator::lanelet_wrapper::pose::toLaneletPose(global_pose, false, 1.0); } enum SearchDirection { FORWARD, BACKWARD, INVALID }; diff --git a/test_runner/random_test_runner/src/test_randomizer.cpp b/test_runner/random_test_runner/src/test_randomizer.cpp index f47b7ab0746..5ad27362105 100644 --- a/test_runner/random_test_runner/src/test_randomizer.cpp +++ b/test_runner/random_test_runner/src/test_randomizer.cpp @@ -51,7 +51,7 @@ TestDescription TestRandomizer::generate() test_suite_parameters_.ego_goal_lanelet_id, test_suite_parameters_.ego_goal_s, test_suite_parameters_.ego_goal_partial_randomization, test_suite_parameters_.ego_goal_partial_randomization_distance); - ret.ego_goal_pose = lanelet_utils_->toMapPose(ret.ego_goal_position, false).pose; + ret.ego_goal_pose = traffic_simulator::pose::toMapPose(ret.ego_goal_position); std::vector lanelets_around_start = lanelet_utils_->getLanesWithinDistance( ret.ego_start_position, test_suite_parameters_.npc_min_spawn_distance_from_ego, From 8dd8afc3e7d206f2138f39e4763a33d747da0f78 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 3 Dec 2024 18:38:37 +0100 Subject: [PATCH 07/62] feat(traffic_simulator): use lanelet_wrapper::pose in the rest of hdmap_utils, adapt hdmap_utils test to lanelet_wrapper::pose --- .../hdmap_utils/hdmap_utils.hpp | 72 --- .../src/hdmap_utils/hdmap_utils.cpp | 423 +----------------- .../test/src/hdmap_utils/test_hdmap_utils.cpp | 246 +++++----- 3 files changed, 156 insertions(+), 585 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 07dced38f8f..b32788d7a44 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 @@ -71,15 +71,6 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingConfiguration().routing_graph_type) const -> bool; - auto canonicalizeLaneletPose(const traffic_simulator_msgs::msg::LaneletPose &) const - -> std::tuple< - std::optional, std::optional>; - - auto canonicalizeLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose &, const lanelet::Ids & route_lanelets) const - -> std::tuple< - std::optional, std::optional>; - auto clipTrajectoryFromLaneletIds( const lanelet::Id, const double s, const lanelet::Ids &, const double forward_distance = 20) const -> std::vector; @@ -94,15 +85,6 @@ class HdMapUtils auto generateMarker() const -> visualization_msgs::msg::MarkerArray; - auto getAllCanonicalizedLaneletPoses(const traffic_simulator_msgs::msg::LaneletPose &) const - -> std::vector; - - auto getAlongLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose & from, const double along, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> traffic_simulator_msgs::msg::LaneletPose; - auto getCenterPoints(const lanelet::Ids &) const -> std::vector; auto getCenterPoints(const lanelet::Id) const -> std::vector; @@ -202,10 +184,6 @@ class HdMapUtils auto getLeftBound(const lanelet::Id) const -> std::vector; - auto getLeftLaneletIds( - const lanelet::Id, const traffic_simulator::RoutingGraphType, - const bool include_opposite_direction = true) const -> lanelet::Ids; - auto getLongitudinalDistance( const traffic_simulator_msgs::msg::LaneletPose & from_pose, const traffic_simulator_msgs::msg::LaneletPose & to_pose, @@ -267,10 +245,6 @@ class HdMapUtils auto getRightBound(const lanelet::Id) const -> std::vector; - auto getRightLaneletIds( - const lanelet::Id, const traffic_simulator::RoutingGraphType, - const bool include_opposite_direction = true) const -> lanelet::Ids; - auto getRightOfWayLaneletIds(const lanelet::Ids &) const -> std::unordered_map; @@ -329,57 +303,11 @@ class HdMapUtils constexpr static double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; public: - auto matchToLane( - const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, - const bool include_crosswalk, const double matching_distance = 1.0, - const double reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> std::optional; - - auto toLaneletPose( - const geometry_msgs::msg::Pose &, const bool include_crosswalk, - const double matching_distance = 1.0) const - -> std::optional; - - auto toLaneletPose( - const geometry_msgs::msg::Pose &, const lanelet::Ids &, - const double matching_distance = 1.0) const - -> std::optional; - - auto toLaneletPose( - const geometry_msgs::msg::Point &, const traffic_simulator_msgs::msg::BoundingBox &, - const bool include_crosswalk, const double matching_distance = 1.0, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> std::optional; - - auto toLaneletPose( - const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, - const bool include_crosswalk, const double matching_distance = 1.0, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> std::optional; - - auto toLaneletPose( - const geometry_msgs::msg::Pose &, const lanelet::Id, const double matching_distance = 1.0) const - -> std::optional; - - auto toLaneletPoses( - const geometry_msgs::msg::Pose &, const lanelet::Id, const double matching_distance = 5.0, - const bool include_opposite_direction = true, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> std::vector; - auto toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin; auto toMapPoints(const lanelet::Id, const std::vector & s) const -> std::vector; - auto toMapPose(const traffic_simulator_msgs::msg::LaneletPose &, const bool fill_pitch = true) - const -> geometry_msgs::msg::PoseStamped; - private: /** @defgroup cache * Declared mutable for caching diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index b6f9406a8d8..2cb2292311f 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -50,12 +50,15 @@ #include #include #include +#include #include #include #include namespace hdmap_utils { +using namespace traffic_simulator::lanelet_wrapper; + HdMapUtils::HdMapUtils( const boost::filesystem::path & lanelet2_map_path, const geographic_msgs::msg::GeoPoint &) { @@ -79,129 +82,8 @@ HdMapUtils::HdMapUtils( overwriteLaneletsCenterline(); } -auto HdMapUtils::getAllCanonicalizedLaneletPoses( - const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) const - -> std::vector -{ - /// @note Define lambda functions for canonicalizing previous/next lanelet. - const auto canonicalize_to_previous_lanelet = - [this](const auto & lanelet_pose) -> std::vector { - if (const auto ids = getPreviousLaneletIds(lanelet_pose.lanelet_id); !ids.empty()) { - std::vector canonicalized_all; - for (const auto id : ids) { - const auto lanelet_pose_tmp = traffic_simulator::helper::constructLaneletPose( - id, lanelet_pose.s + getLaneletLength(id), lanelet_pose.offset); - if (const auto canonicalized_lanelet_poses = - getAllCanonicalizedLaneletPoses(lanelet_pose_tmp); - canonicalized_lanelet_poses.empty()) { - canonicalized_all.emplace_back(lanelet_pose_tmp); - } else { - std::copy( - canonicalized_lanelet_poses.begin(), canonicalized_lanelet_poses.end(), - std::back_inserter(canonicalized_all)); - } - } - return canonicalized_all; - } else { - return {}; - } - }; - const auto canonicalize_to_next_lanelet = - [this](const auto & lanelet_pose) -> std::vector { - if (const auto ids = getNextLaneletIds(lanelet_pose.lanelet_id); !ids.empty()) { - std::vector canonicalized_all; - for (const auto id : ids) { - const auto lanelet_pose_tmp = traffic_simulator::helper::constructLaneletPose( - id, lanelet_pose.s - getLaneletLength(lanelet_pose.lanelet_id), lanelet_pose.offset); - if (const auto canonicalized_lanelet_poses = - getAllCanonicalizedLaneletPoses(lanelet_pose_tmp); - canonicalized_lanelet_poses.empty()) { - canonicalized_all.emplace_back(lanelet_pose_tmp); - } else { - std::copy( - canonicalized_lanelet_poses.begin(), canonicalized_lanelet_poses.end(), - std::back_inserter(canonicalized_all)); - } - } - return canonicalized_all; - } else { - return {}; - } - }; - - /// @note If s value under 0, it means this pose is on the previous lanelet. - if (lanelet_pose.s < 0) { - return canonicalize_to_previous_lanelet(lanelet_pose); - } - /// @note If s value overs it's lanelet length, it means this pose is on the next lanelet. - else if (lanelet_pose.s > (getLaneletLength(lanelet_pose.lanelet_id))) { - return canonicalize_to_next_lanelet(lanelet_pose); - } - /// @note If s value is in range [0,length_of_the_lanelet], return lanelet_pose. - else { - return {lanelet_pose}; - } -} - // If route is not specified, the lanelet_id with the lowest array index is used as a candidate for // canonicalize destination. -auto HdMapUtils::canonicalizeLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) const - -> std::tuple, std::optional> -{ - auto canonicalized = lanelet_pose; - while (canonicalized.s < 0) { - if (const auto ids = getPreviousLaneletIds(canonicalized.lanelet_id); ids.empty()) { - return {std::nullopt, canonicalized.lanelet_id}; - } else { - canonicalized.s += getLaneletLength(ids[0]); - canonicalized.lanelet_id = ids[0]; - } - } - while (canonicalized.s > getLaneletLength(canonicalized.lanelet_id)) { - if (const auto ids = getNextLaneletIds(canonicalized.lanelet_id); ids.empty()) { - return {std::nullopt, canonicalized.lanelet_id}; - } else { - canonicalized.s -= getLaneletLength(canonicalized.lanelet_id); - canonicalized.lanelet_id = ids[0]; - } - } - return {canonicalized, std::nullopt}; -} - -auto HdMapUtils::canonicalizeLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose, - const lanelet::Ids & route_lanelets) const - -> std::tuple, std::optional> -{ - auto canonicalized = lanelet_pose; - while (canonicalized.s < 0) { - // When canonicalizing to backward lanelet_id, do not consider route - if (const auto ids = getPreviousLaneletIds(canonicalized.lanelet_id); ids.empty()) { - return {std::nullopt, canonicalized.lanelet_id}; - } else { - canonicalized.s += getLaneletLength(ids[0]); - canonicalized.lanelet_id = ids[0]; - } - } - while (canonicalized.s > getLaneletLength(canonicalized.lanelet_id)) { - bool next_lanelet_found = false; - // When canonicalizing to forward lanelet_id, consider route - for (const auto id : getNextLaneletIds(canonicalized.lanelet_id)) { - if (std::any_of(route_lanelets.begin(), route_lanelets.end(), [id](auto id_on_route) { - return id == id_on_route; - })) { - canonicalized.s -= getLaneletLength(canonicalized.lanelet_id); - canonicalized.lanelet_id = id; - next_lanelet_found = true; - } - } - if (!next_lanelet_found) { - return {std::nullopt, canonicalized.lanelet_id}; - } - } - return {canonicalized, std::nullopt}; -} auto HdMapUtils::countLaneChanges( const traffic_simulator_msgs::msg::LaneletPose & from, @@ -220,11 +102,11 @@ auto HdMapUtils::countLaneChanges( if (auto followings = getNextLaneletIds(previous, routing_configuration.routing_graph_type); std::find(followings.begin(), followings.end(), current) == followings.end()) { - if (auto lefts = getLeftLaneletIds(previous, routing_configuration.routing_graph_type); + if (auto lefts = pose::leftLaneletIds(previous, routing_configuration.routing_graph_type); std::find(lefts.begin(), lefts.end(), current) != lefts.end()) { lane_changes.first++; } else if (auto rights = - getRightLaneletIds(previous, routing_configuration.routing_graph_type); + pose::rightLaneletIds(previous, routing_configuration.routing_graph_type); std::find(rights.begin(), rights.end(), current) != rights.end()) { lane_changes.second++; } @@ -335,7 +217,7 @@ auto HdMapUtils::getNearbyLaneletIds( auto HdMapUtils::getHeight(const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) const -> double { - return toMapPose(lanelet_pose).pose.position.z; + return pose::toMapPose(lanelet_pose).pose.position.z; } auto HdMapUtils::getCollisionPointInLaneCoordinate( @@ -438,7 +320,7 @@ auto HdMapUtils::clipTrajectoryFromLaneletIds( if (rest_distance < l) { for (double s_val = 0; s_val < rest_distance; s_val = s_val + 1.0) { ret.emplace_back( - toMapPose(traffic_simulator::helper::constructLaneletPose(*id_itr, s_val, 0)) + pose::toMapPose(traffic_simulator::helper::constructLaneletPose(*id_itr, s_val, 0)) .pose.position); } break; @@ -446,7 +328,7 @@ auto HdMapUtils::clipTrajectoryFromLaneletIds( rest_distance = rest_distance - l; for (double s_val = 0; s_val < l; s_val = s_val + 1.0) { ret.emplace_back( - toMapPose(traffic_simulator::helper::constructLaneletPose(*id_itr, s_val, 0.0)) + pose::toMapPose(traffic_simulator::helper::constructLaneletPose(*id_itr, s_val, 0.0)) .pose.position); } continue; @@ -457,7 +339,7 @@ auto HdMapUtils::clipTrajectoryFromLaneletIds( if ((s + forward_distance) < l) { for (double s_val = s; s_val < s + forward_distance; s_val = s_val + 1.0) { ret.emplace_back( - toMapPose(traffic_simulator::helper::constructLaneletPose(lanelet_id, s_val, 0.0)) + pose::toMapPose(traffic_simulator::helper::constructLaneletPose(lanelet_id, s_val, 0.0)) .pose.position); } break; @@ -465,7 +347,7 @@ auto HdMapUtils::clipTrajectoryFromLaneletIds( rest_distance = rest_distance - (l - s); for (double s_val = s; s_val < l; s_val = s_val + 1.0) { ret.emplace_back( - toMapPose(traffic_simulator::helper::constructLaneletPose(lanelet_id, s_val, 0.0)) + pose::toMapPose(traffic_simulator::helper::constructLaneletPose(lanelet_id, s_val, 0.0)) .pose.position); } continue; @@ -523,175 +405,6 @@ auto HdMapUtils::toPoint2d(const geometry_msgs::msg::Point & point) const -> lan return lanelet::BasicPoint2d{point.x, point.y}; } -auto HdMapUtils::matchToLane( - const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, - const bool include_crosswalk, const double matching_distance, const double reduction_ratio, - const traffic_simulator::RoutingGraphType type) const -> std::optional -{ - lanelet::matching::Object2d obj; - obj.pose.translation() = toPoint2d(pose.position); - obj.pose.linear() = - Eigen::Rotation2D(math::geometry::convertQuaternionToEulerAngle(pose.orientation).z) - .matrix(); - obj.absoluteHull = absoluteHull( - lanelet::matching::Hull2d{ - lanelet::BasicPoint2d{ - bbox.center.x + bbox.dimensions.x * 0.5 * reduction_ratio, - bbox.center.y + bbox.dimensions.y * 0.5 * reduction_ratio}, - lanelet::BasicPoint2d{ - bbox.center.x - bbox.dimensions.x * 0.5 * reduction_ratio, - bbox.center.y - bbox.dimensions.y * 0.5 * reduction_ratio}}, - obj.pose); - auto matches = - lanelet::matching::getDeterministicMatches(*lanelet_map_ptr_, obj, matching_distance); - if (!include_crosswalk) { - matches = lanelet::matching::removeNonRuleCompliantMatches( - matches, routing_graphs_->traffic_rule(type)); - } - if (matches.empty()) { - return std::nullopt; - } - std::vector> id_and_distance; - for (const auto & match : matches) { - if (const auto lanelet_pose = toLaneletPose(pose, match.lanelet.id(), matching_distance)) { - id_and_distance.emplace_back(lanelet_pose->lanelet_id, lanelet_pose->offset); - } - } - if (id_and_distance.empty()) { - return std::nullopt; - } - const auto min_id_and_distance = std::min_element( - id_and_distance.begin(), id_and_distance.end(), - [](auto const & lhs, auto const & rhs) { return lhs.second < rhs.second; }); - return min_id_and_distance->first; -} - -auto HdMapUtils::toLaneletPose( - const geometry_msgs::msg::Pose & pose, const bool include_crosswalk, - const double matching_distance) const -> std::optional -{ - const auto lanelet_ids = getNearbyLaneletIds(pose.position, 0.1, include_crosswalk); - if (lanelet_ids.empty()) { - return std::nullopt; - } - for (const auto & id : lanelet_ids) { - const auto lanelet_pose = toLaneletPose(pose, id, matching_distance); - if (lanelet_pose) { - return lanelet_pose; - } - } - return std::nullopt; -} - -auto HdMapUtils::toLaneletPose( - const geometry_msgs::msg::Pose & pose, const lanelet::Id lanelet_id, - const double matching_distance) const -> std::optional -{ - const auto spline = getCenterPointsSpline(lanelet_id); - const auto s = spline->getSValue(pose, matching_distance); - if (!s) { - return std::nullopt; - } - auto pose_on_centerline = spline->getPose(s.value()); - auto rpy = math::geometry::convertQuaternionToEulerAngle( - math::geometry::getRotation(pose_on_centerline.orientation, pose.orientation)); - double offset = std::sqrt(spline->getSquaredDistanceIn2D(pose.position, s.value())); - /** - * @note Hard coded parameter - */ - double yaw_threshold = 0.25; - if (M_PI * yaw_threshold < std::fabs(rpy.z) && std::fabs(rpy.z) < M_PI * (1 - yaw_threshold)) { - return std::nullopt; - } - double inner_prod = math::geometry::innerProduct( - spline->getNormalVector(s.value()), spline->getSquaredDistanceVector(pose.position, s.value())); - if (inner_prod < 0) { - offset = offset * -1; - } - traffic_simulator_msgs::msg::LaneletPose lanelet_pose; - lanelet_pose.lanelet_id = lanelet_id; - lanelet_pose.s = s.value(); - lanelet_pose.offset = offset; - lanelet_pose.rpy = rpy; - return lanelet_pose; -} - -auto HdMapUtils::toLaneletPose( - const geometry_msgs::msg::Pose & pose, const lanelet::Ids & lanelet_ids, - const double matching_distance) const -> std::optional -{ - for (const auto id : lanelet_ids) { - if (const auto lanelet_pose = toLaneletPose(pose, id, matching_distance); lanelet_pose) { - return lanelet_pose.value(); - } - } - return std::nullopt; -} - -auto HdMapUtils::toLaneletPose( - const geometry_msgs::msg::Point & position, const traffic_simulator_msgs::msg::BoundingBox & bbox, - const bool include_crosswalk, const double matching_distance, - const traffic_simulator::RoutingGraphType type) const - -> std::optional -{ - return toLaneletPose( - geometry_msgs::build().position(position).orientation( - geometry_msgs::build().x(0).y(0).z(0).w(1)), - bbox, include_crosswalk, matching_distance, type); -} - -auto HdMapUtils::toLaneletPose( - const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, - const bool include_crosswalk, const double matching_distance, - const traffic_simulator::RoutingGraphType type) const - -> std::optional -{ - const auto lanelet_id = matchToLane( - pose, bbox, include_crosswalk, matching_distance, DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, type); - if (!lanelet_id) { - return toLaneletPose(pose, include_crosswalk, matching_distance); - } - const auto pose_in_target_lanelet = toLaneletPose(pose, lanelet_id.value(), matching_distance); - if (pose_in_target_lanelet) { - return pose_in_target_lanelet; - } - const auto previous = getPreviousLaneletIds(lanelet_id.value(), type); - for (const auto id : previous) { - const auto pose_in_previous = toLaneletPose(pose, id, matching_distance); - if (pose_in_previous) { - return pose_in_previous; - } - } - const auto next = getNextLaneletIds(lanelet_id.value(), type); - for (const auto id : previous) { - const auto pose_in_next = toLaneletPose(pose, id, matching_distance); - if (pose_in_next) { - return pose_in_next; - } - } - return toLaneletPose(pose, include_crosswalk, matching_distance); -} - -auto HdMapUtils::toLaneletPoses( - const geometry_msgs::msg::Pose & pose, const lanelet::Id lanelet_id, - const double matching_distance, const bool include_opposite_direction, - const traffic_simulator::RoutingGraphType type) const - -> std::vector -{ - std::vector ret; - std::vector lanelet_ids = {lanelet_id}; - lanelet_ids += getLeftLaneletIds(lanelet_id, type, include_opposite_direction); - lanelet_ids += getRightLaneletIds(lanelet_id, type, include_opposite_direction); - lanelet_ids += getPreviousLaneletIds(lanelet_ids, type); - lanelet_ids += getNextLaneletIds(lanelet_ids, type); - for (const auto & id : sortAndUnique(lanelet_ids)) { - if (const auto & lanelet_pose = toLaneletPose(pose, id, matching_distance)) { - ret.emplace_back(lanelet_pose.value()); - } - } - return ret; -} - auto HdMapUtils::getClosestLaneletId( const geometry_msgs::msg::Pose & pose, const double distance_thresh, const bool include_crosswalk) const -> std::optional @@ -1128,44 +841,6 @@ auto HdMapUtils::getTrafficLightBulbPosition( return std::nullopt; } -auto HdMapUtils::getAlongLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose & from_pose, const double along, - const traffic_simulator::RoutingGraphType type) const -> traffic_simulator_msgs::msg::LaneletPose -{ - traffic_simulator_msgs::msg::LaneletPose along_pose = from_pose; - along_pose.s = along_pose.s + along; - if (along_pose.s >= 0) { - while (along_pose.s >= getLaneletLength(along_pose.lanelet_id)) { - auto next_ids = getNextLaneletIds(along_pose.lanelet_id, "straight", type); - if (next_ids.empty()) { - next_ids = getNextLaneletIds(along_pose.lanelet_id, type); - if (next_ids.empty()) { - THROW_SEMANTIC_ERROR( - "failed to calculate along pose (id,s) = (", from_pose.lanelet_id, ",", - from_pose.s + along, "), next lanelet of id = ", along_pose.lanelet_id, "is empty."); - } - } - along_pose.s = along_pose.s - getLaneletLength(along_pose.lanelet_id); - along_pose.lanelet_id = next_ids[0]; - } - } else { - while (along_pose.s < 0) { - auto previous_ids = getPreviousLaneletIds(along_pose.lanelet_id, "straight", type); - if (previous_ids.empty()) { - previous_ids = getPreviousLaneletIds(along_pose.lanelet_id, type); - if (previous_ids.empty()) { - THROW_SEMANTIC_ERROR( - "failed to calculate along pose (id,s) = (", from_pose.lanelet_id, ",", - from_pose.s + along, "), next lanelet of id = ", along_pose.lanelet_id, "is empty."); - } - } - along_pose.s = along_pose.s + getLaneletLength(previous_ids[0]); - along_pose.lanelet_id = previous_ids[0]; - } - } - return along_pose; -} - auto HdMapUtils::getLeftBound(const lanelet::Id lanelet_id) const -> std::vector { @@ -1178,32 +853,6 @@ auto HdMapUtils::getRightBound(const lanelet::Id lanelet_id) const return toPolygon(lanelet_map_ptr_->laneletLayer.get(lanelet_id).rightBound()); } -auto HdMapUtils::getLeftLaneletIds( - const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type, - const bool include_opposite_direction) const -> lanelet::Ids -{ - if (include_opposite_direction) { - return getLaneletIds( - routing_graphs_->routing_graph(type)->lefts(lanelet_map_ptr_->laneletLayer.get(lanelet_id))); - } else { - return getLaneletIds(routing_graphs_->routing_graph(type)->adjacentLefts( - lanelet_map_ptr_->laneletLayer.get(lanelet_id))); - } -} - -auto HdMapUtils::getRightLaneletIds( - const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type, - const bool include_opposite_direction) const -> lanelet::Ids -{ - if (include_opposite_direction) { - return getLaneletIds( - routing_graphs_->routing_graph(type)->rights(lanelet_map_ptr_->laneletLayer.get(lanelet_id))); - } else { - return getLaneletIds(routing_graphs_->routing_graph(type)->adjacentRights( - lanelet_map_ptr_->laneletLayer.get(lanelet_id))); - } -} - auto HdMapUtils::getLaneChangeTrajectory( const traffic_simulator_msgs::msg::LaneletPose & from_pose, const traffic_simulator::lane_change::Parameter & lane_change_parameter) const @@ -1228,13 +877,13 @@ auto HdMapUtils::getLaneChangeTrajectory( traffic_simulator::lane_change::Parameter::default_lanechange_distance; break; } - const auto along_pose = getAlongLaneletPose(from_pose, longitudinal_distance); + const auto along_pose = pose::alongLaneletPose(from_pose, longitudinal_distance); // clang-format off const auto left_point = - toMapPose(traffic_simulator::helper::constructLaneletPose( + pose::toMapPose(traffic_simulator::helper::constructLaneletPose( along_pose.lanelet_id, along_pose.s, along_pose.offset + 5.0)).pose.position; const auto right_point = - toMapPose(traffic_simulator::helper::constructLaneletPose( + pose::toMapPose(traffic_simulator::helper::constructLaneletPose( along_pose.lanelet_id, along_pose.s, along_pose.offset - 5.0)).pose.position; // clang-format on const auto collision_point = getCenterPointsSpline(lane_change_parameter.target.lanelet_id) @@ -1245,15 +894,15 @@ auto HdMapUtils::getLaneChangeTrajectory( const auto to_pose = traffic_simulator::helper::constructLaneletPose( lane_change_parameter.target.lanelet_id, collision_point.value(), lane_change_parameter.target.offset); - const auto goal_pose_in_map = toMapPose(to_pose).pose; - const auto from_pose_in_map = toMapPose(from_pose).pose; + const auto goal_pose_in_map = pose::toMapPose(to_pose).pose; + const auto from_pose_in_map = pose::toMapPose(from_pose).pose; double start_to_goal_distance = std::sqrt( std::pow(from_pose_in_map.position.x - goal_pose_in_map.position.x, 2) + std::pow(from_pose_in_map.position.y - goal_pose_in_map.position.y, 2) + std::pow(from_pose_in_map.position.z - goal_pose_in_map.position.z, 2)); auto traj = getLaneChangeTrajectory( - toMapPose(from_pose).pose, to_pose, lane_change_parameter.trajectory_shape, + pose::toMapPose(from_pose).pose, to_pose, lane_change_parameter.trajectory_shape, start_to_goal_distance * 0.5); return std::make_pair(traj, collision_point.value()); } @@ -1270,7 +919,7 @@ auto HdMapUtils::getLaneChangeTrajectory( std::vector curves; for (double to_s = 0; to_s < to_length; to_s = to_s + 1.0) { - auto goal_pose = toMapPose(traffic_simulator::helper::constructLaneletPose( + auto goal_pose = pose::toMapPose(traffic_simulator::helper::constructLaneletPose( lane_change_parameter.target.lanelet_id, to_s)); if ( math::geometry::getRelativePose(from_pose, goal_pose.pose).position.x <= @@ -1309,7 +958,7 @@ auto HdMapUtils::getLaneChangeTrajectory( { geometry_msgs::msg::Vector3 start_vec; geometry_msgs::msg::Vector3 to_vec; - geometry_msgs::msg::Pose goal_pose = toMapPose(to_pose).pose; + geometry_msgs::msg::Pose goal_pose = pose::toMapPose(to_pose).pose; double tangent_vector_size_in_curve = 0.0; switch (trajectory_shape) { case traffic_simulator::lane_change::TrajectoryShape::CUBIC: @@ -1366,38 +1015,6 @@ auto HdMapUtils::toMapPoints(const lanelet::Id lanelet_id, const std::vector geometry_msgs::msg::PoseStamped -{ - using math::geometry::operator*; - using math::geometry::operator+=; - if ( - const auto pose = std::get>( - canonicalizeLaneletPose(lanelet_pose))) { - geometry_msgs::msg::PoseStamped ret; - ret.header.frame_id = "map"; - const auto spline = getCenterPointsSpline(pose->lanelet_id); - ret.pose = spline->getPose(pose->s); - const auto normal_vec = spline->getNormalVector(pose->s); - const auto diff = math::geometry::normalize(normal_vec) * pose->offset; //this - ret.pose.position += diff; - const auto tangent_vec = spline->getTangentVector(pose->s); - geometry_msgs::msg::Vector3 rpy; - rpy.x = 0.0; - rpy.y = fill_pitch ? std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)) : 0.0; - rpy.z = std::atan2(tangent_vec.y, tangent_vec.x); - ret.pose.orientation = math::geometry::convertEulerAngleToQuaternion(rpy) * - math::geometry::convertEulerAngleToQuaternion(pose->rpy); - return ret; - } else { - THROW_SEMANTIC_ERROR( - "Lanelet pose (id=", lanelet_pose.lanelet_id, ",s=", lanelet_pose.s, - ",offset=", lanelet_pose.offset, ",rpy.x=", lanelet_pose.rpy.x, ",rpy.y=", lanelet_pose.rpy.y, - ",rpy.z=", lanelet_pose.rpy.z, ") is invalid, please check lanelet length and connection."); - } -} - auto HdMapUtils::getTangentVector(const lanelet::Id lanelet_id, const double s) const -> std::optional { @@ -1438,14 +1055,14 @@ auto HdMapUtils::getLateralDistance( if ( auto next_lanelet_origin_from_current_lanelet = - toLaneletPose(toMapPose(next_lanelet_pose).pose, route[i], 10.0)) { + 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 = - toLaneletPose(toMapPose(current_lanelet_pose).pose, route[i + 1], 10.0)) { + 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; 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 eaa36c849a4..f7b931f156d 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 "../expect_eq_macros.hpp" #include "../helper_functions.hpp" @@ -189,16 +190,20 @@ TEST_F(HdMapUtilsTest_StandardMap, matchToLane) { const auto bbox = makeSmallBoundingBox(); { - const auto id = hdmap_utils.matchToLane( - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(120659, 1)).pose, bbox, - false); + const auto id = traffic_simulator::lanelet_wrapper::pose::matchToLane( + traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(120659, 1)) + .pose, + bbox, false); EXPECT_TRUE(id); EXPECT_EQ(id.value(), 120659); } { - const auto id = hdmap_utils.matchToLane( - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34411, 1)).pose, bbox, - false); + const auto id = traffic_simulator::lanelet_wrapper::pose::matchToLane( + traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34411, 1)) + .pose, + bbox, false); EXPECT_TRUE(id); EXPECT_EQ(id.value(), 34411); } @@ -213,16 +218,20 @@ TEST_F(HdMapUtilsTest_StandardMap, matchToLane_includeCrosswalk) { auto bbox = makeSmallBoundingBox(); { - const auto id = hdmap_utils.matchToLane( - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34399, 1)).pose, bbox, - true); + const auto id = traffic_simulator::lanelet_wrapper::pose::matchToLane( + traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34399, 1)) + .pose, + bbox, true); EXPECT_TRUE(id.has_value()); EXPECT_EQ(id.value(), 34399); } { - const auto id = hdmap_utils.matchToLane( - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34399, 1)).pose, bbox, - false); + const auto id = traffic_simulator::lanelet_wrapper::pose::matchToLane( + traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34399, 1)) + .pose, + bbox, false); if (id.has_value()) { EXPECT_NE(id.value(), 34399); } @@ -240,15 +249,19 @@ TEST_F(HdMapUtilsTest_StandardMap, matchToLane_noMatch) { auto bbox = makeSmallBoundingBox(); { - const auto id = hdmap_utils.matchToLane( - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34392, 0)).pose, bbox, - false); + const auto id = traffic_simulator::lanelet_wrapper::pose::matchToLane( + traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34392, 0)) + .pose, + bbox, false); EXPECT_FALSE(id.has_value()); } { - const auto id = hdmap_utils.matchToLane( - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34378, 0)).pose, bbox, - false); + const auto id = traffic_simulator::lanelet_wrapper::pose::matchToLane( + traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34378, 0)) + .pose, + bbox, false); EXPECT_FALSE(id.has_value()); } } @@ -261,11 +274,13 @@ TEST_F(HdMapUtilsTest_StandardMap, matchToLane_noMatch) TEST_F(HdMapUtilsTest_StandardMap, AlongLaneletPose_insideDistance) { EXPECT_DOUBLE_EQ( - hdmap_utils.getAlongLaneletPose(traffic_simulator::helper::constructLaneletPose(34513, 0), 30.0) + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + traffic_simulator::helper::constructLaneletPose(34513, 0), 30.0) .s, 30.0); EXPECT_EQ( - hdmap_utils.getAlongLaneletPose(traffic_simulator::helper::constructLaneletPose(34513, 0), 30.0) + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + traffic_simulator::helper::constructLaneletPose(34513, 0), 30.0) .lanelet_id, 34513); } @@ -279,14 +294,14 @@ TEST_F(HdMapUtilsTest_StandardMap, AlongLaneletPose_insideDistance) TEST_F(HdMapUtilsTest_StandardMap, AlongLaneletPose_outsideDistance) { EXPECT_EQ( - hdmap_utils.getAlongLaneletPose(traffic_simulator::helper::constructLaneletPose(34513, 0), 30) + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + traffic_simulator::helper::constructLaneletPose(34513, 0), 30) .lanelet_id, 34513); EXPECT_EQ( - hdmap_utils - .getAlongLaneletPose( - traffic_simulator::helper::constructLaneletPose(34513, 0), - hdmap_utils.getLaneletLength(34513) + 10.0) + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + traffic_simulator::helper::constructLaneletPose(34513, 0), + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34513) + 10.0) .lanelet_id, 34510); } @@ -300,15 +315,15 @@ TEST_F(HdMapUtilsTest_StandardMap, AlongLaneletPose_outsideDistance) TEST_F(HdMapUtilsTest_StandardMap, AlongLaneletPose_negativeDistance) { EXPECT_EQ( - hdmap_utils - .getAlongLaneletPose(traffic_simulator::helper::constructLaneletPose(34513, 0), -10.0) + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + traffic_simulator::helper::constructLaneletPose(34513, 0), -10.0) .lanelet_id, 34684); EXPECT_DOUBLE_EQ( - hdmap_utils - .getAlongLaneletPose(traffic_simulator::helper::constructLaneletPose(34513, 0), -10.0) + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + traffic_simulator::helper::constructLaneletPose(34513, 0), -10.0) .s, - hdmap_utils.getLaneletLength(34684) - 10.0); + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34684) - 10.0); } /** @@ -319,7 +334,7 @@ TEST_F(HdMapUtilsTest_StandardMap, AlongLaneletPose_negativeDistance) TEST_F(HdMapUtilsTest_FourTrackHighwayMap, AlongLaneletPose_afterLast) { EXPECT_THROW( - hdmap_utils.getAlongLaneletPose( + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( traffic_simulator::helper::constructLaneletPose(206, 15.0), 30.0), common::SemanticError); } @@ -332,7 +347,7 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, AlongLaneletPose_afterLast) TEST_F(HdMapUtilsTest_FourTrackHighwayMap, AlongLaneletPose_beforeFirst) { EXPECT_THROW( - hdmap_utils.getAlongLaneletPose( + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( traffic_simulator::helper::constructLaneletPose(3002178, 15.0), -30.0), common::SemanticError); } @@ -347,15 +362,16 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, AlongLaneletPose_beforeFirst) TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeNegative) { double non_canonicalized_lanelet_s = -22.0; - const auto canonicalized_lanelet_pose = - std::get>(hdmap_utils.canonicalizeLaneletPose( + const auto canonicalized_lanelet_pose = std::get>( + traffic_simulator::lanelet_wrapper::pose::canonicalizeLaneletPose( traffic_simulator::helper::constructLaneletPose(34564, non_canonicalized_lanelet_s))); EXPECT_EQ(canonicalized_lanelet_pose.value().lanelet_id, 34576); EXPECT_EQ( - canonicalized_lanelet_pose.value().s, non_canonicalized_lanelet_s + - hdmap_utils.getLaneletLength(34570) + - hdmap_utils.getLaneletLength(34576)); + canonicalized_lanelet_pose.value().s, + non_canonicalized_lanelet_s + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34570) + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34576)); } /** @@ -368,15 +384,16 @@ TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeNegative) TEST_F(HdMapUtilsTest_StandardMap, CanonicalizePositive) { double non_canonicalized_lanelet_s = 30.0; - const auto canonicalized_lanelet_pose = - std::get>(hdmap_utils.canonicalizeLaneletPose( + const auto canonicalized_lanelet_pose = std::get>( + traffic_simulator::lanelet_wrapper::pose::canonicalizeLaneletPose( traffic_simulator::helper::constructLaneletPose(34981, non_canonicalized_lanelet_s))); EXPECT_EQ(canonicalized_lanelet_pose.value().lanelet_id, 34579); EXPECT_EQ( - canonicalized_lanelet_pose.value().s, non_canonicalized_lanelet_s - - hdmap_utils.getLaneletLength(34585) - - hdmap_utils.getLaneletLength(34981)); + canonicalized_lanelet_pose.value().s, + non_canonicalized_lanelet_s - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34585) - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34981)); } /** @@ -387,8 +404,8 @@ TEST_F(HdMapUtilsTest_StandardMap, CanonicalizePositive) TEST_F(HdMapUtilsTest_StandardMap, Canonicalize) { const double non_canonicalized_lanelet_s = 2.0; - const auto canonicalized_lanelet_pose = - std::get>(hdmap_utils.canonicalizeLaneletPose( + const auto canonicalized_lanelet_pose = std::get>( + traffic_simulator::lanelet_wrapper::pose::canonicalizeLaneletPose( traffic_simulator::helper::constructLaneletPose(34981, non_canonicalized_lanelet_s))); EXPECT_EQ(canonicalized_lanelet_pose.value().lanelet_id, 34981); @@ -409,25 +426,29 @@ TEST_F(HdMapUtilsTest_StandardMap, Canonicalize) TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeAllNegative) { const double non_canonicalized_lanelet_s = -22.0; - const auto canonicalized_lanelet_poses = hdmap_utils.getAllCanonicalizedLaneletPoses( - traffic_simulator::helper::constructLaneletPose(34564, non_canonicalized_lanelet_s)); + const auto canonicalized_lanelet_poses = + traffic_simulator::lanelet_wrapper::pose::alternativeLaneletPoses( + traffic_simulator::helper::constructLaneletPose(34564, non_canonicalized_lanelet_s)); EXPECT_EQ(canonicalized_lanelet_poses.size(), static_cast(3)); EXPECT_EQ(canonicalized_lanelet_poses[0].lanelet_id, 34576); EXPECT_EQ( - canonicalized_lanelet_poses[0].s, non_canonicalized_lanelet_s + - hdmap_utils.getLaneletLength(34570) + - hdmap_utils.getLaneletLength(34576)); + canonicalized_lanelet_poses[0].s, + non_canonicalized_lanelet_s + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34570) + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34576)); EXPECT_EQ(canonicalized_lanelet_poses[1].lanelet_id, 34981); EXPECT_EQ( - canonicalized_lanelet_poses[1].s, non_canonicalized_lanelet_s + - hdmap_utils.getLaneletLength(34636) + - hdmap_utils.getLaneletLength(34981)); + canonicalized_lanelet_poses[1].s, + non_canonicalized_lanelet_s + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34636) + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34981)); EXPECT_EQ(canonicalized_lanelet_poses[2].lanelet_id, 34600); EXPECT_EQ( - canonicalized_lanelet_poses[2].s, non_canonicalized_lanelet_s + - hdmap_utils.getLaneletLength(34648) + - hdmap_utils.getLaneletLength(34600)); + canonicalized_lanelet_poses[2].s, + non_canonicalized_lanelet_s + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34648) + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34600)); } /** @@ -444,25 +465,29 @@ TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeAllNegative) TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeAllPositive) { const double non_canonicalized_lanelet_s = 30.0; - const auto canonicalized_lanelet_poses = hdmap_utils.getAllCanonicalizedLaneletPoses( - traffic_simulator::helper::constructLaneletPose(34981, non_canonicalized_lanelet_s)); + const auto canonicalized_lanelet_poses = + traffic_simulator::lanelet_wrapper::pose::alternativeLaneletPoses( + traffic_simulator::helper::constructLaneletPose(34981, non_canonicalized_lanelet_s)); EXPECT_EQ(canonicalized_lanelet_poses.size(), static_cast(3)); EXPECT_EQ(canonicalized_lanelet_poses[0].lanelet_id, 34579); EXPECT_EQ( - canonicalized_lanelet_poses[0].s, non_canonicalized_lanelet_s - - hdmap_utils.getLaneletLength(34585) - - hdmap_utils.getLaneletLength(34981)); + canonicalized_lanelet_poses[0].s, + non_canonicalized_lanelet_s - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34585) - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34981)); EXPECT_EQ(canonicalized_lanelet_poses[1].lanelet_id, 34564); EXPECT_EQ( - canonicalized_lanelet_poses[1].s, non_canonicalized_lanelet_s - - hdmap_utils.getLaneletLength(34636) - - hdmap_utils.getLaneletLength(34981)); + canonicalized_lanelet_poses[1].s, + non_canonicalized_lanelet_s - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34636) - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34981)); EXPECT_EQ(canonicalized_lanelet_poses[2].lanelet_id, 34630); EXPECT_EQ( - canonicalized_lanelet_poses[2].s, non_canonicalized_lanelet_s - - hdmap_utils.getLaneletLength(34651) - - hdmap_utils.getLaneletLength(34981)); + canonicalized_lanelet_poses[2].s, + non_canonicalized_lanelet_s - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34651) - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34981)); } /** @@ -473,8 +498,9 @@ TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeAllPositive) TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeAll) { const double non_canonicalized_lanelet_s = 2.0; - const auto canonicalized_lanelet_poses = hdmap_utils.getAllCanonicalizedLaneletPoses( - traffic_simulator::helper::constructLaneletPose(34981, non_canonicalized_lanelet_s)); + const auto canonicalized_lanelet_poses = + traffic_simulator::lanelet_wrapper::pose::alternativeLaneletPoses( + traffic_simulator::helper::constructLaneletPose(34981, non_canonicalized_lanelet_s)); EXPECT_EQ(canonicalized_lanelet_poses.size(), static_cast(1)); EXPECT_EQ(canonicalized_lanelet_poses[0].lanelet_id, 34981); @@ -664,7 +690,7 @@ TEST_F(HdMapUtilsTest_StandardMap, getCollisionPointInLaneCoordinate_invalidCros */ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_correct) { - const auto lanelet_pose = hdmap_utils.toLaneletPose( + const auto lanelet_pose = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(3790.0, 73757.0), makeQuaternionFromYaw(M_PI + M_PI_2 / 3.0)), false); // angle to make pose aligned with the lanelet @@ -702,7 +728,7 @@ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_negativeOffset) 73757.0 + std::sin(offset_yaw) * std::abs(offset)), makeQuaternionFromYaw(yaw)); - const auto lanelet_pose = hdmap_utils.toLaneletPose(pose, false); + const auto lanelet_pose = traffic_simulator::lanelet_wrapper::pose::toLaneletPose(pose, false); const auto reference_lanelet_pose = traffic_simulator_msgs::build() @@ -724,9 +750,8 @@ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_negativeOffset) TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_reverse) { EXPECT_FALSE( - hdmap_utils - .toLaneletPose( - makePose(makePoint(3790.0, 73757.0), makeQuaternionFromYaw(M_PI_2 + M_PI_2 / 3.0)), false) + traffic_simulator::lanelet_wrapper::pose::toLaneletPose( + makePose(makePoint(3790.0, 73757.0), makeQuaternionFromYaw(M_PI_2 + M_PI_2 / 3.0)), false) .has_value()); // angle to make pose reverse aligned with the lanelet } @@ -737,11 +762,9 @@ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_reverse) TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_notOnLanelet) { EXPECT_FALSE( - hdmap_utils - .toLaneletPose( - makePose( - makePoint(3790.0 + 5.0, 73757.0 - 5.0), makeQuaternionFromYaw(M_PI + M_PI_2 / 3.0)), - true) + traffic_simulator::lanelet_wrapper::pose::toLaneletPose( + makePose(makePoint(3790.0 + 5.0, 73757.0 - 5.0), makeQuaternionFromYaw(M_PI + M_PI_2 / 3.0)), + true) .has_value()); // angle to make pose aligned with the lanelet } @@ -751,10 +774,9 @@ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_notOnLanelet) */ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_empty) { - EXPECT_FALSE(hdmap_utils - .toLaneletPose( - makePose(makePoint(3790.0, 73757.0), makeQuaternionFromYaw(M_PI + M_PI_2 / 3.0)), - lanelet::Ids{}) + EXPECT_FALSE(traffic_simulator::lanelet_wrapper::pose::toLaneletPose( + makePose(makePoint(3790.0, 73757.0), makeQuaternionFromYaw(M_PI + M_PI_2 / 3.0)), + lanelet::Ids{}) .has_value()); // angle to make pose aligned with the lanelet } @@ -771,13 +793,11 @@ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_empty) TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_boundingBoxMatchPrevious) { EXPECT_LANELET_POSE_NEAR( - hdmap_utils - .toLaneletPose( - makePose( - makePoint(3774.9, 73749.2), - makeQuaternionFromYaw( - M_PI + M_PI_2 / 3.0)), // angle to make pose aligned with the lanelet - makeBoundingBox(), false, 0.5) + traffic_simulator::lanelet_wrapper::pose::toLaneletPose( + makePose( + makePoint(3774.9, 73749.2), + makeQuaternionFromYaw(M_PI + M_PI_2 / 3.0)), // angle to make pose aligned with the lanelet + makeBoundingBox(), false, 0.5) .value(), traffic_simulator_msgs::build() .lanelet_id(34600) @@ -1086,7 +1106,8 @@ TEST_F(HdMapUtilsTest_StandardMap, isInLanelet_correct) TEST_F(HdMapUtilsTest_StandardMap, isInLanelet_after) { const lanelet::Id lanelet_id = 34696; - EXPECT_FALSE(hdmap_utils.isInLanelet(lanelet_id, hdmap_utils.getLaneletLength(lanelet_id) + 5.0)); + EXPECT_FALSE(hdmap_utils.isInLanelet( + lanelet_id, traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(lanelet_id) + 5.0)); } /** @@ -1135,7 +1156,8 @@ TEST_F(HdMapUtilsTest_StandardMap, toMapPoints_sLargerThanLaneletLength) { const lanelet::Id lanelet_id = 34696; - const auto lanelet_length = hdmap_utils.getLaneletLength(lanelet_id); + const auto lanelet_length = + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(lanelet_id); const auto points = hdmap_utils.toMapPoints( lanelet_id, std::vector{lanelet_length + 10.0, lanelet_length + 20.0, lanelet_length + 30.0}); @@ -1166,8 +1188,8 @@ TEST_F(HdMapUtilsTest_StandardMap, toMapPoints_empty) */ TEST_F(HdMapUtilsTest_StandardMap, toMapPose_onlyOffset) { - const auto map_pose = - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34696, 10.0, 0.5)); + const auto map_pose = traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34696, 10.0, 0.5)); EXPECT_STREQ(map_pose.header.frame_id.c_str(), "map"); EXPECT_POSE_NEAR( @@ -1182,7 +1204,7 @@ TEST_F(HdMapUtilsTest_StandardMap, toMapPose_onlyOffset) */ TEST_F(HdMapUtilsTest_StandardMap, toMapPose_additionalRotation) { - const auto map_pose = hdmap_utils.toMapPose( + const auto map_pose = traffic_simulator::lanelet_wrapper::pose::toMapPose( traffic_simulator::helper::constructLaneletPose(34696, 10.0, 0.0, 0.0, 0.0, M_PI_4)); EXPECT_STREQ(map_pose.header.frame_id.c_str(), "map"); @@ -1198,8 +1220,8 @@ TEST_F(HdMapUtilsTest_StandardMap, toMapPose_negativeS) { geometry_msgs::msg::PoseStamped map_pose; EXPECT_NO_THROW( - map_pose = - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34696, -10.0))); + map_pose = traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34696, -10.0))); EXPECT_STREQ(map_pose.header.frame_id.c_str(), "map"); EXPECT_POSE_NEAR( @@ -1215,8 +1237,10 @@ TEST_F(HdMapUtilsTest_StandardMap, toMapPose_sLargerThanLaneletLength) geometry_msgs::msg::PoseStamped map_pose; EXPECT_NO_THROW( - map_pose = hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose( - lanelet_id, hdmap_utils.getLaneletLength(lanelet_id) + 10.0))); + map_pose = traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose( + lanelet_id, + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(lanelet_id) + 10.0))); EXPECT_STREQ(map_pose.header.frame_id.c_str(), "map"); EXPECT_POSE_NEAR( @@ -1977,7 +2001,7 @@ TEST_F(HdMapUtilsTest_StandardMap, isTrafficLightRegulatoryElement_invalidId) */ TEST_F(HdMapUtilsTest_StandardMap, getLaneletLength_simple) { - EXPECT_NEAR(hdmap_utils.getLaneletLength(34468), 55.5, 1.0); + EXPECT_NEAR(traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34468), 55.5, 1.0); } /** @@ -1989,7 +2013,9 @@ TEST_F(HdMapUtilsTest_StandardMap, getLaneletLength_cache) { const lanelet::Id id = 34468; - EXPECT_EQ(hdmap_utils.getLaneletLength(id), hdmap_utils.getLaneletLength(id)); + EXPECT_EQ( + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(id), + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(id)); } /** @@ -2036,9 +2062,9 @@ TEST_F(HdMapUtilsTest_StandardMap, getTrafficLightIdsOnPath_empty) */ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLanelet) { - const auto pose_from = hdmap_utils.toLaneletPose( + const auto pose_from = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(3812.65, 73810.13, -2.80), makeQuaternionFromYaw(90.0)), lanelet::Id{34606}); - const auto pose_to = hdmap_utils.toLaneletPose( + const auto pose_to = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(3825.10, 73786.34, -1.82), makeQuaternionFromYaw(90.0)), lanelet::Id{34606}); ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); @@ -2057,9 +2083,9 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLanelet) */ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLaneletBehind) { - const auto pose_to = hdmap_utils.toLaneletPose( + const auto pose_to = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(3812.65, 73810.13, -2.80), makeQuaternionFromYaw(90.0)), lanelet::Id{34606}); - const auto pose_from = hdmap_utils.toLaneletPose( + const auto pose_from = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(3825.10, 73786.34, -1.82), makeQuaternionFromYaw(90.0)), lanelet::Id{34606}); ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); @@ -2076,10 +2102,10 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLaneletBehind) */ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_differentLanelet) { - const auto pose_from = - hdmap_utils.toLaneletPose(makePose(makePoint(3801.19, 73812.70, -2.86)), lanelet::Id{120660}); - const auto pose_to = - hdmap_utils.toLaneletPose(makePose(makePoint(3724.70, 73773.00, -1.20)), lanelet::Id{34462}); + const auto pose_from = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( + makePose(makePoint(3801.19, 73812.70, -2.86)), lanelet::Id{120660}); + const auto pose_to = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( + makePose(makePoint(3724.70, 73773.00, -1.20)), lanelet::Id{34462}); ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); @@ -2097,9 +2123,9 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_differentLanelet) */ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLongitudinalDistance_differentLaneletNoRoute) { - const auto pose_to = hdmap_utils.toLaneletPose( + const auto pose_to = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(81590.79, 50067.66), makeQuaternionFromYaw(90.0)), lanelet::Id{3002185}); - const auto pose_from = hdmap_utils.toLaneletPose( + const auto pose_from = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(81596.20, 50068.04), makeQuaternionFromYaw(90.0)), lanelet::Id{3002166}); ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); From 9d6e3ce488ab98c38b50ed0ea01a81345ea4889f Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 3 Dec 2024 19:27:36 +0100 Subject: [PATCH 08/62] feat(traffic_simulator, behavior_tree_plugin): use lanelet_wrapper::lanelet_map in parts previusly overlooked --- .../behavior_tree_plugin/action_node.hpp | 1 + .../src/pedestrian/pedestrian_action_node.cpp | 1 - .../move_backward_action.cpp | 2 +- .../traffic_simulator/utils/lanelet_map.hpp | 2 + .../src/lanelet_wrapper/lanelet_map.cpp | 96 ------------------- .../src/traffic/traffic_controller.cpp | 15 +-- .../src/utils/lanelet_map.cpp | 14 +++ 7 files changed, 23 insertions(+), 108 deletions(-) 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 4bcba6c41cc..ce95f4b1bca 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 @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include diff --git a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp index c7f8a35a3a4..1443370e8a0 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp @@ -18,7 +18,6 @@ #include #include #include -#include namespace entity_behavior { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp index d1412b22029..dbd4de401d4 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp @@ -51,7 +51,7 @@ const traffic_simulator_msgs::msg::WaypointsArray MoveBackwardAction::calculateW s_in_spline = s_in_spline + lanelet_pose.s; break; } else { - s_in_spline = hdmap_utils->getLaneletLength(id) + s_in_spline; + s_in_spline = traffic_simulator::lanelet_map::laneletLength(id) + s_in_spline; } } traffic_simulator_msgs::msg::WaypointsArray waypoints; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp index 76cdc500f1e..344c19d9c1a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp @@ -36,6 +36,8 @@ inline auto activate(Ts &&... xs) } auto laneletLength(const lanelet::Id lanelet_id) -> double; + +auto borderlinePoses() -> std::vector>; } // namespace lanelet_map } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__UTILS__LANELET_MAP_HPP_ diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 25c31ee957a..7feeb7a778f 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -221,102 +221,6 @@ 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; -// } - -// auto conflictingCrosswalkIds(const lanelet::Ids & lanelet_ids) -> lanelet::Ids -// { -// constexpr size_t routing_graph_id{1}; -// constexpr double height_clearance{4}; -// /// @note it is not clear if the distinction for crosswalks only is implemented here -// lanelet::Ids conflicting_crosswalk_ids; -// lanelet::routing::RoutingGraphContainer graphs_container( -// {LaneletWrapper::routingGraph(RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER), -// LaneletWrapper::routingGraph(RoutingGraphType::PEDESTRIAN)}); -// for (const auto & lanelet_id : lanelet_ids) { -// const auto & conflicting_crosswalks = graphs_container.conflictingInGraph( -// LaneletWrapper::map()->laneletLayer.get(lanelet_id), routing_graph_id, height_clearance); -// for (const auto & conflicting_crosswalk : conflicting_crosswalks) { -// conflicting_crosswalk_ids.emplace_back(conflicting_crosswalk.id()); -// } -// } -// return conflicting_crosswalk_ids; -// } - -// auto conflictingLaneIds(const lanelet::Ids & lanelet_ids, const RoutingGraphType type) -// -> lanelet::Ids -// { -// lanelet::Ids conflicting_lanes_ids; -// for (const auto & lanelet_id : lanelet_ids) { -// const auto & conflicting_lanelets = lanelet::utils::getConflictingLanelets( -// LaneletWrapper::routingGraph(type), LaneletWrapper::map()->laneletLayer.get(lanelet_id)); -// for (const auto & conflicting_lanelet : conflicting_lanelets) { -// conflicting_lanes_ids.emplace_back(conflicting_lanelet.id()); -// } -// } -// return conflicting_lanes_ids; -// } } // namespace lanelet_map } // namespace lanelet_wrapper } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 92ddb0c8349..55c49055c30 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -55,16 +55,11 @@ TrafficController::TrafficController( void TrafficController::autoSink() { - for (const auto & lanelet_id : hdmap_utils_->getLaneletIds()) { - if (hdmap_utils_->getNextLaneletIds(lanelet_id).empty()) { - LaneletPose lanelet_pose; - lanelet_pose.lanelet_id = lanelet_id; - lanelet_pose.s = lanelet_map::laneletLength(lanelet_id); - const auto pose = pose::toMapPose(lanelet_pose); - addModule( - lanelet_id, 1, pose.position, get_entity_names_function, get_entity_pose_function, - despawn_function); - } + constexpr double sink_radius{1.0}; + for (const auto & [lanelet_id, pose] : lanelet_map::borderlinePoses()) { + addModule( + lanelet_id, sink_radius, pose.position, get_entity_names_function, get_entity_pose_function, + despawn_function); } } diff --git a/simulation/traffic_simulator/src/utils/lanelet_map.cpp b/simulation/traffic_simulator/src/utils/lanelet_map.cpp index e04f122d4a1..a55c2638b30 100644 --- a/simulation/traffic_simulator/src/utils/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/utils/lanelet_map.cpp @@ -22,5 +22,19 @@ auto laneletLength(const lanelet::Id lanelet_id) -> double { return lanelet_wrapper::lanelet_map::laneletLength(lanelet_id); } + +auto borderlinePoses() -> std::vector> +{ + std::vector> borderline_poses; + for (const auto & lanelet_id : lanelet_wrapper::lanelet_map::laneletIds()) { + if (lanelet_wrapper::lanelet_map::nextLaneletIds(lanelet_id).empty()) { + LaneletPose lanelet_pose; + lanelet_pose.lanelet_id = lanelet_id; + lanelet_pose.s = lanelet_map::laneletLength(lanelet_id); + borderline_poses.push_back({lanelet_id, pose::toMapPose(lanelet_pose)}); + } + } + return borderline_poses; +} } // namespace lanelet_map } // namespace traffic_simulator From ba476252e10fb624b3c28b48c31994df251bb849 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 3 Dec 2024 19:28:36 +0100 Subject: [PATCH 09/62] feat(traffic_simulator): use lanelet_wrapper::lanelet_map in the rest of hdmap_utils, adapt hdmap_utils test to lanelet_wrapper::lanelet_map --- .../hdmap_utils/hdmap_utils.hpp | 42 ----- .../src/hdmap_utils/hdmap_utils.cpp | 143 +++--------------- .../test/src/hdmap_utils/test_hdmap_utils.cpp | 35 +++-- 3 files changed, 42 insertions(+), 178 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 b32788d7a44..e997dde211a 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 @@ -170,8 +170,6 @@ class HdMapUtils auto getLaneletIds() const -> lanelet::Ids; - auto getLaneletLength(const lanelet::Id) const -> double; - auto getLaneletPolygon(const lanelet::Id) const -> std::vector; auto getLanelets(const lanelet::Ids &) const -> lanelet::Lanelets; @@ -198,46 +196,6 @@ class HdMapUtils const geometry_msgs::msg::Point &, const double distance_threshold, const std::size_t search_count = 5) const -> lanelet::Ids; - auto getNextLaneletIds( - const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> lanelet::Ids; - - auto getNextLaneletIds( - const lanelet::Ids &, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - - auto getNextLaneletIds( - const lanelet::Id, const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> lanelet::Ids; - - auto getNextLaneletIds( - const lanelet::Id, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - - auto getPreviousLaneletIds( - const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> lanelet::Ids; - - auto getPreviousLaneletIds( - const lanelet::Ids &, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - - auto getPreviousLaneletIds( - const lanelet::Id, const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> lanelet::Ids; - - auto getPreviousLaneletIds( - const lanelet::Id, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - auto getPreviousLanelets( const lanelet::Id, const double backward_horizon = 100, const traffic_simulator::RoutingGraphType type = diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 2cb2292311f..2cabfb0f4ab 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -100,7 +101,8 @@ auto HdMapUtils::countLaneChanges( const auto & previous = route[i - 1]; const auto & current = route[i]; - if (auto followings = getNextLaneletIds(previous, routing_configuration.routing_graph_type); + 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); std::find(lefts.begin(), lefts.end(), current) != lefts.end()) { @@ -315,7 +317,7 @@ auto HdMapUtils::clipTrajectoryFromLaneletIds( bool on_traj = false; double rest_distance = forward_distance; for (auto id_itr = lanelet_ids.begin(); id_itr != lanelet_ids.end(); id_itr++) { - double l = getLaneletLength(*id_itr); + double l = lanelet_map::laneletLength(*id_itr); if (on_traj) { if (rest_distance < l) { for (double s_val = 0; s_val < rest_distance; s_val = s_val + 1.0) { @@ -511,13 +513,14 @@ auto HdMapUtils::getPreviousLanelets( while (total_distance < backward_horizon) { const auto & reference_lanelet_id = previous_lanelets_ids.back(); if (const auto straight_lanelet_ids = - getPreviousLaneletIds(reference_lanelet_id, "straight", type); + lanelet_map::previousLaneletIds(reference_lanelet_id, "straight", type); not straight_lanelet_ids.empty()) { - total_distance = total_distance + getLaneletLength(straight_lanelet_ids[0]); + total_distance = total_distance + lanelet_map::laneletLength(straight_lanelet_ids[0]); previous_lanelets_ids.push_back(straight_lanelet_ids[0]); - } else if (auto non_straight_lanelet_ids = getPreviousLaneletIds(reference_lanelet_id, type); + } else if (auto non_straight_lanelet_ids = + lanelet_map::previousLaneletIds(reference_lanelet_id, type); not non_straight_lanelet_ids.empty()) { - total_distance = total_distance + getLaneletLength(non_straight_lanelet_ids[0]); + total_distance = total_distance + lanelet_map::laneletLength(non_straight_lanelet_ids[0]); previous_lanelets_ids.push_back(non_straight_lanelet_ids[0]); } else { break; @@ -538,7 +541,7 @@ auto HdMapUtils::getFollowingLanelets( { const auto is_following_lanelet = [this, type](const auto & current_lanelet, const auto & candidate_lanelet) { - const auto next_ids = getNextLaneletIds(current_lanelet, type); + const auto next_ids = lanelet_map::nextLaneletIds(current_lanelet, type); return std::find(next_ids.cbegin(), next_ids.cend(), candidate_lanelet) != next_ids.cend(); }; @@ -560,7 +563,7 @@ auto HdMapUtils::getFollowingLanelets( candidate_lanelet_id + " is not the follower of lanelet " + previous_lanelet); } following_lanelets_ids.push_back(candidate_lanelet_id); - total_distance += getLaneletLength(candidate_lanelet_id); + total_distance += lanelet_map::laneletLength(candidate_lanelet_id); if (total_distance > horizon) { break; } @@ -594,14 +597,15 @@ auto HdMapUtils::getFollowingLanelets( } lanelet::Id end_lanelet_id = lanelet_id; while (total_distance < distance) { - if (const auto straight_ids = getNextLaneletIds(end_lanelet_id, "straight", type); + if (const auto straight_ids = lanelet_map::nextLaneletIds(end_lanelet_id, "straight", type); !straight_ids.empty()) { - total_distance = total_distance + getLaneletLength(straight_ids[0]); + total_distance = total_distance + lanelet_map::laneletLength(straight_ids[0]); ret.push_back(straight_ids[0]); end_lanelet_id = straight_ids[0]; continue; - } else if (const auto ids = getNextLaneletIds(end_lanelet_id, type); ids.size() != 0) { - total_distance = total_distance + getLaneletLength(ids[0]); + } else if (const auto ids = lanelet_map::nextLaneletIds(end_lanelet_id, type); + ids.size() != 0) { + total_distance = total_distance + lanelet_map::laneletLength(ids[0]); ret.push_back(ids[0]); end_lanelet_id = ids[0]; continue; @@ -680,112 +684,6 @@ auto HdMapUtils::getCenterPoints(const lanelet::Id lanelet_id) const return ret; } -auto HdMapUtils::getLaneletLength(const lanelet::Id lanelet_id) const -> double -{ - if (lanelet_length_cache_.exists(lanelet_id)) { - return lanelet_length_cache_.getLength(lanelet_id); - } - double ret = lanelet::utils::getLaneletLength2d(lanelet_map_ptr_->laneletLayer.get(lanelet_id)); - lanelet_length_cache_.appendData(lanelet_id, ret); - return ret; -} - -auto HdMapUtils::getPreviousLaneletIds( - const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type) const - -> lanelet::Ids -{ - lanelet::Ids ids; - const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & llt : routing_graphs_->routing_graph(type)->previous(lanelet)) { - ids.push_back(llt.id()); - } - return ids; -} - -auto HdMapUtils::getPreviousLaneletIds( - const lanelet::Ids & lanelet_ids, const traffic_simulator::RoutingGraphType type) const - -> lanelet::Ids -{ - lanelet::Ids ids; - for (const auto & id : lanelet_ids) { - ids += getNextLaneletIds(id, type); - } - return sortAndUnique(ids); -} - -auto HdMapUtils::getPreviousLaneletIds( - const lanelet::Id lanelet_id, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids -{ - lanelet::Ids ids; - const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & llt : routing_graphs_->routing_graph(type)->previous(lanelet)) { - if (llt.attributeOr("turn_direction", "else") == turn_direction) { - ids.push_back(llt.id()); - } - } - return ids; -} - -auto HdMapUtils::getPreviousLaneletIds( - const lanelet::Ids & lanelet_ids, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids -{ - lanelet::Ids ids; - for (const auto & id : lanelet_ids) { - ids += getPreviousLaneletIds(id, turn_direction, type); - } - return sortAndUnique(ids); -} - -auto HdMapUtils::getNextLaneletIds( - const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type) const - -> lanelet::Ids -{ - lanelet::Ids ids; - const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & llt : routing_graphs_->routing_graph(type)->following(lanelet)) { - ids.push_back(llt.id()); - } - return ids; -} - -auto HdMapUtils::getNextLaneletIds( - const lanelet::Ids & lanelet_ids, const traffic_simulator::RoutingGraphType type) const - -> lanelet::Ids -{ - lanelet::Ids ids; - for (const auto & id : lanelet_ids) { - ids += getNextLaneletIds(id, type); - } - return sortAndUnique(ids); -} - -auto HdMapUtils::getNextLaneletIds( - const lanelet::Id lanelet_id, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids -{ - lanelet::Ids ids; - const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & llt : routing_graphs_->routing_graph(type)->following(lanelet)) { - if (llt.attributeOr("turn_direction", "else") == turn_direction) { - ids.push_back(llt.id()); - } - } - return ids; -} - -auto HdMapUtils::getNextLaneletIds( - const lanelet::Ids & lanelet_ids, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids -{ - lanelet::Ids ids; - for (const auto & id : lanelet_ids) { - ids += getNextLaneletIds(id, turn_direction, type); - } - return sortAndUnique(ids); -} - auto HdMapUtils::getTrafficLightIds() const -> lanelet::Ids { using namespace lanelet::utils::query; @@ -914,7 +812,7 @@ auto HdMapUtils::getLaneChangeTrajectory( const double forward_distance_threshold) const -> std::optional> { - double to_length = getLaneletLength(lane_change_parameter.target.lanelet_id); + double to_length = lanelet_map::laneletLength(lane_change_parameter.target.lanelet_id); std::vector evaluation, target_s; std::vector curves; @@ -1043,7 +941,8 @@ auto HdMapUtils::getLateralDistance( 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 = getNextLaneletIds(route[i], routing_configuration.routing_graph_type); + 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]; }); @@ -1086,7 +985,7 @@ auto HdMapUtils::getLongitudinalDistance( const lanelet::Id current_lanelet, const lanelet::Id next_lanelet) -> bool { const auto next_lanelet_ids = - getNextLaneletIds(current_lanelet, routing_configuration.routing_graph_type); + 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; }); @@ -1133,7 +1032,7 @@ auto HdMapUtils::getLongitudinalDistance( return std::nullopt; } } else { - accumulated_distance += getLaneletLength(route[i - 1UL]); + accumulated_distance += lanelet_map::laneletLength(route[i - 1UL]); } } // subtract the distance already traveled on the first lanelet: from_pose.s 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 f7b931f156d..0a6d7ab2dfa 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 "../expect_eq_macros.hpp" @@ -923,7 +924,8 @@ TEST_F(HdMapUtilsTest_EmptyMap, getClosestLaneletId_emptyMap) */ TEST_F(HdMapUtilsTest_StandardMap, getPreviousLaneletIds) { - const auto result_ids = hdmap_utils.getPreviousLaneletIds(34468); + const auto result_ids = + traffic_simulator::lanelet_wrapper::lanelet_map::previousLaneletIds(34468); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(120660)); @@ -937,7 +939,8 @@ TEST_F(HdMapUtilsTest_StandardMap, getPreviousLaneletIds) */ TEST_F(HdMapUtilsTest_WithRoadShoulderMap, getPreviousLaneletIds_RoadShoulder) { - const auto result_ids = hdmap_utils.getPreviousLaneletIds(34768); + const auto result_ids = + traffic_simulator::lanelet_wrapper::lanelet_map::previousLaneletIds(34768); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(34696)); @@ -952,7 +955,7 @@ TEST_F(HdMapUtilsTest_WithRoadShoulderMap, getPreviousLaneletIds_RoadShoulder) TEST_F(HdMapUtilsTest_StandardMap, getPreviousLaneletIds_multiplePrevious) { lanelet::Ids prev_lanelets = {34411, 34465}; - auto result_ids = hdmap_utils.getPreviousLaneletIds(34462); + auto result_ids = traffic_simulator::lanelet_wrapper::lanelet_map::previousLaneletIds(34462); std::sort(prev_lanelets.begin(), prev_lanelets.end()); std::sort(result_ids.begin(), result_ids.end()); @@ -974,14 +977,16 @@ TEST_F(HdMapUtilsTest_StandardMap, getPreviousLaneletIds_direction) const lanelet::Id prev_lanelet_straight = 34465; { - const auto result_ids = hdmap_utils.getPreviousLaneletIds(curr_lanelet, "left"); + const auto result_ids = + traffic_simulator::lanelet_wrapper::lanelet_map::previousLaneletIds(curr_lanelet, "left"); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(prev_lanelet_left)); } } { - const auto result_ids = hdmap_utils.getPreviousLaneletIds(curr_lanelet, "straight"); + const auto result_ids = + traffic_simulator::lanelet_wrapper::lanelet_map::previousLaneletIds(curr_lanelet, "straight"); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(prev_lanelet_straight)); @@ -994,9 +999,9 @@ TEST_F(HdMapUtilsTest_StandardMap, getPreviousLaneletIds_direction) * Test next lanelets id obtaining correctness * with a lanelet that has a lanelet following it. */ -TEST_F(HdMapUtilsTest_StandardMap, getNextLaneletIds) +TEST_F(HdMapUtilsTest_StandardMap, nextLaneletIds) { - const auto result_ids = hdmap_utils.getNextLaneletIds(120660); + const auto result_ids = traffic_simulator::lanelet_wrapper::lanelet_map::nextLaneletIds(120660); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(34468)); @@ -1008,9 +1013,9 @@ TEST_F(HdMapUtilsTest_StandardMap, getNextLaneletIds) * Test next lanelets id obtaining correctness * with a lanelet that has a lanelet following it and is a shoulder lane. */ -TEST_F(HdMapUtilsTest_WithRoadShoulderMap, getNextLaneletIds_RoadShoulder) +TEST_F(HdMapUtilsTest_WithRoadShoulderMap, nextLaneletIds_RoadShoulder) { - const auto result_ids = hdmap_utils.getNextLaneletIds(34696); + const auto result_ids = traffic_simulator::lanelet_wrapper::lanelet_map::nextLaneletIds(34696); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(34768)); @@ -1022,10 +1027,10 @@ TEST_F(HdMapUtilsTest_WithRoadShoulderMap, getNextLaneletIds_RoadShoulder) * Test next lanelets id obtaining correctness * with a lanelet that has several lanelets following it. */ -TEST_F(HdMapUtilsTest_StandardMap, getNextLaneletIds_multipleNext) +TEST_F(HdMapUtilsTest_StandardMap, nextLaneletIds_multipleNext) { lanelet::Ids next_lanelets = {34438, 34465}; - auto result_ids = hdmap_utils.getNextLaneletIds(34468); + auto result_ids = traffic_simulator::lanelet_wrapper::lanelet_map::nextLaneletIds(34468); std::sort(next_lanelets.begin(), next_lanelets.end()); std::sort(result_ids.begin(), result_ids.end()); @@ -1040,19 +1045,21 @@ TEST_F(HdMapUtilsTest_StandardMap, getNextLaneletIds_multipleNext) * - the goal is to test the function specialization that takes a direction as an argument * and returns only the next lanelets that have this turn direction. */ -TEST_F(HdMapUtilsTest_StandardMap, getNextLaneletIds_direction) +TEST_F(HdMapUtilsTest_StandardMap, nextLaneletIds_direction) { const lanelet::Id curr_lanelet = 34468; { - const auto result_ids = hdmap_utils.getNextLaneletIds(curr_lanelet, "left"); + const auto result_ids = + traffic_simulator::lanelet_wrapper::lanelet_map::nextLaneletIds(curr_lanelet, "left"); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(34438)); } } { - const auto result_ids = hdmap_utils.getNextLaneletIds(curr_lanelet, "straight"); + const auto result_ids = + traffic_simulator::lanelet_wrapper::lanelet_map::nextLaneletIds(curr_lanelet, "straight"); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(34465)); From f4394a8f032ec7a17b89e47c8706813430cf2c43 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 4 Dec 2024 10:56:51 +0100 Subject: [PATCH 10/62] fix(traffic_simmulator): fix spell check --- .../src/lanelet_wrapper/lanelet_map.cpp | 2 +- .../src/lanelet_wrapper/pose.cpp | 20 +++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 7feeb7a778f..bc6bebf8379 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -173,7 +173,7 @@ auto nextLaneletIds( return lanelet::Ids(next_lanelet_ids_set.begin(), next_lanelet_ids_set.end()); } -// Previus lanelet +// Previous lanelet auto previousLaneletIds(const lanelet::Id lanelet_id, const RoutingGraphType type) -> lanelet::Ids { lanelet::Ids previous_lanelet_ids; diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index d3dbca38ffd..c71730a6ecb 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -193,14 +193,14 @@ auto alternativeLaneletPoses(const LaneletPose & lanelet_pose) -> std::vector std::vector Date: Wed, 4 Dec 2024 10:58:58 +0100 Subject: [PATCH 11/62] fix(traffic_simulator): add EOF to traffic_rules --- .../traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp index 13124229b4a..35e192ca2d7 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp @@ -24,4 +24,4 @@ lanelet::traffic_rules::RegisterTrafficRules germanRoadShoulderPassableVehicleRules( Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle); } -} // namespace traffic_simulator \ No newline at end of file +} // namespace traffic_simulator From 6bcbbb2037c5a81366826df82338f8a5ba9a27bc Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 4 Dec 2024 11:26:01 +0100 Subject: [PATCH 12/62] fix(traffic_simulator): fix hdmap_utils tests --- .../test/src/hdmap_utils/test_hdmap_utils.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) 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 0a6d7ab2dfa..4b969db3b2c 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 @@ -44,6 +44,7 @@ class HdMapUtilsTest_StandardMap : public testing::Test .longitude(139.78066608243) .altitude(0.0)) { + activateLaneletWrapper("standard_map"); } hdmap_utils::HdMapUtils hdmap_utils; @@ -60,6 +61,7 @@ class HdMapUtilsTest_WithRoadShoulderMap : public testing::Test .longitude(139.78066608243) .altitude(0.0)) { + activateLaneletWrapper("with_road_shoulder"); } hdmap_utils::HdMapUtils hdmap_utils; @@ -76,6 +78,7 @@ class HdMapUtilsTest_EmptyMap : public testing::Test .longitude(0.0) .altitude(0.0)) { + activateLaneletWrapper("empty"); } hdmap_utils::HdMapUtils hdmap_utils; @@ -92,6 +95,7 @@ class HdMapUtilsTest_FourTrackHighwayMap : public testing::Test .longitude(138.8024583466017) .altitude(0.0)) { + activateLaneletWrapper("four_track_highway"); } hdmap_utils::HdMapUtils hdmap_utils; @@ -108,6 +112,7 @@ class HdMapUtilsTest_CrossroadsWithStoplinesMap : public testing::Test .longitude(139.9009591876285) .altitude(0.0)) { + activateLaneletWrapper("crossroads_with_stoplines"); } hdmap_utils::HdMapUtils hdmap_utils; @@ -123,6 +128,9 @@ class HdMapUtilsTest_KashiwanohaMap : public testing::Test .longitude(0.0) .altitude(0.0)) { + const auto lanelet_path = + ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(lanelet_path); } hdmap_utils::HdMapUtils hdmap_utils; @@ -139,6 +147,7 @@ class HdMapUtilsTest_IntersectionMap : public testing::Test .longitude(139.74821144562) .altitude(0.0)) { + activateLaneletWrapper("intersection"); } hdmap_utils::HdMapUtils hdmap_utils; From 8101375c8334f8f546ed2658bbbbf8a549058a5f Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 4 Dec 2024 16:22:54 +0100 Subject: [PATCH 13/62] ref(traffic_simulator): improve Configuration, traffic_rules, lanelet_wrapper --- .../cpp_mock_scenarios/cpp_scenario_node.hpp | 3 +- .../src/openscenario_interpreter.cpp | 14 +-- simulation/traffic_simulator/CMakeLists.txt | 3 +- .../traffic_simulator/api/configuration.hpp | 6 +- .../hdmap_utils/hdmap_utils.hpp | 1 - .../hdmap_utils/traffic_rules.hpp | 71 ------------- .../lanelet_wrapper/lanelet_map.hpp | 11 +- .../lanelet_wrapper/lanelet_wrapper.hpp | 36 ++++--- .../lanelet_wrapper/pose.hpp | 17 +-- .../traffic_simulator/utils/lanelet_map.hpp | 2 +- .../include/traffic_simulator/utils/pose.hpp | 6 +- .../src/hdmap_utils/hdmap_utils.cpp | 1 + .../src/hdmap_utils/traffic_rules.cpp | 21 ---- .../src/lanelet_wrapper/lanelet_map.cpp | 5 +- .../src/lanelet_wrapper/lanelet_wrapper.cpp | 100 +++++++++--------- .../src/lanelet_wrapper/pose.cpp | 9 +- .../src/utils/lanelet_map.cpp | 2 +- .../traffic_simulator/src/utils/pose.cpp | 4 +- .../test/src/traffic_lights/helper.hpp | 2 +- .../traffic_lights/test_traffic_lights.cpp | 2 +- .../src/random_test_runner.cpp | 2 +- 21 files changed, 105 insertions(+), 213 deletions(-) delete mode 100644 simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp delete mode 100644 simulation/traffic_simulator/src/hdmap_utils/traffic_rules.cpp diff --git a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp index 630c6d91c2f..a2b7b26ec39 100644 --- a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp +++ b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp @@ -73,8 +73,9 @@ class CppScenarioNode : public rclcpp::Node const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose) -> traffic_simulator::Configuration { + constexpr bool auto_sink{true}; auto configuration = - traffic_simulator::Configuration(map_path, lanelet2_map_file, scenario_filename); + traffic_simulator::Configuration(map_path, lanelet2_map_file, scenario_filename, true); configuration.verbose = verbose; checkConfiguration(configuration); return configuration; diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index e65f6677705..040f78b74f2 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -67,15 +67,17 @@ auto Interpreter::makeCurrentConfiguration() const -> traffic_simulator::Configu { constexpr bool auto_sink{false}; const auto logic_file = currentScenarioDefinition()->road_network.logic_file; + const auto is_directly_lanelet2_map_file = + not logic_file.isDirectory() and logic_file.filepath.extension() == ".osm"; + const auto map_files_path = + is_directly_lanelet2_map_file ? logic_file.filepath.parent_path() : logic_file; + // XXX DIRTY HACK!!! - if (not logic_file.isDirectory() and logic_file.filepath.extension() == ".osm") { + if (is_directly_lanelet2_map_file) { return traffic_simulator::Configuration( - logic_file.isDirectory() ? logic_file : logic_file.filepath.parent_path(), - logic_file.filepath.filename().string(), osc_path, auto_sink); + map_files_path, logic_file.filepath.filename().string(), osc_path, auto_sink); } else { - return traffic_simulator::Configuration( - logic_file.isDirectory() ? logic_file : logic_file.filepath.parent_path(), osc_path, - auto_sink); + return traffic_simulator::Configuration(map_files_path, osc_path, auto_sink); } } diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index 9eecfc78100..d902f5c9c17 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -45,7 +45,6 @@ ament_auto_add_library(traffic_simulator SHARED src/entity/pedestrian_entity.cpp src/entity/vehicle_entity.cpp src/hdmap_utils/hdmap_utils.cpp - src/hdmap_utils/traffic_rules.cpp src/helper/helper.cpp src/job/job.cpp src/job/job_list.cpp @@ -64,8 +63,8 @@ ament_auto_add_library(traffic_simulator SHARED src/traffic_lights/traffic_lights.cpp src/traffic_lights/traffic_lights_base.cpp src/utils/distance.cpp - src/utils/pose.cpp src/utils/lanelet_map.cpp + src/utils/pose.cpp ) ament_auto_add_library(visualization_component SHARED diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index e62833cbd90..312dc4ffe86 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -66,7 +66,7 @@ struct Configuration const Pathname scenario_path; explicit Configuration( - const Pathname & map_path, const Pathname & scenario_path = "", const bool auto_sink = true) + const Pathname & map_path, const Pathname & scenario_path, const bool auto_sink) : auto_sink(auto_sink), map_path(assertMapPath(map_path)), lanelet2_map_file(findLexicographicallyFirstFilenameOf(map_path, ".osm")), @@ -77,8 +77,8 @@ struct Configuration } explicit Configuration( - const Pathname & map_path, const Filename & lanelet2_map_file, - const Pathname & scenario_path = "", const bool auto_sink = true) + const Pathname & map_path, const Filename & lanelet2_map_file, const Pathname & scenario_path, + const bool auto_sink) : auto_sink(auto_sink), map_path(assertMapPath(map_path)), lanelet2_map_file(lanelet2_map_file), 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 e997dde211a..fddee73f040 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 @@ -48,7 +48,6 @@ #include #include #include -#include #include #include #include diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp deleted file mode 100644 index 7acbbeb5e5b..00000000000 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2015 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__HDMAP_UTILS__TRAFFIC_RULES_HPP_ -#define TRAFFIC_SIMULATOR__HDMAP_UTILS__TRAFFIC_RULES_HPP_ - -#include - -namespace hdmap_utils -{ -struct Locations -{ - /// @note DIRTY HACK!! Originally, a location code should be an ISO country code. - /// @sa https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/507033b82d9915f086f2539d56c3b62e71802438/lanelet2_traffic_rules/include/lanelet2_traffic_rules/TrafficRules.h#L97 - static constexpr char RoadShoulderPassableGermany[] = "de_road_shoulder_passable"; -}; - -class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanVehicle -{ -public: - using lanelet::traffic_rules::GermanVehicle::GermanVehicle; - -protected: - /// @note this function overrides and adds road shoulder handling to GenericTrafficRules::canPass - lanelet::Optional canPass( - const std::string & type, const std::string & /*location*/) const override - { - using lanelet::AttributeValueString; - using lanelet::Participants; - const static std::map> participants_map{ - // clang-format off - {"", {Participants::Vehicle}}, - {AttributeValueString::Road, {Participants::Vehicle, Participants::Bicycle}}, - {"road_shoulder", {Participants::Vehicle, Participants::Bicycle}}, // add road_shoulder - {AttributeValueString::Highway, {Participants::Vehicle}}, - {AttributeValueString::BicycleLane, {Participants::Bicycle}}, - {AttributeValueString::PlayStreet, {Participants::Pedestrian, Participants::Bicycle, Participants::Vehicle}}, - {AttributeValueString::EmergencyLane, {Participants::VehicleEmergency}}, - {AttributeValueString::Exit, {Participants::Pedestrian, Participants::Bicycle, Participants::Vehicle}}, - {AttributeValueString::Walkway, {Participants::Pedestrian}}, - {AttributeValueString::Crosswalk, {Participants::Pedestrian}}, - {AttributeValueString::Stairs, {Participants::Pedestrian}}, - {AttributeValueString::SharedWalkway, {Participants::Pedestrian, Participants::Bicycle}} - // clang-format on - }; - auto participants = participants_map.find(type); - if (participants == participants_map.end()) { - return {}; - } - - auto startsWith = [](const std::string & str, const std::string & substr) { - return str.compare(0, substr.size(), substr) == 0; - }; - return lanelet::utils::anyOf(participants->second, [this, startsWith](auto & participant) { - return startsWith(this->participant(), participant); - }); - } -}; -} // namespace hdmap_utils -#endif // TRAFFIC_SIMULATOR__HDMAP_UTILS__TRAFFIC_RULES_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 654e731e652..dcfcbcf2ca6 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 @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 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. @@ -18,10 +18,7 @@ #include #include -#include -#include #include -#include namespace traffic_simulator { @@ -29,9 +26,6 @@ namespace lanelet_wrapper { namespace lanelet_map { -using Point = geometry_msgs::msg::Point; -using Spline = math::geometry::CatmullRomSpline; - auto isInLanelet(const lanelet::Id lanelet_id, const double lanelet_pose_s) -> bool; auto isInLanelet(const lanelet::Id lanelet_id, const Point point) -> bool; @@ -54,14 +48,12 @@ 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; @@ -78,7 +70,6 @@ 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; 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 801120f590f..38491551bdb 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 @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 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. @@ -25,10 +25,17 @@ #include #include #include +#include +#include +#include #include #include #include #include +#include +#include +#include +#include namespace std { @@ -53,16 +60,21 @@ namespace traffic_simulator { namespace lanelet_wrapper { +using BoundingBox = traffic_simulator_msgs::msg::BoundingBox; +using EntityType = traffic_simulator_msgs::msg::EntityType; +using LaneletPose = traffic_simulator_msgs::msg::LaneletPose; using Point = geometry_msgs::msg::Point; +using Pose = geometry_msgs::msg::Pose; +using PoseStamped = geometry_msgs::msg::PoseStamped; using Spline = math::geometry::CatmullRomSpline; +using Vector3 = geometry_msgs::msg::Vector3; class RouteCache { public: auto getRoute( const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, - const lanelet::LaneletMapPtr & lanelet_map, - const traffic_simulator::RoutingConfiguration & routing_configuration, + const lanelet::LaneletMapPtr & lanelet_map, const RoutingConfiguration & routing_configuration, const lanelet::routing::RoutingGraphConstPtr & routing_graph) -> lanelet::Ids { if (!exists(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change)) { @@ -138,7 +150,7 @@ class CenterPointsCache THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); } std::lock_guard lock(mutex_); - return splines_[lanelet_id]; + return splines_.at(lanelet_id); } auto getCenterPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) @@ -159,7 +171,7 @@ class CenterPointsCache appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); } std::lock_guard lock(mutex_); - return splines_[lanelet_id]; + return splines_.at(lanelet_id); } std::unordered_map> data_; @@ -211,7 +223,7 @@ class LaneletLengthCache THROW_SIMULATION_ERROR("length of : ", lanelet_id, " does not exists on route cache."); } std::lock_guard lock(mutex_); - return data_[lanelet_id]; + return data_.at(lanelet_id); } auto getLength(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) -> double @@ -221,7 +233,7 @@ class LaneletLengthCache lanelet_id, lanelet::utils::getLaneletLength2d(lanelet_map->laneletLayer.get(lanelet_id))); } std::lock_guard lock(mutex_); - return data_[lanelet_id]; + return data_.at(lanelet_id); } std::unordered_map data_; @@ -266,23 +278,23 @@ class LaneletWrapper LaneletWrapper(const std::filesystem::path & lanelet_map_path); static LaneletWrapper & getInstance(); - auto calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) - -> std::vector; + auto overwriteLaneletsCenterline() -> void; auto resamplePoints( const lanelet::ConstLineString3d & line_string, const std::int32_t num_segments) -> lanelet::BasicPoints3d; - auto overwriteLaneletsCenterline() -> void; + auto calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) + -> std::vector; inline static std::unique_ptr instance{nullptr}; inline static std::string lanelet_map_path_{""}; inline static std::mutex mutex_; - lanelet::projection::MGRSProjector mgrs_projector_; + const lanelet::LaneletMapPtr lanelet_map_ptr_; + const lanelet::projection::MGRSProjector mgrs_projector_; lanelet::ErrorMessages lanelet_errors_; - const lanelet::LaneletMapPtr lanelet_map_ptr_; /// @todo It is worth trying to add const to each attribute of type TrafficRulesWithRoutingGraph TrafficRulesWithRoutingGraph vehicle_; TrafficRulesWithRoutingGraph vehicle_with_road_shoulder_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp index 9a8069f4df2..ca31741fb98 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 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. @@ -17,14 +17,7 @@ #include -#include -#include -#include -#include #include -#include -#include -#include namespace traffic_simulator { @@ -32,14 +25,6 @@ namespace lanelet_wrapper { namespace pose { -using Point = geometry_msgs::msg::Point; -using Vector3 = geometry_msgs::msg::Vector3; -using Pose = geometry_msgs::msg::Pose; -using PoseStamped = geometry_msgs::msg::PoseStamped; -using BoundingBox = traffic_simulator_msgs::msg::BoundingBox; -using LaneletPose = traffic_simulator_msgs::msg::LaneletPose; -using EntityType = traffic_simulator_msgs::msg::EntityType; - /// @note This value was determined experimentally by @hakuturu583 and not theoretically. /// @sa https://github.com/tier4/scenario_simulator_v2/commit/4c8e9f496b061b00bec799159d59c33f2ba46b3a constexpr static double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp index 344c19d9c1a..a3fd0dea976 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 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. diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index a42fe6622b1..db4a230b6e8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -85,7 +85,7 @@ auto boundingBoxRelativePose( // Relative LaneletPose auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - const traffic_simulator::RoutingConfiguration & routing_configuration, + const RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose; auto boundingBoxRelativeLaneletPose( @@ -93,7 +93,7 @@ auto boundingBoxRelativeLaneletPose( 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 RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose; // Others @@ -107,8 +107,6 @@ auto isAtEndOfLanelets( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const std::shared_ptr & hdmap_utils_ptr) -> bool; -// auto isAtEndOfLanelets(const CanonicalizedLaneletPose & canonicalized_lanelet_pose) -> bool; - namespace pedestrian { auto transformToCanonicalizedLaneletPose( diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 2cabfb0f4ab..01e24c889d7 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include diff --git a/simulation/traffic_simulator/src/hdmap_utils/traffic_rules.cpp b/simulation/traffic_simulator/src/hdmap_utils/traffic_rules.cpp deleted file mode 100644 index 2140970be62..00000000000 --- a/simulation/traffic_simulator/src/hdmap_utils/traffic_rules.cpp +++ /dev/null @@ -1,21 +0,0 @@ -// Copyright 2015 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 - -lanelet::traffic_rules::RegisterTrafficRules - germanRoadShoulderPassableVehicleRules( - hdmap_utils::Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle); diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index bc6bebf8379..cc647acee17 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 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. @@ -97,7 +97,6 @@ auto nearbyLaneletIds( } } -// Center points auto centerPoints(const lanelet::Ids & lanelet_ids) -> std::vector { if (lanelet_ids.empty()) { @@ -125,7 +124,6 @@ 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; @@ -173,7 +171,6 @@ 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; diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp index 9356b902e50..a856cccceee 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 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. @@ -142,26 +142,43 @@ LaneletWrapper::LaneletWrapper(const std::filesystem::path & lanelet_map_path) pedestrian_.graph = lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_.rules); } -auto LaneletWrapper::calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) - -> std::vector +auto LaneletWrapper::overwriteLaneletsCenterline() -> void { - auto calculateSegmentDistances = - [](const lanelet::ConstLineString3d & line_string) -> std::vector { - std::vector segment_distances; - segment_distances.reserve(line_string.size() - 1); - for (size_t i = 1; i < line_string.size(); ++i) { - segment_distances.push_back(lanelet::geometry::distance(line_string[i], line_string[i - 1])); + constexpr double resolution{2.0}; + + auto generateFineCenterline = + [&](const lanelet::ConstLanelet & lanelet_obj) -> lanelet::LineString3d { + // Get length of longer border + const double left_length = + static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); + const double right_length = + static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); + const double longer_distance = (left_length > right_length) ? left_length : right_length; + const int32_t num_segments = + std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d centerline(lanelet::utils::getId()); + for (size_t i = 0; i < static_cast(num_segments + 1); i++) { + // Add ID for the average point of left and right + const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2.0; + const lanelet::Point3d center_point( + lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(), + center_basic_point.z()); + centerline.push_back(center_point); } - return segment_distances; + return centerline; }; - const auto segment_distances = calculateSegmentDistances(line_string); - std::vector accumulated_lengths{0}; - accumulated_lengths.reserve(segment_distances.size() + 1); - std::partial_sum( - std::begin(segment_distances), std::end(segment_distances), - std::back_inserter(accumulated_lengths)); - return accumulated_lengths; + for (auto & lanelet_obj : lanelet_map_ptr_->laneletLayer) { + if (!lanelet_obj.hasCustomCenterline()) { + lanelet_obj.setCenterline(generateFineCenterline(lanelet_obj)); + } + } } auto LaneletWrapper::resamplePoints( @@ -215,43 +232,26 @@ auto LaneletWrapper::resamplePoints( return resampled_points; } -auto LaneletWrapper::overwriteLaneletsCenterline() -> void +auto LaneletWrapper::calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) + -> std::vector { - constexpr double resolution{2.0}; - - auto generateFineCenterline = - [&](const lanelet::ConstLanelet & lanelet_obj) -> lanelet::LineString3d { - // Get length of longer border - const double left_length = - static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); - const double right_length = - static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); - const double longer_distance = (left_length > right_length) ? left_length : right_length; - const int32_t num_segments = - std::max(static_cast(ceil(longer_distance / resolution)), 1); - - // Resample points - const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); - const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); - - // Create centerline - lanelet::LineString3d centerline(lanelet::utils::getId()); - for (size_t i = 0; i < static_cast(num_segments + 1); i++) { - // Add ID for the average point of left and right - const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2.0; - const lanelet::Point3d center_point( - lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(), - center_basic_point.z()); - centerline.push_back(center_point); + auto calculateSegmentDistances = + [](const lanelet::ConstLineString3d & line_string) -> std::vector { + std::vector segment_distances; + segment_distances.reserve(line_string.size() - 1); + for (size_t i = 1; i < line_string.size(); ++i) { + segment_distances.push_back(lanelet::geometry::distance(line_string[i], line_string[i - 1])); } - return centerline; + return segment_distances; }; - for (auto & lanelet_obj : lanelet_map_ptr_->laneletLayer) { - if (!lanelet_obj.hasCustomCenterline()) { - lanelet_obj.setCenterline(generateFineCenterline(lanelet_obj)); - } - } + const auto segment_distances = calculateSegmentDistances(line_string); + std::vector accumulated_lengths{0}; + accumulated_lengths.reserve(segment_distances.size() + 1); + std::partial_sum( + std::begin(segment_distances), std::end(segment_distances), + std::back_inserter(accumulated_lengths)); + return accumulated_lengths; } } // namespace lanelet_wrapper } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index c71730a6ecb..c0449bf7ba7 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 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. @@ -162,8 +162,7 @@ auto toLaneletPose( auto toLaneletPoses( const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance, - const bool include_opposite_direction, const traffic_simulator::RoutingGraphType type) - -> std::vector + const bool include_opposite_direction, const RoutingGraphType type) -> std::vector { std::vector lanelet_poses; std::set lanelet_ids_set{lanelet_id}; @@ -424,8 +423,8 @@ auto matchToLane( } auto leftLaneletIds( - const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type, - const bool include_opposite_direction) -> lanelet::Ids + const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) + -> lanelet::Ids { if (include_opposite_direction) { return lanelet_map::laneletIds(LaneletWrapper::routingGraph(type)->lefts( diff --git a/simulation/traffic_simulator/src/utils/lanelet_map.cpp b/simulation/traffic_simulator/src/utils/lanelet_map.cpp index a55c2638b30..c3eea682c97 100644 --- a/simulation/traffic_simulator/src/utils/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/utils/lanelet_map.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 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. diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 8d73628c447..ace0a5a7b28 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -226,7 +226,7 @@ auto boundingBoxRelativePose( /// @todo HdMapUtils will be removed when lanelet_wrapper::distance is added auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - const traffic_simulator::RoutingConfiguration & routing_configuration, + const RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose { constexpr bool include_adjacent_lanelet{false}; @@ -255,7 +255,7 @@ auto boundingBoxRelativeLaneletPose( 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 RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose { constexpr bool include_adjacent_lanelet{false}; diff --git a/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp b/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp index a85c91a2c75..7709ec5a522 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 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. diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp index ea549b6d785..d01dd88e2f4 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// Copyright 2015 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. diff --git a/test_runner/random_test_runner/src/random_test_runner.cpp b/test_runner/random_test_runner/src/random_test_runner.cpp index 0a407dbf5bd..5d4a29721ca 100644 --- a/test_runner/random_test_runner/src/random_test_runner.cpp +++ b/test_runner/random_test_runner/src/random_test_runner.cpp @@ -70,7 +70,7 @@ RandomTestRunner::RandomTestRunner(const rclcpp::NodeOptions & option) message = fmt::format("Map path found: {}", map_path); RCLCPP_INFO_STREAM(get_logger(), message); - traffic_simulator::Configuration configuration(map_path); + traffic_simulator::Configuration configuration(map_path, "", true); configuration.simulator_host = test_control_parameters.simulator_host; auto lanelet_utils = std::make_shared(configuration.lanelet2_map_path()); From a396ebcf56c39d1f39b06898f5ec621cd8dc5fdf Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 4 Dec 2024 17:18:29 +0100 Subject: [PATCH 14/62] feat(traffic_simulator): separate lanelet_loader, provide const TrafficRulesWithRoutingGraph --- simulation/traffic_simulator/CMakeLists.txt | 1 + .../lanelet_wrapper/lanelet_loader.hpp | 40 +++++ .../lanelet_wrapper/lanelet_wrapper.hpp | 28 ++- .../src/lanelet_wrapper/lanelet_loader.cpp | 162 +++++++++++++++++ .../src/lanelet_wrapper/lanelet_wrapper.cpp | 168 ++---------------- 5 files changed, 230 insertions(+), 169 deletions(-) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_loader.hpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index d902f5c9c17..f55539afe1f 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -48,6 +48,7 @@ ament_auto_add_library(traffic_simulator SHARED src/helper/helper.cpp src/job/job.cpp src/job/job_list.cpp + src/lanelet_wrapper/lanelet_loader.cpp src/lanelet_wrapper/lanelet_map.cpp src/lanelet_wrapper/lanelet_wrapper.cpp src/lanelet_wrapper/pose.cpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_loader.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_loader.hpp new file mode 100644 index 00000000000..d3da002a735 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_loader.hpp @@ -0,0 +1,40 @@ +// Copyright 2015 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_LOADER_HPP_ +#define TRAFFIC_SIMULATOR__LANELET_LOADER_HPP_ + +#include + +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +class LaneletLoader +{ +public: + static auto load(const std::filesystem::path & lanelet_map_path) -> lanelet::LaneletMapPtr; + +private: + static auto overwriteLaneletsCenterline(lanelet::LaneletMapPtr) -> void; + static auto resamplePoints( + const lanelet::ConstLineString3d & line_string, const std::int32_t num_segments) + -> lanelet::BasicPoints3d; + static auto calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) + -> std::vector; +}; +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__LANELET_LOADER_HPP_ 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 38491551bdb..434005f030c 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 @@ -15,12 +15,10 @@ #ifndef TRAFFIC_SIMULATOR__LANELET_WRAPPER_HPP_ #define TRAFFIC_SIMULATOR__LANELET_WRAPPER_HPP_ -#include #include #include #include -#include #include #include #include @@ -257,6 +255,14 @@ struct TrafficRulesWithRoutingGraph lanelet::traffic_rules::TrafficRulesPtr rules; lanelet::routing::RoutingGraphConstPtr graph; mutable RouteCache route_cache; + + TrafficRulesWithRoutingGraph( + const lanelet::LaneletMapPtr lanelet_map_ptr, const std::string & locations, + const std::string & participants) + { + rules = lanelet::traffic_rules::TrafficRulesFactory::create(locations, participants); + graph = lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *rules); + } }; class LaneletWrapper @@ -278,27 +284,15 @@ class LaneletWrapper LaneletWrapper(const std::filesystem::path & lanelet_map_path); static LaneletWrapper & getInstance(); - auto overwriteLaneletsCenterline() -> void; - - auto resamplePoints( - const lanelet::ConstLineString3d & line_string, const std::int32_t num_segments) - -> lanelet::BasicPoints3d; - - auto calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) - -> std::vector; - inline static std::unique_ptr instance{nullptr}; inline static std::string lanelet_map_path_{""}; inline static std::mutex mutex_; const lanelet::LaneletMapPtr lanelet_map_ptr_; - const lanelet::projection::MGRSProjector mgrs_projector_; - lanelet::ErrorMessages lanelet_errors_; - /// @todo It is worth trying to add const to each attribute of type TrafficRulesWithRoutingGraph - TrafficRulesWithRoutingGraph vehicle_; - TrafficRulesWithRoutingGraph vehicle_with_road_shoulder_; - TrafficRulesWithRoutingGraph pedestrian_; + const TrafficRulesWithRoutingGraph vehicle_; + const TrafficRulesWithRoutingGraph vehicle_with_road_shoulder_; + const TrafficRulesWithRoutingGraph pedestrian_; mutable CenterPointsCache center_points_cache_; mutable LaneletLengthCache lanelet_length_cache_; diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp new file mode 100644 index 00000000000..1f04643ade2 --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp @@ -0,0 +1,162 @@ +// Copyright 2015 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 + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +auto LaneletLoader::load(const std::filesystem::path & lanelet_map_path) -> lanelet::LaneletMapPtr +{ + lanelet::projection::MGRSProjector mgrs_projector; + lanelet::ErrorMessages lanelet_errors; + lanelet::LaneletMapPtr lanelet_map_ptr = + lanelet::load(lanelet_map_path.string(), mgrs_projector, &lanelet_errors); + + if (!lanelet_errors.empty()) { + std::stringstream ss; + ss << "Failed to load lanelet map, errors:\n"; + for (const auto & error : lanelet_errors) { + ss << " - " << error << "\n"; + } + THROW_SIMULATION_ERROR(ss.str()); + } + + if (lanelet_map_ptr->laneletLayer.empty()) { + THROW_SIMULATION_ERROR("Lanelet layer is empty!"); + } + + overwriteLaneletsCenterline(lanelet_map_ptr); + return lanelet_map_ptr; +} + +auto LaneletLoader::overwriteLaneletsCenterline(lanelet::LaneletMapPtr lanelet_map_ptr) -> void +{ + constexpr double resolution{2.0}; + + auto generateFineCenterline = + [&](const lanelet::ConstLanelet & lanelet_obj) -> lanelet::LineString3d { + // Get length of longer border + const double left_length = + static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); + const double right_length = + static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); + const double longer_distance = (left_length > right_length) ? left_length : right_length; + const int32_t num_segments = + std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d centerline(lanelet::utils::getId()); + for (size_t i = 0; i < static_cast(num_segments + 1); i++) { + // Add ID for the average point of left and right + const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2.0; + const lanelet::Point3d center_point( + lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(), + center_basic_point.z()); + centerline.push_back(center_point); + } + return centerline; + }; + + for (auto & lanelet_obj : lanelet_map_ptr->laneletLayer) { + if (!lanelet_obj.hasCustomCenterline()) { + lanelet_obj.setCenterline(generateFineCenterline(lanelet_obj)); + } + } +} + +auto LaneletLoader::resamplePoints( + const lanelet::ConstLineString3d & line_string, const std::int32_t num_segments) + -> lanelet::BasicPoints3d +{ + const auto accumulated_lengths = calculateAccumulatedLengths(line_string); + + auto findNearestIndexPair = + [&](const double target_length) -> std::pair { + const auto N = accumulated_lengths.size(); + if (target_length < accumulated_lengths.at(1)) { // Front + return std::make_pair(0, 1); + } else if (target_length > accumulated_lengths.at(N - 2)) { // Back + return std::make_pair(N - 2, N - 1); + } else // Middle + { + for (size_t i = 1; i < N; ++i) { + if ( + accumulated_lengths.at(i - 1) <= target_length && + target_length <= accumulated_lengths.at(i)) { + return std::make_pair(i - 1, i); + } + } + } + THROW_SEMANTIC_ERROR("findNearestIndexPair(): No nearest point found."); + }; + + // Create each segment + lanelet::BasicPoints3d resampled_points; + for (auto i = 0; i <= num_segments; ++i) { + // Find two nearest points + const double target_length = (static_cast(i) / num_segments) * + static_cast(lanelet::geometry::length(line_string)); + const auto index_pair = findNearestIndexPair(target_length); + + // Apply linear interpolation + const lanelet::BasicPoint3d back_point = line_string[index_pair.first]; + const lanelet::BasicPoint3d front_point = line_string[index_pair.second]; + const auto direction_vector = (front_point - back_point); + + const auto back_length = accumulated_lengths.at(index_pair.first); + const auto front_length = accumulated_lengths.at(index_pair.second); + const auto segment_length = front_length - back_length; + const auto target_point = + back_point + (direction_vector * (target_length - back_length) / segment_length); + + // Add to list + resampled_points.push_back(target_point); + } + return resampled_points; +} + +auto LaneletLoader::calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) + -> std::vector +{ + auto calculateSegmentDistances = + [](const lanelet::ConstLineString3d & line_string) -> std::vector { + std::vector segment_distances; + segment_distances.reserve(line_string.size() - 1); + for (size_t i = 1; i < line_string.size(); ++i) { + segment_distances.push_back(lanelet::geometry::distance(line_string[i], line_string[i - 1])); + } + return segment_distances; + }; + + const auto segment_distances = calculateSegmentDistances(line_string); + std::vector accumulated_lengths{0}; + accumulated_lengths.reserve(segment_distances.size() + 1); + std::partial_sum( + std::begin(segment_distances), std::end(segment_distances), + std::back_inserter(accumulated_lengths)); + return accumulated_lengths; +} +} // namespace lanelet_wrapper +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp index a856cccceee..11dfaabf7d5 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp @@ -12,10 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include -#include +#include #include #include @@ -32,20 +29,6 @@ auto LaneletWrapper::activate(const std::string & lanelet_map_path) -> void } } -LaneletWrapper & LaneletWrapper::getInstance() -{ - std::lock_guard lock(mutex_); - if (!instance) { - if (!lanelet_map_path_.empty()) { - instance.reset(new LaneletWrapper(lanelet_map_path_)); - } else { - THROW_SIMULATION_ERROR( - "lanelet_map_path is empty! Please call lanelet_map::activate() first."); - } - } - return *instance; -} - auto LaneletWrapper::map() -> const lanelet::LaneletMapPtr { return getInstance().lanelet_map_ptr_; @@ -112,146 +95,27 @@ auto LaneletWrapper::laneletLengthCache() -> LaneletLengthCache & } LaneletWrapper::LaneletWrapper(const std::filesystem::path & lanelet_map_path) -: lanelet_map_ptr_(lanelet::load(lanelet_map_path.string(), mgrs_projector_, &lanelet_errors_)) +: lanelet_map_ptr_(LaneletLoader::load(lanelet_map_path)), + vehicle_(lanelet_map_ptr_, lanelet::Locations::Germany, lanelet::Participants::Vehicle), + vehicle_with_road_shoulder_( + lanelet_map_ptr_, Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle), + pedestrian_(lanelet_map_ptr_, lanelet::Locations::Germany, lanelet::Participants::Pedestrian) { - if (!lanelet_errors_.empty()) { - std::stringstream ss; - ss << "Failed to load lanelet map, errors:\n"; - for (const auto & error : lanelet_errors_) { - ss << " - " << error << "\n"; - } - THROW_SIMULATION_ERROR(ss.str()); - } - - if (lanelet_map_ptr_->laneletLayer.empty()) { - THROW_SIMULATION_ERROR("Lanelet layer is empty!"); - } - - overwriteLaneletsCenterline(); - vehicle_.rules = lanelet::traffic_rules::TrafficRulesFactory::create( - lanelet::Locations::Germany, lanelet::Participants::Vehicle); - vehicle_.graph = lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *vehicle_.rules); - - vehicle_with_road_shoulder_.rules = lanelet::traffic_rules::TrafficRulesFactory::create( - Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle); - vehicle_with_road_shoulder_.graph = - lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *vehicle_with_road_shoulder_.rules); - - pedestrian_.rules = lanelet::traffic_rules::TrafficRulesFactory::create( - lanelet::Locations::Germany, lanelet::Participants::Pedestrian); - pedestrian_.graph = lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_.rules); } -auto LaneletWrapper::overwriteLaneletsCenterline() -> void -{ - constexpr double resolution{2.0}; - - auto generateFineCenterline = - [&](const lanelet::ConstLanelet & lanelet_obj) -> lanelet::LineString3d { - // Get length of longer border - const double left_length = - static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); - const double right_length = - static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); - const double longer_distance = (left_length > right_length) ? left_length : right_length; - const int32_t num_segments = - std::max(static_cast(ceil(longer_distance / resolution)), 1); - - // Resample points - const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); - const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); - - // Create centerline - lanelet::LineString3d centerline(lanelet::utils::getId()); - for (size_t i = 0; i < static_cast(num_segments + 1); i++) { - // Add ID for the average point of left and right - const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2.0; - const lanelet::Point3d center_point( - lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(), - center_basic_point.z()); - centerline.push_back(center_point); - } - return centerline; - }; - - for (auto & lanelet_obj : lanelet_map_ptr_->laneletLayer) { - if (!lanelet_obj.hasCustomCenterline()) { - lanelet_obj.setCenterline(generateFineCenterline(lanelet_obj)); - } - } -} - -auto LaneletWrapper::resamplePoints( - const lanelet::ConstLineString3d & line_string, const std::int32_t num_segments) - -> lanelet::BasicPoints3d +LaneletWrapper & LaneletWrapper::getInstance() { - const auto accumulated_lengths = calculateAccumulatedLengths(line_string); - - auto findNearestIndexPair = - [&](const double target_length) -> std::pair { - const auto N = accumulated_lengths.size(); - if (target_length < accumulated_lengths.at(1)) { // Front - return std::make_pair(0, 1); - } else if (target_length > accumulated_lengths.at(N - 2)) { // Back - return std::make_pair(N - 2, N - 1); - } else // Middle - { - for (size_t i = 1; i < N; ++i) { - if ( - accumulated_lengths.at(i - 1) <= target_length && - target_length <= accumulated_lengths.at(i)) { - return std::make_pair(i - 1, i); - } - } + std::lock_guard lock(mutex_); + if (!instance) { + if (!lanelet_map_path_.empty()) { + /// `new` is intentionally used here instead of `make_unique` since the LaneletWrapper constructor is private + instance.reset(new LaneletWrapper(lanelet_map_path_)); + } else { + THROW_SIMULATION_ERROR( + "LaneletWrapper::lanelet_map_path_ is empty! Please call lanelet_map::activate() first."); } - THROW_SEMANTIC_ERROR("findNearestIndexPair(): No nearest point found."); - }; - - // Create each segment - lanelet::BasicPoints3d resampled_points; - for (auto i = 0; i <= num_segments; ++i) { - // Find two nearest points - const double target_length = (static_cast(i) / num_segments) * - static_cast(lanelet::geometry::length(line_string)); - const auto index_pair = findNearestIndexPair(target_length); - - // Apply linear interpolation - const lanelet::BasicPoint3d back_point = line_string[index_pair.first]; - const lanelet::BasicPoint3d front_point = line_string[index_pair.second]; - const auto direction_vector = (front_point - back_point); - - const auto back_length = accumulated_lengths.at(index_pair.first); - const auto front_length = accumulated_lengths.at(index_pair.second); - const auto segment_length = front_length - back_length; - const auto target_point = - back_point + (direction_vector * (target_length - back_length) / segment_length); - - // Add to list - resampled_points.push_back(target_point); } - return resampled_points; -} - -auto LaneletWrapper::calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) - -> std::vector -{ - auto calculateSegmentDistances = - [](const lanelet::ConstLineString3d & line_string) -> std::vector { - std::vector segment_distances; - segment_distances.reserve(line_string.size() - 1); - for (size_t i = 1; i < line_string.size(); ++i) { - segment_distances.push_back(lanelet::geometry::distance(line_string[i], line_string[i - 1])); - } - return segment_distances; - }; - - const auto segment_distances = calculateSegmentDistances(line_string); - std::vector accumulated_lengths{0}; - accumulated_lengths.reserve(segment_distances.size() + 1); - std::partial_sum( - std::begin(segment_distances), std::end(segment_distances), - std::back_inserter(accumulated_lengths)); - return accumulated_lengths; + return *instance; } } // namespace lanelet_wrapper } // namespace traffic_simulator From 657e8ea32fa68a5d5b05c001644e97d7a64eec78 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 4 Dec 2024 18:27:27 +0100 Subject: [PATCH 15/62] ref(traffic_simulator): apply sonar recommendations to lanelet_wrapper and utils --- .../behavior_tree_plugin/action_node.hpp | 4 +- .../behavior_tree_plugin/src/action_node.cpp | 9 +- .../lanelet_wrapper/lanelet_wrapper.hpp | 4 +- .../include/traffic_simulator/utils/pose.hpp | 2 +- .../src/lanelet_wrapper/lanelet_loader.cpp | 29 ++++--- .../src/lanelet_wrapper/lanelet_map.cpp | 14 ++-- .../src/lanelet_wrapper/pose.cpp | 82 ++++++++++--------- .../src/lanelet_wrapper/traffic_rules.cpp | 2 +- 8 files changed, 74 insertions(+), 72 deletions(-) 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 ce95f4b1bca..8e13996926e 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 @@ -107,10 +107,10 @@ class ActionNode : public BT::ActionNodeBase auto setCanonicalizedEntityStatus(const traffic_simulator::EntityStatus & entity_status) -> void; auto calculateUpdatedEntityStatus( - double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const + const double local_target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const -> traffic_simulator::EntityStatus; auto calculateUpdatedEntityStatusInWorldFrame( - double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const + const double local_target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const -> traffic_simulator::EntityStatus; protected: diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 2d5e9f48418..147a9b48ac2 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -408,7 +408,7 @@ auto ActionNode::foundConflictingEntity(const lanelet::Ids & following_lanelets) } auto ActionNode::calculateUpdatedEntityStatus( - const double target_speed, + const double local_target_speed, const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const -> traffic_simulator::EntityStatus { @@ -416,7 +416,7 @@ auto ActionNode::calculateUpdatedEntityStatus( traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( step_time, canonicalized_entity_status->getName()); const auto dynamics = speed_planner.getDynamicStates( - target_speed, constraints, canonicalized_entity_status->getTwist(), + local_target_speed, constraints, canonicalized_entity_status->getTwist(), canonicalized_entity_status->getAccel()); const double linear_jerk_new = std::get<2>(dynamics); @@ -449,7 +449,8 @@ auto ActionNode::calculateUpdatedEntityStatus( } auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( - double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const + const double local_target_speed, + const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const -> traffic_simulator::EntityStatus { using math::geometry::operator*; @@ -457,7 +458,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( step_time, canonicalized_entity_status->getName()); const auto dynamics = speed_planner.getDynamicStates( - target_speed, constraints, canonicalized_entity_status->getTwist(), + local_target_speed, constraints, canonicalized_entity_status->getTwist(), canonicalized_entity_status->getAccel()); double linear_jerk_new = std::get<2>(dynamics); geometry_msgs::msg::Accel accel_new = std::get<1>(dynamics); 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 434005f030c..765e73079eb 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 @@ -189,7 +189,7 @@ class CenterPointsCache splines_[lanelet_id] = std::make_shared(route); } - auto centerPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) + auto centerPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) const -> std::vector { std::vector center_points; @@ -281,7 +281,7 @@ class LaneletWrapper [[nodiscard]] static auto laneletLengthCache() -> LaneletLengthCache &; private: - LaneletWrapper(const std::filesystem::path & lanelet_map_path); + explicit LaneletWrapper(const std::filesystem::path & lanelet_map_path); static LaneletWrapper & getInstance(); inline static std::unique_ptr instance{nullptr}; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index db4a230b6e8..92371213c72 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -65,7 +65,7 @@ auto transformRelativePoseToGlobal( const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose) -> geometry_msgs::msg::Pose; -// Relative msg::geometry_msgs::msg::Pose +// Relative msg::Pose auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) -> std::optional; diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp index 1f04643ade2..73f026a0529 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp @@ -54,13 +54,12 @@ auto LaneletLoader::overwriteLaneletsCenterline(lanelet::LaneletMapPtr lanelet_m auto generateFineCenterline = [&](const lanelet::ConstLanelet & lanelet_obj) -> lanelet::LineString3d { // Get length of longer border - const double left_length = + const auto left_length = static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); - const double right_length = + const auto right_length = static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); - const double longer_distance = (left_length > right_length) ? left_length : right_length; - const int32_t num_segments = - std::max(static_cast(ceil(longer_distance / resolution)), 1); + const auto longer_distance = (left_length > right_length) ? left_length : right_length; + const auto num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); // Resample points const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); @@ -118,21 +117,21 @@ auto LaneletLoader::resamplePoints( // Find two nearest points const double target_length = (static_cast(i) / num_segments) * static_cast(lanelet::geometry::length(line_string)); - const auto index_pair = findNearestIndexPair(target_length); + const auto [first_index, second_index] = findNearestIndexPair(target_length); // Apply linear interpolation - const lanelet::BasicPoint3d back_point = line_string[index_pair.first]; - const lanelet::BasicPoint3d front_point = line_string[index_pair.second]; + const lanelet::BasicPoint3d back_point = line_string[first_index]; + const lanelet::BasicPoint3d front_point = line_string[second_index]; const auto direction_vector = (front_point - back_point); - const auto back_length = accumulated_lengths.at(index_pair.first); - const auto front_length = accumulated_lengths.at(index_pair.second); + const auto back_length = accumulated_lengths.at(first_index); + const auto front_length = accumulated_lengths.at(second_index); const auto segment_length = front_length - back_length; const auto target_point = back_point + (direction_vector * (target_length - back_length) / segment_length); // Add to list - resampled_points.push_back(target_point); + resampled_points.emplace_back(target_point); } return resampled_points; } @@ -141,11 +140,11 @@ auto LaneletLoader::calculateAccumulatedLengths(const lanelet::ConstLineString3d -> std::vector { auto calculateSegmentDistances = - [](const lanelet::ConstLineString3d & line_string) -> std::vector { + [](const lanelet::ConstLineString3d & line) -> std::vector { std::vector segment_distances; - segment_distances.reserve(line_string.size() - 1); - for (size_t i = 1; i < line_string.size(); ++i) { - segment_distances.push_back(lanelet::geometry::distance(line_string[i], line_string[i - 1])); + segment_distances.reserve(line.size() - 1); + for (size_t i = 1; i < line.size(); ++i) { + segment_distances.push_back(lanelet::geometry::distance(line[i], line[i - 1])); } return segment_distances; }; diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index cc647acee17..43d24d0cc6e 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -63,10 +63,10 @@ auto nearbyLaneletIds( const char subtype[]) -> std::vector> { std::vector> filtered_lanelets; for (const auto & pair : pair_distance_lanelet) { - if (pair.second.hasAttribute(lanelet::AttributeName::Subtype)) { - if (pair.second.attribute(lanelet::AttributeName::Subtype).value() != subtype) { - filtered_lanelets.push_back(pair); - } + if ( + pair.second.hasAttribute(lanelet::AttributeName::Subtype) && + pair.second.attribute(lanelet::AttributeName::Subtype).value() != subtype) { + filtered_lanelets.push_back(pair); } } return filtered_lanelets; @@ -88,9 +88,9 @@ auto nearbyLaneletIds( return {}; } else { lanelet::Ids target_lanelet_ids; - for (const auto & lanelet : nearest_lanelets) { - if (lanelet.first <= distance_thresh) { - target_lanelet_ids.emplace_back(lanelet.second.id()); + for (const auto & [distance, lanelet] : nearest_lanelets) { + if (distance <= distance_thresh) { + target_lanelet_ids.emplace_back(lanelet.id()); } } return target_lanelet_ids; diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index c0449bf7ba7..4ff31179246 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -130,31 +130,33 @@ auto toLaneletPose( const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk, const double matching_distance, const RoutingGraphType type) -> std::optional { - if ( - const auto lanelet_id_using_bounding_box = matchToLane( - map_pose, bounding_box, include_crosswalk, matching_distance, - DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, type)) { + const auto lanelet_id_using_bounding_box = matchToLane( + map_pose, bounding_box, include_crosswalk, matching_distance, + DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, type); + + if (!lanelet_id_using_bounding_box) { + return toLaneletPose(map_pose, include_crosswalk, matching_distance); + } else if ( + const auto lanelet_pose_using_bounding_box = + toLaneletPose(map_pose, lanelet_id_using_bounding_box.value(), matching_distance)) { + return lanelet_pose_using_bounding_box; + } + + for (const auto & previous_lanelet_id : + lanelet_map::previousLaneletIds(lanelet_id_using_bounding_box.value(), type)) { if ( - const auto lanelet_pose_using_bounding_box = - toLaneletPose(map_pose, lanelet_id_using_bounding_box.value(), matching_distance)) { - return lanelet_pose_using_bounding_box; - } else { - for (const auto & previous_lanelet_id : - lanelet_map::previousLaneletIds(lanelet_id_using_bounding_box.value(), type)) { - if ( - const auto lanelet_pose_in_previous_lanelet = - toLaneletPose(map_pose, previous_lanelet_id, matching_distance)) { - return lanelet_pose_in_previous_lanelet; - } - } - for (const auto & next_lanelet_id : - lanelet_map::nextLaneletIds(lanelet_id_using_bounding_box.value(), type)) { - if ( - const auto lanelet_pose_in_next_lanelet = - toLaneletPose(map_pose, next_lanelet_id, matching_distance)) { - return lanelet_pose_in_next_lanelet; - } - } + const auto lanelet_pose_in_previous_lanelet = + toLaneletPose(map_pose, previous_lanelet_id, matching_distance)) { + return lanelet_pose_in_previous_lanelet; + } + } + + for (const auto & next_lanelet_id : + lanelet_map::nextLaneletIds(lanelet_id_using_bounding_box.value(), type)) { + if ( + const auto lanelet_pose_in_next_lanelet = + toLaneletPose(map_pose, next_lanelet_id, matching_distance)) { + return lanelet_pose_in_next_lanelet; } } return toLaneletPose(map_pose, include_crosswalk, matching_distance); @@ -165,16 +167,16 @@ auto toLaneletPoses( const bool include_opposite_direction, const RoutingGraphType type) -> std::vector { std::vector lanelet_poses; - std::set lanelet_ids_set{lanelet_id}; - auto insertIds = [&](const auto & new_ids) { - lanelet_ids_set.insert(new_ids.begin(), new_ids.end()); + std::set new_lanelet_ids_set{lanelet_id}; + auto insertNewIds = [&](const auto & new_ids) { + new_lanelet_ids_set.insert(new_ids.begin(), new_ids.end()); }; - insertIds(leftLaneletIds(lanelet_id, type, include_opposite_direction)); - insertIds(rightLaneletIds(lanelet_id, type, include_opposite_direction)); - insertIds(lanelet_map::previousLaneletIds({lanelet_id}, type)); - insertIds(lanelet_map::nextLaneletIds({lanelet_id}, type)); - for (const auto & lanelet_id : lanelet_ids_set) { - if (const auto & lanelet_pose = toLaneletPose(map_pose, lanelet_id, matching_distance)) { + insertNewIds(leftLaneletIds(lanelet_id, type, include_opposite_direction)); + insertNewIds(rightLaneletIds(lanelet_id, type, include_opposite_direction)); + insertNewIds(lanelet_map::previousLaneletIds({lanelet_id}, type)); + insertNewIds(lanelet_map::nextLaneletIds({lanelet_id}, type)); + for (const auto & new_lanelet_id : new_lanelet_ids_set) { + if (const auto & lanelet_pose = toLaneletPose(map_pose, new_lanelet_id, matching_distance)) { lanelet_poses.emplace_back(lanelet_pose.value()); } } @@ -290,7 +292,8 @@ auto alongLaneletPose( auto lanelet_pose = from_pose; lanelet_pose.s += distance; const auto canonicalized = canonicalizeLaneletPose(lanelet_pose, route_lanelets); - if (const auto canonicalized_lanelet_pose = std::get>(canonicalized)) { + if ( + const auto & canonicalized_lanelet_pose = std::get>(canonicalized)) { // If canonicalize succeed, just return canonicalized pose return canonicalized_lanelet_pose.value(); } else { @@ -410,12 +413,11 @@ auto matchToLane( } else { std::optional> min_pair_id_offset; for (const auto & match : matches) { - if ( - const auto lanelet_pose = - pose::toLaneletPose(map_pose, match.lanelet.id(), matching_distance)) { - if (!min_pair_id_offset || lanelet_pose->offset < min_pair_id_offset->second) { - min_pair_id_offset = std::make_pair(lanelet_pose->lanelet_id, lanelet_pose->offset); - } + if (const auto lanelet_pose = + pose::toLaneletPose(map_pose, match.lanelet.id(), matching_distance); + lanelet_pose && + (!min_pair_id_offset || lanelet_pose->offset < min_pair_id_offset->second)) { + min_pair_id_offset = std::make_pair(lanelet_pose->lanelet_id, lanelet_pose->offset); } } return min_pair_id_offset ? std::optional(min_pair_id_offset->first) : std::nullopt; diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp index 35e192ca2d7..db403b83cf3 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp @@ -20,7 +20,7 @@ namespace traffic_simulator { namespace lanelet_wrapper { -lanelet::traffic_rules::RegisterTrafficRules +const lanelet::traffic_rules::RegisterTrafficRules germanRoadShoulderPassableVehicleRules( Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle); } From d61cd26b1fce168a75a3ec25179ece625a95119a Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Mon, 9 Dec 2024 18:15:53 +0100 Subject: [PATCH 16/62] fix(traffic_simulator, simple_sensor_simulator): fix after merge, apply sonar recommendations --- .../ego_entity_simulation.cpp | 6 +- .../test_ego_entity_simulation.cpp | 4 +- .../lanelet_wrapper/pose.hpp | 3 +- .../lanelet_wrapper/traffic_rules.hpp | 4 +- .../src/lanelet_wrapper/lanelet_map.cpp | 3 +- .../src/lanelet_wrapper/pose.cpp | 92 ++++++++----------- 6 files changed, 53 insertions(+), 59 deletions(-) diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp index 1b1ca8e7d6b..79e29b5383a 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace vehicle_simulation @@ -343,9 +344,12 @@ auto EgoEntitySimulation::calculateAccelerationBySlope() const -> double { if (consider_acceleration_by_road_slope_) { constexpr double gravity_acceleration = -9.81; + /// @todo why there is a need to recalculate orientation using getLaneletPose? + /// status_.getMapPose().orientation already contains the orientation const double ego_pitch_angle = math::geometry::convertQuaternionToEulerAngle( - hdmap_utils_ptr_->toMapPose(status_.getLaneletPose(), true).pose.orientation) + traffic_simulator::lanelet_wrapper::pose::toMapPose(status_.getLaneletPose(), true) + .pose.orientation) .y; return -std::sin(ego_pitch_angle) * gravity_acceleration; } else { diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp index 92f43a57e9b..42faf9bb479 100644 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -58,7 +59,8 @@ TEST(EgoEntitySimulation, calculateAccelerationBySlope) initial_status.name = "ego"; // use pitch-filled map pose initial_status.lanelet_pose_valid = false; - initial_status.pose = traffic_simulator::pose::toMapPose(lanelet_pose, true).pose; + initial_status.pose = + traffic_simulator::lanelet_wrapper::pose::toMapPose(lanelet_pose, true).pose; EgoEntitySimulation ego_entity_simulation( initial_status, traffic_simulator_msgs::msg::VehicleParameters(), 1.f / 30.f, hdmap_utils, diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp index ca31741fb98..47c7c8383a8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp @@ -55,7 +55,8 @@ auto toLaneletPoses( const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> std::vector; -auto alternativeLaneletPoses(const LaneletPose & lanelet_pose) -> std::vector; +auto alternativeLaneletPoses(const LaneletPose & reference_lanelet_pose) + -> std::vector; auto alongLaneletPose( const LaneletPose & from_pose, const lanelet::Ids & route_lanelets, const double distance) diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp index 41743932542..7383f44686f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp @@ -35,8 +35,8 @@ class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanV protected: /// @note this function overrides and adds road shoulder handling to GenericTrafficRules::canPass - lanelet::Optional canPass( - const std::string & type, const std::string & /*location*/) const override + auto canPass(const std::string & type, const std::string & /*location*/) const + -> lanelet::Optional override { using lanelet::AttributeValueString; using lanelet::Participants; diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 43d24d0cc6e..482bf7135e5 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -73,7 +73,8 @@ auto nearbyLaneletIds( }; auto nearest_lanelets = lanelet::geometry::findNearest( - LaneletWrapper::map()->laneletLayer, lanelet::BasicPoint2d(point.x, point.y), search_count); + LaneletWrapper::map()->laneletLayer, lanelet::BasicPoint2d(point.x, point.y), + static_cast(search_count)); // check for current content, if not empty then optionally apply filter if (nearest_lanelets.empty() || nearest_lanelets.front().first > distance_thresh) { diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 4ff31179246..2d21a04caa2 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -183,64 +183,50 @@ auto toLaneletPoses( return lanelet_poses; } -auto alternativeLaneletPoses(const LaneletPose & lanelet_pose) -> std::vector +auto alternativeLaneletPoses(const LaneletPose & reference_lanelet_pose) -> std::vector { - const auto alternativesInPreviousLanelet = - [](const auto & lanelet_pose) -> std::vector { - std::vector lanelet_poses_in_previous_lanelet; - if (const auto previous_lanelet_ids = lanelet_map::previousLaneletIds(lanelet_pose.lanelet_id); - !previous_lanelet_ids.empty()) { - for (const auto & previous_lanelet_id : previous_lanelet_ids) { - const auto lanelet_pose_in_previous_lanelet = helper::constructLaneletPose( - previous_lanelet_id, lanelet_pose.s + lanelet_map::laneletLength(previous_lanelet_id), - lanelet_pose.offset); - if (const auto recursive_alternative_poses = - alternativeLaneletPoses(lanelet_pose_in_previous_lanelet); - recursive_alternative_poses.empty()) { - lanelet_poses_in_previous_lanelet.emplace_back(lanelet_pose_in_previous_lanelet); + // Helper function to process previous or next lanelets + const auto constructAlternativePoses = + [&](const auto & alternativeLaneletIdsFunc, const auto & constructLaneletPoseFunc) { + std::vector alternative_lanelet_poses; + for (const auto & lanelet_id : alternativeLaneletIdsFunc(reference_lanelet_pose.lanelet_id)) { + const auto lanelet_pose = constructLaneletPoseFunc(lanelet_id); + // Recursively collect alternative poses + if (const auto recursive_poses = alternativeLaneletPoses(lanelet_pose); + recursive_poses.empty()) { + alternative_lanelet_poses.push_back(lanelet_pose); } else { - lanelet_poses_in_previous_lanelet.insert( - lanelet_poses_in_previous_lanelet.end(), recursive_alternative_poses.begin(), - recursive_alternative_poses.end()); + alternative_lanelet_poses.insert( + alternative_lanelet_poses.end(), recursive_poses.begin(), recursive_poses.end()); } } - } - return lanelet_poses_in_previous_lanelet; - }; + return alternative_lanelet_poses; + }; - const auto alternativesInNextLanelet = [](const auto & lanelet_pose) -> std::vector { - std::vector lanelet_poses_in_next_lanelet; - if (const auto next_lanelet_ids = lanelet_map::nextLaneletIds(lanelet_pose.lanelet_id); - !next_lanelet_ids.empty()) { - for (const auto & next_lanelet_id : next_lanelet_ids) { - const auto lanelet_pose_in_next_lanelet = helper::constructLaneletPose( - next_lanelet_id, lanelet_pose.s - lanelet_map::laneletLength(lanelet_pose.lanelet_id), - lanelet_pose.offset); - if (const auto recursive_alternative_poses = - alternativeLaneletPoses(lanelet_pose_in_next_lanelet); - recursive_alternative_poses.empty()) { - lanelet_poses_in_next_lanelet.emplace_back(lanelet_pose_in_next_lanelet); - } else { - lanelet_poses_in_next_lanelet.insert( - lanelet_poses_in_next_lanelet.end(), recursive_alternative_poses.begin(), - recursive_alternative_poses.end()); - } - } - } - return lanelet_poses_in_next_lanelet; - }; - - /// @note If s value under 0, it means this pose is on the previous lanelet. - if (lanelet_pose.s < 0) { - return alternativesInPreviousLanelet(lanelet_pose); + /// If s value under 0, it means this pose is on the previous lanelet. + if (reference_lanelet_pose.s < 0) { + return constructAlternativePoses( + [](const auto & lanelet_id) { return lanelet_map::previousLaneletIds(lanelet_id); }, + [&reference_lanelet_pose](const auto & lanelet_id) { + return helper::constructLaneletPose( + lanelet_id, reference_lanelet_pose.s + lanelet_map::laneletLength(lanelet_id), + reference_lanelet_pose.offset); + }); } - /// @note If s value overs it's lanelet length, it means this pose is on the next lanelet. - else if (lanelet_pose.s > (lanelet_map::laneletLength(lanelet_pose.lanelet_id))) { - return alternativesInNextLanelet(lanelet_pose); + /// If s value overs it's lanelet length, it means this pose is on the next lanelet. + else if ( + reference_lanelet_pose.s > lanelet_map::laneletLength(reference_lanelet_pose.lanelet_id)) { + return constructAlternativePoses( + [](const auto & lanelet_id) { return lanelet_map::nextLaneletIds(lanelet_id); }, + [&reference_lanelet_pose](const auto & lanelet_id) { + return helper::constructLaneletPose( + lanelet_id, reference_lanelet_pose.s - lanelet_map::laneletLength(lanelet_id), + reference_lanelet_pose.offset); + }); } - /// @note If s value is in range [0,length_of_the_lanelet], return lanelet_pose. + /// If s value is in range [0, length_of_the_lanelet], return lanelet_pose. else { - return {lanelet_pose}; + return {reference_lanelet_pose}; } } @@ -377,8 +363,8 @@ auto matchToLane( const double matching_distance, const double reduction_ratio, const RoutingGraphType type) -> std::optional { - const auto absoluteHullPolygon = - [&reduction_ratio](const auto & bounding_box, const auto & pose) -> lanelet::BasicPolygon2d { + const auto absoluteHullPolygon = [&reduction_ratio, + &bounding_box](const auto & pose) -> lanelet::BasicPolygon2d { auto relative_hull = lanelet::matching::Hull2d{ lanelet::BasicPoint2d{ bounding_box.center.x + bounding_box.dimensions.x * 0.5 * reduction_ratio, @@ -399,7 +385,7 @@ auto matchToLane( bounding_box_object.pose.translation() = lanelet::BasicPoint2d(map_pose.position.x, map_pose.position.y); bounding_box_object.pose.linear() = Eigen::Rotation2D(yaw).matrix(); - bounding_box_object.absoluteHull = absoluteHullPolygon(bounding_box, bounding_box_object.pose); + bounding_box_object.absoluteHull = absoluteHullPolygon(bounding_box_object.pose); // find matches and optionally filter auto matches = lanelet::matching::getDeterministicMatches( *LaneletWrapper::map(), bounding_box_object, matching_distance); From 870bfdeed6fcc2f8bdec7dffef826432b3b0975e Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Mon, 9 Dec 2024 22:02:13 +0100 Subject: [PATCH 17/62] fix(traffic_simulator): fix lanelet_wrapper::pose::alternativeLaneletPoses --- .../src/lanelet_wrapper/pose.cpp | 78 +++++++++++-------- 1 file changed, 44 insertions(+), 34 deletions(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 2d21a04caa2..6cfc08acc07 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -185,46 +185,56 @@ auto toLaneletPoses( auto alternativeLaneletPoses(const LaneletPose & reference_lanelet_pose) -> std::vector { - // Helper function to process previous or next lanelets - const auto constructAlternativePoses = - [&](const auto & alternativeLaneletIdsFunc, const auto & constructLaneletPoseFunc) { - std::vector alternative_lanelet_poses; - for (const auto & lanelet_id : alternativeLaneletIdsFunc(reference_lanelet_pose.lanelet_id)) { - const auto lanelet_pose = constructLaneletPoseFunc(lanelet_id); - // Recursively collect alternative poses - if (const auto recursive_poses = alternativeLaneletPoses(lanelet_pose); - recursive_poses.empty()) { - alternative_lanelet_poses.push_back(lanelet_pose); - } else { - alternative_lanelet_poses.insert( - alternative_lanelet_poses.end(), recursive_poses.begin(), recursive_poses.end()); - } + const auto alternativesInPreviousLanelet = + [](const auto & lanelet_pose) -> std::vector { + std::vector lanelet_poses_in_previous_lanelet; + for (const auto & previous_lanelet_id : + lanelet_map::previousLaneletIds(lanelet_pose.lanelet_id)) { + const auto lanelet_pose_in_previous_lanelet = helper::constructLaneletPose( + previous_lanelet_id, lanelet_pose.s + lanelet_map::laneletLength(previous_lanelet_id), + lanelet_pose.offset); + if (const auto recursive_alternative_poses = + alternativeLaneletPoses(lanelet_pose_in_previous_lanelet); + recursive_alternative_poses.empty()) { + lanelet_poses_in_previous_lanelet.emplace_back(lanelet_pose_in_previous_lanelet); + } else { + lanelet_poses_in_previous_lanelet.insert( + lanelet_poses_in_previous_lanelet.end(), recursive_alternative_poses.begin(), + recursive_alternative_poses.end()); } - return alternative_lanelet_poses; - }; + } + return lanelet_poses_in_previous_lanelet; + }; + + const auto alternativesInNextLanelet = [](const auto & lanelet_pose) -> std::vector { + std::vector lanelet_poses_in_next_lanelet; + for (const auto & next_lanelet_id : lanelet_map::nextLaneletIds(lanelet_pose.lanelet_id)) { + const auto lanelet_pose_in_next_lanelet = helper::constructLaneletPose( + next_lanelet_id, lanelet_pose.s - lanelet_map::laneletLength(lanelet_pose.lanelet_id), + lanelet_pose.offset); + if (const auto recursive_alternative_poses = + alternativeLaneletPoses(lanelet_pose_in_next_lanelet); + recursive_alternative_poses.empty()) { + lanelet_poses_in_next_lanelet.emplace_back(lanelet_pose_in_next_lanelet); + } else { + lanelet_poses_in_next_lanelet.insert( + lanelet_poses_in_next_lanelet.end(), recursive_alternative_poses.begin(), + recursive_alternative_poses.end()); + } + } + return lanelet_poses_in_next_lanelet; + }; - /// If s value under 0, it means this pose is on the previous lanelet. + /// @note If s value under 0, it means this pose is on the previous lanelet. if (reference_lanelet_pose.s < 0) { - return constructAlternativePoses( - [](const auto & lanelet_id) { return lanelet_map::previousLaneletIds(lanelet_id); }, - [&reference_lanelet_pose](const auto & lanelet_id) { - return helper::constructLaneletPose( - lanelet_id, reference_lanelet_pose.s + lanelet_map::laneletLength(lanelet_id), - reference_lanelet_pose.offset); - }); + return alternativesInPreviousLanelet(reference_lanelet_pose); } - /// If s value overs it's lanelet length, it means this pose is on the next lanelet. + /// @note If s value overs it's lanelet length, it means this pose is on the next lanelet. else if ( - reference_lanelet_pose.s > lanelet_map::laneletLength(reference_lanelet_pose.lanelet_id)) { - return constructAlternativePoses( - [](const auto & lanelet_id) { return lanelet_map::nextLaneletIds(lanelet_id); }, - [&reference_lanelet_pose](const auto & lanelet_id) { - return helper::constructLaneletPose( - lanelet_id, reference_lanelet_pose.s - lanelet_map::laneletLength(lanelet_id), - reference_lanelet_pose.offset); - }); + reference_lanelet_pose.s > (lanelet_map::laneletLength(reference_lanelet_pose.lanelet_id))) { + return alternativesInNextLanelet(reference_lanelet_pose); } - /// If s value is in range [0, length_of_the_lanelet], return lanelet_pose. + /// @note If s value is in range [0,length_of_the_lanelet], return lanelet_pose. else { return {reference_lanelet_pose}; } From bd674393563654ec1473ee3bf1edaf3be6703411 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 10 Dec 2024 15:41:40 +0100 Subject: [PATCH 18/62] fix(traffic_simulator): adapt lanelet-wapper::pose to changes in hdmap_utils --- .../traffic_simulator/lanelet_wrapper/pose.hpp | 8 ++++---- .../src/lanelet_wrapper/pose.cpp | 15 +++++++++------ 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp index 47c7c8383a8..367a23cffbe 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp @@ -81,12 +81,12 @@ auto matchToLane( -> std::optional; auto leftLaneletIds( - const lanelet::Id lanelet_id, const RoutingGraphType type, - const bool include_opposite_direction = true) -> lanelet::Ids; + const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) + -> lanelet::Ids; auto rightLaneletIds( - const lanelet::Id lanelet_id, const RoutingGraphType type, - const bool include_opposite_direction = true) -> lanelet::Ids; + const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) + -> lanelet::Ids; } // namespace pose } // namespace lanelet_wrapper } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 6cfc08acc07..609726441bd 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -425,10 +425,11 @@ auto leftLaneletIds( -> lanelet::Ids { if (include_opposite_direction) { - return lanelet_map::laneletIds(LaneletWrapper::routingGraph(type)->lefts( - LaneletWrapper::map()->laneletLayer.get(lanelet_id))); + throw common::Error( + "lanelet_wrapper::pose::leftLaneletIds with include_opposite_direction=true is not " + "implemented yet."); } else { - return lanelet_map::laneletIds(LaneletWrapper::routingGraph(type)->adjacentLefts( + return lanelet_map::laneletIds(LaneletWrapper::routingGraph(type)->lefts( LaneletWrapper::map()->laneletLayer.get(lanelet_id))); } } @@ -438,10 +439,12 @@ auto rightLaneletIds( -> lanelet::Ids { if (include_opposite_direction) { - return lanelet_map::laneletIds(LaneletWrapper::routingGraph(type)->rights( - LaneletWrapper::map()->laneletLayer.get(lanelet_id))); + throw common::Error( + "lanelet_wrapper::pose::rightLaneletIds with include_opposite_direction=true is not " + "implemented " + "yet."); } else { - return lanelet_map::laneletIds(LaneletWrapper::routingGraph(type)->adjacentRights( + return lanelet_map::laneletIds(LaneletWrapper::routingGraph(type)->rights( LaneletWrapper::map()->laneletLayer.get(lanelet_id))); } } From 8fa1f2b768f04e997136331e7134ed5e86d03a92 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 10 Dec 2024 16:01:27 +0100 Subject: [PATCH 19/62] ref(traffic_simulator): improve hdmaputils::countLaneChanges --- .../src/hdmap_utils/hdmap_utils.cpp | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 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..6db9c0f2047 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 = + if (const 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++; - } + 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 37d902b5bf52ec1117b816e96d986e4a952b305d Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 17 Dec 2024 16:51:52 +0100 Subject: [PATCH 20/62] fix(traffic_simulator): fix follow_trajectory_action issue, add orientation to distance calc, remove toCanonicalizedLaneletPose(point...) because it can cause a another issues --- .../quaternion/direction_to_quaternion.hpp | 46 +++++++++++++++++++ .../include/traffic_simulator/utils/pose.hpp | 5 -- .../src/behavior/follow_trajectory.cpp | 22 +++++---- .../traffic_simulator/src/utils/pose.cpp | 11 ----- 4 files changed, 59 insertions(+), 25 deletions(-) create mode 100644 common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp diff --git a/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp b/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp new file mode 100644 index 00000000000..7a31f056a7f --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp @@ -0,0 +1,46 @@ +// Copyright 2015 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 GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_ +#define GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_ + +#include +#include +#include +#include +#include + +namespace math +{ +namespace geometry +{ +auto convertDirectionToQuaternion(const double dx, const double dy, const double dz) +{ + const auto euler_angles = geometry_msgs::build() + .x(0.0) + .y(std::atan2(-dz, std::hypot(dx, dy))) + .z(std::atan2(dy, dx)); + return math::geometry::convertEulerAngleToQuaternion(euler_angles); +} + +template < + typename T, std::enable_if_t>, std::nullptr_t> = nullptr> +auto convertDirectionToQuaternion(const T & v) +{ + return convertDirectionToQuaternion(v.x, v.y, v.z); +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_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..fa85c4f26a4 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -45,11 +45,6 @@ auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const bool include_crosswalk) -> std::optional; -auto toCanonicalizedLaneletPose( - const geometry_msgs::msg::Point & map_point, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk, - const double matching_distance) -> std::optional; - auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk, diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 92e1ad14b46..2e1df5ddadd 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include #include @@ -68,11 +68,20 @@ auto makeUpdatedStatus( auto distance_along_lanelet = [&](const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double { + const auto quaternion = math::geometry::convertDirectionToQuaternion( + geometry_msgs::build() + .x(to.x - from.x) + .y(to.y - from.y) + .z(to.z - from.z)); + const auto from_pose = + geometry_msgs::build().position(from).orientation(quaternion); + const auto to_pose = + geometry_msgs::build().position(to).orientation(quaternion); if (const auto from_lanelet_pose = pose::toCanonicalizedLaneletPose( - from, entity_status.bounding_box, false, matching_distance); + from_pose, entity_status.bounding_box, false, matching_distance); from_lanelet_pose) { if (const auto to_lanelet_pose = pose::toCanonicalizedLaneletPose( - to, entity_status.bounding_box, false, matching_distance); + to_pose, entity_status.bounding_box, false, matching_distance); to_lanelet_pose) { if (const auto distance = hdmap_utils->getLongitudinalDistance( static_cast(from_lanelet_pose.value()), @@ -561,12 +570,7 @@ auto makeUpdatedStatus( return entity_status.pose.orientation; } else { // if there is a designed_velocity vector, set the orientation in the direction of it - const geometry_msgs::msg::Vector3 direction = - geometry_msgs::build() - .x(0.0) - .y(std::atan2(-desired_velocity.z, std::hypot(desired_velocity.x, desired_velocity.y))) - .z(std::atan2(desired_velocity.y, desired_velocity.x)); - return math::geometry::convertEulerAngleToQuaternion(direction); + return math::geometry::convertDirectionToQuaternion(desired_velocity); } }(); diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index ace0a5a7b28..72e3b7dd67b 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -134,17 +134,6 @@ auto toCanonicalizedLaneletPose( } } -auto toCanonicalizedLaneletPose( - const geometry_msgs::msg::Point & map_point, - const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk, - const double matching_distance) -> std::optional -{ - return toCanonicalizedLaneletPose( - geometry_msgs::build().position(map_point).orientation( - geometry_msgs::build().x(0).y(0).z(0).w(1)), - bounding_box, include_crosswalk, matching_distance); -} - auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, From 312bfd72ed30377a2be94f4134a7cc4ed9c9d6a3 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 17 Dec 2024 18:25:22 +0100 Subject: [PATCH 21/62] ref(traffic_simulator): refactor laneletAltitude and isAltitudeMatching after merge --- .../behavior_tree_plugin/src/action_node.cpp | 24 ++++++++++--------- .../data_type/lanelet_pose.hpp | 1 + .../lanelet_wrapper/lanelet_map.hpp | 4 ++++ .../lanelet_wrapper/pose.hpp | 2 ++ .../traffic_simulator/utils/lanelet_map.hpp | 6 +++++ .../include/traffic_simulator/utils/pose.hpp | 4 ++++ .../src/data_type/entity_status.cpp | 3 ++- .../src/lanelet_wrapper/lanelet_map.cpp | 12 ++++++++++ .../src/lanelet_wrapper/pose.cpp | 24 +++++++++++++++++-- .../src/traffic/traffic_source.cpp | 5 ++-- .../traffic_simulator/src/utils/pose.cpp | 8 +++++++ 11 files changed, 77 insertions(+), 16 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 6b14ac2b83c..1e786c6f85c 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -302,13 +302,9 @@ auto ActionNode::getDistanceToTargetEntityPolygon( double width_extension_right, double width_extension_left, double length_extension_front, double length_extension_rear) const -> std::optional { - const auto & status = getEntityStatus(target_name); - if (status.laneMatchingSucceed()) { - return getDistanceToTargetEntityPolygon( - spline, status, width_extension_right, width_extension_left, length_extension_front, - length_extension_rear); - } - return std::nullopt; + return getDistanceToTargetEntityPolygon( + spline, getEntityStatus(target_name), width_extension_right, width_extension_left, + length_extension_front, length_extension_rear); } auto ActionNode::getDistanceToTargetEntityPolygon( @@ -317,21 +313,27 @@ auto ActionNode::getDistanceToTargetEntityPolygon( double width_extension_left, double length_extension_front, double length_extension_rear) const -> std::optional { - if (status.laneMatchingSucceed() && isOtherEntityAtConsideredAltitude(status)) { + if (isOtherEntityAtConsideredAltitude(status)) { const auto polygon = math::geometry::transformPoints( status.getMapPose(), math::geometry::getPointsFromBbox( status.getBoundingBox(), width_extension_right, width_extension_left, length_extension_front, length_extension_rear)); return spline.getCollisionPointIn2D(polygon, false); + } else { + return std::nullopt; } - return std::nullopt; } auto ActionNode::isOtherEntityAtConsideredAltitude( const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool { - return hdmap_utils->isAltitudeMatching( - canonicalized_entity_status->getAltitude(), entity_status.getAltitude()); + if (canonicalized_entity_status->laneMatchingSucceed() && entity_status.laneMatchingSucceed()) { + return traffic_simulator::pose::isAltitudeMatching( + canonicalized_entity_status->getCanonicalizedLaneletPose().value(), + entity_status.getCanonicalizedLaneletPose().value()); + } else { + return false; + } } auto ActionNode::getDistanceToConflictingEntity( 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..90e2fb11a2e 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 @@ -44,6 +44,7 @@ class CanonicalizedLaneletPose explicit operator geometry_msgs::msg::Pose() const noexcept { return map_pose_; } auto getLaneletPose() const -> const LaneletPose & { return lanelet_pose_; } + auto getAltitude() const -> double { return map_pose_.position.z; } auto getLaneletId() const noexcept -> lanelet::Id { return lanelet_pose_.lanelet_id; } auto alignOrientationToLanelet() -> void; auto hasAlternativeLaneletPose() const -> bool { return lanelet_poses_.size() > 1; } 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..b0969574350 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 @@ -32,6 +32,10 @@ auto isInLanelet(const lanelet::Id lanelet_id, const Point point) -> bool; auto laneletLength(const lanelet::Id lanelet_id) -> double; +auto laneletAltitude( + const lanelet::Id & lanelet_id, const geometry_msgs::msg::Pose & pose, + const double matching_distance) -> std::optional; + template auto laneletIds(const std::vector & lanelets) -> lanelet::Ids { diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp index 367a23cffbe..4198371514e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp @@ -31,6 +31,8 @@ constexpr static double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; auto toMapPose(const LaneletPose & lanelet_pose, const bool fill_pitch = true) -> PoseStamped; +auto isAltitudeMatching(const double current_altitude, const double target_altitude) -> bool; + auto toLaneletPose( const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance = 1.0) -> std::optional; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp index a3fd0dea976..4edb58df8aa 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp @@ -37,6 +37,12 @@ inline auto activate(Ts &&... xs) auto laneletLength(const lanelet::Id lanelet_id) -> double; +template +inline auto laneletAltitude(Ts &&... xs) +{ + return lanelet_wrapper::lanelet_map::laneletAltitude(std::forward(xs)...); +} + auto borderlinePoses() -> std::vector>; } // namespace lanelet_map } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index fa85c4f26a4..c2c6553e79f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -78,6 +78,10 @@ auto boundingBoxRelativePose( -> std::optional; // Relative LaneletPose +auto isAltitudeMatching( + const CanonicalizedLaneletPose & lanelet_pose, + const CanonicalizedLaneletPose & target_lanelet_pose) -> bool; + auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, const RoutingConfiguration & routing_configuration, diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index 42a08de9fea..4d32f1a71f4 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -122,7 +122,8 @@ auto CanonicalizedEntityStatus::getMapPose() const noexcept -> const geometry_ms auto CanonicalizedEntityStatus::getAltitude() const -> double { - return entity_status_.pose.position.z; + return canonicalized_lanelet_pose_ ? canonicalized_lanelet_pose_->getAltitude() + : entity_status_.pose.position.z; } auto CanonicalizedEntityStatus::getLaneletPose() const noexcept -> const LaneletPose & diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 482bf7135e5..4cf778efeb2 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -44,6 +44,18 @@ auto laneletLength(const lanelet::Id lanelet_id) -> double return LaneletWrapper::laneletLengthCache().getLength(lanelet_id, LaneletWrapper::map()); } +auto laneletAltitude( + const lanelet::Id & lanelet_id, const geometry_msgs::msg::Pose & pose, + const double matching_distance) -> std::optional +{ + if (const auto spline = centerPointsSpline(lanelet_id)) { + if (const auto s = spline->getSValue(pose, matching_distance)) { + return spline->getPoint(s.value()).z; + } + } + return std::nullopt; +} + auto laneletIds() -> lanelet::Ids { lanelet::Ids ids; diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 609726441bd..662842960c8 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -68,6 +68,24 @@ auto toMapPose(const LaneletPose & lanelet_pose, const bool fill_pitch) -> PoseS } } +auto isAltitudeMatching(const double current_altitude, const double target_altitude) -> bool +{ + /* + Using a fixed `altitude_threshold` value of 1.0 [m] is justified because the + entity's Z-position is always relative to its base. This eliminates the + need to dynamically adjust the threshold based on the entity's dimensions, + ensuring consistent altitude matching regardless of the entity type. + + The position of the entity is defined relative to its base, typically + the center of the rear axle projected onto the ground in the case of vehicles. + + There is no technical basis for this value; it was determined based on + experiments. + */ + static constexpr double altitude_threshold = 1.0; + return std::abs(current_altitude - target_altitude) <= altitude_threshold; +} + auto toLaneletPose( const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance) -> std::optional @@ -77,10 +95,12 @@ auto toLaneletPose( if (const auto lanelet_pose_s = lanelet_spline->getSValue(map_pose, matching_distance); !lanelet_pose_s) { return std::nullopt; + } else if (const auto pose_on_centerline = lanelet_spline->getPose(lanelet_pose_s.value()); + !isAltitudeMatching(map_pose.position.z, pose_on_centerline.position.z)) { + return std::nullopt; } else { - const auto lanelet_quaternion = lanelet_spline->getPose(lanelet_pose_s.value()).orientation; if (const auto lanelet_pose_rpy = math::geometry::convertQuaternionToEulerAngle( - math::geometry::getRotation(lanelet_quaternion, map_pose.orientation)); + math::geometry::getRotation(pose_on_centerline.orientation, map_pose.orientation)); std::fabs(lanelet_pose_rpy.z) > M_PI * yaw_threshold && std::fabs(lanelet_pose_rpy.z) < M_PI * (1 - yaw_threshold)) { return std::nullopt; diff --git a/simulation/traffic_simulator/src/traffic/traffic_source.cpp b/simulation/traffic_simulator/src/traffic/traffic_source.cpp index ce0149891e7..347ea1cf7a6 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_source.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_source.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -91,8 +92,8 @@ auto TrafficSource::makeRandomPose( !nearby_lanelets.empty()) { // Get the altitude of the first nearby lanelet if ( - const auto altitude = - hdmap_utils_->getLaneletAltitude(nearby_lanelets.front(), random_pose, radius)) { + const auto altitude = traffic_simulator::lanelet_map::laneletAltitude( + nearby_lanelets.front(), random_pose, radius)) { random_pose.position.z = altitude.value(); } } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 72e3b7dd67b..97d3fd872e8 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -169,6 +169,14 @@ auto transformRelativePoseToGlobal( } // Relative msg::Pose +auto isAltitudeMatching( + const CanonicalizedLaneletPose & lanelet_pose, + const CanonicalizedLaneletPose & target_lanelet_pose) -> bool +{ + return lanelet_wrapper::pose::isAltitudeMatching( + lanelet_pose.getAltitude(), target_lanelet_pose.getAltitude()); +} + auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) -> std::optional { From 2645a653162bf9cc7ed7e998f64c22e5387a67ee Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 7 Jan 2025 12:57:19 +0100 Subject: [PATCH 22/62] ref(traffic_simulator): apply sonar required changes --- .../traffic_simulator/hdmap_utils/cache.hpp | 20 +++++------ .../lanelet_wrapper/lanelet_map.hpp | 8 ++--- .../lanelet_wrapper/lanelet_wrapper.hpp | 34 +++++++++---------- .../lanelet_wrapper/traffic_rules.hpp | 3 +- .../src/hdmap_utils/hdmap_utils.cpp | 2 +- .../src/lanelet_wrapper/lanelet_map.cpp | 28 +++++++-------- .../src/lanelet_wrapper/lanelet_wrapper.cpp | 13 +++---- .../src/lanelet_wrapper/pose.cpp | 8 ++--- .../src/utils/lanelet_map.cpp | 2 +- 9 files changed, 57 insertions(+), 61 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp index f8895a0a3c2..fa20308d0cc 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp @@ -31,7 +31,7 @@ class RouteCache public: auto exists(lanelet::Id from, lanelet::Id to, bool allow_lane_change) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); std::tuple key = {from, to, allow_lane_change}; return data_.find(key) != data_.end(); } @@ -44,7 +44,7 @@ class RouteCache "route from : ", from, " to : ", to, (allow_lane_change ? " with" : " without"), " lane change does not exists on route cache."); } else { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.at({from, to, allow_lane_change}); } } @@ -53,7 +53,7 @@ class RouteCache lanelet::Id from, lanelet::Id to, const bool allow_lane_change, const lanelet::Ids & route) -> void { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[{from, to, allow_lane_change}] = route; } @@ -68,7 +68,7 @@ class CenterPointsCache public: auto exists(lanelet::Id lanelet_id) -> bool { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } @@ -77,7 +77,7 @@ class CenterPointsCache if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } @@ -86,13 +86,13 @@ class CenterPointsCache if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return splines_[lanelet_id]; } auto appendData(lanelet::Id lanelet_id, const std::vector & route) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[lanelet_id] = route; splines_[lanelet_id] = std::make_shared(route); } @@ -110,7 +110,7 @@ class LaneletLengthCache public: auto exists(lanelet::Id lanelet_id) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } @@ -119,13 +119,13 @@ class LaneletLengthCache if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("length of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_[lanelet_id]; } auto appendData(lanelet::Id lanelet_id, double length) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[lanelet_id] = length; } 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 b0969574350..ebda8439df7 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 @@ -67,11 +67,11 @@ auto nextLaneletIds( const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; auto nextLaneletIds( - const lanelet::Id lanelet_id, const std::string & turn_direction, + const lanelet::Id lanelet_id, std::string_view turn_direction, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; auto nextLaneletIds( - const lanelet::Ids & lanelet_ids, const std::string & turn_direction, + const lanelet::Ids & lanelet_ids, std::string_view turn_direction, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; auto previousLaneletIds( @@ -83,11 +83,11 @@ auto previousLaneletIds( const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; auto previousLaneletIds( - const lanelet::Id lanelet_id, const std::string & turn_direction, + const lanelet::Id lanelet_id, std::string_view turn_direction, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; auto previousLaneletIds( - const lanelet::Ids & lanelet_ids, const std::string & turn_direction, + const lanelet::Ids & lanelet_ids, std::string_view turn_direction, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; } // namespace lanelet_map } // namespace lanelet_wrapper 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..7d01b2a7dee 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 @@ -94,7 +94,7 @@ class RouteCache shortest_path_ids); } } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.at({from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change}); } @@ -106,7 +106,7 @@ class RouteCache "route from : ", from, " to : ", to, (allow_lane_change ? " with" : " without"), " lane change does not exists on route cache."); } else { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.at({from, to, allow_lane_change}); } } @@ -116,7 +116,7 @@ class RouteCache auto exists(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) -> bool { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); std::tuple key = {from, to, allow_lane_change}; return data_.find(key) != data_.end(); } @@ -125,7 +125,7 @@ class RouteCache const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change, const lanelet::Ids & route) -> void { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[{from, to, allow_lane_change}] = route; } }; @@ -138,7 +138,7 @@ class CenterPointsCache if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } @@ -147,7 +147,7 @@ class CenterPointsCache if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return splines_.at(lanelet_id); } @@ -157,7 +157,7 @@ class CenterPointsCache if (!exists(lanelet_id)) { appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } @@ -168,7 +168,7 @@ class CenterPointsCache if (!exists(lanelet_id)) { appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return splines_.at(lanelet_id); } @@ -178,13 +178,13 @@ class CenterPointsCache auto exists(const lanelet::Id lanelet_id) -> bool { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } auto appendData(const lanelet::Id lanelet_id, const std::vector & route) -> void { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[lanelet_id] = route; splines_[lanelet_id] = std::make_shared(route); } @@ -220,7 +220,7 @@ class LaneletLengthCache if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("length of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } @@ -230,7 +230,7 @@ class LaneletLengthCache appendData( lanelet_id, lanelet::utils::getLaneletLength2d(lanelet_map->laneletLayer.get(lanelet_id))); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } @@ -239,13 +239,13 @@ class LaneletLengthCache auto exists(const lanelet::Id lanelet_id) -> bool { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } auto appendData(const lanelet::Id lanelet_id, double length) -> void { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[lanelet_id] = length; } }; @@ -270,11 +270,11 @@ class LaneletWrapper public: static auto activate(const std::string & lanelet_map_path) -> void; - [[nodiscard]] static auto map() -> const lanelet::LaneletMapPtr; + [[nodiscard]] static auto map() -> lanelet::LaneletMapPtr; [[nodiscard]] static auto routingGraph(const RoutingGraphType type) - -> const lanelet::routing::RoutingGraphConstPtr; + -> lanelet::routing::RoutingGraphConstPtr; [[nodiscard]] static auto trafficRules(const RoutingGraphType type) - -> const lanelet::traffic_rules::TrafficRulesPtr; + -> lanelet::traffic_rules::TrafficRulesPtr; [[nodiscard]] static auto routeCache(const RoutingGraphType type) -> RouteCache &; [[nodiscard]] static auto centerPointsCache() -> CenterPointsCache &; diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp index cbeda304ca0..f96a3486a75 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp @@ -31,6 +31,7 @@ struct Locations class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanVehicle { public: + using lanelet::traffic_rules::GenericTrafficRules::canPass; using lanelet::traffic_rules::GermanVehicle::GermanVehicle; protected: @@ -40,7 +41,7 @@ class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanV { using lanelet::AttributeValueString; using lanelet::Participants; - const static std::map> participants_map{ + const static std::map, std::less<>> participants_map{ // clang-format off {"", {Participants::Vehicle}}, {AttributeValueString::Road, {Participants::Vehicle, Participants::Bicycle}}, diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 90d2ed5ef55..af964f22341 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -988,7 +988,7 @@ auto HdMapUtils::getLongitudinalDistance( const traffic_simulator::RoutingConfiguration & routing_configuration) const -> std::optional { - const auto is_lane_change_required = [this, routing_configuration]( + const auto is_lane_change_required = [routing_configuration]( const lanelet::Id current_lanelet, const lanelet::Id next_lanelet) -> bool { const auto next_lanelet_ids = diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 4cf778efeb2..3bf62e09fb0 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -72,17 +72,17 @@ auto nearbyLaneletIds( auto excludeSubtypeLanelets = []( const std::vector> & pair_distance_lanelet, - const char subtype[]) -> std::vector> { - std::vector> filtered_lanelets; - for (const auto & pair : pair_distance_lanelet) { - if ( - pair.second.hasAttribute(lanelet::AttributeName::Subtype) && - pair.second.attribute(lanelet::AttributeName::Subtype).value() != subtype) { - filtered_lanelets.push_back(pair); + const char subtype[]) { + std::vector> filtered_lanelets; + for (const auto & pair : pair_distance_lanelet) { + if ( + pair.second.hasAttribute(lanelet::AttributeName::Subtype) && + pair.second.attribute(lanelet::AttributeName::Subtype).value() != subtype) { + filtered_lanelets.push_back(pair); + } } - } - return filtered_lanelets; - }; + return filtered_lanelets; + }; auto nearest_lanelets = lanelet::geometry::findNearest( LaneletWrapper::map()->laneletLayer, lanelet::BasicPoint2d(point.x, point.y), @@ -158,7 +158,7 @@ auto nextLaneletIds(const lanelet::Ids & lanelet_ids, const RoutingGraphType typ } auto nextLaneletIds( - const lanelet::Id lanelet_id, const std::string & turn_direction, const RoutingGraphType type) + const lanelet::Id lanelet_id, std::string_view turn_direction, const RoutingGraphType type) -> lanelet::Ids { lanelet::Ids next_lanelet_ids; @@ -173,7 +173,7 @@ auto nextLaneletIds( } auto nextLaneletIds( - const lanelet::Ids & lanelet_ids, const std::string & turn_direction, const RoutingGraphType type) + const lanelet::Ids & lanelet_ids, std::string_view turn_direction, const RoutingGraphType type) -> lanelet::Ids { std::set next_lanelet_ids_set; @@ -206,7 +206,7 @@ auto previousLaneletIds(const lanelet::Ids & lanelet_ids, const RoutingGraphType } auto previousLaneletIds( - const lanelet::Id lanelet_id, const std::string & turn_direction, const RoutingGraphType type) + const lanelet::Id lanelet_id, std::string_view turn_direction, const RoutingGraphType type) -> lanelet::Ids { lanelet::Ids previous_lanelet_ids; @@ -221,7 +221,7 @@ auto previousLaneletIds( } auto previousLaneletIds( - const lanelet::Ids & lanelet_ids, const std::string & turn_direction, const RoutingGraphType type) + const lanelet::Ids & lanelet_ids, std::string_view turn_direction, const RoutingGraphType type) -> lanelet::Ids { std::set previous_lanelet_ids_set; diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp index 11dfaabf7d5..afb8248cbd7 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp @@ -24,18 +24,15 @@ auto LaneletWrapper::activate(const std::string & lanelet_map_path) -> void { lanelet_map_path_ = lanelet_map_path; if (instance) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); instance.reset(); } } -auto LaneletWrapper::map() -> const lanelet::LaneletMapPtr -{ - return getInstance().lanelet_map_ptr_; -} +auto LaneletWrapper::map() -> lanelet::LaneletMapPtr { return getInstance().lanelet_map_ptr_; } auto LaneletWrapper::routingGraph(const RoutingGraphType type) - -> const lanelet::routing::RoutingGraphConstPtr + -> lanelet::routing::RoutingGraphConstPtr { switch (type) { case RoutingGraphType::VEHICLE: @@ -52,7 +49,7 @@ auto LaneletWrapper::routingGraph(const RoutingGraphType type) } auto LaneletWrapper::trafficRules(const RoutingGraphType type) - -> const lanelet::traffic_rules::TrafficRulesPtr + -> lanelet::traffic_rules::TrafficRulesPtr { switch (type) { case RoutingGraphType::VEHICLE: @@ -105,7 +102,7 @@ LaneletWrapper::LaneletWrapper(const std::filesystem::path & lanelet_map_path) LaneletWrapper & LaneletWrapper::getInstance() { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); if (!instance) { if (!lanelet_map_path_.empty()) { /// `new` is intentionally used here instead of `make_unique` since the LaneletWrapper constructor is private diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 662842960c8..d620aa825df 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -205,8 +205,7 @@ auto toLaneletPoses( auto alternativeLaneletPoses(const LaneletPose & reference_lanelet_pose) -> std::vector { - const auto alternativesInPreviousLanelet = - [](const auto & lanelet_pose) -> std::vector { + const auto alternativesInPreviousLanelet = [](const auto & lanelet_pose) { std::vector lanelet_poses_in_previous_lanelet; for (const auto & previous_lanelet_id : lanelet_map::previousLaneletIds(lanelet_pose.lanelet_id)) { @@ -226,7 +225,7 @@ auto alternativeLaneletPoses(const LaneletPose & reference_lanelet_pose) -> std: return lanelet_poses_in_previous_lanelet; }; - const auto alternativesInNextLanelet = [](const auto & lanelet_pose) -> std::vector { + const auto alternativesInNextLanelet = [](const auto & lanelet_pose) { std::vector lanelet_poses_in_next_lanelet; for (const auto & next_lanelet_id : lanelet_map::nextLaneletIds(lanelet_pose.lanelet_id)) { const auto lanelet_pose_in_next_lanelet = helper::constructLaneletPose( @@ -393,8 +392,7 @@ auto matchToLane( const double matching_distance, const double reduction_ratio, const RoutingGraphType type) -> std::optional { - const auto absoluteHullPolygon = [&reduction_ratio, - &bounding_box](const auto & pose) -> lanelet::BasicPolygon2d { + const auto absoluteHullPolygon = [&reduction_ratio, &bounding_box](const auto & pose) { auto relative_hull = lanelet::matching::Hull2d{ lanelet::BasicPoint2d{ bounding_box.center.x + bounding_box.dimensions.x * 0.5 * reduction_ratio, diff --git a/simulation/traffic_simulator/src/utils/lanelet_map.cpp b/simulation/traffic_simulator/src/utils/lanelet_map.cpp index c3eea682c97..81566a28cc1 100644 --- a/simulation/traffic_simulator/src/utils/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/utils/lanelet_map.cpp @@ -31,7 +31,7 @@ auto borderlinePoses() -> std::vector> LaneletPose lanelet_pose; lanelet_pose.lanelet_id = lanelet_id; lanelet_pose.s = lanelet_map::laneletLength(lanelet_id); - borderline_poses.push_back({lanelet_id, pose::toMapPose(lanelet_pose)}); + borderline_poses.emplace_back(lanelet_id, pose::toMapPose(lanelet_pose)); } } return borderline_poses; From c35aed6952ff63436c32d8f0ad62634cd7a4c6d7 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 7 Jan 2025 18:32:17 +0100 Subject: [PATCH 23/62] ref(traffic_simulator): apply solar changes --- .../include/traffic_simulator/api/configuration.hpp | 4 ++-- .../traffic_simulator/lanelet_wrapper/traffic_rules.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index 2e2aa6fc990..4be810be98d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -68,7 +68,7 @@ struct Configuration explicit Configuration( const Pathname & map_path, const Pathname & scenario_path, - const std::set auto_sink_entity_types = {}) + const std::set & auto_sink_entity_types = {}) : auto_sink_entity_types(auto_sink_entity_types), map_path(assertMapPath(map_path)), lanelet2_map_file(findLexicographicallyFirstFilenameOf(map_path, ".osm")), @@ -80,7 +80,7 @@ struct Configuration explicit Configuration( const Pathname & map_path, const Filename & lanelet2_map_file, const Pathname & scenario_path, - const std::set auto_sink_entity_types = {}) + const std::set & auto_sink_entity_types = {}) : auto_sink_entity_types(auto_sink_entity_types), map_path(assertMapPath(map_path)), lanelet2_map_file(lanelet2_map_file), diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp index f96a3486a75..0ea608c2999 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp @@ -62,7 +62,7 @@ class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanV return {}; } - auto startsWith = [](const std::string & str, const std::string & substr) { + auto startsWith = [](std::string_view str, std::string_view substr) { return str.compare(0, substr.size(), substr) == 0; }; return lanelet::utils::anyOf(participants->second, [this, startsWith](auto & participant) { From 46a142a0f8846093eda368bcd0b3f7859eebc9fb Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 15 Jan 2025 11:22:01 +0100 Subject: [PATCH 24/62] ref(cpp_scenario_mock): remove unused auto_sink variable --- .../include/cpp_mock_scenarios/cpp_scenario_node.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp index 0301fef3d0e..3b407791ef8 100644 --- a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp +++ b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp @@ -75,7 +75,6 @@ class CppScenarioNode : public rclcpp::Node const std::string & scenario_filename, const bool verbose, const std::set & auto_sink_entity_types = {}) -> traffic_simulator::Configuration { - constexpr bool auto_sink{true}; auto configuration = traffic_simulator::Configuration( map_path, lanelet2_map_file, scenario_filename, auto_sink_entity_types); configuration.verbose = verbose; From 1d77eb308a7650b87f6569379b0543f341736308 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 11:33:56 +0100 Subject: [PATCH 25/62] Update simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp Co-authored-by: Masaya Kataoka --- .../traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 7d01b2a7dee..27c1c71a045 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 @@ -45,7 +45,7 @@ struct hash> { std::hash lanelet_id_hash; size_t seed = 0; - // hash combine like boost library + /// @note hash combine like boost library 2^32 / phi = 0x9e3779b9 seed ^= lanelet_id_hash(std::get<0>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); seed ^= lanelet_id_hash(std::get<1>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); seed ^= std::hash{}(std::get<2>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); From 0089e7efab1c02b8d885685e6f832e11a9ef5660 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:26:34 +0100 Subject: [PATCH 26/62] Update simulation/traffic_simulator/src/utils/pose.cpp Co-authored-by: Masaya Kataoka --- simulation/traffic_simulator/src/utils/pose.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 879e4202aa6..086168b44b7 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -172,7 +172,7 @@ auto transformRelativePoseToGlobal( return ret; } -// Relative msg::Pose +/// @note Relative msg::Pose auto isAltitudeMatching( const CanonicalizedLaneletPose & lanelet_pose, const CanonicalizedLaneletPose & target_lanelet_pose) -> bool From 6f2316edf06d675de2e985c30fcf26d55fb3a4a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:26:48 +0100 Subject: [PATCH 27/62] Update simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp Co-authored-by: Masaya Kataoka --- simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index d620aa825df..7bf0db184e9 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -309,7 +309,7 @@ auto alongLaneletPose( const auto canonicalized = canonicalizeLaneletPose(lanelet_pose, route_lanelets); if ( const auto & canonicalized_lanelet_pose = std::get>(canonicalized)) { - // If canonicalize succeed, just return canonicalized pose + /// @note If canonicalize succeed, just return canonicalized pose return canonicalized_lanelet_pose.value(); } else { // If canonicalize failed, return lanelet pose as end of road From 8d8b5e8c32c21dbe3b8aaa109db87099564e5bff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:26:55 +0100 Subject: [PATCH 28/62] Update simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp Co-authored-by: Masaya Kataoka --- simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 7bf0db184e9..bfdc463f5ca 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -312,7 +312,7 @@ auto alongLaneletPose( /// @note If canonicalize succeed, just return canonicalized pose return canonicalized_lanelet_pose.value(); } else { - // If canonicalize failed, return lanelet pose as end of road + /// @note If canonicalize failed, return lanelet pose as end of road if (const auto end_of_road_lanelet_id = std::get>(canonicalized)) { return traffic_simulator_msgs::build() .lanelet_id(end_of_road_lanelet_id.value()) From 85aac21bd696bfd12aa51c9c7de02226b2bb3ed3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:28:29 +0100 Subject: [PATCH 29/62] Update simulation/traffic_simulator/src/utils/pose.cpp Co-authored-by: Masaya Kataoka --- simulation/traffic_simulator/src/utils/pose.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 086168b44b7..171ee9396bc 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -223,7 +223,7 @@ auto boundingBoxRelativePose( return std::nullopt; } -// Relative LaneletPose +/// @note Relative LaneletPose /// @todo HdMapUtils will be removed when lanelet_wrapper::distance is added auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, From 0ce2689293a24cb5bf777a0fbb5248e1f78b13a9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:30:07 +0100 Subject: [PATCH 30/62] Update simulation/traffic_simulator/src/utils/pose.cpp Co-authored-by: Masaya Kataoka --- simulation/traffic_simulator/src/utils/pose.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 171ee9396bc..2ad05436e74 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -47,7 +47,7 @@ auto quietNaNLaneletPose() -> LaneletPose .z(std::numeric_limits::quiet_NaN())); } -// Conversions +/// @note Conversions auto canonicalize(const LaneletPose & lanelet_pose) -> LaneletPose { if ( From 6de8a9a64a95004c3626bcff340bb30e0fb46418 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:31:12 +0100 Subject: [PATCH 31/62] Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp Co-authored-by: Masaya Kataoka --- .../traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp index 73f026a0529..5b12ad7d59d 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp @@ -65,7 +65,7 @@ auto LaneletLoader::overwriteLaneletsCenterline(lanelet::LaneletMapPtr lanelet_m const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); - // Create centerline + /// @note Create centerline lanelet::LineString3d centerline(lanelet::utils::getId()); for (size_t i = 0; i < static_cast(num_segments + 1); i++) { // Add ID for the average point of left and right From 19565989723ea81a08bae4c091dde9cd515ff1ce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:31:37 +0100 Subject: [PATCH 32/62] Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp Co-authored-by: Masaya Kataoka --- .../traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp index 5b12ad7d59d..fe4a921e59b 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp @@ -61,7 +61,7 @@ auto LaneletLoader::overwriteLaneletsCenterline(lanelet::LaneletMapPtr lanelet_m const auto longer_distance = (left_length > right_length) ? left_length : right_length; const auto num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); - // Resample points + /// @note Resample points const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); From cbed965f5fe029b9e1816266a0d1cea38b40470c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:40:16 +0100 Subject: [PATCH 33/62] Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp Co-authored-by: Masaya Kataoka --- .../traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp index fe4a921e59b..068b02bdfe6 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp @@ -119,7 +119,7 @@ auto LaneletLoader::resamplePoints( static_cast(lanelet::geometry::length(line_string)); const auto [first_index, second_index] = findNearestIndexPair(target_length); - // Apply linear interpolation + /// @note Apply linear interpolation const lanelet::BasicPoint3d back_point = line_string[first_index]; const lanelet::BasicPoint3d front_point = line_string[second_index]; const auto direction_vector = (front_point - back_point); From 295935bd7c765e3fca81c3b339a72f9d667b94bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:40:31 +0100 Subject: [PATCH 34/62] Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp Co-authored-by: Masaya Kataoka --- .../traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp index 068b02bdfe6..9616de39c78 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp @@ -111,7 +111,7 @@ auto LaneletLoader::resamplePoints( THROW_SEMANTIC_ERROR("findNearestIndexPair(): No nearest point found."); }; - // Create each segment + /// @note Create each segment lanelet::BasicPoints3d resampled_points; for (auto i = 0; i <= num_segments; ++i) { // Find two nearest points From 99beeee8d30b26e848cc93685a0c00361d94c74b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:40:47 +0100 Subject: [PATCH 35/62] Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp Co-authored-by: Masaya Kataoka --- .../traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp index 9616de39c78..f00152e4bad 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp @@ -53,7 +53,7 @@ auto LaneletLoader::overwriteLaneletsCenterline(lanelet::LaneletMapPtr lanelet_m auto generateFineCenterline = [&](const lanelet::ConstLanelet & lanelet_obj) -> lanelet::LineString3d { - // Get length of longer border + /// @note Get length of longer border const auto left_length = static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); const auto right_length = From 4c0cb95b375828f0fb992952f0fa085d071a2768 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:41:18 +0100 Subject: [PATCH 36/62] Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp Co-authored-by: Masaya Kataoka --- .../traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp index f00152e4bad..cab993ce308 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp @@ -68,7 +68,7 @@ auto LaneletLoader::overwriteLaneletsCenterline(lanelet::LaneletMapPtr lanelet_m /// @note Create centerline lanelet::LineString3d centerline(lanelet::utils::getId()); for (size_t i = 0; i < static_cast(num_segments + 1); i++) { - // Add ID for the average point of left and right + /// @note Add ID for the average point of left and right const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2.0; const lanelet::Point3d center_point( lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(), From 707ee46c5c9f86656c1d737d3b154425f96369fd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:41:40 +0100 Subject: [PATCH 37/62] Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp Co-authored-by: Masaya Kataoka --- .../traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp index cab993ce308..c44ac635e9a 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp @@ -130,7 +130,7 @@ auto LaneletLoader::resamplePoints( const auto target_point = back_point + (direction_vector * (target_length - back_length) / segment_length); - // Add to list + /// @note Add to list resampled_points.emplace_back(target_point); } return resampled_points; From 9ef17b3bee4041b780c38d93b67b4647d9cb9a85 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:44:27 +0100 Subject: [PATCH 38/62] Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp Co-authored-by: Masaya Kataoka --- .../traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp index afb8248cbd7..36c8e56d2bf 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp @@ -105,7 +105,7 @@ LaneletWrapper & LaneletWrapper::getInstance() std::lock_guard lock(mutex_); if (!instance) { if (!lanelet_map_path_.empty()) { - /// `new` is intentionally used here instead of `make_unique` since the LaneletWrapper constructor is private + /// @note `new` is intentionally used here instead of `make_unique` since the LaneletWrapper constructor is private instance.reset(new LaneletWrapper(lanelet_map_path_)); } else { THROW_SIMULATION_ERROR( From d62b4ed2e96389289541b9445bb589081ef0ad93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:45:19 +0100 Subject: [PATCH 39/62] Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp Co-authored-by: Masaya Kataoka --- .../traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp index c44ac635e9a..8ac5c2000db 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp @@ -114,7 +114,7 @@ auto LaneletLoader::resamplePoints( /// @note Create each segment lanelet::BasicPoints3d resampled_points; for (auto i = 0; i <= num_segments; ++i) { - // Find two nearest points + /// @note Find two nearest points const double target_length = (static_cast(i) / num_segments) * static_cast(lanelet::geometry::length(line_string)); const auto [first_index, second_index] = findNearestIndexPair(target_length); From 8b2f1b44dd16ee783e64d3a3b02095e3e1048260 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:45:39 +0100 Subject: [PATCH 40/62] Update simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp Co-authored-by: Masaya Kataoka --- simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index bfdc463f5ca..048d59c8afb 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -47,7 +47,7 @@ auto toMapPose(const LaneletPose & lanelet_pose, const bool fill_pitch) -> PoseS math::geometry::normalize(normal_vector) * canonicalized_lanelet_pose->offset; pose_stamped.pose = lanelet_spline->getPose(canonicalized_lanelet_pose->s); pose_stamped.pose.position += offset_transition_vector; - // map orientation + /// @note map orientation const auto tangent_vector = lanelet_spline->getTangentVector(canonicalized_lanelet_pose->s); const auto lanelet_rpy = geometry_msgs::build() From 51e9913e9dfb9303f3da88ab9e971678ae0d43c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:46:04 +0100 Subject: [PATCH 41/62] Update simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp Co-authored-by: Masaya Kataoka --- simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 048d59c8afb..b49d00bd3a9 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -41,7 +41,7 @@ auto toMapPose(const LaneletPose & lanelet_pose, const bool fill_pitch) -> PoseS pose_stamped.header.frame_id = "map"; const auto lanelet_spline = lanelet_map::centerPointsSpline(canonicalized_lanelet_pose->lanelet_id); - // map position + /// @note map position const auto normal_vector = lanelet_spline->getNormalVector(canonicalized_lanelet_pose->s); const auto offset_transition_vector = math::geometry::normalize(normal_vector) * canonicalized_lanelet_pose->offset; From c23d57c9e2d8717f5c54a66f7e475b997824daa6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:46:44 +0100 Subject: [PATCH 42/62] Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp Co-authored-by: Masaya Kataoka --- .../traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 3bf62e09fb0..1b494c54438 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -88,7 +88,7 @@ auto nearbyLaneletIds( LaneletWrapper::map()->laneletLayer, lanelet::BasicPoint2d(point.x, point.y), static_cast(search_count)); - // check for current content, if not empty then optionally apply filter + /// @note check for current content, if not empty then optionally apply filter if (nearest_lanelets.empty() || nearest_lanelets.front().first > distance_thresh) { return {}; } else if (!include_crosswalk) { From aa1ae83207521ceedca6f315b7c8a38b06bd9726 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 15 Jan 2025 12:47:12 +0100 Subject: [PATCH 43/62] Update simulation/traffic_simulator/src/traffic/traffic_controller.cpp Co-authored-by: Masaya Kataoka --- simulation/traffic_simulator/src/traffic/traffic_controller.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 488edc02808..c776787eb6b 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -50,6 +50,7 @@ TrafficController::TrafficController( auto TrafficController::appendAutoSinks(const std::set & auto_sink_entity_types) -> void { + /// @note Hard coded parameter, this value is radius of the traffic sink circle. constexpr double sink_radius{1.0}; for (const auto & [lanelet_id, pose] : lanelet_map::borderlinePoses()) { const auto traffic_sink_config = TrafficSinkConfig( From e9f5d849b7df6dd76a64ca2c445677310858309a Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 15 Jan 2025 14:19:18 +0100 Subject: [PATCH 44/62] ref(traffic_simulator): add comments to lanelet_wrapper::pose, reduce the number of intermediate variables --- .../lanelet_wrapper/lanelet_wrapper.hpp | 2 +- .../lanelet_wrapper/pose.hpp | 18 ++++++++++ .../src/data_type/lanelet_pose.cpp | 9 ++--- .../src/hdmap_utils/hdmap_utils.cpp | 1 + .../src/lanelet_wrapper/lanelet_map.cpp | 8 +++-- .../src/lanelet_wrapper/pose.cpp | 36 ++++++++++--------- 6 files changed, 50 insertions(+), 24 deletions(-) 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 7d01b2a7dee..f8a11a57c56 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 @@ -76,7 +76,7 @@ class RouteCache const lanelet::routing::RoutingGraphConstPtr & routing_graph) -> lanelet::Ids { if (!exists(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change)) { - constexpr int routing_cost_id = 0; + constexpr int routing_cost_id = 0; ///< to use default routing costs: distance along lanelets const auto & from_lanelet = lanelet_map->laneletLayer.get(from_lanelet_id); const auto & to_lanelet = lanelet_map->laneletLayer.get(to_lanelet_id); if (const auto route = routing_graph->getRoute( diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp index 4198371514e..ad0d72fc9ee 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp @@ -57,6 +57,24 @@ auto toLaneletPoses( const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> std::vector; +/** + * @brief Retrieves alternative lanelet poses based on the reference lanelet pose. + * + * This method computes alternative lanelet poses in the previous and next lanelets + * relative to a given reference lanelet pose. It recursively explores the neighboring + * lanelets until no further alternatives are found. The decision of whether a pose belongs + * to a previous or next lanelet is based on the `s` value of the reference pose: + * - If `s` is negative, the pose is assumed to be on the previous lanelet. + * - If `s` exceeds the lanelet length, the pose is assumed to be on the next lanelet. + * - If `s` is within the valid range of the lanelet (from 0 to the lanelet's length), + * the reference lanelet pose is returned directly. + * + * @param reference_lanelet_pose The reference pose on a lanelet, used to determine its position + * and compute alternatives in neighboring lanelets. + * + * @return A vector of alternative `LaneletPose` objects representing poses in the neighboring + * lanelets, or the reference pose if no alternatives are found. + */ auto alternativeLaneletPoses(const LaneletPose & reference_lanelet_pose) -> std::vector; diff --git a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp index 975129d65c9..121bd44af5c 100644 --- a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp +++ b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp @@ -92,10 +92,11 @@ auto CanonicalizedLaneletPose::alignOrientationToLanelet() -> void using math::geometry::convertEulerAngleToQuaternion; using math::geometry::convertQuaternionToEulerAngle; /// @todo it will be changed to route::toSpline(...) - const auto spline = math::geometry::CatmullRomSpline( - lanelet_wrapper::lanelet_map::centerPoints({lanelet_pose_.lanelet_id})); - const auto lanelet_quaternion = spline.getPose(lanelet_pose_.s, true).orientation; - const auto lanelet_rpy = convertQuaternionToEulerAngle(lanelet_quaternion); + const auto lanelet_rpy = convertQuaternionToEulerAngle( + math::geometry::CatmullRomSpline( + lanelet_wrapper::lanelet_map::centerPoints({lanelet_pose_.lanelet_id})) + .getPose(lanelet_pose_.s, true) + .orientation); map_pose_.orientation = convertEulerAngleToQuaternion(geometry_msgs::build() .x(lanelet_rpy.x) diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index af964f22341..cc0db1ee436 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -93,6 +93,7 @@ auto HdMapUtils::countLaneChanges( const traffic_simulator::RoutingConfiguration & routing_configuration) const -> std::optional> { + /// a lane change considers the lanes in the same direction as the original, so ignore the lanes in the opposite direction constexpr bool include_opposite_direction{false}; const auto route = getRoute(from.lanelet_id, to.lanelet_id, routing_configuration); if (route.empty()) { diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 3bf62e09fb0..1d98334bdb2 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -69,6 +69,10 @@ auto nearbyLaneletIds( const Point & point, const double distance_thresh, const bool include_crosswalk, const std::size_t search_count) -> lanelet::Ids { + auto isEmptyOrBeyondThreshold = [&distance_thresh](const auto & lanelets) { + return lanelets.empty() || lanelets.front().first > distance_thresh; + }; + auto excludeSubtypeLanelets = []( const std::vector> & pair_distance_lanelet, @@ -89,7 +93,7 @@ auto nearbyLaneletIds( static_cast(search_count)); // check for current content, if not empty then optionally apply filter - if (nearest_lanelets.empty() || nearest_lanelets.front().first > distance_thresh) { + if (isEmptyOrBeyondThreshold(nearest_lanelets)) { return {}; } else if (!include_crosswalk) { nearest_lanelets = @@ -97,7 +101,7 @@ auto nearbyLaneletIds( } // check again - if (nearest_lanelets.empty() || nearest_lanelets.front().first > distance_thresh) { + if (isEmptyOrBeyondThreshold(nearest_lanelets)) { return {}; } else { lanelet::Ids target_lanelet_ids; diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index d620aa825df..e773e628879 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -42,11 +42,10 @@ auto toMapPose(const LaneletPose & lanelet_pose, const bool fill_pitch) -> PoseS const auto lanelet_spline = lanelet_map::centerPointsSpline(canonicalized_lanelet_pose->lanelet_id); // map position - const auto normal_vector = lanelet_spline->getNormalVector(canonicalized_lanelet_pose->s); - const auto offset_transition_vector = - math::geometry::normalize(normal_vector) * canonicalized_lanelet_pose->offset; pose_stamped.pose = lanelet_spline->getPose(canonicalized_lanelet_pose->s); - pose_stamped.pose.position += offset_transition_vector; + pose_stamped.pose.position += + math::geometry::normalize(lanelet_spline->getNormalVector(canonicalized_lanelet_pose->s)) * + canonicalized_lanelet_pose->offset; // map orientation const auto tangent_vector = lanelet_spline->getTangentVector(canonicalized_lanelet_pose->s); const auto lanelet_rpy = @@ -70,18 +69,19 @@ auto toMapPose(const LaneletPose & lanelet_pose, const bool fill_pitch) -> PoseS auto isAltitudeMatching(const double current_altitude, const double target_altitude) -> bool { - /* - Using a fixed `altitude_threshold` value of 1.0 [m] is justified because the - entity's Z-position is always relative to its base. This eliminates the - need to dynamically adjust the threshold based on the entity's dimensions, - ensuring consistent altitude matching regardless of the entity type. - - The position of the entity is defined relative to its base, typically - the center of the rear axle projected onto the ground in the case of vehicles. - - There is no technical basis for this value; it was determined based on - experiments. - */ + /** + * @brief Justification for using a fixed `altitude_threshold` value of 1.0 [m]. + * + * Using a fixed `altitude_threshold` value of 1.0 [m] is justified because the + * entity's Z-position is always relative to its base. This eliminates the need + * to dynamically adjust the threshold based on the entity's dimensions, ensuring + * consistent altitude matching regardless of the entity type. + * + * The position of the entity is defined relative to its base, typically the center + * of the rear axle projected onto the ground in the case of vehicles. + * + * @note There is no technical basis for this value; it was determined based on experiments. + */ static constexpr double altitude_threshold = 1.0; return std::abs(current_altitude - target_altitude) <= altitude_threshold; } @@ -90,7 +90,9 @@ auto toLaneletPose( const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance) -> std::optional { - constexpr double yaw_threshold = 0.25; + /// yaw_threshold [rad] is used to determine whether the entity is going straight, + /// it defines the maximum allowed rotation with respect to the lanelet centreline + constexpr double yaw_threshold = 0.25; ///< M_PI * 0.25 ≈ 0.7854 rad -> 45 deegres, const auto lanelet_spline = lanelet_map::centerPointsSpline(lanelet_id); if (const auto lanelet_pose_s = lanelet_spline->getSValue(map_pose, matching_distance); !lanelet_pose_s) { From 79517b6cf865163076c399cbafdf96c0a8ac20b3 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 15 Jan 2025 16:21:30 +0100 Subject: [PATCH 45/62] ref(lanelet_wrapper): add toLaneletPose yaw_threshold description --- .../src/lanelet_wrapper/pose.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index e773e628879..a25e6292e19 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -90,10 +90,11 @@ auto toLaneletPose( const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance) -> std::optional { - /// yaw_threshold [rad] is used to determine whether the entity is going straight, - /// it defines the maximum allowed rotation with respect to the lanelet centreline - constexpr double yaw_threshold = 0.25; ///< M_PI * 0.25 ≈ 0.7854 rad -> 45 deegres, - const auto lanelet_spline = lanelet_map::centerPointsSpline(lanelet_id); + /// @note yaw_threshold_deg is used to determine whether the entity is going straight, + /// it defines the maximum allowed rotation with respect to the lanelet centreline. + constexpr double yaw_threshold_deg = 45.0 + + const auto lanelet_spline = lanelet_map::centerPointsSpline(lanelet_id); if (const auto lanelet_pose_s = lanelet_spline->getSValue(map_pose, matching_distance); !lanelet_pose_s) { return std::nullopt; @@ -101,10 +102,12 @@ auto toLaneletPose( !isAltitudeMatching(map_pose.position.z, pose_on_centerline.position.z)) { return std::nullopt; } else { + const double yaw_range_min_rad = M_PI * yaw_threshold_deg / 180.0; + const double yaw_range_max_rad = M_PI - yaw_range_min_rad; if (const auto lanelet_pose_rpy = math::geometry::convertQuaternionToEulerAngle( math::geometry::getRotation(pose_on_centerline.orientation, map_pose.orientation)); - std::fabs(lanelet_pose_rpy.z) > M_PI * yaw_threshold && - std::fabs(lanelet_pose_rpy.z) < M_PI * (1 - yaw_threshold)) { + std::fabs(lanelet_pose_rpy.z) > yaw_range_max_rad || + std::fabs(lanelet_pose_rpy.z) < yaw_range_min_rad) { return std::nullopt; } else { double lanelet_pose_offset = std::sqrt( From ccb059f20d3f7944f1c7d1abfc3a9ecfabbcf660 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 15 Jan 2025 16:28:35 +0100 Subject: [PATCH 46/62] fix(lanelet_wrapper): fix typo centerline --- simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 1054af4233b..4772a2ec65b 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -91,7 +91,7 @@ auto toLaneletPose( -> std::optional { /// @note yaw_threshold_deg is used to determine whether the entity is going straight, - /// it defines the maximum allowed rotation with respect to the lanelet centreline. + /// it defines the maximum allowed rotation with respect to the lanelet centerline. constexpr double yaw_threshold_deg = 45.0 const auto lanelet_spline = lanelet_map::centerPointsSpline(lanelet_id); From 321564d965645227a9a31c19fd37357f70653427 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 15 Jan 2025 16:41:15 +0100 Subject: [PATCH 47/62] fix(lanelet_wrapper): fix pose toLaneletPose typo ";" --- .../include/traffic_simulator/lanelet_wrapper/pose.hpp | 1 - simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp | 5 ++--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp index ad0d72fc9ee..278dbee3c47 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp @@ -92,7 +92,6 @@ auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose) auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose, const lanelet::Ids & route_lanelets) -> std::tuple, std::optional>; -// used only by this namespace auto matchToLane( const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk, const double matching_distance = 1.0, diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 4772a2ec65b..c3f07af225b 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -92,9 +92,9 @@ auto toLaneletPose( { /// @note yaw_threshold_deg is used to determine whether the entity is going straight, /// it defines the maximum allowed rotation with respect to the lanelet centerline. - constexpr double yaw_threshold_deg = 45.0 + constexpr double yaw_threshold_deg = 45.0; - const auto lanelet_spline = lanelet_map::centerPointsSpline(lanelet_id); + const auto lanelet_spline = lanelet_map::centerPointsSpline(lanelet_id); if (const auto lanelet_pose_s = lanelet_spline->getSValue(map_pose, matching_distance); !lanelet_pose_s) { return std::nullopt; @@ -391,7 +391,6 @@ auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose, const lanelet::Id return {canonicalized_lanelet_pose, std::nullopt}; } -// used only by this namespace auto matchToLane( const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk, const double matching_distance, const double reduction_ratio, const RoutingGraphType type) From 380eb7be60f45ee9d3ff89b624954fc87da7b7d0 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 15 Jan 2025 16:48:53 +0100 Subject: [PATCH 48/62] doc(lanelet_wrapper): improve lanelet_wrappe::pose::canonicalizeLaneletPose description --- .../src/hdmap_utils/hdmap_utils.cpp | 2 -- .../src/lanelet_wrapper/pose.cpp | 18 ++++++++++++++++-- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index cc0db1ee436..9af7055df86 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -85,8 +85,6 @@ HdMapUtils::HdMapUtils( overwriteLaneletsCenterline(); } -// 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, diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index c3f07af225b..3e2fb29b063 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -330,8 +330,22 @@ auto alongLaneletPose( } } -// If route is not specified, the lanelet_id with the lowest array index is used as a candidate for -// canonicalize destination. +/** + * @brief Canonicalizes a given LaneletPose by adjusting the longitudinal position (s) to ensure it + * lies within the bounds of the specified lanelet. If the position is out of bounds, it + * traverses to previous or next lanelets to find the canonicalized position. + * + * If the provided pose has a longitudinal position (s) less than 0, the function traverses + * to previous lanelets until a valid position is found or no previous lanelets exist. + * + * If the longitudinal position (s) exceeds the length of the current lanelet, the function traverses + * to next lanelets until the position is valid or no next lanelets exist. + * + * @param lanelet_pose The input LaneletPose to canonicalize, containing a lanelet ID and longitudinal position (s). + * @return A tuple where: + * - The first element is an optional canonicalized LaneletPose (std::nullopt if canonicalization fails). + * - The second element is an optional lanelet ID where the process stopped (std::nullopt if successful). + */ auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose) -> std::tuple, std::optional> { From f3c3e9b15df3a69d4507079c1d85448cb455b76c Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 15 Jan 2025 18:25:09 +0100 Subject: [PATCH 49/62] fix(lanelet_wrapper): fix toLaneletPose --- simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 3e2fb29b063..9a1f4234e3a 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -106,8 +106,8 @@ auto toLaneletPose( const double yaw_range_max_rad = M_PI - yaw_range_min_rad; if (const auto lanelet_pose_rpy = math::geometry::convertQuaternionToEulerAngle( math::geometry::getRotation(pose_on_centerline.orientation, map_pose.orientation)); - std::fabs(lanelet_pose_rpy.z) > yaw_range_max_rad || - std::fabs(lanelet_pose_rpy.z) < yaw_range_min_rad) { + std::fabs(lanelet_pose_rpy.z) > yaw_range_min_rad || + std::fabs(lanelet_pose_rpy.z) < yaw_range_max_rad) { return std::nullopt; } else { double lanelet_pose_offset = std::sqrt( From 1ea2dddf0db2707df19191eea6f290f04ccb2b9d Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 16 Jan 2025 10:39:06 +0100 Subject: [PATCH 50/62] Fix lanelet pose matching calculation bug Signed-off-by: Mateusz Palczuk --- simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 9a1f4234e3a..06896df5dff 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -106,7 +106,7 @@ auto toLaneletPose( const double yaw_range_max_rad = M_PI - yaw_range_min_rad; if (const auto lanelet_pose_rpy = math::geometry::convertQuaternionToEulerAngle( math::geometry::getRotation(pose_on_centerline.orientation, map_pose.orientation)); - std::fabs(lanelet_pose_rpy.z) > yaw_range_min_rad || + std::fabs(lanelet_pose_rpy.z) > yaw_range_min_rad and std::fabs(lanelet_pose_rpy.z) < yaw_range_max_rad) { return std::nullopt; } else { @@ -331,8 +331,8 @@ auto alongLaneletPose( } /** - * @brief Canonicalizes a given LaneletPose by adjusting the longitudinal position (s) to ensure it - * lies within the bounds of the specified lanelet. If the position is out of bounds, it + * @brief Canonicalizes a given LaneletPose by adjusting the longitudinal position (s) to ensure it + * lies within the bounds of the specified lanelet. If the position is out of bounds, it * traverses to previous or next lanelets to find the canonicalized position. * * If the provided pose has a longitudinal position (s) less than 0, the function traverses From bc5a79d727781df3c4c934fa722eb9da9e6f5162 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 16 Jan 2025 10:53:16 +0100 Subject: [PATCH 51/62] Perform calculations at compile time in lanelet_wrapper::pose::toLaneletPose Signed-off-by: Mateusz Palczuk --- simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 06896df5dff..4d3151d4b02 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -102,8 +102,8 @@ auto toLaneletPose( !isAltitudeMatching(map_pose.position.z, pose_on_centerline.position.z)) { return std::nullopt; } else { - const double yaw_range_min_rad = M_PI * yaw_threshold_deg / 180.0; - const double yaw_range_max_rad = M_PI - yaw_range_min_rad; + constexpr double yaw_range_min_rad = M_PI * yaw_threshold_deg / 180.0; + constexpr double yaw_range_max_rad = M_PI - yaw_range_min_rad; if (const auto lanelet_pose_rpy = math::geometry::convertQuaternionToEulerAngle( math::geometry::getRotation(pose_on_centerline.orientation, map_pose.orientation)); std::fabs(lanelet_pose_rpy.z) > yaw_range_min_rad and From 73e2afb0f4d0f8f7440242a2e6f1bb0d042a8a84 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 16 Jan 2025 11:04:12 +0100 Subject: [PATCH 52/62] Add comment explaining what `lanelet_map::borderlinePoses` does Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/utils/lanelet_map.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp index 4edb58df8aa..a93876144ee 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp @@ -43,6 +43,8 @@ inline auto laneletAltitude(Ts &&... xs) return lanelet_wrapper::lanelet_map::laneletAltitude(std::forward(xs)...); } +/// @brief Calculates all poses on the map that have no next lanelet (dead ends) +/// @return A vector of final poses and their corresponding lanelet IDs auto borderlinePoses() -> std::vector>; } // namespace lanelet_map } // namespace traffic_simulator From 94a331507cb9b7afb5473bf2b6b9da749485a26c Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 16 Jan 2025 11:39:53 +0100 Subject: [PATCH 53/62] Rename `borderlinePoses` -> `noNextLaneletPoses` Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/utils/lanelet_map.hpp | 2 +- simulation/traffic_simulator/src/traffic/traffic_controller.cpp | 2 +- simulation/traffic_simulator/src/utils/lanelet_map.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp index a93876144ee..94e87836ad3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp @@ -45,7 +45,7 @@ inline auto laneletAltitude(Ts &&... xs) /// @brief Calculates all poses on the map that have no next lanelet (dead ends) /// @return A vector of final poses and their corresponding lanelet IDs -auto borderlinePoses() -> std::vector>; +auto noNextLaneletPoses() -> std::vector>; } // namespace lanelet_map } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__UTILS__LANELET_MAP_HPP_ diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index d35a38a6cf9..5260db7c2e9 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -53,7 +53,7 @@ auto TrafficController::appendAutoSinks(const std::set & auto_sink { /// @note Hard coded parameter, this value is radius of the traffic sink circle. constexpr double sink_radius{1.0}; - for (const auto & [lanelet_id, pose] : lanelet_map::borderlinePoses()) { + for (const auto & [lanelet_id, pose] : lanelet_map::noNextLaneletPoses()) { const auto traffic_sink_config = TrafficSinkConfig( sink_radius, pose.position, auto_sink_entity_types, std::make_optional(lanelet_id)); addModule(despawn_, entity_manager_ptr_, traffic_sink_config); diff --git a/simulation/traffic_simulator/src/utils/lanelet_map.cpp b/simulation/traffic_simulator/src/utils/lanelet_map.cpp index 81566a28cc1..2c6aefe4822 100644 --- a/simulation/traffic_simulator/src/utils/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/utils/lanelet_map.cpp @@ -23,7 +23,7 @@ auto laneletLength(const lanelet::Id lanelet_id) -> double return lanelet_wrapper::lanelet_map::laneletLength(lanelet_id); } -auto borderlinePoses() -> std::vector> +auto noNextLaneletPoses() -> std::vector> { std::vector> borderline_poses; for (const auto & lanelet_id : lanelet_wrapper::lanelet_map::laneletIds()) { From 54c45850e4879e37ae3e962225ce0623d7af4c2c Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 16 Jan 2025 12:01:55 +0100 Subject: [PATCH 54/62] Rename variable to match function name change Signed-off-by: Mateusz Palczuk --- simulation/traffic_simulator/src/utils/lanelet_map.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/simulation/traffic_simulator/src/utils/lanelet_map.cpp b/simulation/traffic_simulator/src/utils/lanelet_map.cpp index 2c6aefe4822..5c7e5133932 100644 --- a/simulation/traffic_simulator/src/utils/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/utils/lanelet_map.cpp @@ -25,16 +25,16 @@ auto laneletLength(const lanelet::Id lanelet_id) -> double auto noNextLaneletPoses() -> std::vector> { - std::vector> borderline_poses; + std::vector> no_next_lanelet_poses; for (const auto & lanelet_id : lanelet_wrapper::lanelet_map::laneletIds()) { if (lanelet_wrapper::lanelet_map::nextLaneletIds(lanelet_id).empty()) { LaneletPose lanelet_pose; lanelet_pose.lanelet_id = lanelet_id; lanelet_pose.s = lanelet_map::laneletLength(lanelet_id); - borderline_poses.emplace_back(lanelet_id, pose::toMapPose(lanelet_pose)); + no_next_lanelet_poses.emplace_back(lanelet_id, pose::toMapPose(lanelet_pose)); } } - return borderline_poses; + return no_next_lanelet_poses; } } // namespace lanelet_map } // namespace traffic_simulator From 1fca5858b4e1a9cd2fe0c1fc63c21c268d8ccf79 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 16 Jan 2025 12:48:14 +0100 Subject: [PATCH 55/62] Add comment explaining parameters Signed-off-by: Mateusz Palczuk --- simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 4d3151d4b02..8be03186138 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -144,6 +144,8 @@ auto toLaneletPose( const Pose & map_pose, const bool include_crosswalk, const double matching_distance) -> std::optional { + /// @note Hardcoded parameter, this value has no technical basis and is determined based on experimentation. + /// @todo Add doxygen comments as soon as you know the meaning and rationale of the value. constexpr double distance_threshold{0.1}; constexpr std::size_t search_count{5}; const auto nearby_lanelet_ids = lanelet_map::nearbyLaneletIds( From 3ca8c96547adab0f1cadfc85521b678141698b0c Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 16 Jan 2025 15:27:42 +0100 Subject: [PATCH 56/62] Move lanelet_wrapper cache classes member values and some functions to private Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp | 3 +++ 1 file changed, 3 insertions(+) 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 76db2530331..b830419faee 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 @@ -111,6 +111,7 @@ class RouteCache } } +private: std::unordered_map, lanelet::Ids> data_; std::mutex mutex_; @@ -172,6 +173,7 @@ class CenterPointsCache return splines_.at(lanelet_id); } +private: std::unordered_map> data_; std::unordered_map> splines_; std::mutex mutex_; @@ -234,6 +236,7 @@ class LaneletLengthCache return data_.at(lanelet_id); } +private: std::unordered_map data_; std::mutex mutex_; From 06964c3415b31808cb2432da4b7af0c1c3b04549 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 16 Jan 2025 15:29:58 +0100 Subject: [PATCH 57/62] Clean cache code - move cache classes member variables to the end of class declaration Signed-off-by: Mateusz Palczuk --- .../lanelet_wrapper/lanelet_wrapper.hpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) 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 b830419faee..36ee5e67fdc 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 @@ -112,9 +112,6 @@ class RouteCache } private: - std::unordered_map, lanelet::Ids> data_; - std::mutex mutex_; - auto exists(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) -> bool { std::lock_guard lock(mutex_); @@ -129,6 +126,9 @@ class RouteCache std::lock_guard lock(mutex_); data_[{from, to, allow_lane_change}] = route; } + + std::unordered_map, lanelet::Ids> data_; + std::mutex mutex_; }; class CenterPointsCache @@ -174,10 +174,6 @@ class CenterPointsCache } private: - std::unordered_map> data_; - std::unordered_map> splines_; - std::mutex mutex_; - auto exists(const lanelet::Id lanelet_id) -> bool { std::lock_guard lock(mutex_); @@ -212,6 +208,10 @@ class CenterPointsCache } return center_points; } + + std::unordered_map> data_; + std::unordered_map> splines_; + std::mutex mutex_; }; class LaneletLengthCache @@ -237,9 +237,6 @@ class LaneletLengthCache } private: - std::unordered_map data_; - std::mutex mutex_; - auto exists(const lanelet::Id lanelet_id) -> bool { std::lock_guard lock(mutex_); @@ -251,6 +248,9 @@ class LaneletLengthCache std::lock_guard lock(mutex_); data_[lanelet_id] = length; } + + std::unordered_map data_; + std::mutex mutex_; }; struct TrafficRulesWithRoutingGraph From 41f9dd1e226ea827444aed1d01c207d10b9b61fa Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 16 Jan 2025 15:54:12 +0100 Subject: [PATCH 58/62] Refactor lanelet_wrapper cache classes: make top level public member functions acquire resources and make non-public member functions NOT thread safe This change is a proposal of improving thread safety - now the whole bodies of public member functions can be executed at once without the risk of resources being modified in the middle of the function execution Signed-off-by: Mateusz Palczuk --- .../lanelet_wrapper/lanelet_wrapper.hpp | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) 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 36ee5e67fdc..a6ffff97785 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 @@ -75,6 +75,7 @@ class RouteCache const lanelet::LaneletMapPtr & lanelet_map, const RoutingConfiguration & routing_configuration, const lanelet::routing::RoutingGraphConstPtr & routing_graph) -> lanelet::Ids { + std::lock_guard lock(mutex_); if (!exists(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change)) { constexpr int routing_cost_id = 0; ///< to use default routing costs: distance along lanelets const auto & from_lanelet = lanelet_map->laneletLayer.get(from_lanelet_id); @@ -94,36 +95,35 @@ class RouteCache shortest_path_ids); } } - std::lock_guard lock(mutex_); return data_.at({from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change}); } auto getRoute(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) -> decltype(auto) { + std::lock_guard lock(mutex_); if (!exists(from, to, allow_lane_change)) { THROW_SIMULATION_ERROR( "route from : ", from, " to : ", to, (allow_lane_change ? " with" : " without"), " lane change does not exists on route cache."); } else { - std::lock_guard lock(mutex_); return data_.at({from, to, allow_lane_change}); } } private: + /// @note This function is not thread safe auto exists(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) -> bool { - std::lock_guard lock(mutex_); std::tuple key = {from, to, allow_lane_change}; return data_.find(key) != data_.end(); } + /// @note This function is not thread safe auto appendData( const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change, const lanelet::Ids & route) -> void { - std::lock_guard lock(mutex_); data_[{from, to, allow_lane_change}] = route; } @@ -136,29 +136,29 @@ class CenterPointsCache public: auto centerPoints(lanelet::Id lanelet_id) -> decltype(auto) { + std::lock_guard lock(mutex_); if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); return data_.at(lanelet_id); } auto centerPointsSpline(lanelet::Id lanelet_id) -> decltype(auto) { + std::lock_guard lock(mutex_); if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); return splines_.at(lanelet_id); } auto getCenterPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) -> std::vector { + std::lock_guard lock(mutex_); if (!exists(lanelet_id)) { appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); } - std::lock_guard lock(mutex_); return data_.at(lanelet_id); } @@ -166,27 +166,28 @@ class CenterPointsCache const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) -> std::shared_ptr { + std::lock_guard lock(mutex_); if (!exists(lanelet_id)) { appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); } - std::lock_guard lock(mutex_); return splines_.at(lanelet_id); } private: + /// @note This function is not thread safe auto exists(const lanelet::Id lanelet_id) -> bool { - std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } + /// @note This function is not thread safe auto appendData(const lanelet::Id lanelet_id, const std::vector & route) -> void { - std::lock_guard lock(mutex_); data_[lanelet_id] = route; splines_[lanelet_id] = std::make_shared(route); } + /// @note This function is not thread safe auto centerPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) const -> std::vector { @@ -219,33 +220,33 @@ class LaneletLengthCache public: auto getLength(lanelet::Id lanelet_id) { + std::lock_guard lock(mutex_); if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("length of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); return data_.at(lanelet_id); } auto getLength(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) -> double { + std::lock_guard lock(mutex_); if (!exists(lanelet_id)) { appendData( lanelet_id, lanelet::utils::getLaneletLength2d(lanelet_map->laneletLayer.get(lanelet_id))); } - std::lock_guard lock(mutex_); return data_.at(lanelet_id); } private: + /// @note This function is not thread safe auto exists(const lanelet::Id lanelet_id) -> bool { - std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } + /// @note This function is not thread safe auto appendData(const lanelet::Id lanelet_id, double length) -> void { - std::lock_guard lock(mutex_); data_[lanelet_id] = length; } From f4c4503d2325e9d884c81763386ac2345a8ce038 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Mon, 20 Jan 2025 10:52:20 +0100 Subject: [PATCH 59/62] Adjust comments to use "/// @note" convention Signed-off-by: Mateusz Palczuk --- .../lanelet_wrapper/lanelet_wrapper.hpp | 3 ++- .../traffic_simulator/src/hdmap_utils/hdmap_utils.cpp | 2 +- .../src/lanelet_wrapper/lanelet_map.cpp | 2 +- .../traffic_simulator/src/lanelet_wrapper/pose.cpp | 10 +++++----- 4 files changed, 9 insertions(+), 8 deletions(-) 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 a6ffff97785..990620a37eb 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 @@ -77,7 +77,8 @@ class RouteCache { std::lock_guard lock(mutex_); if (!exists(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change)) { - constexpr int routing_cost_id = 0; ///< to use default routing costs: distance along lanelets + /// @note to use default routing costs: distance along lanelets + constexpr int routing_cost_id = 0; const auto & from_lanelet = lanelet_map->laneletLayer.get(from_lanelet_id); const auto & to_lanelet = lanelet_map->laneletLayer.get(to_lanelet_id); if (const auto route = routing_graph->getRoute( diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 9af7055df86..8a4c5043465 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -91,7 +91,7 @@ auto HdMapUtils::countLaneChanges( const traffic_simulator::RoutingConfiguration & routing_configuration) const -> std::optional> { - /// a lane change considers the lanes in the same direction as the original, so ignore the lanes in the opposite direction + /// @note a lane change considers the lanes in the same direction as the original, so ignore the lanes in the opposite direction constexpr bool include_opposite_direction{false}; const auto route = getRoute(from.lanelet_id, to.lanelet_id, routing_configuration); if (route.empty()) { diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 56e91c5ff06..7a8d673cd5f 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -100,7 +100,7 @@ auto nearbyLaneletIds( excludeSubtypeLanelets(nearest_lanelets, lanelet::AttributeValueString::Crosswalk); } - // check again + /// @note check again if (isEmptyOrBeyondThreshold(nearest_lanelets)) { return {}; } else { diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 8be03186138..a0688cd5055 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -381,13 +381,13 @@ auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose, const lanelet::Id -> std::tuple, std::optional> { if (lanelet_pose.s < 0) { - // When canonicalizing to backward lanelet_id, do not consider route + /// @note When canonicalizing to backward lanelet_id, do not consider route return canonicalizeLaneletPose(lanelet_pose); } auto canonicalized_lanelet_pose = lanelet_pose; while (canonicalized_lanelet_pose.s > lanelet_map::laneletLength(canonicalized_lanelet_pose.lanelet_id)) { - // When canonicalizing to forward lanelet_id, consider route + /// @note When canonicalizing to forward lanelet_id, consider route bool found_next_lanelet_in_route = false; for (const auto & next_lanelet_id : lanelet_map::nextLaneletIds(canonicalized_lanelet_pose.lanelet_id)) { @@ -427,21 +427,21 @@ auto matchToLane( } return absolute_hull_polygon; }; - // prepare object for matching + /// @note prepare object for matching const auto yaw = math::geometry::convertQuaternionToEulerAngle(map_pose.orientation).z; lanelet::matching::Object2d bounding_box_object; bounding_box_object.pose.translation() = lanelet::BasicPoint2d(map_pose.position.x, map_pose.position.y); bounding_box_object.pose.linear() = Eigen::Rotation2D(yaw).matrix(); bounding_box_object.absoluteHull = absoluteHullPolygon(bounding_box_object.pose); - // find matches and optionally filter + /// @note find matches and optionally filter auto matches = lanelet::matching::getDeterministicMatches( *LaneletWrapper::map(), bounding_box_object, matching_distance); if (!include_crosswalk) { matches = lanelet::matching::removeNonRuleCompliantMatches(matches, LaneletWrapper::trafficRules(type)); } - // find best match (minimize offset) + /// @note find best match (minimize offset) if (matches.empty()) { return std::nullopt; } else { From b3eeab579528d5f4440d97f85fa39efa817ab7f7 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Mon, 20 Jan 2025 10:59:28 +0100 Subject: [PATCH 60/62] Remove unnecessary comments Signed-off-by: Mateusz Palczuk --- .../src/lanelet_wrapper/lanelet_loader.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp index 8ac5c2000db..59295dd6146 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp @@ -94,12 +94,11 @@ auto LaneletLoader::resamplePoints( auto findNearestIndexPair = [&](const double target_length) -> std::pair { const auto N = accumulated_lengths.size(); - if (target_length < accumulated_lengths.at(1)) { // Front + if (target_length < accumulated_lengths.at(1)) { return std::make_pair(0, 1); - } else if (target_length > accumulated_lengths.at(N - 2)) { // Back + } else if (target_length > accumulated_lengths.at(N - 2)) { return std::make_pair(N - 2, N - 1); - } else // Middle - { + } else { for (size_t i = 1; i < N; ++i) { if ( accumulated_lengths.at(i - 1) <= target_length && From fa8a55200147ae777e2c9637fb1cee4abf3698d4 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 22 Jan 2025 09:43:31 +0100 Subject: [PATCH 61/62] Revert "Refactor lanelet_wrapper cache classes: make top level public member functions acquire resources and make non-public member functions NOT thread safe" This reverts commit 41f9dd1e226ea827444aed1d01c207d10b9b61fa. Signed-off-by: Mateusz Palczuk --- .../lanelet_wrapper/lanelet_wrapper.hpp | 29 +++++++++---------- 1 file changed, 14 insertions(+), 15 deletions(-) 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 990620a37eb..aa0db059390 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 @@ -75,7 +75,6 @@ class RouteCache const lanelet::LaneletMapPtr & lanelet_map, const RoutingConfiguration & routing_configuration, const lanelet::routing::RoutingGraphConstPtr & routing_graph) -> lanelet::Ids { - std::lock_guard lock(mutex_); if (!exists(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change)) { /// @note to use default routing costs: distance along lanelets constexpr int routing_cost_id = 0; @@ -96,35 +95,36 @@ class RouteCache shortest_path_ids); } } + std::lock_guard lock(mutex_); return data_.at({from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change}); } auto getRoute(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) -> decltype(auto) { - std::lock_guard lock(mutex_); if (!exists(from, to, allow_lane_change)) { THROW_SIMULATION_ERROR( "route from : ", from, " to : ", to, (allow_lane_change ? " with" : " without"), " lane change does not exists on route cache."); } else { + std::lock_guard lock(mutex_); return data_.at({from, to, allow_lane_change}); } } private: - /// @note This function is not thread safe auto exists(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) -> bool { + std::lock_guard lock(mutex_); std::tuple key = {from, to, allow_lane_change}; return data_.find(key) != data_.end(); } - /// @note This function is not thread safe auto appendData( const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change, const lanelet::Ids & route) -> void { + std::lock_guard lock(mutex_); data_[{from, to, allow_lane_change}] = route; } @@ -137,29 +137,29 @@ class CenterPointsCache public: auto centerPoints(lanelet::Id lanelet_id) -> decltype(auto) { - std::lock_guard lock(mutex_); if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); } + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } auto centerPointsSpline(lanelet::Id lanelet_id) -> decltype(auto) { - std::lock_guard lock(mutex_); if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); } + std::lock_guard lock(mutex_); return splines_.at(lanelet_id); } auto getCenterPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) -> std::vector { - std::lock_guard lock(mutex_); if (!exists(lanelet_id)) { appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); } + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } @@ -167,28 +167,27 @@ class CenterPointsCache const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) -> std::shared_ptr { - std::lock_guard lock(mutex_); if (!exists(lanelet_id)) { appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); } + std::lock_guard lock(mutex_); return splines_.at(lanelet_id); } private: - /// @note This function is not thread safe auto exists(const lanelet::Id lanelet_id) -> bool { + std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } - /// @note This function is not thread safe auto appendData(const lanelet::Id lanelet_id, const std::vector & route) -> void { + std::lock_guard lock(mutex_); data_[lanelet_id] = route; splines_[lanelet_id] = std::make_shared(route); } - /// @note This function is not thread safe auto centerPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) const -> std::vector { @@ -221,33 +220,33 @@ class LaneletLengthCache public: auto getLength(lanelet::Id lanelet_id) { - std::lock_guard lock(mutex_); if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("length of : ", lanelet_id, " does not exists on route cache."); } + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } auto getLength(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) -> double { - std::lock_guard lock(mutex_); if (!exists(lanelet_id)) { appendData( lanelet_id, lanelet::utils::getLaneletLength2d(lanelet_map->laneletLayer.get(lanelet_id))); } + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } private: - /// @note This function is not thread safe auto exists(const lanelet::Id lanelet_id) -> bool { + std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } - /// @note This function is not thread safe auto appendData(const lanelet::Id lanelet_id, double length) -> void { + std::lock_guard lock(mutex_); data_[lanelet_id] = length; } From 1649df06ab880b6e8a16a810c2167b0e2246903a Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 23 Jan 2025 10:34:37 +0100 Subject: [PATCH 62/62] Add reader functions to cache classes to mitigate direct data members access and having to lock the mutex in different places Signed-off-by: Mateusz Palczuk --- .../lanelet_wrapper/lanelet_wrapper.hpp | 49 +++++++++++++------ 1 file changed, 33 insertions(+), 16 deletions(-) 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 aa0db059390..b0bfca88a94 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 @@ -95,8 +95,7 @@ class RouteCache shortest_path_ids); } } - std::lock_guard lock(mutex_); - return data_.at({from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change}); + return readData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change); } auto getRoute(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) @@ -107,8 +106,7 @@ class RouteCache "route from : ", from, " to : ", to, (allow_lane_change ? " with" : " without"), " lane change does not exists on route cache."); } else { - std::lock_guard lock(mutex_); - return data_.at({from, to, allow_lane_change}); + return readData(from, to, allow_lane_change); } } @@ -120,6 +118,13 @@ class RouteCache return data_.find(key) != data_.end(); } + auto readData(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) + -> lanelet::Ids + { + std::lock_guard lock(mutex_); + return data_.at({from, to, allow_lane_change}); + } + auto appendData( const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change, const lanelet::Ids & route) -> void @@ -140,8 +145,7 @@ class CenterPointsCache if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); - return data_.at(lanelet_id); + return readData(lanelet_id); } auto centerPointsSpline(lanelet::Id lanelet_id) -> decltype(auto) @@ -149,8 +153,7 @@ class CenterPointsCache if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); - return splines_.at(lanelet_id); + return readDataSpline(lanelet_id); } auto getCenterPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) @@ -159,8 +162,7 @@ class CenterPointsCache if (!exists(lanelet_id)) { appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); } - std::lock_guard lock(mutex_); - return data_.at(lanelet_id); + return readData(lanelet_id); } auto getCenterPointsSpline( @@ -170,8 +172,7 @@ class CenterPointsCache if (!exists(lanelet_id)) { appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); } - std::lock_guard lock(mutex_); - return splines_.at(lanelet_id); + return readDataSpline(lanelet_id); } private: @@ -181,6 +182,18 @@ class CenterPointsCache return data_.find(lanelet_id) != data_.end(); } + auto readData(const lanelet::Id lanelet_id) -> std::vector + { + std::lock_guard lock(mutex_); + return data_.at(lanelet_id); + } + + auto readDataSpline(const lanelet::Id lanelet_id) -> std::shared_ptr + { + std::lock_guard lock(mutex_); + return splines_.at(lanelet_id); + } + auto appendData(const lanelet::Id lanelet_id, const std::vector & route) -> void { std::lock_guard lock(mutex_); @@ -223,8 +236,7 @@ class LaneletLengthCache if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("length of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); - return data_.at(lanelet_id); + return readData(lanelet_id); } auto getLength(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) -> double @@ -233,8 +245,7 @@ class LaneletLengthCache appendData( lanelet_id, lanelet::utils::getLaneletLength2d(lanelet_map->laneletLayer.get(lanelet_id))); } - std::lock_guard lock(mutex_); - return data_.at(lanelet_id); + return readData(lanelet_id); } private: @@ -244,6 +255,12 @@ class LaneletLengthCache return data_.find(lanelet_id) != data_.end(); } + auto readData(const lanelet::Id lanelet_id) -> double + { + std::lock_guard lock(mutex_); + return data_.at(lanelet_id); + } + auto appendData(const lanelet::Id lanelet_id, double length) -> void { std::lock_guard lock(mutex_);