Merge branch 'main' into feat_mrm_handler_add_operator_class

Author: veqcc

evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp

@@ -79,7 +79,9 @@ class EvalTest : public ::testing::Test
 
     marker_sub_ = rclcpp::create_subscription<MarkerArray>(
       eval_node, "perception_online_evaluator/markers", 10,
-      [this]([[maybe_unused]] const MarkerArray::SharedPtr msg) { has_received_marker_ = true; });
+      [this]([[maybe_unused]] const MarkerArray::ConstSharedPtr msg) {
+        has_received_marker_ = true;
+      });
     uuid_ = generateUUID();
   }
 

localization/localization_util/src/smart_pose_buffer.cpp

@@ -40,6 +40,7 @@ std::optional<SmartPoseBuffer::InterpolateResult> SmartPoseBuffer::interpolate(
     const rclcpp::Time time_last = pose_buffer_.back()->header.stamp;
 
     if (target_ros_time < time_first) {
+      RCLCPP_INFO(logger_, "Mismatch between pose timestamp and current timestamp");
       return std::nullopt;
     }
 

localization/localization_util/src/util_func.cpp

@@ -241,7 +241,7 @@ void output_pose_with_cov_to_log(
   rpy.y = rpy.y * 180.0 / M_PI;
   rpy.z = rpy.z * 180.0 / M_PI;
 
-  RCLCPP_INFO_STREAM(
+  RCLCPP_DEBUG_STREAM(
     logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << ","
                        << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y
                        << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -182,7 +182,7 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
   const auto duration_micro_sec =
     std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
   const auto exe_time = static_cast<double>(duration_micro_sec) / 1000.0;
-  RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time);
+  RCLCPP_DEBUG(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time);
   return true;  // Updated
 }
 

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -992,7 +992,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose(
   result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose;
 
   output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg);
-  RCLCPP_INFO_STREAM(get_logger(), "best_score," << best_particle_ptr->score);
+  RCLCPP_DEBUG_STREAM(get_logger(), "best_score," << best_particle_ptr->score);
 
   return result_pose_with_cov_msg;
 }

localization/pose_initializer/src/pose_initializer/ndt_module.cpp

@@ -40,7 +40,6 @@ PoseWithCovarianceStamped NdtModule::align_pose(const PoseWithCovarianceStamped
   RCLCPP_INFO(logger_, "Call NDT align server.");
   const auto res = cli_align_->async_send_request(req).get();
   if (!res->success) {
-    RCLCPP_INFO(logger_, "NDT align server failed.");
     throw ServiceException(
       Initialize::Service::Response::ERROR_ESTIMATION, "NDT align server failed.");
   }

perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp

@@ -20,6 +20,8 @@
 #include <euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <shape_estimation/shape_estimator.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
@@ -61,6 +63,11 @@ class Debugger
     divided_objects_pub_ =
       node->create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
         "debug/divided_objects", 1);
+    processing_time_publisher_ =
+      std::make_unique<tier4_autoware_utils::DebugPublisher>(node, "detection_by_tracker");
+    stop_watch_ptr_ =
+      std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
+    this->startStopWatch();
   }
 
   ~Debugger() {}
@@ -80,6 +87,19 @@ class Debugger
   {
     divided_objects_pub_->publish(removeFeature(input));
   }
+  void startStopWatch()
+  {
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
+  void startMeasureProcessingTime() { stop_watch_ptr_->tic("processing_time"); }
+  void publishProcessingTime()
+  {
+    processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true));
+    processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true));
+  }
 
 private:
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
@@ -90,6 +110,9 @@ class Debugger
     merged_objects_pub_;
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
     divided_objects_pub_;
+  // debug publisher
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
 
   autoware_auto_perception_msgs::msg::DetectedObjects removeFeature(
     const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input)

perception/detection_by_tracker/src/detection_by_tracker_core.cpp

