From 46531aa732409360977c92ff303248be083a025d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 31 Oct 2024 10:52:49 +0900 Subject: [PATCH] fix: predicted path generation logic (#1610) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat(lane_change): cancel hysteresis (#6288) * feat(lane_change): cancel hysteresis Signed-off-by: Muhammad Zulfaqar Azmi * Update documentation Signed-off-by: Muhammad Zulfaqar Azmi * fix the explanation of the hysteresis count Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi * Add parked parked RSS Signed-off-by: Zulfaqar Azmi * support new perception_reproducer Signed-off-by: temkei.kem <1041084556@qq.com> * move files Signed-off-by: temkei.kem <1041084556@qq.com> * remove old files. Signed-off-by: temkei.kem <1041084556@qq.com> * fix pre-commit err Signed-off-by: temkei.kem <1041084556@qq.com> * style(pre-commit): autofix * feat(autoware_behavior_path_planner_common): disable feature of turning off blinker at low velocity (#1571) Refactor turn signal decider logic and add support for detecting turn signals in turn lanes Signed-off-by: Kyoichi Sugahara * fix a small bug about perception reproducer * style(pre-commit): autofix * feat(out_of_lane): ignore lanelets beyond the last path point (#1554) * feat(out_of_lane): ignore lanelets beyond the last path point Signed-off-by: Maxime CLEMENT * style(pre-commit): autofix --------- Signed-off-by: Maxime CLEMENT Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * feat(behavior_velocity_run_out_module): exclude obstacle crossing ego… (#1574) feat(behavior_velocity_run_out_module): exclude obstacle crossing ego back line (#6680) * add method to ignore target obstacles that cross ego cut lane * WIP add debug support * add params and finish debug marker * change lane to line * use autoware utils to get the cut line * simplify code wit calcOffsetPose * Update readme and eliminate unused code * update readme * eliminate unused function * readme * comments and readme * eliminate unused include * typo * rename param for consistency * change lane to line for consistency * rename for clarity, add brief * fix indentation * update description * lane ->line * lane -> line --------- Signed-off-by: Daniel Sanchez * fix(lane_change): change stopping logic (RT0-33761) (#1581) * RT0-33761 fix lane change stopping logic Signed-off-by: Zulfaqar Azmi * copied from awf main tested implementation Signed-off-by: Zulfaqar Azmi * doxygen comment Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --------- Signed-off-by: Zulfaqar Azmi Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> * perf: PR 7237 https://github.com/autowarefoundation/autoware.universe/pull/7237 * perf RP8406 https://github.com/autowarefoundation/autoware.universe/pull/8406 * perf PR 8416 * perf PR 8427 * perf PR 8413 * tool PR 8456 * perf PR 8461 * perf PR 8388 * perf PR 8467 * perf PR 8471 * perf PR 8490 * perf PR 8751 * chore: fix format * perf PR 8657 * feat: improve lanelet search logic in getPredictedReferencePath() * sp develop remove non approved change (#1611) Revert "feat: improve lanelet search logic in getPredictedReferencePath()" This reverts commit 5de95b01c51cabadc42cf11f83a534ff7cf4f11e. * feat PR 8811 * fix PR 8973 * feat: improve lanelet search logic in getPredictedReferencePath() --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi Signed-off-by: temkei.kem <1041084556@qq.com> Signed-off-by: Kyoichi Sugahara Signed-off-by: Maxime CLEMENT Signed-off-by: Daniel Sanchez Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: Zulfaqar Azmi Co-authored-by: temkei.kem <1041084556@qq.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kyoichi Sugahara Co-authored-by: Takayuki Murooka Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: danielsanchezaran Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Co-authored-by: Shohei Sakai --- .../tier4_autoware_utils/system/lru_cache.hpp | 142 +++++++ .../test/src/system/test_lru_cache.cpp | 78 ++++ .../map_based_prediction_node.hpp | 39 +- .../map_based_prediction/path_generator.hpp | 31 +- .../src/map_based_prediction_node.cpp | 358 +++++++++++++----- .../src/path_generator.cpp | 306 +++++++++++---- .../multi_object_tracker_core.hpp | 8 +- .../src/multi_object_tracker_core.cpp | 44 ++- 8 files changed, 810 insertions(+), 196 deletions(-) create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/system/lru_cache.hpp create mode 100644 common/tier4_autoware_utils/test/src/system/test_lru_cache.cpp diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/lru_cache.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/lru_cache.hpp new file mode 100644 index 0000000000000..3734f8ecd84ed --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_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 TIER4_AUTOWARE_UTILS__SYSTEM__LRU_CACHE_HPP_ +#define TIER4_AUTOWARE_UTILS__SYSTEM__LRU_CACHE_HPP_ + +#include +#include +#include +#include +#include + +namespace tier4_autoware_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 tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__SYSTEM__LRU_CACHE_HPP_ diff --git a/common/tier4_autoware_utils/test/src/system/test_lru_cache.cpp b/common/tier4_autoware_utils/test/src/system/test_lru_cache.cpp new file mode 100644 index 0000000000000..f1cd2414f50fe --- /dev/null +++ b/common/tier4_autoware_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 "tier4_autoware_utils/system/lru_cache.hpp" + +#include + +#include +#include +#include +#include + +using tier4_autoware_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/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index bdd9ad89bc025..e1005720c0423 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -31,7 +32,9 @@ #include #include +#include #include +#include #include #include @@ -41,6 +44,28 @@ #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 map_based_prediction { struct LateralKinematicsToLanelet @@ -84,6 +109,7 @@ struct LaneletData struct PredictedRefPath { float probability; + double width; PosePath path; Maneuver maneuver; }; @@ -132,6 +158,9 @@ class MapBasedPredictionNode : public rclcpp::Node // Crosswalk Entry Points lanelet::ConstLanelets crosswalks_; + // Fences + lanelet::LaneletMapUPtr fence_layer_{nullptr}; + // Parameters bool enable_delay_compensation_; double prediction_time_horizon_; @@ -194,7 +223,8 @@ class MapBasedPredictionNode : public rclcpp::Node void updateObjectsHistory( const std_msgs::msg::Header & header, const TrackedObject & object, const LaneletsData & current_lanelets_data); - + std::optional searchProperStartingRefPathIndex( + const TrackedObject & object, const PosePath & pose_path) const; std::vector getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, const double object_detected_time); @@ -217,7 +247,12 @@ class MapBasedPredictionNode : public rclcpp::Node const TrackedObject & object, const lanelet::routing::LaneletPaths & candidate_paths, const float path_probability, const ManeuverProbability & maneuver_probability, const Maneuver & maneuver, std::vector & reference_paths); - std::vector convertPathType(const lanelet::routing::LaneletPaths & paths); + + mutable tier4_autoware_utils::LRUCache< + lanelet::routing::LaneletPaths, std::vector>> + 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/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 4da4f62be2ede..7ac9c6311d842 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -22,7 +22,11 @@ #include #include #include +#include #include +#include + +#include #include #include @@ -91,7 +95,7 @@ class PathGenerator PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object); PredictedPath generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths); + const TrackedObject & object, const PosePath & ref_path, const double path_width = 0.0); PredictedPath generatePathForCrosswalkUser( const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const; @@ -109,23 +113,34 @@ class PathGenerator // Member functions PredictedPath generateStraightPath(const TrackedObject & object) const; - PredictedPath generatePolynomialPath(const TrackedObject & object, const PosePath & ref_path); + PredictedPath generatePolynomialPath( + const TrackedObject & object, const PosePath & ref_path, const double path_width, + const double backlash_width) const; FrenetPath generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length); + const FrenetPoint & current_point, const FrenetPoint & target_point, + const double max_length) const; Eigen::Vector3d calcLatCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T); + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const; Eigen::Vector2d calcLonCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T); + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const; + + std::vector interpolationLerp( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys) const; + std::vector interpolationLerp( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys) const; PosePath interpolateReferencePath( - const PosePath & base_path, const FrenetPath & frenet_predicted_path); + const PosePath & base_path, const FrenetPath & frenet_predicted_path) const; PredictedPath convertToPredictedPath( const TrackedObject & object, const FrenetPath & frenet_predicted_path, - const PosePath & ref_path); + const PosePath & ref_path) const; - FrenetPoint getFrenetPoint(const TrackedObject & object, const PosePath & ref_path); + FrenetPoint getFrenetPoint( + const TrackedObject & object, const geometry_msgs::msg::Pose & ref_pose) const; }; } // namespace map_based_prediction diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 85535df4babfe..7100dc2070389 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -14,6 +14,7 @@ #include "map_based_prediction/map_based_prediction_node.hpp" +#include #include #include #include @@ -33,6 +34,7 @@ #include #include +#include #include #include #include @@ -359,46 +361,14 @@ 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 TrackedObject & object, + const std::vector> & surrounding_lanelets_with_dist, const bool use_yaw_information = false) { - 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}; - - lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); - // nearest lanelet - constexpr double search_radius = 10.0; // [m] - 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 & [dist, lanelet] : surrounding_lanelets_with_dist) { + 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) { @@ -406,7 +376,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; } } @@ -414,9 +390,23 @@ bool withinRoadLanelet( return false; } +bool withinRoadLanelet( + const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const bool use_yaw_information = false) +{ + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); + // nearest lanelet + constexpr double search_radius = 10.0; // [m] + const auto surrounding_lanelets_with_dist = + lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, search_point, search_radius); + + return withinRoadLanelet(object, surrounding_lanelets_with_dist, use_yaw_information); +} + boost::optional isReachableCrosswalkEdgePoints( - const TrackedObject & object, const lanelet::ConstLanelet & target_crosswalk, - const CrosswalkEdgePoints & edge_points, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const TrackedObject & object, const lanelet::ConstLanelets & surrounding_lanelets, + 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; @@ -425,11 +415,6 @@ boost::optional isReachableCrosswalkEdgePoints( const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; - lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y); - if (boost::geometry::within(obj_pos_as_lanelet, target_crosswalk.polygon2d().basicPolygon())) { - return {}; - } - const auto & p1 = edge_points.front_center_point; const auto & p2 = edge_points.back_center_point; @@ -446,17 +431,12 @@ boost::optional isReachableCrosswalkEdgePoints( const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto is_stop_object = estimated_velocity < stop_velocity_th; const auto velocity = std::max(min_object_vel, estimated_velocity); - const auto surrounding_lanelets = lanelet::geometry::findNearest( - lanelet_map_ptr->laneletLayer, obj_pos_as_lanelet, time_horizon * velocity); - const auto isAcrossAnyRoad = [&surrounding_lanelets](const Point & p_src, const Point & p_dst) { - const auto withinAnyCrosswalk = [&surrounding_lanelets](const Point & p) { - for (const auto & lanelet : surrounding_lanelets) { - const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); - if ( - (attr.value() == lanelet::AttributeValueString::Crosswalk || - attr.value() == lanelet::AttributeValueString::Walkway) && - boost::geometry::within(p, lanelet.second.polygon2d().basicPolygon())) { + const auto isAcrossAnyRoad = [&surrounding_lanelets, &surrounding_crosswalks]( + const Point & p_src, const Point & p_dst) { + const auto withinAnyCrosswalk = [&surrounding_crosswalks](const Point & p) { + for (const auto & crosswalk : surrounding_crosswalks) { + if (boost::geometry::within(p, crosswalk.polygon2d().basicPolygon())) { return true; } } @@ -475,14 +455,13 @@ boost::optional isReachableCrosswalkEdgePoints( std::vector points_of_intersect; const boost::geometry::model::linestring line{p_src, p_dst}; for (const auto & lanelet : surrounding_lanelets) { - const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + const lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype); if (attr.value() != lanelet::AttributeValueString::Road) { continue; } std::vector tmp_intersects; - boost::geometry::intersection( - line, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); + boost::geometry::intersection(line, lanelet.polygon2d().basicPolygon(), tmp_intersects); for (const auto & p : tmp_intersects) { if (isExist(p, points_of_intersect) || withinAnyCrosswalk(p)) { continue; @@ -828,6 +807,7 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::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_INFO(get_logger(), "[Map Based Prediction]: Map is loaded"); const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); @@ -835,6 +815,16 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::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::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) @@ -845,23 +835,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); @@ -874,12 +847,24 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // result debug visualization_msgs::msg::MarkerArray debug_markers; + // 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 = tier4_autoware_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; @@ -950,7 +935,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // If predicted reference path is empty, assume this object is out of the lane if (ref_paths.empty()) { PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); + path_generator_->generatePathForOffLaneVehicle(transformed_object); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -982,7 +967,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt std::vector predicted_paths; for (const auto & ref_path : ref_paths) { PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( - yaw_fixed_transformed_object, ref_path.path); + yaw_fixed_transformed_object, ref_path.path, ref_path.width); if (predicted_path.path.empty()) { continue; } @@ -1031,10 +1016,13 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) { - 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; } } @@ -1081,10 +1069,26 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } boost::optional crossing_crosswalk{boost::none}; - for (const auto & crosswalk : crosswalks_) { - if (withinLanelet(object, crosswalk)) { - crossing_crosswalk = crosswalk; - break; + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; + const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); + const auto velocity = std::max(min_crosswalk_user_velocity_, estimated_velocity); + const auto surrounding_lanelets_with_dist = lanelet::geometry::findWithin2d( + lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y}, + prediction_time_horizon_ * velocity); + lanelet::ConstLanelets surrounding_lanelets; + 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); + if ( + attr.value() == lanelet::AttributeValueString::Crosswalk || + attr.value() == lanelet::AttributeValueString::Walkway) { + const auto & crosswalk = lanelet; + surrounding_crosswalks.push_back(crosswalk); + if (std::abs(dist) < 1e-5) { + crossing_crosswalk = crosswalk; + } } } @@ -1114,7 +1118,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( // If the object is not crossing the crosswalk, in the road lanelets, try to find the closest // crosswalk and generate path to the crosswalk edge - } else if (withinRoadLanelet(object, lanelet_map_ptr_)) { + } else if (withinRoadLanelet(object, surrounding_lanelets_with_dist)) { lanelet::ConstLanelet closest_crosswalk{}; const auto & obj_pose = object.kinematics.pose_with_covariance.pose; const auto found_closest_crosswalk = @@ -1144,8 +1148,14 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } } } - // try to find the edge points for all crosswalks and generate path to the crosswalk edge - for (const auto & crosswalk : crosswalks_) { + + // try to find the edge points for other surrounding crosswalks and generate path to the crosswalk + // edge + for (const auto & crosswalk : surrounding_crosswalks) { + if (crossing_crosswalk && crossing_crosswalk.get() == crosswalk) { + continue; + } + const auto edge_points = getCrosswalkEdgePoints(crosswalk); const auto reachable_first = hasPotentialToReach( @@ -1162,7 +1172,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( - object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_, + object, surrounding_lanelets, surrounding_crosswalks, edge_points, prediction_time_horizon_, min_crosswalk_user_velocity_); if (!reachable_crosswalk) { @@ -1485,6 +1495,90 @@ void MapBasedPredictionNode::updateObjectsHistory( } } +std::optional MapBasedPredictionNode::searchProperStartingRefPathIndex( + const TrackedObject & object, const PosePath & pose_path) const +{ + bool is_position_found = false; + std::optional opt_index{std::nullopt}; + auto & index = opt_index.emplace(); + + // starting segment index is a segment close enough to the object + const auto obj_point = object.kinematics.pose_with_covariance.pose.position; + { + double min_dist_sq = std::numeric_limits::max(); + constexpr double acceptable_dist_sq = 1.0; // [m2] + for (size_t i = 0; i < pose_path.size(); i++) { + const double dx = pose_path.at(i).position.x - obj_point.x; + const double dy = pose_path.at(i).position.y - obj_point.y; + const double dist_sq = dx * dx + dy * dy; + if (dist_sq < min_dist_sq) { + min_dist_sq = dist_sq; + index = i; + } + if (dist_sq < acceptable_dist_sq) { + break; + } + } + } + + // calculate score that object can reach the target path smoothly, and search the + // starting segment index that have the best score + size_t idx = 0; + { // find target segmentation index + constexpr double search_distance = 22.0; // [m] + constexpr double yaw_diff_limit = M_PI / 3.0; // 60 degrees + + const double obj_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const size_t search_segment_count = + static_cast(std::floor(search_distance / reference_path_resolution_)); + const size_t search_segment_num = + std::min(search_segment_count, static_cast(pose_path.size() - index)); + + // search for the best score, which is the smallest + double best_score = 1e9; // initial value is large enough + for (size_t i = 0; i < search_segment_num; ++i) { + const auto & path_pose = pose_path.at(index + i); + // yaw difference + const double path_yaw = tf2::getYaw(path_pose.orientation); + const double relative_path_yaw = autoware_utils::normalize_radian(path_yaw - obj_yaw); + if (std::abs(relative_path_yaw) > yaw_diff_limit) { + continue; + } + + const double dx = path_pose.position.x - obj_point.x; + const double dy = path_pose.position.y - obj_point.y; + const double dx_cp = std::cos(obj_yaw) * dx + std::sin(obj_yaw) * dy; + const double dy_cp = -std::sin(obj_yaw) * dx + std::cos(obj_yaw) * dy; + const double neutral_yaw = std::atan2(dy_cp, dx_cp) * 2.0; + const double delta_yaw = autoware_utils::normalize_radian(path_yaw - obj_yaw - neutral_yaw); + if (std::abs(delta_yaw) > yaw_diff_limit) { + continue; + } + + // objective function score + constexpr double weight_ratio = 0.01; + double score = delta_yaw * delta_yaw + weight_ratio * neutral_yaw * neutral_yaw; + constexpr double acceptable_score = 1e-3; + + if (score < best_score) { + best_score = score; + idx = i; + is_position_found = true; + if (score < acceptable_score) { + // if the score is small enough, we can break the loop + break; + } + } + } + } + + // update starting segment index + index += idx; + index = std::clamp(index, 0ul, pose_path.size() - 1); + + return is_position_found ? opt_index : std::nullopt; +} + std::vector MapBasedPredictionNode::getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, const double object_detected_time) @@ -1541,6 +1635,15 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( if (!unconnected_lanelets.empty()) { return unconnected_lanelets.front(); } + // search side of the next lanelet + const lanelet::ConstLanelets next_lanelet = routing_graph_ptr_->following(lanelet); + if (!next_lanelet.empty()) { + const auto next = get_left ? routing_graph_ptr_->left(next_lanelet.front()) + : routing_graph_ptr_->right(next_lanelet.front()); + if (!!next) { + return *next; + } + } } // if no candidate lanelet found, return empty @@ -1589,6 +1692,26 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( addReferencePathsLocal(center_paths, Maneuver::LANE_FOLLOW); } + // Step 3. Search starting point for each reference path + for (auto it = all_ref_paths.begin(); it != all_ref_paths.end();) { + auto & pose_path = it->path; + if (pose_path.empty()) { + continue; + } + + const std::optional opt_starting_idx = + searchProperStartingRefPathIndex(object, pose_path); + + if (opt_starting_idx.has_value()) { + // Trim the reference path + pose_path.erase(pose_path.begin(), pose_path.begin() + opt_starting_idx.value()); + ++it; + } else { + // Proper starting point is not found, remove the reference path + it = all_ref_paths.erase(it); + } + } + return all_ref_paths; } @@ -1912,7 +2035,8 @@ void MapBasedPredictionNode::addReferencePaths( for (const auto & converted_path : converted_paths) { PredictedRefPath predicted_path; predicted_path.probability = maneuver_probability.at(maneuver) * path_probability; - predicted_path.path = converted_path; + predicted_path.path = converted_path.first; + predicted_path.width = converted_path.second; predicted_path.maneuver = maneuver; reference_paths.push_back(predicted_path); } @@ -1983,12 +2107,17 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( return maneuver_prob; } -std::vector MapBasedPredictionNode::convertPathType( - const lanelet::routing::LaneletPaths & paths) +std::vector> MapBasedPredictionNode::convertPathType( + const lanelet::routing::LaneletPaths & paths) const { - std::vector converted_paths; + 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; + double width = 10.0; // Initialize with a large value // Insert Positions. Note that we start inserting points from previous lanelet if (!path.empty()) { @@ -2008,7 +2137,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 = tier4_autoware_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; } @@ -2039,18 +2174,39 @@ 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 = tier4_autoware_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; } + + // Update minimum width + const auto left_bound = lanelet.leftBound2d(); + const auto right_bound = lanelet.rightBound2d(); + const double lanelet_width_front = std::hypot( + left_bound.front().x() - right_bound.front().x(), + left_bound.front().y() - right_bound.front().y()); + width = std::min(width, lanelet_width_front); } // Resample Path - const auto resampled_converted_path = - motion_utils::resamplePoseVector(converted_path, reference_path_resolution_); - converted_paths.push_back(resampled_converted_path); - } - + 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 = motion_utils::resamplePoseVector( + converted_path, reference_path_resolution_, use_akima_spline_for_xy, use_lerp_for_z); + converted_paths.push_back(std::make_pair(resampled_converted_path, width)); + } + + lru_cache_of_convert_path_type_.put(paths, converted_paths); return converted_paths; } diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 5cb7e186b7cc4..1c3e23bfe68f1 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -14,7 +14,7 @@ #include "map_based_prediction/path_generator.hpp" -#include +#include #include #include @@ -149,13 +149,32 @@ PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & } PredictedPath PathGenerator::generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths) + const TrackedObject & object, const PosePath & ref_path, const double path_width) { - if (ref_paths.size() < 2) { + if (ref_path.size() < 2) { return generateStraightPath(object); } - return generatePolynomialPath(object, ref_paths); + // if the object is moving backward, we generate a straight path + if (object.kinematics.twist_with_covariance.twist.linear.x < 0.0) { + return generateStraightPath(object); + } + + // get object width + double object_width = 5.0; // a large number + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + object_width = object.shape.dimensions.y; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + object_width = object.shape.dimensions.x; + } + // Calculate the backlash width, which represents the maximum distance the object can be biased + // from the reference path + constexpr double margin = + 0.5; // Set a safety margin of 0.5m for the object to stay away from the edge of the lane + double backlash_width = (path_width - object_width) / 2.0 - margin; + backlash_width = std::max(backlash_width, 0.0); // minimum is 0.0 + + return generatePolynomialPath(object, ref_path, path_width, backlash_width); } PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) const @@ -178,22 +197,63 @@ PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) } PredictedPath PathGenerator::generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path) + const TrackedObject & object, const PosePath & ref_path, const double path_width, + const double backlash_width) const { // Get current Frenet Point const double ref_path_len = motion_utils::calcArcLength(ref_path); - const auto current_point = getFrenetPoint(object, ref_path); + const auto current_point = getFrenetPoint(object, ref_path.at(0)); // Step1. Set Target Frenet Point // Note that we do not set position s, // since we don't know the target longitudinal position FrenetPoint terminal_point; - terminal_point.s_vel = current_point.s_vel; + terminal_point.s_vel = std::hypot(current_point.s_vel, current_point.d_vel); terminal_point.s_acc = 0.0; terminal_point.d = 0.0; terminal_point.d_vel = 0.0; terminal_point.d_acc = 0.0; + // if the object is behind of the reference path adjust the lateral_time_horizon_ to reach the + // start of the reference path + double lateral_duration_adjusted = lateral_time_horizon_; + if (current_point.s < 0.0) { + const double distance_to_start = -current_point.s; + const double duration_to_reach = distance_to_start / terminal_point.s_vel; + lateral_duration_adjusted = std::max(lateral_duration_adjusted, duration_to_reach); + } + + // calculate terminal d position, based on backlash width + { + if (backlash_width < 0.01 /*m*/) { + // If the backlash width is less than 0.01m, do not consider the backlash width and reduce + // calculation cost + terminal_point.d = 0.0; + } else { + const double return_width = path_width / 2.0; // [m] + const double current_momentum_d = + current_point.d + 0.5 * current_point.d_vel * lateral_duration_adjusted; + const double momentum_d_abs = std::abs(current_momentum_d); + + if (momentum_d_abs < backlash_width) { + // If the object momentum is within the backlash width, we set the target d position to the + // current momentum + terminal_point.d = current_momentum_d; + } else if ( + momentum_d_abs >= backlash_width && momentum_d_abs < backlash_width + return_width) { + // If the object momentum is within the return zone, we set the target d position close to + // the zero gradually + terminal_point.d = + (backlash_width + return_width - momentum_d_abs) * backlash_width / return_width; + terminal_point.d *= (current_momentum_d > 0) ? 1 : -1; + } else { + // If the object momentum is outside the backlash width + return zone, we set the target d + // position to 0 + terminal_point.d = 0.0; + } + } + } + // Step2. Generate Predicted Path on a Frenet coordinate const auto frenet_predicted_path = generateFrenetPath(current_point, terminal_point, ref_path_len); @@ -210,7 +270,8 @@ PredictedPath PathGenerator::generatePolynomialPath( } FrenetPath PathGenerator::generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length) + const FrenetPoint & current_point, const FrenetPoint & target_point, + const double max_length) const { FrenetPath path; const double duration = time_horizon_; @@ -223,28 +284,26 @@ FrenetPath PathGenerator::generateFrenetPath( path.reserve(static_cast(duration / sampling_time_interval_)); for (double t = 0.0; t <= duration; t += sampling_time_interval_) { + const auto t2 = t * t; + const auto t3 = t2 * t; + const auto t4 = t3 * t; + const auto t5 = t4 * t; const double current_acc = 0.0; // Currently we assume the object is traveling at a constant speed - const double d_next_ = current_point.d + current_point.d_vel * t + - current_acc * 2.0 * std::pow(t, 2) + lat_coeff(0) * std::pow(t, 3) + - lat_coeff(1) * std::pow(t, 4) + lat_coeff(2) * std::pow(t, 5); - // t > lateral_duration: 0.0, else d_next_ - const double d_next = t > lateral_duration ? 0.0 : d_next_; - const double s_next = current_point.s + current_point.s_vel * t + - 2.0 * current_acc * std::pow(t, 2) + lon_coeff(0) * std::pow(t, 3) + - lon_coeff(1) * std::pow(t, 4); + const double d_next_ = current_point.d + current_point.d_vel * t + current_acc * 2.0 * t2 + + lat_coeff(0) * t3 + lat_coeff(1) * t4 + lat_coeff(2) * t5; + // t > lateral_duration: target_point.d, else d_next_ + const double d_next = t > lateral_duration ? target_point.d : d_next_; + const double s_next = current_point.s + current_point.s_vel * t + 2.0 * current_acc * t2 + + lon_coeff(0) * t3 + lon_coeff(1) * t4; if (s_next > max_length) { break; } // We assume the object is traveling at a constant speed along s direction FrenetPoint point; - point.s = std::max(s_next, 0.0); - point.s_vel = current_point.s_vel; - point.s_acc = current_point.s_acc; + point.s = s_next; point.d = d_next; - point.d_vel = current_point.d_vel; - point.d_acc = current_point.d_acc; path.push_back(point); } @@ -252,7 +311,7 @@ FrenetPath PathGenerator::generateFrenetPath( } Eigen::Vector3d PathGenerator::calcLatCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const { // Lateral Path Calculation // Quintic polynomial for d @@ -265,10 +324,14 @@ Eigen::Vector3d PathGenerator::calcLatCoefficients( // b = np.matrix([[xe - self.a0 - self.a1 * T - self.a2 * T**2], // [vxe - self.a1 - 2 * self.a2 * T], // [axe - 2 * self.a2]]) + const auto T2 = T * T; + const auto T3 = T2 * T; + const auto T4 = T3 * T; + const auto T5 = T4 * T; + Eigen::Matrix3d A_lat_inv; - A_lat_inv << 10 / std::pow(T, 3), -4 / std::pow(T, 2), 1 / (2 * T), -15 / std::pow(T, 4), - 7 / std::pow(T, 3), -1 / std::pow(T, 2), 6 / std::pow(T, 5), -3 / std::pow(T, 4), - 1 / (2 * std::pow(T, 3)); + A_lat_inv << 10 / T3, -4 / T2, 1 / (2 * T), -15 / T4, 7 / T3, -1 / T2, 6 / T5, -3 / T4, + 1 / (2 * T3); Eigen::Vector3d b_lat; b_lat[0] = target_point.d - current_point.d - current_point.d_vel * T; b_lat[1] = target_point.d_vel - current_point.d_vel; @@ -278,7 +341,7 @@ Eigen::Vector3d PathGenerator::calcLatCoefficients( } Eigen::Vector2d PathGenerator::calcLonCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const { // Longitudinal Path Calculation // Quadric polynomial @@ -286,17 +349,112 @@ Eigen::Vector2d PathGenerator::calcLonCoefficients( // [-1/(2*T**3), 1/(4*T**2)]]) // b = np.matrix([[vxe - self.a1 - 2 * self.a2 * T], // [axe - 2 * self.a2]]) + const auto T2 = T * T; + const auto T3 = T2 * T; + Eigen::Matrix2d A_lon_inv; - A_lon_inv << 1 / std::pow(T, 2), -1 / (3 * T), -1 / (2 * std::pow(T, 3)), - 1 / (4 * std::pow(T, 2)); + A_lon_inv << 1 / T2, -1 / (3 * T), -1 / (2 * T3), 1 / (4 * T2); Eigen::Vector2d b_lon; b_lon[0] = target_point.s_vel - current_point.s_vel; b_lon[1] = 0.0; return A_lon_inv * b_lon; } +std::vector PathGenerator::interpolationLerp( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys) const +{ + // calculate linear interpolation + // extrapolate the value if the query key is out of the base key range + std::vector query_values; + size_t key_index = 0; + double last_query_key = query_keys.at(0); + for (const auto query_key : query_keys) { + // search for the closest key index + // if current query key is larger than the last query key, search base_keys increasing order + if (query_key >= last_query_key) { + while (base_keys.at(key_index + 1) < query_key) { + if (key_index == base_keys.size() - 2) { + break; + } + ++key_index; + } + } else { + // if current query key is smaller than the last query key, search base_keys decreasing order + while (base_keys.at(key_index) > query_key) { + if (key_index == 0) { + break; + } + --key_index; + } + } + last_query_key = query_key; + + const double & src_val = base_values.at(key_index); + const double & dst_val = base_values.at(key_index + 1); + const double ratio = (query_key - base_keys.at(key_index)) / + (base_keys.at(key_index + 1) - base_keys.at(key_index)); + + const double interpolated_val = src_val + (dst_val - src_val) * ratio; + query_values.push_back(interpolated_val); + } + + return query_values; +} + +std::vector PathGenerator::interpolationLerp( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys) const +{ + // calculate linear interpolation + // extrapolate the value if the query key is out of the base key range + std::vector query_values; + size_t key_index = 0; + double last_query_key = query_keys.at(0); + for (const auto query_key : query_keys) { + // search for the closest key index + // if current query key is larger than the last query key, search base_keys increasing order + if (query_key >= last_query_key) { + while (base_keys.at(key_index + 1) < query_key) { + if (key_index == base_keys.size() - 2) { + break; + } + ++key_index; + } + } else { + // if current query key is smaller than the last query key, search base_keys decreasing order + while (base_keys.at(key_index) > query_key) { + if (key_index == 0) { + break; + } + --key_index; + } + } + last_query_key = query_key; + + const tf2::Quaternion & src_val = base_values.at(key_index); + const tf2::Quaternion & dst_val = base_values.at(key_index + 1); + const double ratio = (query_key - base_keys.at(key_index)) / + (base_keys.at(key_index + 1) - base_keys.at(key_index)); + + // in case of extrapolation, export the nearest quaternion + if (ratio < 0.0) { + query_values.push_back(src_val); + continue; + } + if (ratio > 1.0) { + query_values.push_back(dst_val); + continue; + } + const auto interpolated_quat = tf2::slerp(src_val, dst_val, ratio); + query_values.push_back(interpolated_quat); + } + + return query_values; +} + PosePath PathGenerator::interpolateReferencePath( - const PosePath & base_path, const FrenetPath & frenet_predicted_path) + const PosePath & base_path, const FrenetPath & frenet_predicted_path) const { PosePath interpolated_path; const size_t interpolate_num = frenet_predicted_path.size(); @@ -308,100 +466,102 @@ PosePath PathGenerator::interpolateReferencePath( std::vector base_path_x(base_path.size()); std::vector base_path_y(base_path.size()); std::vector base_path_z(base_path.size()); + std::vector base_path_orientation(base_path.size()); std::vector base_path_s(base_path.size(), 0.0); for (size_t i = 0; i < base_path.size(); ++i) { base_path_x.at(i) = base_path.at(i).position.x; base_path_y.at(i) = base_path.at(i).position.y; base_path_z.at(i) = base_path.at(i).position.z; + tf2::Quaternion src_tf; + tf2::fromMsg(base_path.at(i).orientation, src_tf); + base_path_orientation.at(i) = src_tf; if (i > 0) { base_path_s.at(i) = base_path_s.at(i - 1) + tier4_autoware_utils::calcDistance2d( base_path.at(i - 1), base_path.at(i)); } } + // Prepare resampled s vector std::vector resampled_s(frenet_predicted_path.size()); for (size_t i = 0; i < frenet_predicted_path.size(); ++i) { resampled_s.at(i) = frenet_predicted_path.at(i).s; } - if (resampled_s.front() > resampled_s.back()) { - 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); + // Linear Interpolation for x, y, z, and orientation + std::vector lerp_ref_path_x = interpolationLerp(base_path_s, base_path_x, resampled_s); + std::vector lerp_ref_path_y = interpolationLerp(base_path_s, base_path_y, resampled_s); + std::vector lerp_ref_path_z = interpolationLerp(base_path_s, base_path_z, resampled_s); + std::vector lerp_ref_path_orientation = + interpolationLerp(base_path_s, base_path_orientation, resampled_s); interpolated_path.resize(interpolate_num); - for (size_t i = 0; i < interpolate_num - 1; ++i) { + for (size_t i = 0; i < interpolate_num; ++i) { geometry_msgs::msg::Pose interpolated_pose; - const auto current_point = - tier4_autoware_utils::createPoint(spline_ref_path_x.at(i), spline_ref_path_y.at(i), 0.0); - const auto next_point = tier4_autoware_utils::createPoint( - spline_ref_path_x.at(i + 1), spline_ref_path_y.at(i + 1), 0.0); - const double yaw = tier4_autoware_utils::calcAzimuthAngle(current_point, next_point); interpolated_pose.position = tier4_autoware_utils::createPoint( - spline_ref_path_x.at(i), spline_ref_path_y.at(i), spline_ref_path_z.at(i)); - interpolated_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + lerp_ref_path_x.at(i), lerp_ref_path_y.at(i), lerp_ref_path_z.at(i)); + interpolated_pose.orientation = tf2::toMsg(lerp_ref_path_orientation.at(i)); interpolated_path.at(i) = interpolated_pose; } - interpolated_path.back().position = tier4_autoware_utils::createPoint( - spline_ref_path_x.back(), spline_ref_path_y.back(), spline_ref_path_z.back()); - interpolated_path.back().orientation = interpolated_path.at(interpolate_num - 2).orientation; - return interpolated_path; } PredictedPath PathGenerator::convertToPredictedPath( - const TrackedObject & object, const FrenetPath & frenet_predicted_path, const PosePath & ref_path) + const TrackedObject & object, const FrenetPath & frenet_predicted_path, + const PosePath & ref_path) const { + // Object position + const auto & object_pose = object.kinematics.pose_with_covariance.pose; + const double object_height = object.shape.dimensions.z / 2.0; + + // Convert Frenet Path to Cartesian Path PredictedPath predicted_path; predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); predicted_path.path.resize(ref_path.size()); - for (size_t i = 0; i < predicted_path.path.size(); ++i) { + + // Set the first point as the object's current position + predicted_path.path.at(0) = object_pose; + + for (size_t i = 1; i < predicted_path.path.size(); ++i) { // Reference Point from interpolated reference path const auto & ref_pose = ref_path.at(i); // Frenet Point from frenet predicted path const auto & frenet_point = frenet_predicted_path.at(i); + double d_offset = frenet_point.d; // Converted Pose - auto predicted_pose = tier4_autoware_utils::calcOffsetPose(ref_pose, 0.0, frenet_point.d, 0.0); - predicted_pose.position.z = object.kinematics.pose_with_covariance.pose.position.z; - if (i == 0) { - predicted_pose.orientation = object.kinematics.pose_with_covariance.pose.orientation; - } else { - const double yaw = tier4_autoware_utils::calcAzimuthAngle( - predicted_path.path.at(i - 1).position, predicted_pose.position); - predicted_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - } + auto predicted_pose = tier4_autoware_utils::calcOffsetPose(ref_pose, 0.0, d_offset, 0.0); + predicted_pose.position.z += object_height; + + const double yaw = tier4_autoware_utils::calcAzimuthAngle( + predicted_path.path.at(i - 1).position, predicted_pose.position); + predicted_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + predicted_path.path.at(i) = predicted_pose; } return predicted_path; } -FrenetPoint PathGenerator::getFrenetPoint(const TrackedObject & object, const PosePath & ref_path) +FrenetPoint PathGenerator::getFrenetPoint( + const TrackedObject & object, const geometry_msgs::msg::Pose & ref_pose) const { FrenetPoint frenet_point; + + // 1. Position const auto obj_point = object.kinematics.pose_with_covariance.pose.position; + const float obj_yaw = + static_cast(tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); + const float lane_yaw = static_cast(tf2::getYaw(ref_pose.orientation)); + frenet_point.s = (obj_point.x - ref_pose.position.x) * cos(lane_yaw) + + (obj_point.y - ref_pose.position.y) * sin(lane_yaw); + frenet_point.d = -(obj_point.x - ref_pose.position.x) * sin(lane_yaw) + + (obj_point.y - ref_pose.position.y) * cos(lane_yaw); - const size_t nearest_segment_idx = motion_utils::findNearestSegmentIndex(ref_path, obj_point); - const double l = - motion_utils::calcLongitudinalOffsetToSegment(ref_path, nearest_segment_idx, obj_point); + // 2. Velocity const float vx = static_cast(object.kinematics.twist_with_covariance.twist.linear.x); const float vy = static_cast(object.kinematics.twist_with_covariance.twist.linear.y); - const float obj_yaw = - static_cast(tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - const float lane_yaw = - static_cast(tf2::getYaw(ref_path.at(nearest_segment_idx).orientation)); const float delta_yaw = obj_yaw - lane_yaw; - - frenet_point.s = motion_utils::calcSignedArcLength(ref_path, 0, nearest_segment_idx) + l; - frenet_point.d = motion_utils::calcLateralOffset(ref_path, obj_point); frenet_point.s_vel = vx * std::cos(delta_yaw) - vy * std::sin(delta_yaw); frenet_point.d_vel = vx * std::sin(delta_yaw) + vy * std::cos(delta_yaw); frenet_point.s_acc = 0.0; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 95d33b78ff184..7e9f3a8754695 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -98,6 +98,11 @@ class MultiObjectTracker : public rclcpp::Node rclcpp::Subscription::SharedPtr detected_object_sub_; rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer + rclcpp::Time last_published_time_; + rclcpp::Time last_updated_time_; + double publisher_period_; + static constexpr double minimum_publish_interval_ratio = 0.85; + static constexpr double maximum_publish_interval_ratio = 1.05; // debugger class std::unique_ptr debugger_; @@ -116,8 +121,7 @@ class MultiObjectTracker : public rclcpp::Node std::unique_ptr data_association_; void checkTrackerLifeCycle( - std::list> & list_tracker, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform); + std::list> & list_tracker, const rclcpp::Time & time); void sanitizeTracker( std::list> & list_tracker, const rclcpp::Time & time); std::shared_ptr createNewTracker( diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index d28833241bd5f..2acc4f447cb50 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -174,6 +174,8 @@ void TrackerDebugger::startMeasurementTime(const rclcpp::Time & measurement_head MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("multi_object_tracker", node_options), + last_published_time_(this->now()), + last_updated_time_(this->now()), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { @@ -196,11 +198,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) this->get_node_base_interface(), this->get_node_timers_interface()); tf_buffer_.setCreateTimerInterface(cti); - // Create ROS time based timer + // Create ROS time based timer. + // If the delay compensation is enabled, the timer is used to publish the output at the correct + // time. if (enable_delay_compensation) { - const auto period_ns = rclcpp::Rate(publish_rate).period(); + publisher_period_ = 1.0 / publish_rate; // [s] + constexpr double timer_multiplier = 10.0; // 10 times frequent for publish timing check + const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period(); publish_timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&MultiObjectTracker::onTimer, this)); + this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this)); } const auto tmp = this->declare_parameter>("can_assign_matrix"); @@ -236,6 +242,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) void MultiObjectTracker::onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) { + last_updated_time_ = this->now(); + /* keep the latest input stamp and check transform*/ debugger_->startMeasurementTime(rclcpp::Time(input_objects_msg->header.stamp)); const auto self_transform = getTransformAnonymous( @@ -277,7 +285,7 @@ void MultiObjectTracker::onMeasurement( } /* life cycle check */ - checkTrackerLifeCycle(list_tracker_, measurement_time, *self_transform); + checkTrackerLifeCycle(list_tracker_, measurement_time); /* sanitize trackers */ sanitizeTracker(list_tracker_, measurement_time); @@ -326,14 +334,28 @@ std::shared_ptr MultiObjectTracker::createNewTracker( void MultiObjectTracker::onTimer() { rclcpp::Time current_time = this->now(); - const auto self_transform = - getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time); - if (!self_transform) { + + // ensure minimum interval: room for the next process(prediction) + const double minimum_publish_interval = publisher_period_ * minimum_publish_interval_ratio; + const auto elapsed_time = (current_time - last_published_time_).seconds(); + if (elapsed_time < minimum_publish_interval) { + return; + } + + // if there was update after publishing, publish new messages + bool should_publish = last_published_time_ < last_updated_time_; + + // if there was no update, publish if the elapsed time is longer than the maximum publish latency + // in this case, it will perform extrapolate/remove old objects + const double maximum_publish_interval = publisher_period_ * maximum_publish_interval_ratio; + should_publish = should_publish || elapsed_time > maximum_publish_interval; + + if (!should_publish) { return; } /* life cycle check */ - checkTrackerLifeCycle(list_tracker_, current_time, *self_transform); + checkTrackerLifeCycle(list_tracker_, current_time); /* sanitize trackers */ sanitizeTracker(list_tracker_, current_time); @@ -342,8 +364,7 @@ void MultiObjectTracker::onTimer() } void MultiObjectTracker::checkTrackerLifeCycle( - std::list> & list_tracker, const rclcpp::Time & time, - [[maybe_unused]] const geometry_msgs::msg::Transform & self_transform) + std::list> & list_tracker, const rclcpp::Time & time) { /* params */ constexpr float max_elapsed_time = 1.0; @@ -464,6 +485,9 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) // Publish tracked_objects_pub_->publish(output_msg); + // Update last published time + last_published_time_ = this->now(); + // Debugger Publish if enabled debugger_->publishProcessingTime(); debugger_->publishTentativeObjects(tentative_objects_msg);