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 ce1c587cc3b01..1c677c9d0f6e4 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 @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -222,6 +223,9 @@ class MapBasedPredictionNode : public rclcpp::Node bool remember_lost_crosswalk_users_; std::unique_ptr published_time_publisher_; + rclcpp::Publisher::SharedPtr + detailed_processing_time_publisher_; + mutable autoware::universe_utils::TimeKeeper time_keeper_; // Member Functions void mapCallback(const LaneletMapBin::ConstSharedPtr msg); 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 f505cb7cc5592..eaea3c06041b3 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 @@ -863,6 +863,11 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ 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_); + set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); @@ -922,6 +927,8 @@ 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( @@ -938,6 +945,8 @@ void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg void MapBasedPredictionNode::trafficSignalsCallback( const TrafficLightGroupArray::ConstSharedPtr msg) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + traffic_signal_id_map_.clear(); for (const auto & signal : msg->traffic_light_groups) { traffic_signal_id_map_[signal.traffic_light_group_id] = signal; @@ -946,6 +955,8 @@ void MapBasedPredictionNode::trafficSignalsCallback( void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + stop_watch_ptr_->toc("processing_time", true); // take traffic_signal @@ -1237,6 +1248,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt 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_); + CrosswalkUserData crosswalk_user_data; crosswalk_user_data.header = header; crosswalk_user_data.tracked_object = object; @@ -1252,6 +1265,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_); + const auto known_match_opt = [&]() -> std::optional { if (!known_matches_.count(object_id)) { return std::nullopt; @@ -1309,6 +1324,7 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared( bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); const lanelet::ConstLineStrings3d & all_fences = lanelet::utils::query::getAllFences(lanelet_map_ptr_); for (const auto & fence_line : all_fences) { @@ -1322,6 +1338,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_); + // check whether the predicted path cross with fence for (size_t i = 0; i < predicted_path.path.size() - 1; ++i) { for (size_t j = 0; j < fence_line.size() - 1; ++j) { @@ -1350,6 +1368,8 @@ bool MapBasedPredictionNode::isIntersecting( PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const TrackedObject & object) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + auto predicted_object = convertToPredictedObject(object); { PredictedPath predicted_path = @@ -1482,6 +1502,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( void MapBasedPredictionNode::updateObjectData(TrackedObject & object) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + if ( object.kinematics.orientation_availability == autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { @@ -1532,6 +1554,8 @@ 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(), @@ -1548,6 +1572,8 @@ void MapBasedPredictionNode::removeStaleTrafficLightInfo( LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & object) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + // obstacle point lanelet::BasicPoint2d search_point( object.kinematics.pose_with_covariance.pose.position.x, @@ -1638,6 +1664,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_); + // Step1. If we only have one point in the centerline, we will ignore the lanelet if (lanelet.second.centerline().size() <= 1) { return false; @@ -1684,6 +1712,8 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( float MapBasedPredictionNode::calculateLocalLikelihood( const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + const auto & obj_point = object.kinematics.pose_with_covariance.pose.position; // compute yaw difference between the object and lane @@ -1719,6 +1749,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::string object_id = autoware::universe_utils::toHexString(object.object_id); const auto current_lanelets = getLanelets(current_lanelets_data); @@ -1760,6 +1792,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_); + const double obj_vel = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); @@ -1921,6 +1955,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_); + // calculate maneuver const auto current_maneuver = [&]() { if (lane_change_detection_method_ == "time_to_change_lane") { @@ -1971,6 +2007,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_); + // Step1. Check if we have the object in the buffer const std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { @@ -2042,6 +2080,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_); + // Step1. Check if we have the object in the buffer const std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { @@ -2185,6 +2225,8 @@ 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(); @@ -2204,6 +2246,8 @@ double MapBasedPredictionNode::calcLeftLateralOffset( void MapBasedPredictionNode::updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::string object_id = autoware::universe_utils::toHexString(object.object_id); if (road_users_history.count(object_id) == 0) { return; @@ -2228,6 +2272,8 @@ void MapBasedPredictionNode::addReferencePaths( const Maneuver & maneuver, std::vector & reference_paths, const double speed_limit) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + if (!candidate_paths.empty()) { updateFuturePossibleLanelets(object, candidate_paths); const auto converted_paths = convertPathType(candidate_paths); @@ -2247,6 +2293,8 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( const lanelet::routing::LaneletPaths & right_paths, const lanelet::routing::LaneletPaths & center_paths) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + float left_lane_change_probability = 0.0; float right_lane_change_probability = 0.0; float lane_follow_probability = 0.0; @@ -2309,6 +2357,8 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( std::vector MapBasedPredictionNode::convertPathType( const lanelet::routing::LaneletPaths & paths) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + std::vector converted_paths; for (const auto & path : paths) { PosePath converted_path; @@ -2381,6 +2431,8 @@ 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(); @@ -2398,6 +2450,8 @@ 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; @@ -2414,6 +2468,8 @@ 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()) { @@ -2430,6 +2486,8 @@ std::optional MapBasedPredictionNode::getTrafficSignalId( std::optional MapBasedPredictionNode::getTrafficSignalElement( const lanelet::Id & id) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + if (traffic_signal_id_map_.count(id) != 0) { const auto & signal_elements = traffic_signal_id_map_.at(id).elements; if (signal_elements.size() > 1) { @@ -2446,6 +2504,8 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal( const TrackedObject & object, const lanelet::ConstLanelet & crosswalk, const lanelet::Id & signal_id) { + autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + const auto signal_color = [&] { const auto elem_opt = getTrafficSignalElement(signal_id); return elem_opt ? elem_opt.value().color : TrafficLightElement::UNKNOWN;