@@ -206,6 +206,7 @@ void DetectionByTracker::setMaxSearchRange()
 void DetectionByTracker::onObjects(
   const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg)
 {
+  debugger_->startMeasureProcessingTime();
   autoware_auto_perception_msgs::msg::DetectedObjects detected_objects;
   detected_objects.header = input_msg->header;
 
@@ -250,6 +251,7 @@ void DetectionByTracker::onObjects(
   }
 
   objects_pub_->publish(detected_objects);
+  debugger_->publishProcessingTime();
 }
 
 void DetectionByTracker::divideUnderSegmentedObjects(

perception/map_based_prediction/README.md

@@ -218,11 +218,13 @@ If the target object is inside the road or crosswalk, this module outputs one or
 
 ### Output
 
-| Name                     | Type                                                   | Description                                                                           |
-| ------------------------ | ------------------------------------------------------ | ------------------------------------------------------------------------------------- |
-| `~/input/objects`        | `autoware_auto_perception_msgs::msg::TrackedObjects`   | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` |
-| `~/output/objects`       | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path.                                                 |
-| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray`                 | marker for visualization.                                                             |
+| Name                         | Type                                                   | Description                                                                           |
+| ---------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------------------------- |
+| `~/input/objects`            | `autoware_auto_perception_msgs::msg::TrackedObjects`   | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` |
+| `~/output/objects`           | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path.                                                 |
+| `~/objects_path_markers`     | `visualization_msgs::msg::MarkerArray`                 | marker for visualization.                                                             |
+| `~/debug/processing_time_ms` | `std_msgs::msg::Float64`                               | processing time of this module.                                                       |
+| `~/debug/cyclic_time_ms`     | `std_msgs::msg::Float64`                               | cyclic time of this module.                                                           |
 
 ## Parameters
 

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -21,6 +21,7 @@
 #include "tier4_autoware_utils/ros/update_param.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
 #include <tier4_autoware_utils/ros/transform_listener.hpp>
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
@@ -33,7 +34,6 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
-#include <tier4_debug_msgs/msg/string_stamped.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <lanelet2_core/Forward.h>
@@ -125,11 +125,14 @@ class MapBasedPredictionNode : public rclcpp::Node
   // ROS Publisher and Subscriber
   rclcpp::Publisher<PredictedObjects>::SharedPtr pub_objects_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_debug_markers_;
-  rclcpp::Publisher<StringStamped>::SharedPtr pub_calculation_time_;
   rclcpp::Subscription<TrackedObjects>::SharedPtr sub_objects_;
   rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
   rclcpp::Subscription<TrafficSignalArray>::SharedPtr sub_traffic_signals_;
 
+  // debug publisher
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
+
   // Object History
   std::unordered_map<std::string, std::deque<ObjectData>> objects_history_;
   std::map<std::pair<std::string, lanelet::Id>, rclcpp::Time> stopped_times_against_green_;
@@ -196,9 +199,6 @@ class MapBasedPredictionNode : public rclcpp::Node
   std::vector<double> distance_set_for_no_intention_to_walk_;
   std::vector<double> timeout_set_for_no_intention_to_walk_;
 
-  // Stop watch
-  StopWatch<std::chrono::milliseconds> stop_watch_;
-
   // Member Functions
   void mapCallback(const HADMapBin::ConstSharedPtr msg);
   void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg);

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -646,14 +646,6 @@ ObjectClassification::_label_type changeLabelForPrediction(
   }
 }
 
-StringStamped createStringStamped(const rclcpp::Time & now, const double data)
-{
-  StringStamped msg;
-  msg.stamp = now;
-  msg.data = std::to_string(data);
-  return msg;
-}
-
 // NOTE: These two functions are copied from the route_handler package.
 lanelet::Lanelets getRightOppositeLanelets(
   const std::shared_ptr<lanelet::LaneletMap> & lanelet_map_ptr,
@@ -816,10 +808,15 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
   pub_objects_ = this->create_publisher<PredictedObjects>("~/output/objects", rclcpp::QoS{1});
   pub_debug_markers_ =
     this->create_publisher<visualization_msgs::msg::MarkerArray>("maneuver", rclcpp::QoS{1});
-  pub_calculation_time_ = create_publisher<StringStamped>("~/debug/calculation_time", 1);
+  processing_time_publisher_ =
+    std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "map_based_prediction");
 
   set_param_res_ = this->add_on_set_parameters_callback(
     std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1));
+
+  stop_watch_ptr_ = std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
+  stop_watch_ptr_->tic("cyclic_time");
+  stop_watch_ptr_->tic("processing_time");
 }
 
 rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam(
@@ -893,7 +890,7 @@ void MapBasedPredictionNode::trafficSignalsCallback(const TrafficSignalArray::Co
 
 void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects)
 {
-  stop_watch_.tic();
+  stop_watch_ptr_->toc("processing_time", true);
   // Guard for map pointer and frame transformation
   if (!lanelet_map_ptr_) {
     return;
@@ -1116,8 +1113,12 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
   // Publish Results
   pub_objects_->publish(output);
   pub_debug_markers_->publish(debug_markers);
-  const auto calculation_time_msg = createStringStamped(now(), stop_watch_.toc());
-  pub_calculation_time_->publish(calculation_time_msg);
+  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<tier4_debug_msgs::msg::Float64Stamped>(
+    "debug/cyclic_time_ms", cyclic_time_ms);
+  processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+    "debug/processing_time_ms", processing_time_ms);
 }
 
 bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path)

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -148,7 +148,7 @@ void TrackerDebugger::publishProcessingTime()
     processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
       "debug/processing_time_ms", processing_time_ms);
     processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
-      "debug/elapsed_time_from_sensor_input_ms", elapsed_time_from_sensor_input_ * 1e3);
+      "debug/pipeline_latency_ms", elapsed_time_from_sensor_input_ * 1e3);
   }
 }
 

perception/object_merger/include/object_merger/node.hpp

@@ -18,6 +18,8 @@
 #include "object_merger/data_association/data_association.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 
@@ -81,6 +83,10 @@ class ObjectAssociationMergerNode : public rclcpp::Node
     std::map<int, double> generalized_iou_threshold;
     std::map<int /*class label*/, double /*distance_threshold*/> distance_threshold_map;
   } overlapped_judge_param_;
+
+  // debug publisher
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
 };
 }  // namespace object_association
 

perception/object_merger/src/object_association_merger/node.cpp

@@ -118,6 +118,13 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio
 
   merged_object_pub_ = create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
     "output/object", rclcpp::QoS{1});
+
+  // Debug publisher
+  processing_time_publisher_ =
+    std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "object_association_merger");
+  stop_watch_ptr_ = std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
+  stop_watch_ptr_->tic("cyclic_time");
+  stop_watch_ptr_->tic("processing_time");
 }
 
 void ObjectAssociationMergerNode::objectsCallback(
@@ -128,6 +135,7 @@ void ObjectAssociationMergerNode::objectsCallback(
   if (merged_object_pub_->get_subscription_count() < 1) {
     return;
   }
+  stop_watch_ptr_->toc("processing_time", true);
 
   /* transform to base_link coordinate */
   autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects0, transformed_objects1;
@@ -215,6 +223,11 @@ void ObjectAssociationMergerNode::objectsCallback(
 
   // publish output msg
   merged_object_pub_->publish(output_msg);
+  // publish processing time
+  processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+    "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true));
+  processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+    "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true));
 }
 }  // namespace object_association
 

perception/shape_estimation/src/node.cpp

@@ -50,10 +50,17 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option
   RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer);
   estimator_ =
     std::make_unique<ShapeEstimator>(use_corrector, use_filter, use_boost_bbox_optimizer);
+
+  processing_time_publisher_ =
+    std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "shape_estimation");
+  stop_watch_ptr_ = std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
+  stop_watch_ptr_->tic("cyclic_time");
+  stop_watch_ptr_->tic("processing_time");
 }
 
 void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg)
 {
+  stop_watch_ptr_->toc("processing_time", true);
   // Guard
   if (pub_->get_subscription_count() < 1) {
     return;
@@ -108,6 +115,11 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared
 
   // Publish
   pub_->publish(output_msg);
+
+  processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+    "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true));
+  processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+    "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true));
 }
 
 #include <rclcpp_components/register_node_macro.hpp>

perception/shape_estimation/src/node.hpp

@@ -18,6 +18,8 @@
 #include "shape_estimation/shape_estimator.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
@@ -33,6 +35,10 @@ class ShapeEstimationNode : public rclcpp::Node
   rclcpp::Publisher<DetectedObjectsWithFeature>::SharedPtr pub_;
   rclcpp::Subscription<DetectedObjectsWithFeature>::SharedPtr sub_;
 
+  // debug publisher
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
+
   void callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg);
 
   std::unique_ptr<ShapeEstimator> estimator_;

perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp

@@ -20,6 +20,8 @@
 #include "tracking_object_merger/utils/utils.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
 #include <std_msgs/msg/header.hpp>
@@ -87,6 +89,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
     debug_object_pub_;
   bool publish_interpolated_sub_objects_;
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
 
   /* handle objects */
   std::unordered_map<MEASUREMENT_STATE, std::function<void(TrackedObject &, const TrackedObject &)>>