diff --git a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp index fb7642356e8d9..0acba3d2dd58d 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include @@ -61,6 +63,11 @@ class Debugger divided_objects_pub_ = node->create_publisher( "debug/divided_objects", 1); + processing_time_publisher_ = + std::make_unique(node, "detection_by_tracker"); + stop_watch_ptr_ = + std::make_unique>(); + 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( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); + } private: rclcpp::Publisher::SharedPtr @@ -90,6 +110,9 @@ class Debugger merged_objects_pub_; rclcpp::Publisher::SharedPtr divided_objects_pub_; + // debug publisher + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; autoware_auto_perception_msgs::msg::DetectedObjects removeFeature( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input) diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 2595315e1f9b2..7afc3e41e4683 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/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( diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index a58e19db70304..7989a5fbf6af9 100644 --- a/perception/map_based_prediction/README.md +++ b/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 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 321342c3d8367..26dee3eacb08d 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 @@ -21,6 +21,7 @@ #include "tier4_autoware_utils/ros/update_param.hpp" #include +#include #include #include #include @@ -33,7 +34,6 @@ #include #include #include -#include #include #include @@ -125,11 +125,14 @@ class MapBasedPredictionNode : public rclcpp::Node // ROS Publisher and Subscriber rclcpp::Publisher::SharedPtr pub_objects_; rclcpp::Publisher::SharedPtr pub_debug_markers_; - rclcpp::Publisher::SharedPtr pub_calculation_time_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_traffic_signals_; + // debug publisher + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + // Object History std::unordered_map> objects_history_; std::map, rclcpp::Time> stopped_times_against_green_; @@ -196,9 +199,6 @@ class MapBasedPredictionNode : public rclcpp::Node std::vector distance_set_for_no_intention_to_walk_; std::vector timeout_set_for_no_intention_to_walk_; - // Stop watch - StopWatch stop_watch_; - // Member Functions void mapCallback(const HADMapBin::ConstSharedPtr msg); void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg); 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 21697bb1b5f1a..549619950cf82 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/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_map_ptr, @@ -816,10 +808,15 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ pub_objects_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); - pub_calculation_time_ = create_publisher("~/debug/calculation_time", 1); + processing_time_publisher_ = + std::make_unique(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>(); + 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( + "debug/cyclic_time_ms", cyclic_time_ms); + processing_time_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); } bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) 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 1d4d50c7eab4c..d955745bb8c47 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -148,7 +148,7 @@ void TrackerDebugger::publishProcessingTime() processing_time_publisher_->publish( "debug/processing_time_ms", processing_time_ms); processing_time_publisher_->publish( - "debug/elapsed_time_from_sensor_input_ms", elapsed_time_from_sensor_input_ * 1e3); + "debug/pipeline_latency_ms", elapsed_time_from_sensor_input_ * 1e3); } } diff --git a/perception/object_merger/include/object_merger/node.hpp b/perception/object_merger/include/object_merger/node.hpp index 8341ce490ca72..ba89a04553476 100644 --- a/perception/object_merger/include/object_merger/node.hpp +++ b/perception/object_merger/include/object_merger/node.hpp @@ -18,6 +18,8 @@ #include "object_merger/data_association/data_association.hpp" #include +#include +#include #include @@ -81,6 +83,10 @@ class ObjectAssociationMergerNode : public rclcpp::Node std::map generalized_iou_threshold; std::map distance_threshold_map; } overlapped_judge_param_; + + // debug publisher + std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; }; } // namespace object_association diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 1ffc2791e8f4e..15b1939911394 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -118,6 +118,13 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio merged_object_pub_ = create_publisher( "output/object", rclcpp::QoS{1}); + + // Debug publisher + processing_time_publisher_ = + std::make_unique(this, "object_association_merger"); + stop_watch_ptr_ = std::make_unique>(); + 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( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } } // namespace object_association diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 624ca604d8fbf..22f31fffec21a 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/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(use_corrector, use_filter, use_boost_bbox_optimizer); + + processing_time_publisher_ = + std::make_unique(this, "shape_estimation"); + stop_watch_ptr_ = std::make_unique>(); + 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( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } #include diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/node.hpp index 95ee4afe8d9e8..87df44f08f836 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/node.hpp @@ -18,6 +18,8 @@ #include "shape_estimation/shape_estimator.hpp" #include +#include +#include #include #include @@ -33,6 +35,10 @@ class ShapeEstimationNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_; rclcpp::Subscription::SharedPtr sub_; + // debug publisher + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + void callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg); std::unique_ptr estimator_; diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp index 0509fb2a07dc5..56843f4c5e1e0 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp @@ -20,6 +20,8 @@ #include "tracking_object_merger/utils/utils.hpp" #include +#include +#include #include #include @@ -87,6 +89,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_object_pub_; bool publish_interpolated_sub_objects_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; /* handle objects */ std::unordered_map> diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index b22b8d5fa2948..af0de2a02bbe7 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -151,6 +151,13 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio set3dDataAssociation("lidar-radar", data_association_map_); // radar-radar association matrix set3dDataAssociation("radar-radar", data_association_map_); + + // debug publisher + processing_time_publisher_ = + std::make_unique(this, "decorative_object_merger_node"); + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } void DecorativeTrackerMergerNode::set3dDataAssociation( @@ -182,6 +189,7 @@ void DecorativeTrackerMergerNode::set3dDataAssociation( void DecorativeTrackerMergerNode::mainObjectsCallback( const TrackedObjects::ConstSharedPtr & main_objects) { + stop_watch_ptr_->toc("processing_time", true); // try to merge sub object if (!sub_objects_buffer_.empty()) { // get interpolated sub objects @@ -214,6 +222,10 @@ void DecorativeTrackerMergerNode::mainObjectsCallback( this->decorativeMerger(main_sensor_type_, main_objects); merged_object_pub_->publish(getTrackedObjects(main_objects->header)); + processing_time_publisher_->publish( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } /**