From 398ab6ef9cc76fc461ef082e944387d4bc7d4285 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 15 Aug 2024 11:26:00 +0900 Subject: [PATCH] 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;