feat(map_based_prediction): add time_keeper (autowarefoundation#8176) (…

…#1426)

Signed-off-by: Mamoru Sobue <[email protected]>

perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -26,6 +26,7 @@
 #include <autoware/universe_utils/ros/transform_listener.hpp>
 #include <autoware/universe_utils/ros/uuid_helper.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
+#include <autoware/universe_utils/system/time_keeper.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
@@ -222,6 +223,9 @@ class MapBasedPredictionNode : public rclcpp::Node
   bool remember_lost_crosswalk_users_;
 
   std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
+  rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr
+    detailed_processing_time_publisher_;
+  mutable autoware::universe_utils::TimeKeeper time_keeper_;
 
   // Member Functions
   void mapCallback(const LaneletMapBin::ConstSharedPtr msg);

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<autoware::universe_utils::PublishedTimePublisher>(this);
+  detailed_processing_time_publisher_ =
+    this->create_publisher<autoware::universe_utils::ProcessingTimeDetail>(
+      "~/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::LaneletMap>();
   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<std::string, TrackedObject> & current_users)
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+
   const auto known_match_opt = [&]() -> std::optional<std::string> {
     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<double, lanelet::Lanelet> & 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<PredictedRefPath> 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<geometry_msgs::msg::Point> 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<PredictedRefPath> & 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<PosePath> MapBasedPredictionNode::convertPathType(
   const lanelet::routing::LaneletPaths & paths)
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+
   std::vector<PosePath> converted_paths;
   for (const auto & path : paths) {
     PosePath converted_path;
@@ -2381,6 +2431,8 @@ bool MapBasedPredictionNode::isDuplicated(
   const std::pair<double, lanelet::ConstLanelet> & 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<PredictedPath> & 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<lanelet::Id> MapBasedPredictionNode::getTrafficSignalId(
   const lanelet::ConstLanelet & way_lanelet)
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+
   const auto traffic_light_reg_elems =
     way_lanelet.regulatoryElementsAs<const lanelet::TrafficLight>();
   if (traffic_light_reg_elems.empty()) {
@@ -2430,6 +2486,8 @@ std::optional<lanelet::Id> MapBasedPredictionNode::getTrafficSignalId(
 std::optional<TrafficLightElement> 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;