From ec740774c38f2882e0931cdbf0ddc0d0690e4d2c Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 15 Aug 2024 10:16:09 +0900 Subject: [PATCH] perf(autoware_map_based_prediction autoware_universe_utils): improve performance of map_based_prediction (#1464) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix(autoware_map_based_prediction): fix argument order (#8031) fix(autoware_map_based_prediction): fix argument order in call `getFrenetPoint()` Signed-off-by: yucedagonurcan Co-authored-by: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> * perf(map_based_prediction): remove unncessary withinRoadLanelet() (#8403) Signed-off-by: Mamoru Sobue * perf(map_based_prediction): create a fence LineString layer and use rtree query (#8406) use fence layer Signed-off-by: Mamoru Sobue * perf(map_based_prediction): apply lerp instead of spline (#8416) perf: apply lerp interpolation instead of spline Signed-off-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> * perf(autoware_map_based_prediction): improve orientation calculation and resample converted path (#8427) * refactor: improve orientation calculation and resample converted path with linear interpolation Simplify the calculation of the orientation for each pose in the convertPathType function by directly calculating the sine and cosine of half the yaw angle. This improves efficiency and readability. Also, improve the resampling of the converted path by using linear interpolation for better performance. Signed-off-by: Taekjin LEE * Update perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> * Update perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> --------- Signed-off-by: Taekjin LEE Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> * perf(map_based_prediction): improve world to map transform calculation (#8413) * perf(map_based_prediction): improve world to map transform calculation 1. remove unused transforms 2. make transform loading late as possible Signed-off-by: Taekjin LEE * perf(map_based_prediction): get transform only when it is necessary Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE * feat(autoware_universe_utils): add LRU Cache (#8456) Signed-off-by: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> * perf(autoware_map_based_prediction): speed up map based prediction by using lru cache in convertPathType (#8461) feat(autoware_map_based_prediction): speed up map based prediction by using lru cache in convertPathType Signed-off-by: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> * fix(autoware_map_based_prediction): use surrounding_crosswalks instead of external_surrounding_crosswalks (#8467) Signed-off-by: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> --------- Signed-off-by: Mamoru Sobue Signed-off-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Signed-off-by: Taekjin LEE Signed-off-by: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Co-authored-by: Onur Can Yücedağ Co-authored-by: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Co-authored-by: Mamoru Sobue Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> --- .../examples/example_lru_cache.cpp | 73 +++++++++ .../universe_utils/system/lru_cache.hpp | 142 ++++++++++++++++++ .../test/src/system/test_lru_cache.cpp | 78 ++++++++++ .../map_based_prediction_node.hpp | 32 +++- .../src/map_based_prediction_node.cpp | 111 +++++++++----- .../src/path_generator.cpp | 23 ++- 6 files changed, 406 insertions(+), 53 deletions(-) create mode 100644 common/autoware_universe_utils/examples/example_lru_cache.cpp create mode 100644 common/autoware_universe_utils/include/autoware/universe_utils/system/lru_cache.hpp create mode 100644 common/autoware_universe_utils/test/src/system/test_lru_cache.cpp diff --git a/common/autoware_universe_utils/examples/example_lru_cache.cpp b/common/autoware_universe_utils/examples/example_lru_cache.cpp new file mode 100644 index 0000000000000..95decf8eb336d --- /dev/null +++ b/common/autoware_universe_utils/examples/example_lru_cache.cpp @@ -0,0 +1,73 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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 "autoware/universe_utils/system/lru_cache.hpp" + +#include +#include +#include + +using autoware::universe_utils::LRUCache; + +// Fibonacci calculation with LRU cache +int64_t fibonacci_with_cache(int n, LRUCache * cache) +{ + if (n <= 1) return n; + + if (cache->contains(n)) { + return *cache->get(n); + } + int64_t result = fibonacci_with_cache(n - 1, cache) + fibonacci_with_cache(n - 2, cache); + cache->put(n, result); + return result; +} + +// Fibonacci calculation without cache +int64_t fibonacci_no_cache(int n) +{ + if (n <= 1) return n; + return fibonacci_no_cache(n - 1) + fibonacci_no_cache(n - 2); +} + +// Helper function to measure execution time +template +int64_t measure_time(Func func, Args &&... args) +{ + auto start = std::chrono::high_resolution_clock::now(); + func(std::forward(args)...); + auto end = std::chrono::high_resolution_clock::now(); + return std::chrono::duration_cast(end - start).count(); +} + +int main() +{ + const int max_n = 40; // Increased for more noticeable time difference + LRUCache cache(max_n); // Cache size set to MAX_N + + std::cout << "Comparing Fibonacci calculation time with and without LRU cache:\n\n"; + std::cout << "n\tWith Cache (μs)\tWithout Cache (μs)\n"; + std::cout << std::string(43, '-') << "\n"; + + for (int i = 30; i <= max_n; ++i) { // Starting from 30 for more significant differences + int64_t time_with_cache = measure_time([i, &cache]() { fibonacci_with_cache(i, &cache); }); + int64_t time_without_cache = measure_time([i]() { fibonacci_no_cache(i); }); + + std::cout << i << "\t" << time_with_cache << "\t\t" << time_without_cache << "\n"; + + // Clear the cache after each iteration to ensure fair comparison + cache.clear(); + } + + return 0; +} diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/lru_cache.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/lru_cache.hpp new file mode 100644 index 0000000000000..ecd8fc7e6c01a --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/lru_cache.hpp @@ -0,0 +1,142 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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 AUTOWARE__UNIVERSE_UTILS__SYSTEM__LRU_CACHE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__LRU_CACHE_HPP_ + +#include +#include +#include +#include +#include + +namespace autoware::universe_utils +{ + +/** + * @brief A template class for LRU (Least Recently Used) Cache. + * + * This class implements a simple LRU cache using a combination of a list and a hash map. + * + * @tparam Key The type of keys. + * @tparam Value The type of values. + * @tparam Map The type of underlying map, defaulted to std::unordered_map. + */ +template class Map = std::unordered_map> +class LRUCache +{ +private: + size_t capacity_; ///< The maximum capacity of the cache. + std::list> cache_list_; ///< List to maintain the order of elements. + Map>::iterator> + cache_map_; ///< Map for fast access to elements. + +public: + /** + * @brief Construct a new LRUCache object. + * + * @param size The capacity of the cache. + */ + explicit LRUCache(size_t size) : capacity_(size) {} + + /** + * @brief Get the capacity of the cache. + * + * @return The capacity of the cache. + */ + [[nodiscard]] size_t capacity() const { return capacity_; } + + /** + * @brief Insert a key-value pair into the cache. + * + * If the key already exists, its value is updated and it is moved to the front. + * If the cache exceeds its capacity, the least recently used element is removed. + * + * @param key The key to insert. + * @param value The value to insert. + */ + void put(const Key & key, const Value & value) + { + auto it = cache_map_.find(key); + if (it != cache_map_.end()) { + cache_list_.erase(it->second); + } + cache_list_.push_front({key, value}); + cache_map_[key] = cache_list_.begin(); + + if (cache_map_.size() > capacity_) { + auto last = cache_list_.back(); + cache_map_.erase(last.first); + cache_list_.pop_back(); + } + } + + /** + * @brief Retrieve a value from the cache. + * + * If the key does not exist in the cache, std::nullopt is returned. + * If the key exists, the value is returned and the element is moved to the front. + * + * @param key The key to retrieve. + * @return The value associated with the key, or std::nullopt if the key does not exist. + */ + std::optional get(const Key & key) + { + auto it = cache_map_.find(key); + if (it == cache_map_.end()) { + return std::nullopt; + } + cache_list_.splice(cache_list_.begin(), cache_list_, it->second); + return it->second->second; + } + + /** + * @brief Clear the cache. + * + * This removes all elements from the cache. + */ + void clear() + { + cache_list_.clear(); + cache_map_.clear(); + } + + /** + * @brief Get the current size of the cache. + * + * @return The number of elements in the cache. + */ + [[nodiscard]] size_t size() const { return cache_map_.size(); } + + /** + * @brief Check if the cache is empty. + * + * @return True if the cache is empty, false otherwise. + */ + [[nodiscard]] bool empty() const { return cache_map_.empty(); } + + /** + * @brief Check if a key exists in the cache. + * + * @param key The key to check. + * @return True if the key exists, false otherwise. + */ + [[nodiscard]] bool contains(const Key & key) const + { + return cache_map_.find(key) != cache_map_.end(); + } +}; + +} // namespace autoware::universe_utils + +#endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__LRU_CACHE_HPP_ diff --git a/common/autoware_universe_utils/test/src/system/test_lru_cache.cpp b/common/autoware_universe_utils/test/src/system/test_lru_cache.cpp new file mode 100644 index 0000000000000..f30f732431fa6 --- /dev/null +++ b/common/autoware_universe_utils/test/src/system/test_lru_cache.cpp @@ -0,0 +1,78 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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 "autoware/universe_utils/system/lru_cache.hpp" + +#include + +#include +#include +#include +#include + +using autoware::universe_utils::LRUCache; + +// Fibonacci calculation with LRU cache +int64_t fibonacci_with_cache(int n, LRUCache * cache) +{ + if (n <= 1) return n; + + if (cache->contains(n)) { + return *cache->get(n); + } + int64_t result = fibonacci_with_cache(n - 1, cache) + fibonacci_with_cache(n - 2, cache); + cache->put(n, result); + return result; +} + +// Fibonacci calculation without cache +int64_t fibonacci_no_cache(int n) +{ + if (n <= 1) return n; + return fibonacci_no_cache(n - 1) + fibonacci_no_cache(n - 2); +} + +// Helper function to measure execution time +template +std::pair()(std::declval()...))> measure_time( + Func func, Args &&... args) +{ + auto start = std::chrono::high_resolution_clock::now(); + auto result = func(std::forward(args)...); + auto end = std::chrono::high_resolution_clock::now(); + return {std::chrono::duration_cast(end - start).count(), result}; +} + +// Test case to verify Fibonacci calculation results with and without cache +TEST(FibonacciLRUCacheTest, CompareResultsAndPerformance) +{ + const int max_n = 40; // Test range + LRUCache cache(max_n); // Cache with capacity set to max_n + + for (int i = 0; i <= max_n; ++i) { + // Measure time for performance comparison + auto [time_with_cache, result_with_cache] = + measure_time([i, &cache]() { return fibonacci_with_cache(i, &cache); }); + auto [time_without_cache, result_without_cache] = + measure_time([i]() { return fibonacci_no_cache(i); }); + + EXPECT_EQ(result_with_cache, result_without_cache) << "Mismatch at n = " << i; + + // Print the calculation time + std::cout << "n = " << i << ": With Cache = " << time_with_cache + << " μs, Without Cache = " << time_without_cache << " μs\n"; + + // // Clear the cache after each iteration to ensure fair comparison + cache.clear(); + } +} diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 1c677c9d0f6e4..1f13391ae442a 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -39,7 +40,9 @@ #include #include +#include #include +#include #include #include @@ -53,6 +56,27 @@ #include #include +namespace std +{ +template <> +struct hash +{ + // 0x9e3779b9 is a magic number. See + // https://stackoverflow.com/questions/4948780/magic-number-in-boosthash-combine + size_t operator()(const lanelet::routing::LaneletPaths & paths) const + { + size_t seed = 0; + for (const auto & path : paths) { + for (const auto & lanelet : path) { + seed ^= hash{}(lanelet.id()) + 0x9e3779b9 + (seed << 6U) + (seed >> 2U); + } + // Add a separator between paths + seed ^= hash{}(0) + 0x9e3779b9 + (seed << 6U) + (seed >> 2U); + } + return seed; + } +}; +} // namespace std namespace autoware::map_based_prediction { struct LateralKinematicsToLanelet @@ -178,6 +202,9 @@ class MapBasedPredictionNode : public rclcpp::Node // Crosswalk Entry Points lanelet::ConstLanelets crosswalks_; + // Fences + lanelet::LaneletMapUPtr fence_layer_{nullptr}; + // Parameters bool enable_delay_compensation_; PredictionTimeHorizon prediction_time_horizon_; @@ -287,7 +314,10 @@ class MapBasedPredictionNode : public rclcpp::Node const float path_probability, const ManeuverProbability & maneuver_probability, const Maneuver & maneuver, std::vector & reference_paths, const double speed_limit = 0.0); - std::vector convertPathType(const lanelet::routing::LaneletPaths & paths); + + mutable universe_utils::LRUCache> + lru_cache_of_convert_path_type_{1000}; + std::vector convertPathType(const lanelet::routing::LaneletPaths & paths) const; void updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths); diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 29cfe52855d24..649a92ac981f5 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -411,8 +411,8 @@ bool withinRoadLanelet( boost::optional isReachableCrosswalkEdgePoints( const TrackedObject & object, const lanelet::ConstLanelets & surrounding_lanelets, - const lanelet::ConstLanelets & external_surrounding_crosswalks, - const CrosswalkEdgePoints & edge_points, const double time_horizon, const double min_object_vel) + const lanelet::ConstLanelets & surrounding_crosswalks, const CrosswalkEdgePoints & edge_points, + const double time_horizon, const double min_object_vel) { using Point = boost::geometry::model::d2::point_xy; @@ -439,10 +439,10 @@ boost::optional isReachableCrosswalkEdgePoints( const auto is_stop_object = estimated_velocity < stop_velocity_th; const auto velocity = std::max(min_object_vel, estimated_velocity); - const auto isAcrossAnyRoad = [&surrounding_lanelets, &external_surrounding_crosswalks]( + const auto isAcrossAnyRoad = [&surrounding_lanelets, &surrounding_crosswalks]( const Point & p_src, const Point & p_dst) { - const auto withinAnyCrosswalk = [&external_surrounding_crosswalks](const Point & p) { - for (const auto & crosswalk : external_surrounding_crosswalks) { + const auto withinAnyCrosswalk = [&surrounding_crosswalks](const Point & p) { + for (const auto & crosswalk : surrounding_crosswalks) { if (boost::geometry::within(p, crosswalk.polygon2d().basicPolygon())) { return true; } @@ -662,7 +662,6 @@ ObjectClassification::_label_type changeLabelForPrediction( } case ObjectClassification::PEDESTRIAN: { - const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); const float max_velocity_for_human_mps = autoware::universe_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h const double abs_speed = std::hypot( @@ -670,7 +669,6 @@ ObjectClassification::_label_type changeLabelForPrediction( object.kinematics.twist_with_covariance.twist.linear.y); const bool high_speed_object = abs_speed > max_velocity_for_human_mps; // fast, human-like object: like segway - if (within_road_lanelet && high_speed_object) return label; // currently do nothing // return ObjectClassification::MOTORCYCLE; if (high_speed_object) return label; // currently do nothing // fast human outside road lanelet will move like unknown object @@ -924,6 +922,7 @@ void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + lru_cache_of_convert_path_type_.clear(); // clear cache RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Map is loaded"); const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); @@ -931,6 +930,16 @@ void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg const auto walkways = lanelet::utils::query::walkwayLanelets(all_lanelets); crosswalks_.insert(crosswalks_.end(), crosswalks.begin(), crosswalks.end()); crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end()); + + lanelet::LineStrings3d fences; + for (const auto & linestring : lanelet_map_ptr_->lineStringLayer) { + if (const std::string type = linestring.attributeOr(lanelet::AttributeName::Type, "none"); + type == "fence") { + fences.push_back(lanelet::LineString3d( + std::const_pointer_cast(linestring.constData()))); + } + } + fence_layer_ = lanelet::utils::createMap(fences); } void MapBasedPredictionNode::trafficSignalsCallback( @@ -963,23 +972,6 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt return; } - auto world2map_transform = transform_listener_.getTransform( - "map", // target - in_objects->header.frame_id, // src - in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); - auto map2world_transform = transform_listener_.getTransform( - in_objects->header.frame_id, // target - "map", // src - in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); - auto debug_map2lidar_transform = transform_listener_.getTransform( - "base_link", // target - "map", // src - rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)); - - if (!world2map_transform || !map2world_transform || !debug_map2lidar_transform) { - return; - } - // Remove old objects information in object history const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds(); removeOldObjectsHistory(objects_detected_time, object_buffer_time_length_, road_users_history); @@ -1018,12 +1010,24 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } std::unordered_set predicted_crosswalk_users_ids; + // get world to map transform + geometry_msgs::msg::TransformStamped::ConstSharedPtr world2map_transform; + + bool is_object_not_in_map_frame = in_objects->header.frame_id != "map"; + if (is_object_not_in_map_frame) { + world2map_transform = transform_listener_.getTransform( + "map", // target + in_objects->header.frame_id, // src + in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1)); + if (!world2map_transform) return; + } + for (const auto & object : in_objects->objects) { std::string object_id = autoware::universe_utils::toHexString(object.object_id); TrackedObject transformed_object = object; // transform object frame if it's based on map frame - if (in_objects->header.frame_id != "map") { + if (is_object_not_in_map_frame) { geometry_msgs::msg::PoseStamped pose_in_map; geometry_msgs::msg::PoseStamped pose_orig; pose_orig.pose = object.kinematics.pose_with_covariance.pose; @@ -1316,10 +1320,14 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared( bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) { autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); - const lanelet::ConstLineStrings3d & all_fences = - lanelet::utils::query::getAllFences(lanelet_map_ptr_); - for (const auto & fence_line : all_fences) { - if (doesPathCrossFence(predicted_path, fence_line)) { + + lanelet::BasicLineString2d predicted_path_ls; + for (const auto & p : predicted_path.path) + predicted_path_ls.emplace_back(p.position.x, p.position.y); + const auto candidates = + fence_layer_->lineStringLayer.search(lanelet::geometry::boundingBox2d(predicted_path_ls)); + for (const auto & candidate : candidates) { + if (doesPathCrossFence(predicted_path, candidate)) { return true; } } @@ -1381,7 +1389,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y}, prediction_time_horizon_.pedestrian * velocity); lanelet::ConstLanelets surrounding_lanelets; - lanelet::ConstLanelets external_surrounding_crosswalks; + lanelet::ConstLanelets surrounding_crosswalks; for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) { surrounding_lanelets.push_back(lanelet); const auto attr = lanelet.attribute(lanelet::AttributeName::Subtype); @@ -1389,10 +1397,9 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( attr.value() == lanelet::AttributeValueString::Crosswalk || attr.value() == lanelet::AttributeValueString::Walkway) { const auto & crosswalk = lanelet; + surrounding_crosswalks.push_back(crosswalk); if (withinLanelet(object, crosswalk)) { crossing_crosswalk = crosswalk; - } else { - external_surrounding_crosswalks.push_back(crosswalk); } } } @@ -1456,7 +1463,10 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( // try to find the edge points for other surrounding crosswalks and generate path to the crosswalk // edge - for (const auto & crosswalk : external_surrounding_crosswalks) { + for (const auto & crosswalk : surrounding_crosswalks) { + if (crossing_crosswalk && crossing_crosswalk.get() == crosswalk) { + continue; + } const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk); if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) { if (!calcIntentionToCrossWithTrafficSignal( @@ -1481,7 +1491,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( - object, surrounding_lanelets, external_surrounding_crosswalks, edge_points, + object, surrounding_lanelets, surrounding_crosswalks, edge_points, prediction_time_horizon_.pedestrian, min_crosswalk_user_velocity_); if (!reachable_crosswalk) { @@ -2366,10 +2376,14 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( } std::vector MapBasedPredictionNode::convertPathType( - const lanelet::routing::LaneletPaths & paths) + const lanelet::routing::LaneletPaths & paths) const { autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + if (lru_cache_of_convert_path_type_.contains(paths)) { + return *lru_cache_of_convert_path_type_.get(paths); + } + std::vector converted_paths; for (const auto & path : paths) { PosePath converted_path; @@ -2392,7 +2406,13 @@ std::vector MapBasedPredictionNode::convertPathType( const double lane_yaw = std::atan2( current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x); - current_p.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); + const double sin_yaw_half = std::sin(lane_yaw / 2.0); + const double cos_yaw_half = std::cos(lane_yaw / 2.0); + current_p.orientation.x = 0.0; + current_p.orientation.y = 0.0; + current_p.orientation.z = sin_yaw_half; + current_p.orientation.w = cos_yaw_half; + converted_path.push_back(current_p); prev_p = current_p; } @@ -2423,18 +2443,31 @@ std::vector MapBasedPredictionNode::convertPathType( const double lane_yaw = std::atan2( current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x); - current_p.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); + const double sin_yaw_half = std::sin(lane_yaw / 2.0); + const double cos_yaw_half = std::cos(lane_yaw / 2.0); + current_p.orientation.x = 0.0; + current_p.orientation.y = 0.0; + current_p.orientation.z = sin_yaw_half; + current_p.orientation.w = cos_yaw_half; + converted_path.push_back(current_p); prev_p = current_p; } } // Resample Path - const auto resampled_converted_path = - autoware::motion_utils::resamplePoseVector(converted_path, reference_path_resolution_); + const bool use_akima_spline_for_xy = true; + const bool use_lerp_for_z = true; + // the options use_akima_spline_for_xy and use_lerp_for_z are set to true + // but the implementation of use_akima_spline_for_xy in resamplePoseVector and + // resamplePointVector is opposite to the options so the options are set to true to use linear + // interpolation for xy + const auto resampled_converted_path = autoware::motion_utils::resamplePoseVector( + converted_path, reference_path_resolution_, use_akima_spline_for_xy, use_lerp_for_z); converted_paths.push_back(resampled_converted_path); } + lru_cache_of_convert_path_type_.put(paths, converted_paths); return converted_paths; } diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index aeb1b0fedd33f..b483503215cc6 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include @@ -188,7 +188,7 @@ PredictedPath PathGenerator::generatePolynomialPath( { // Get current Frenet Point const double ref_path_len = autoware::motion_utils::calcArcLength(ref_path); - const auto current_point = getFrenetPoint(object, ref_path, speed_limit, duration); + const auto current_point = getFrenetPoint(object, ref_path, duration, speed_limit); // Step1. Set Target Frenet Point // Note that we do not set position s, @@ -332,29 +332,26 @@ PosePath PathGenerator::interpolateReferencePath( std::reverse(resampled_s.begin(), resampled_s.end()); } - // Spline Interpolation - std::vector spline_ref_path_x = - interpolation::spline(base_path_s, base_path_x, resampled_s); - std::vector spline_ref_path_y = - interpolation::spline(base_path_s, base_path_y, resampled_s); - std::vector spline_ref_path_z = - interpolation::spline(base_path_s, base_path_z, resampled_s); + // Lerp Interpolation + std::vector lerp_ref_path_x = interpolation::lerp(base_path_s, base_path_x, resampled_s); + std::vector lerp_ref_path_y = interpolation::lerp(base_path_s, base_path_y, resampled_s); + std::vector lerp_ref_path_z = interpolation::lerp(base_path_s, base_path_z, resampled_s); interpolated_path.resize(interpolate_num); for (size_t i = 0; i < interpolate_num - 1; ++i) { geometry_msgs::msg::Pose interpolated_pose; const auto current_point = - autoware::universe_utils::createPoint(spline_ref_path_x.at(i), spline_ref_path_y.at(i), 0.0); + autoware::universe_utils::createPoint(lerp_ref_path_x.at(i), lerp_ref_path_y.at(i), 0.0); const auto next_point = autoware::universe_utils::createPoint( - spline_ref_path_x.at(i + 1), spline_ref_path_y.at(i + 1), 0.0); + lerp_ref_path_x.at(i + 1), lerp_ref_path_y.at(i + 1), 0.0); const double yaw = autoware::universe_utils::calcAzimuthAngle(current_point, next_point); interpolated_pose.position = autoware::universe_utils::createPoint( - spline_ref_path_x.at(i), spline_ref_path_y.at(i), spline_ref_path_z.at(i)); + lerp_ref_path_x.at(i), lerp_ref_path_y.at(i), lerp_ref_path_z.at(i)); interpolated_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); interpolated_path.at(i) = interpolated_pose; } interpolated_path.back().position = autoware::universe_utils::createPoint( - spline_ref_path_x.back(), spline_ref_path_y.back(), spline_ref_path_z.back()); + lerp_ref_path_x.back(), lerp_ref_path_y.back(), lerp_ref_path_z.back()); interpolated_path.back().orientation = interpolated_path.at(interpolate_num - 2).orientation; return interpolated_path;