From ec740774c38f2882e0931cdbf0ddc0d0690e4d2c Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 15 Aug 2024 10:16:09 +0900 Subject: [PATCH 1/3] 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; From 398ab6ef9cc76fc461ef082e944387d4bc7d4285 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 15 Aug 2024 11:26:00 +0900 Subject: [PATCH 2/3] chore(map_based_prediction): debug options (#1466) refactor(autoware_map_based_prediction): map based pred time keeper ptr (#8462) * refactor(map_based_prediction): implement time keeper by pointer * feat(map_based_prediction): set time keeper in path generator * feat: use scoped time track only when the timekeeper ptr is not null * refactor: define publish function to measure time * chore: add debug parameters for map-based prediction * chore: remove unnecessary ScopedTimeTrack instances * feat: replace member to pointer --------- Signed-off-by: Taekjin LEE --- .../config/map_based_prediction.param.yaml | 5 + .../map_based_prediction_node.hpp | 6 +- .../map_based_prediction/path_generator.hpp | 5 + .../src/map_based_prediction_node.cpp | 181 +++++++++++------- .../src/path_generator.cpp | 41 ++++ 5 files changed, 170 insertions(+), 68 deletions(-) diff --git a/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml b/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml index a5b7b0ad6be01..b45cc25f82c1c 100644 --- a/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml @@ -50,3 +50,8 @@ consider_only_routable_neighbours: false reference_path_resolution: 0.5 #[m] + + # debug parameters + publish_processing_time: false + publish_processing_time_detail: false + publish_debug_markers: false 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 1f13391ae442a..3078fe89444b8 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 @@ -252,7 +252,7 @@ class MapBasedPredictionNode : public rclcpp::Node std::unique_ptr published_time_publisher_; rclcpp::Publisher::SharedPtr detailed_processing_time_publisher_; - mutable autoware::universe_utils::TimeKeeper time_keeper_; + std::shared_ptr time_keeper_; // Member Functions void mapCallback(const LaneletMapBin::ConstSharedPtr msg); @@ -343,6 +343,10 @@ class MapBasedPredictionNode : public rclcpp::Node const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time); + void publish( + const PredictedObjects & output, + const visualization_msgs::msg::MarkerArray & debug_markers) const; + // NOTE: This function is copied from the motion_velocity_smoother package. // TODO(someone): Consolidate functions and move them to a common inline std::vector calcTrajectoryCurvatureFrom3Points( diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp index bd098894f8a88..8861598ae7fb2 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -16,6 +16,7 @@ #define MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_ #include +#include #include #include @@ -82,6 +83,8 @@ class PathGenerator public: PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity); + void setTimeKeeper(std::shared_ptr time_keeper_ptr); + PredictedPath generatePathForNonVehicleObject( const TrackedObject & object, const double duration) const; @@ -119,6 +122,8 @@ class PathGenerator bool use_vehicle_acceleration_; double acceleration_exponential_half_life_; + std::shared_ptr time_keeper_; + // Member functions PredictedPath generateStraightPath(const TrackedObject & object, const double duration) const; 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 649a92ac981f5..e95113641104d 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 @@ -52,6 +52,7 @@ namespace autoware::map_based_prediction { +using autoware::universe_utils::ScopedTimeTrack; namespace { @@ -831,12 +832,18 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ timeout_set_for_no_intention_to_walk_ = declare_parameter>( "crosswalk_with_signal.timeout_set_for_no_intention_to_walk"); + // debug parameter + bool use_time_publisher = declare_parameter("publish_processing_time"); + bool use_time_keeper = declare_parameter("publish_processing_time_detail"); + bool use_debug_marker = declare_parameter("publish_debug_markers"); + path_generator_ = std::make_shared( prediction_sampling_time_interval_, min_crosswalk_user_velocity_); path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); + // subscribers sub_objects_ = this->create_subscription( "~/input/objects", 1, std::bind(&MapBasedPredictionNode::objectsCallback, this, std::placeholders::_1)); @@ -844,26 +851,37 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ "/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&MapBasedPredictionNode::mapCallback, this, std::placeholders::_1)); + // publishers pub_objects_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); - pub_debug_markers_ = - this->create_publisher("maneuver", rclcpp::QoS{1}); - processing_time_publisher_ = - std::make_unique(this, "map_based_prediction"); - - published_time_publisher_ = - std::make_unique(this); - detailed_processing_time_publisher_ = - this->create_publisher( - "~/debug/processing_time_detail_ms", 1); - time_keeper_ = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); + // debug publishers + if (use_time_publisher) { + processing_time_publisher_ = + std::make_unique(this, "map_based_prediction"); + published_time_publisher_ = + std::make_unique(this); + stop_watch_ptr_ = + std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + + if (use_time_keeper) { + detailed_processing_time_publisher_ = + this->create_publisher( + "~/debug/processing_time_detail_ms", 1); + auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); + path_generator_->setTimeKeeper(time_keeper_); + } + + if (use_debug_marker) { + pub_debug_markers_ = + this->create_publisher("maneuver", rclcpp::QoS{1}); + } + // dynamic reconfigure set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); - - stop_watch_ptr_ = - std::make_unique>(); - stop_watch_ptr_->tic("cyclic_time"); - stop_watch_ptr_->tic("processing_time"); } rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( @@ -916,8 +934,6 @@ PredictedObject MapBasedPredictionNode::convertToPredictedObject( void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); - RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Start loading lanelet"); lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( @@ -945,7 +961,8 @@ void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg void MapBasedPredictionNode::trafficSignalsCallback( const TrafficLightGroupArray::ConstSharedPtr msg) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); traffic_signal_id_map_.clear(); for (const auto & signal : msg->traffic_light_groups) { @@ -955,9 +972,10 @@ void MapBasedPredictionNode::trafficSignalsCallback( void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - stop_watch_ptr_->toc("processing_time", true); + if (stop_watch_ptr_) stop_watch_ptr_->toc("processing_time", true); // take traffic_signal { @@ -1116,14 +1134,16 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } // Get Debug Marker for On Lane Vehicles - const auto max_prob_path = std::max_element( - ref_paths.begin(), ref_paths.end(), - [](const PredictedRefPath & a, const PredictedRefPath & b) { - return a.probability < b.probability; - }); - const auto debug_marker = - getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); - debug_markers.markers.push_back(debug_marker); + if (pub_debug_markers_) { + const auto max_prob_path = std::max_element( + ref_paths.begin(), ref_paths.end(), + [](const PredictedRefPath & a, const PredictedRefPath & b) { + return a.probability < b.probability; + }); + const auto debug_marker = + getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); + debug_markers.markers.push_back(debug_marker); + } // Fix object angle if its orientation unreliable (e.g. far object by radar sensor) // This prevent bending predicted path @@ -1229,21 +1249,36 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } // Publish Results + publish(output, debug_markers); + + // Publish Processing Time + if (stop_watch_ptr_) { + const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + processing_time_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + processing_time_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } +} + +void MapBasedPredictionNode::publish( + const PredictedObjects & output, const visualization_msgs::msg::MarkerArray & debug_markers) const +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + pub_objects_->publish(output); - published_time_publisher_->publish_if_subscribed(pub_objects_, output.header.stamp); - pub_debug_markers_->publish(debug_markers); - const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - processing_time_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - processing_time_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); + if (published_time_publisher_) + published_time_publisher_->publish_if_subscribed(pub_objects_, output.header.stamp); + if (pub_debug_markers_) pub_debug_markers_->publish(debug_markers); } void MapBasedPredictionNode::updateCrosswalkUserHistory( const std_msgs::msg::Header & header, const TrackedObject & object, const std::string & object_id) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); CrosswalkUserData crosswalk_user_data; crosswalk_user_data.header = header; @@ -1260,7 +1295,8 @@ void MapBasedPredictionNode::updateCrosswalkUserHistory( std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared( const std::string & object_id, std::unordered_map & current_users) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); const auto known_match_opt = [&]() -> std::optional { if (!known_matches_.count(object_id)) { @@ -1319,7 +1355,8 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared( bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); lanelet::BasicLineString2d predicted_path_ls; for (const auto & p : predicted_path.path) @@ -1337,7 +1374,8 @@ bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predict bool MapBasedPredictionNode::doesPathCrossFence( const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // check whether the predicted path cross with fence for (size_t i = 0; i < predicted_path.path.size() - 1; ++i) { @@ -1356,6 +1394,9 @@ bool MapBasedPredictionNode::isIntersecting( const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + const auto p1 = autoware::universe_utils::createPoint(point1.x, point1.y, 0.0); const auto p2 = autoware::universe_utils::createPoint(point2.x, point2.y, 0.0); const auto p3 = autoware::universe_utils::createPoint(point3.x(), point3.y(), 0.0); @@ -1367,7 +1408,8 @@ bool MapBasedPredictionNode::isIntersecting( PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const TrackedObject & object) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); auto predicted_object = convertToPredictedObject(object); { @@ -1523,7 +1565,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( void MapBasedPredictionNode::updateObjectData(TrackedObject & object) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); if ( object.kinematics.orientation_availability == @@ -1575,8 +1618,6 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) void MapBasedPredictionNode::removeStaleTrafficLightInfo( const TrackedObjects::ConstSharedPtr in_objects) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); - for (auto it = stopped_times_against_green_.begin(); it != stopped_times_against_green_.end();) { const bool isDisappeared = std::none_of( in_objects->objects.begin(), in_objects->objects.end(), @@ -1593,7 +1634,8 @@ void MapBasedPredictionNode::removeStaleTrafficLightInfo( LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & object) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // obstacle point lanelet::BasicPoint2d search_point( @@ -1685,7 +1727,8 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob bool MapBasedPredictionNode::checkCloseLaneletCondition( const std::pair & lanelet, const TrackedObject & object) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // Step1. If we only have one point in the centerline, we will ignore the lanelet if (lanelet.second.centerline().size() <= 1) { @@ -1733,7 +1776,8 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( float MapBasedPredictionNode::calculateLocalLikelihood( const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); const auto & obj_point = object.kinematics.pose_with_covariance.pose.position; @@ -1770,7 +1814,8 @@ void MapBasedPredictionNode::updateRoadUsersHistory( const std_msgs::msg::Header & header, const TrackedObject & object, const LaneletsData & current_lanelets_data) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); std::string object_id = autoware::universe_utils::toHexString(object.object_id); const auto current_lanelets = getLanelets(current_lanelets_data); @@ -1813,7 +1858,8 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, const double object_detected_time, const double time_horizon) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); const double obj_vel = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, @@ -1976,7 +2022,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver( const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // calculate maneuver const auto current_maneuver = [&]() { @@ -2028,7 +2075,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange( const TrackedObject & object, const LaneletData & current_lanelet_data, const double /*object_detected_time*/) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // Step1. Check if we have the object in the buffer const std::string object_id = autoware::universe_utils::toHexString(object.object_id); @@ -2101,7 +2149,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance( const TrackedObject & object, const LaneletData & current_lanelet_data, const double /*object_detected_time*/) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); // Step1. Check if we have the object in the buffer const std::string object_id = autoware::universe_utils::toHexString(object.object_id); @@ -2246,8 +2295,6 @@ geometry_msgs::msg::Pose MapBasedPredictionNode::compensateTimeDelay( double MapBasedPredictionNode::calcRightLateralOffset( const lanelet::ConstLineString2d & boundary_line, const geometry_msgs::msg::Pose & search_pose) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); - std::vector boundary_path(boundary_line.size()); for (size_t i = 0; i < boundary_path.size(); ++i) { const double x = boundary_line[i].x(); @@ -2267,7 +2314,8 @@ double MapBasedPredictionNode::calcLeftLateralOffset( void MapBasedPredictionNode::updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { @@ -2293,7 +2341,8 @@ void MapBasedPredictionNode::addReferencePaths( const Maneuver & maneuver, std::vector & reference_paths, const double speed_limit) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); if (!candidate_paths.empty()) { updateFuturePossibleLanelets(object, candidate_paths); @@ -2314,7 +2363,8 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( const lanelet::routing::LaneletPaths & right_paths, const lanelet::routing::LaneletPaths & center_paths) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); float left_lane_change_probability = 0.0; float right_lane_change_probability = 0.0; @@ -2378,7 +2428,8 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( std::vector MapBasedPredictionNode::convertPathType( const lanelet::routing::LaneletPaths & paths) const { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); if (lru_cache_of_convert_path_type_.contains(paths)) { return *lru_cache_of_convert_path_type_.get(paths); @@ -2475,8 +2526,6 @@ bool MapBasedPredictionNode::isDuplicated( const std::pair & target_lanelet, const LaneletsData & lanelets_data) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); - const double CLOSE_LANELET_THRESHOLD = 0.1; for (const auto & lanelet_data : lanelets_data) { const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back(); @@ -2494,8 +2543,6 @@ bool MapBasedPredictionNode::isDuplicated( bool MapBasedPredictionNode::isDuplicated( const PredictedPath & predicted_path, const std::vector & predicted_paths) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); - const double CLOSE_PATH_THRESHOLD = 0.1; for (const auto & prev_predicted_path : predicted_paths) { const auto prev_path_end = prev_predicted_path.path.back().position; @@ -2512,8 +2559,6 @@ bool MapBasedPredictionNode::isDuplicated( std::optional MapBasedPredictionNode::getTrafficSignalId( const lanelet::ConstLanelet & way_lanelet) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); - const auto traffic_light_reg_elems = way_lanelet.regulatoryElementsAs(); if (traffic_light_reg_elems.empty()) { @@ -2530,7 +2575,8 @@ std::optional MapBasedPredictionNode::getTrafficSignalId( std::optional MapBasedPredictionNode::getTrafficSignalElement( const lanelet::Id & id) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); if (traffic_signal_id_map_.count(id) != 0) { const auto & signal_elements = traffic_signal_id_map_.at(id).elements; @@ -2548,7 +2594,8 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal( const TrackedObject & object, const lanelet::ConstLanelet & crosswalk, const lanelet::Id & signal_id) { - autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); const auto signal_color = [&] { const auto elem_opt = getTrafficSignalElement(signal_id); diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index b483503215cc6..6c61876176b68 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -22,6 +22,8 @@ namespace autoware::map_based_prediction { +using autoware::universe_utils::ScopedTimeTrack; + PathGenerator::PathGenerator( const double sampling_time_interval, const double min_crosswalk_user_velocity) : sampling_time_interval_(sampling_time_interval), @@ -29,15 +31,27 @@ PathGenerator::PathGenerator( { } +void PathGenerator::setTimeKeeper( + std::shared_ptr time_keeper_ptr) +{ + time_keeper_ = std::move(time_keeper_ptr); +} + PredictedPath PathGenerator::generatePathForNonVehicleObject( const TrackedObject & object, const double duration) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + return generateStraightPath(object, duration); } PredictedPath PathGenerator::generatePathToTargetPoint( const TrackedObject & object, const Eigen::Vector2d & point) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + PredictedPath predicted_path{}; const double ep = 0.001; @@ -77,6 +91,9 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk, const double duration) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + PredictedPath predicted_path{}; const double ep = 0.001; @@ -135,6 +152,9 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( PredictedPath PathGenerator::generatePathForLowSpeedVehicle( const TrackedObject & object, const double duration) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + PredictedPath path; path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); const double ep = 0.001; @@ -148,6 +168,9 @@ PredictedPath PathGenerator::generatePathForLowSpeedVehicle( PredictedPath PathGenerator::generatePathForOffLaneVehicle( const TrackedObject & object, const double duration) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + return generateStraightPath(object, duration); } @@ -155,6 +178,9 @@ PredictedPath PathGenerator::generatePathForOnLaneVehicle( const TrackedObject & object, const PosePath & ref_paths, const double duration, const double lateral_duration, const double speed_limit) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (ref_paths.size() < 2) { return generateStraightPath(object, duration); } @@ -186,6 +212,9 @@ PredictedPath PathGenerator::generatePolynomialPath( const TrackedObject & object, const PosePath & ref_path, const double duration, const double lateral_duration, const double speed_limit) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + // Get current Frenet Point const double ref_path_len = autoware::motion_utils::calcArcLength(ref_path); const auto current_point = getFrenetPoint(object, ref_path, duration, speed_limit); @@ -219,6 +248,9 @@ FrenetPath PathGenerator::generateFrenetPath( const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length, const double duration, const double lateral_duration) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + FrenetPath path; // Compute Lateral and Longitudinal Coefficients to generate the trajectory @@ -303,6 +335,9 @@ Eigen::Vector2d PathGenerator::calcLonCoefficients( PosePath PathGenerator::interpolateReferencePath( const PosePath & base_path, const FrenetPath & frenet_predicted_path) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + PosePath interpolated_path; const size_t interpolate_num = frenet_predicted_path.size(); if (interpolate_num < 2) { @@ -361,6 +396,9 @@ PredictedPath PathGenerator::convertToPredictedPath( const TrackedObject & object, const FrenetPath & frenet_predicted_path, const PosePath & ref_path) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + PredictedPath predicted_path; predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); predicted_path.path.resize(ref_path.size()); @@ -392,6 +430,9 @@ FrenetPoint PathGenerator::getFrenetPoint( const TrackedObject & object, const PosePath & ref_path, const double duration, const double speed_limit) const { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; From 83b3e4c4153d76dc3be6c3a0f74c90458157938b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 15 Aug 2024 14:21:54 +0900 Subject: [PATCH 3/3] perf(autoware_map_based_prediction): enhance speed by removing unnecessary calculation (#1469) perf(autoware_map_based_prediction): enhance speed by removing unnecessary calculation (#8471) * fix(autoware_map_based_prediction): use surrounding_crosswalks instead of external_surrounding_crosswalks * perf(autoware_map_based_prediction): enhance speed by removing unnecessary calculation --------- Signed-off-by: Y.Hisaki Co-authored-by: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> --- .../src/map_based_prediction_node.cpp | 41 +++++++------------ 1 file changed, 14 insertions(+), 27 deletions(-) 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 e95113641104d..8c595c75eb481 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 @@ -359,28 +359,6 @@ CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswa back_center_point, r_p_back, l_p_back}; } -bool withinLanelet( - const TrackedObject & object, const lanelet::ConstLanelet & lanelet, - const bool use_yaw_information = false, const float yaw_threshold = 0.6) -{ - using Point = boost::geometry::model::d2::point_xy; - - const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; - const Point p_object{obj_pos.x, obj_pos.y}; - - auto polygon = lanelet.polygon2d().basicPolygon(); - boost::geometry::correct(polygon); - bool with_in_polygon = boost::geometry::within(p_object, polygon); - - if (!use_yaw_information) return with_in_polygon; - - // use yaw angle to compare - const double abs_yaw_diff = calcAbsYawDiffBetweenLaneletAndObject(object, lanelet); - if (abs_yaw_diff < yaw_threshold) return with_in_polygon; - - return false; -} - bool withinRoadLanelet( const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, const bool use_yaw_information = false) @@ -392,9 +370,12 @@ bool withinRoadLanelet( const auto surrounding_lanelets = lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, search_radius); - for (const auto & lanelet : surrounding_lanelets) { - if (lanelet.second.hasAttribute(lanelet::AttributeName::Subtype)) { - lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + for (const auto & lanelet_with_dist : surrounding_lanelets) { + const auto & dist = lanelet_with_dist.first; + const auto & lanelet = lanelet_with_dist.second; + + if (lanelet.hasAttribute(lanelet::AttributeName::Subtype)) { + lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype); if ( attr.value() == lanelet::AttributeValueString::Crosswalk || attr.value() == lanelet::AttributeValueString::Walkway) { @@ -402,7 +383,13 @@ bool withinRoadLanelet( } } - if (withinLanelet(object, lanelet.second, use_yaw_information)) { + constexpr float yaw_threshold = 0.6; + bool within_lanelet = std::abs(dist) < 1e-5; + if (use_yaw_information) { + within_lanelet = + within_lanelet && calcAbsYawDiffBetweenLaneletAndObject(object, lanelet) < yaw_threshold; + } + if (within_lanelet) { return true; } } @@ -1440,7 +1427,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( attr.value() == lanelet::AttributeValueString::Walkway) { const auto & crosswalk = lanelet; surrounding_crosswalks.push_back(crosswalk); - if (withinLanelet(object, crosswalk)) { + if (std::abs(dist) < 1e-5) { crossing_crosswalk = crosswalk; } }