|
| 1 | +// Copyright 2022 TIER IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#ifndef POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ |
| 16 | +#define POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ |
| 17 | + |
| 18 | +#include "pointcloud_preprocessor/filter.hpp" |
| 19 | + |
| 20 | +#include <diagnostic_updater/diagnostic_updater.hpp> |
| 21 | +#include <image_transport/image_transport.hpp> |
| 22 | +#include <opencv2/highgui/highgui.hpp> |
| 23 | +#include <rclcpp/rclcpp.hpp> |
| 24 | + |
| 25 | +#include <diagnostic_msgs/msg/diagnostic_array.hpp> |
| 26 | +#include <sensor_msgs/msg/point_cloud2.hpp> |
| 27 | +#include <std_msgs/msg/header.hpp> |
| 28 | +#include <tier4_debug_msgs/msg/float32_stamped.hpp> |
| 29 | + |
| 30 | +#include <cv_bridge/cv_bridge.h> |
| 31 | + |
| 32 | +#include <string> |
| 33 | +#include <vector> |
| 34 | + |
| 35 | +namespace pointcloud_preprocessor |
| 36 | +{ |
| 37 | +using diagnostic_updater::DiagnosticStatusWrapper; |
| 38 | +using diagnostic_updater::Updater; |
| 39 | + |
| 40 | +class BlockageDiagComponent : public pointcloud_preprocessor::Filter |
| 41 | +{ |
| 42 | +protected: |
| 43 | + virtual void filter( |
| 44 | + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, |
| 45 | + PointCloud2 & output); |
| 46 | + /** \brief Parameter service callback result : needed to be hold */ |
| 47 | + OnSetParametersCallbackHandle::SharedPtr set_param_res_; |
| 48 | + |
| 49 | + /** \brief Parameter service callback */ |
| 50 | + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p); |
| 51 | + image_transport::Publisher lidar_depth_map_pub_; |
| 52 | + image_transport::Publisher blockage_mask_pub_; |
| 53 | + rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr ground_blockage_ratio_pub_; |
| 54 | + rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr sky_blockage_ratio_pub_; |
| 55 | + |
| 56 | +private: |
| 57 | + void onBlockageChecker(DiagnosticStatusWrapper & stat); |
| 58 | + Updater updater_{this}; |
| 59 | + uint vertical_bins_; |
| 60 | + std::vector<double> angle_range_deg_; |
| 61 | + uint horizontal_ring_id_ = 12; |
| 62 | + float blockage_ratio_threshold_; |
| 63 | + float ground_blockage_ratio_ = -1.0f; |
| 64 | + float sky_blockage_ratio_ = -1.0f; |
| 65 | + std::vector<float> ground_blockage_range_deg_ = {0.0f, 0.0f}; |
| 66 | + std::vector<float> sky_blockage_range_deg_ = {0.0f, 0.0f}; |
| 67 | + uint erode_kernel_ = 10; |
| 68 | + uint ground_blockage_count_ = 0; |
| 69 | + uint sky_blockage_count_ = 0; |
| 70 | + uint blockage_count_threshold_; |
| 71 | + std::string lidar_model_; |
| 72 | + |
| 73 | +public: |
| 74 | + EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
| 75 | + explicit BlockageDiagComponent(const rclcpp::NodeOptions & options); |
| 76 | +}; |
| 77 | + |
| 78 | +} // namespace pointcloud_preprocessor |
| 79 | + |
| 80 | +#endif // POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ |
![pre-commit-ci[bot]](https://avatars.githubusercontent.com/in/68672?v=4&size=40){kind=avatar}
0 commit comments