diff --git a/perception/compare_map_segmentation/package.xml b/perception/compare_map_segmentation/package.xml index 2943180c50d96..93b7a5ced8c66 100644 --- a/perception/compare_map_segmentation/package.xml +++ b/perception/compare_map_segmentation/package.xml @@ -24,6 +24,7 @@ rclcpp rclcpp_components sensor_msgs + tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp index 78bdf191f3d8a..270ff891d4ecb 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp @@ -28,6 +28,16 @@ VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapF const rclcpp::NodeOptions & options) : Filter("VoxelBasedApproximateCompareMapFilter", options) { + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = + std::make_unique(this, "voxel_based_approximate_compare_map_filter"); + stop_watch_ptr_->tic(); + } + distance_threshold_ = static_cast(declare_parameter("distance_threshold", 0.3)); set_map_in_voxel_grid_ = false; @@ -50,6 +60,12 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter( output = *input; return; } + // add processing time for debug + if (stop_watch_ptr_) { + const double processing_time_ms = stop_watch_ptr_->toc(true); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp index 7afce3d35bc69..c75b2c84eac36 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp @@ -28,6 +28,15 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent( const rclcpp::NodeOptions & options) : Filter("VoxelBasedCompareMapFilter", options) { + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "voxel_based_compare_map_filter"); + stop_watch_ptr_->tic(); + } + distance_threshold_ = static_cast(declare_parameter("distance_threshold", 0.3)); set_map_in_voxel_grid_ = false; @@ -50,6 +59,12 @@ void VoxelBasedCompareMapFilterComponent::filter( output = *input; return; } + // add processing time for debug + if (stop_watch_ptr_) { + const double processing_time_ms = stop_watch_ptr_->toc(true); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp index 0c1cc660b61ac..bfea09983cbc3 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp @@ -15,6 +15,8 @@ #include "lidar_apollo_instance_segmentation/debugger.hpp" #include +#include +#include #include #include @@ -40,6 +42,8 @@ class LidarInstanceSegmentationNode : public rclcpp::Node std::shared_ptr detector_ptr_; std::shared_ptr debugger_ptr_; void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; public: explicit LidarInstanceSegmentationNode(const rclcpp::NodeOptions & node_options); diff --git a/perception/lidar_apollo_instance_segmentation/package.xml b/perception/lidar_apollo_instance_segmentation/package.xml index 4fe135fcded3f..f5743d5688147 100755 --- a/perception/lidar_apollo_instance_segmentation/package.xml +++ b/perception/lidar_apollo_instance_segmentation/package.xml @@ -20,6 +20,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_perception_msgs ament_cmake_cppcheck diff --git a/perception/lidar_apollo_instance_segmentation/src/node.cpp b/perception/lidar_apollo_instance_segmentation/src/node.cpp index 653995efa4809..d36131cb13fe6 100644 --- a/perception/lidar_apollo_instance_segmentation/src/node.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/node.cpp @@ -16,11 +16,22 @@ #include "lidar_apollo_instance_segmentation/detector.hpp" +#include +#include + LidarInstanceSegmentationNode::LidarInstanceSegmentationNode( const rclcpp::NodeOptions & node_options) : Node("lidar_apollo_instance_segmentation_node", node_options) { using std::placeholders::_1; + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "lidar_apollo_instance_segmentation"); + stop_watch_ptr_->tic(); + } detector_ptr_ = std::make_shared(this); debugger_ptr_ = std::make_shared(this); pointcloud_sub_ = this->create_subscription( @@ -34,6 +45,12 @@ LidarInstanceSegmentationNode::LidarInstanceSegmentationNode( void LidarInstanceSegmentationNode::pointCloudCallback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { + // add processing time for debug + if (stop_watch_ptr_) { + const double processing_time_ms = stop_watch_ptr_->toc(true); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } tier4_perception_msgs::msg::DetectedObjectsWithFeature output_msg; detector_ptr_->detectDynamicObjects(*msg, output_msg); dynamic_objects_pub_->publish(output_msg); diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index 4ae0fde705e18..c471f0ab71510 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -114,6 +114,8 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node // Debugger std::shared_ptr debugger_ptr_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; // ROS Parameters std::string map_frame_; diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 8b47cee943138..f1661743ed833 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -176,6 +176,15 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( const rclcpp::NodeOptions & options) : Node("OccupancyGridMapOutlierFilter", options) { + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "occupancy_grid_map_outlier_filter"); + stop_watch_ptr_->tic(); + } + /* params */ map_frame_ = declare_parameter("map_frame", "map"); base_link_frame_ = declare_parameter("base_link_frame", "base_link"); @@ -215,6 +224,12 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( if (!transformPointcloud(*input_pc, *tf2_, input_ogm->header.frame_id, ogm_frame_pc)) { return; } + // add processing time for debug + if (stop_watch_ptr_) { + const double processing_time_ms = stop_watch_ptr_->toc(true); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } // Occupancy grid map based filter PclPointCloud high_confidence_pc{}; PclPointCloud low_confidence_pc{}; diff --git a/sensing/pointcloud_preprocessor/config/processing_time_ms.xml b/sensing/pointcloud_preprocessor/config/processing_time_ms.xml new file mode 100644 index 0000000000000..6d24eab30a0a1 --- /dev/null +++ b/sensing/pointcloud_preprocessor/config/processing_time_ms.xml @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index d121fdbf269f0..865c056e496fe 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -27,6 +27,10 @@ #include #include +// Include tier4 autoware utils +#include +#include + #include #include @@ -56,6 +60,9 @@ class DistortionCorrectorComponent : public rclcpp::Node rclcpp::Subscription::SharedPtr velocity_report_sub_; rclcpp::Publisher::SharedPtr undistorted_points_pub_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + tf2_ros::Buffer tf2_buffer_{get_clock()}; tf2_ros::TransformListener tf2_listener_{tf2_buffer_}; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp index 194f58a6fe7ea..d0d79907eb1dd 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp @@ -77,6 +77,10 @@ #include #include +// Include tier4 autoware utils +#include +#include + namespace pointcloud_preprocessor { namespace sync_policies = message_filters::sync_policies; @@ -168,6 +172,10 @@ class Filter : public rclcpp::Node /** \brief Internal mutex. */ boost::mutex mutex_; + /** \brief processing time publisher. **/ + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + /** \brief Virtual abstract filter method. To be implemented by every child. * \param input the input point cloud dataset. * \param indices a pointer to the vector of point indices to use. diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index 52f092b6bc89a..24b0c0e6a8490 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -60,6 +60,15 @@ namespace pointcloud_preprocessor CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & options) : Filter("CropBoxFilter", options) { + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "crop_box_filter"); + stop_watch_ptr_->tic(); + } + // set initial parameters { auto & p = param_; @@ -91,6 +100,12 @@ void CropBoxFilterComponent::filter( PointCloud2 & output) { boost::mutex::scoped_lock lock(mutex_); + // add processing time for debug + if (stop_watch_ptr_) { + const double processing_time_ms = stop_watch_ptr_->toc(true); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } output.data.resize(input->data.size()); Eigen::Vector3f pt(Eigen::Vector3f::Zero()); diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 1a73d6c301b9b..9f7b04244ce13 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -24,6 +24,15 @@ namespace pointcloud_preprocessor DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOptions & options) : Node("distortion_corrector_node", options) { + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "distortion_corrector"); + stop_watch_ptr_->tic(); + } + // Parameter time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp"); @@ -69,6 +78,13 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms return; } + // add processing time for debug + if (stop_watch_ptr_) { + const double processing_time_ms = stop_watch_ptr_->toc(true); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } + tf2::Transform tf2_base_link_to_sensor{}; getTransform(points_msg->header.frame_id, base_link_frame_, &tf2_base_link_to_sensor); diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index 7a842665e5f14..708209b92997e 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -21,6 +21,15 @@ namespace pointcloud_preprocessor RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options) : Filter("RingOutlierFilter", options) { + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "ring_outlier_filter"); + stop_watch_ptr_->tic(); + } + // set initial parameters { distance_ratio_ = static_cast(declare_parameter("distance_ratio", 1.03)); @@ -39,6 +48,12 @@ void RingOutlierFilterComponent::filter( PointCloud2 & output) { boost::mutex::scoped_lock lock(mutex_); + // add processing time for debug + if (stop_watch_ptr_) { + const double processing_time_ms = stop_watch_ptr_->toc(true); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + } std::unordered_map> input_ring_map; input_ring_map.reserve(128); sensor_msgs::msg::PointCloud2::SharedPtr input_ptr =