diff --git a/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml b/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml index 662362c4fd7c5..347cb57b484e9 100644 --- a/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml +++ b/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml @@ -12,3 +12,6 @@ filter_scope_max_x: 100.0 filter_scope_max_y: 100.0 filter_scope_max_z: 100.0 + + # debug parameters + publish_processing_time_detail: false diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index c9b4f1f7b903f..04ecf89faa0d4 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -142,6 +143,11 @@ class FusionNode : public rclcpp::Node /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; + + // timekeeper + rclcpp::Publisher::SharedPtr + detailed_processing_time_publisher_; + std::shared_ptr time_keeper_; }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 2c0283a190023..be05c0a1c4bc6 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -53,6 +53,7 @@ class RoiClusterFusionNode double fusion_distance_; double trust_object_distance_; std::string non_trust_object_iou_mode_{"iou_x"}; + bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold); bool out_of_scope(const DetectedObjectWithFeature & obj) override; double cal_iou_by_mode( diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 4de49df61a071..9baf754e224a7 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -19,8 +19,10 @@ #include +#include #include #include + namespace autoware::image_projection_based_fusion { class RoiPointCloudFusionNode diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index b09e9521bdcdb..0bec3195bb402 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 6c575752b52e5..bd4d57e45c582 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -42,6 +42,7 @@ static double processing_time_ms = 0; namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; template FusionNode::FusionNode( @@ -130,6 +131,17 @@ FusionNode::FusionNode( debugger_ = std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics_); } + + // time keeper + bool use_time_keeper = declare_parameter("publish_processing_time_detail"); + 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); + } + point_project_to_unrectified_image_ = declare_parameter("point_project_to_unrectified_image"); @@ -170,6 +182,9 @@ template void FusionNode::subCallback( const typename TargetMsg3D::ConstSharedPtr input_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (cached_msg_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); timer_->cancel(); @@ -306,6 +321,9 @@ template void FusionNode::roiCallback( const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + stop_watch_ptr_->toc("processing_time", true); int64_t timestamp_nsec = (*input_roi_msg).header.stamp.sec * static_cast(1e9) + @@ -378,6 +396,9 @@ void FusionNode::postprocess(TargetMsg3D & output_msg template void FusionNode::timer_callback() { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + using std::chrono_literals::operator""ms; timer_->cancel(); if (mutex_cached_msgs_.try_lock()) { diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index d4601e26dda46..4d12de2685c95 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -35,6 +36,7 @@ namespace { +using autoware::universe_utils::ScopedTimeTrack; Eigen::Affine3f _transformToEigen(const geometry_msgs::msg::Transform & t) { @@ -202,6 +204,9 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); @@ -273,6 +278,9 @@ void PointPaintingFusionNode::fuseOnSingleImage( if (!checkCameraInfo(camera_info)) return; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::vector debug_image_rois; std::vector debug_image_points; @@ -372,6 +380,10 @@ dc | dc dc dc dc ||zc| } if (debugger_) { + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); + debugger_->image_rois_ = debug_image_rois; debugger_->obstacle_points_ = debug_image_points; debugger_->publishImage(image_id, input_roi_msg.header.stamp); @@ -380,6 +392,9 @@ dc | dc dc dc dc ||zc| void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + const auto objects_sub_count = obj_pub_ptr_->get_subscription_count() + obj_pub_ptr_->get_intra_process_subscription_count(); if (objects_sub_count < 1) { diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 9a94613a2b788..7520647544684 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -16,12 +16,14 @@ #include #include +#include #include #include #include #include +#include #include #include #include @@ -36,6 +38,7 @@ namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) : FusionNode( @@ -55,6 +58,9 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + // reset cluster semantic type if (!use_cluster_semantic_type_) { for (auto & feature_object : output_cluster_msg.feature_objects) { @@ -67,6 +73,9 @@ void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluste void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_cluster_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (!remove_unknown_) { return; } @@ -91,6 +100,9 @@ void RoiClusterFusionNode::fuseOnSingleImage( { if (!checkCameraInfo(camera_info)) return; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + image_geometry::PinholeCameraModel pinhole_camera_model; pinhole_camera_model.fromCameraInfo(camera_info); @@ -110,6 +122,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( } std::map m_cluster_roi; + for (std::size_t i = 0; i < input_cluster_msg.feature_objects.size(); ++i) { if (input_cluster_msg.feature_objects.at(i).feature.cluster.data.empty()) { continue; diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 3ef3af8168f53..035bc259ab73c 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -18,14 +18,17 @@ #include #include +#include #include #include +#include #include #include namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) : FusionNode( @@ -49,6 +52,9 @@ RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptio void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::vector passthrough_object_flags, fused_object_flags, ignored_object_flags; passthrough_object_flags.resize(output_msg.objects.size()); fused_object_flags.resize(output_msg.objects.size()); @@ -81,6 +87,9 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg __attribute__((unused))) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (!checkCameraInfo(camera_info)) return; Eigen::Affine3d object2camera_affine; @@ -117,6 +126,9 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( const Eigen::Affine3d & object2camera_affine, const image_geometry::PinholeCameraModel & pinhole_camera_model) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::map object_roi_map; int64_t timestamp_nsec = input_object_msg.header.stamp.sec * static_cast(1e9) + input_object_msg.header.stamp.nanosec; @@ -202,6 +214,9 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( const std::vector & image_rois, const std::map & object_roi_map) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + int64_t timestamp_nsec = input_object_msg.header.stamp.sec * static_cast(1e9) + input_object_msg.header.stamp.nanosec; if (fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { diff --git a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index f9eb4ef909282..206a5f77a0235 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -17,6 +17,9 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" +#include + +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -31,6 +34,8 @@ namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; + RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) : FusionNode( "roi_pointcloud_fusion", options) @@ -47,12 +52,18 @@ RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & opt void RoiPointCloudFusionNode::preprocess( __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + return; } void RoiPointCloudFusionNode::postprocess( __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + pub_objects_ptr_->get_intra_process_subscription_count(); if (objects_sub_count < 1) { @@ -81,6 +92,9 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( const sensor_msgs::msg::CameraInfo & camera_info, __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (input_pointcloud_msg.data.empty()) { return; } diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index e678a956bc64e..afeff213c0afe 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -17,6 +17,7 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" +#include #include #include @@ -32,6 +33,8 @@ namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; + SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) : FusionNode("segmentation_pointcloud_fusion", options) { @@ -49,11 +52,17 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + return; } void SegmentPointCloudFusionNode::postprocess(PointCloud2 & pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + auto original_cloud = std::make_shared(pointcloud_msg); int point_step = original_cloud->point_step; @@ -82,6 +91,9 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info, __attribute__((unused)) PointCloud2 & output_cloud) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (input_pointcloud_msg.data.empty()) { return; }