Skip to content

Commit 4b8cf17

Browse files
swiftfilepre-commit-ci[bot]
swiftfile
and
pre-commit-ci[bot]
authored
feat(blockage_diag): time series blockage diag (autowarefoundation#1004)
* Add basic pixel value sum function Signed-off-by: swift_file <sky.y.m.318@gmail.com> * update change sum topic(depth_map->mask_image) Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * update to use circular buffer Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * refactor : rename topic_name and publisher_name Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * fix : properly colorized Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * fix normalize process Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * fix bug Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * fix typo Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * update set initial parameter Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * update: change the number of frames to variable Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * feat :Periodically add frames to the buffer ,style Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * chore rename variable Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * add Diagnostics item Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * add debug statement of rosparam Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * update : time series processing integrated to blockage diag Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * chore Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * refactor: change variable name Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * delete non used publisher Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * docs: add description about new parameter Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * update flowchart Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Signed-off-by: swift_file <sky.y.m.318@gmail.com> * fix: delete unnecessary item of Diagnotics Signed-off-by: swift_file <sky.y.m.318@gmail.com> * update:calc blockage_ratio by multi frame blockage mask Signed-off-by: swift_file <sky.y.m.318@gmail.com> * ci(pre-commit): autofix Signed-off-by: swift_file <sky.y.m.318@gmail.com> Signed-off-by: YusukeMizoguchi <sky.y.m.318@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent eae1714 commit 4b8cf17

File tree

4 files changed

+45
-6
lines changed

4 files changed

+45
-6
lines changed

‎sensing/pointcloud_preprocessor/docs/blockage_diag.md

+2
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,8 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
4545
| `angle_range` | vector | The effective range of LiDAR |
4646
| `vertical_bins` | int | The LiDAR channel number |
4747
| `model` | string | The LiDAR model |
48+
| `buffering_frames` | uint | The number of buffering [range:1-200] |
49+
| `buffering_interval` | uint | The interval of buffering |
4850

4951
## Assumptions / Known limits
5052

‎sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg

+1-1
Loading

‎sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -69,6 +69,8 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter
6969
uint sky_blockage_count_ = 0;
7070
uint blockage_count_threshold_;
7171
std::string lidar_model_;
72+
uint buffering_frames_ = 100;
73+
uint buffering_interval_ = 5;
7274

7375
public:
7476
PCL_MAKE_ALIGNED_OPERATOR_NEW

‎sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp

+40-5
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616

1717
#include "autoware_point_types/types.hpp"
1818

19-
#include <boost/thread/detail/platform_time.hpp>
19+
#include <boost/circular_buffer.hpp>
2020

2121
#include <algorithm>
2222

@@ -38,6 +38,8 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options
3838
lidar_model_ = static_cast<std::string>(declare_parameter("model", "Pandar40P"));
3939
blockage_count_threshold_ =
4040
static_cast<uint>(declare_parameter("blockage_count_threshold", 50));
41+
buffering_frames_ = static_cast<uint>(declare_parameter("buffering_frames", 100));
42+
buffering_interval_ = static_cast<uint>(declare_parameter("buffering_interval", 5));
4143
}
4244

4345
updater_.setHardwareID("blockage_diag");
@@ -73,7 +75,6 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat)
7375
stat.add(
7476
"sky_blockage_range_deg", "[" + std::to_string(sky_blockage_range_deg_[0]) + "," +
7577
std::to_string(sky_blockage_range_deg_[1]) + "]");
76-
7778
// TODO(badai-nguyen): consider sky_blockage_ratio_ for DiagnosticsStatus." [todo]
7879

7980
auto level = DiagnosticStatus::OK;
@@ -151,16 +152,44 @@ void BlockageDiagComponent::filter(
151152
lidar_depth_map.convertTo(lidar_depth_map, CV_8UC1, 1.0 / 100.0);
152153
cv::Mat no_return_mask;
153154
cv::inRange(lidar_depth_map_8u, 0, 1, no_return_mask);
155+
154156
cv::Mat erosion_dst;
155157
cv::Mat element = cv::getStructuringElement(
156158
cv::MORPH_RECT, cv::Size(2 * erode_kernel_ + 1, 2 * erode_kernel_ + 1),
157159
cv::Point(erode_kernel_, erode_kernel_));
158160
cv::erode(no_return_mask, erosion_dst, element);
159161
cv::dilate(erosion_dst, no_return_mask, element);
162+
163+
static boost::circular_buffer<cv::Mat> no_return_mask_buffer(buffering_frames_);
164+
165+
cv::Mat no_return_mask_result(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
166+
cv::Mat time_series_blockage_mask(
167+
cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
168+
cv::Mat no_return_mask_binarized(
169+
cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
170+
171+
static uint frame_count;
172+
frame_count++;
173+
if (buffering_interval_ != 0) {
174+
no_return_mask_binarized = no_return_mask / 255;
175+
if (frame_count == buffering_interval_) {
176+
no_return_mask_buffer.push_back(no_return_mask_binarized);
177+
frame_count = 0;
178+
}
179+
for (const auto & binary_mask : no_return_mask_buffer) {
180+
time_series_blockage_mask += binary_mask;
181+
}
182+
cv::inRange(
183+
time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(),
184+
no_return_mask_result);
185+
} else {
186+
no_return_mask.copyTo(no_return_mask_result);
187+
}
160188
cv::Mat ground_no_return_mask;
161189
cv::Mat sky_no_return_mask;
162-
no_return_mask(cv::Rect(0, 0, horizontal_bins, horizontal_ring_id_)).copyTo(sky_no_return_mask);
163-
no_return_mask(
190+
no_return_mask_result(cv::Rect(0, 0, horizontal_bins, horizontal_ring_id_))
191+
.copyTo(sky_no_return_mask);
192+
no_return_mask_result(
164193
cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_))
165194
.copyTo(ground_no_return_mask);
166195
ground_blockage_ratio_ =
@@ -203,7 +232,7 @@ void BlockageDiagComponent::filter(
203232
lidar_depth_map_pub_.publish(lidar_depth_msg);
204233

205234
cv::Mat blockage_mask_colorized;
206-
cv::applyColorMap(no_return_mask, blockage_mask_colorized, cv::COLORMAP_JET);
235+
cv::applyColorMap(no_return_mask_result, blockage_mask_colorized, cv::COLORMAP_JET);
207236
sensor_msgs::msg::Image::SharedPtr blockage_mask_msg =
208237
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg();
209238
blockage_mask_msg->header = input->header;
@@ -249,6 +278,12 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback(
249278
get_logger(), " Setting new angle_range to: [%f , %f].", angle_range_deg_[0],
250279
angle_range_deg_[1]);
251280
}
281+
if (get_param(p, "buffering_frames", buffering_frames_)) {
282+
RCLCPP_DEBUG(get_logger(), "Setting new buffering_frames to: %d.", buffering_frames_);
283+
}
284+
if (get_param(p, "buffering_interval", buffering_interval_)) {
285+
RCLCPP_DEBUG(get_logger(), "Setting new buffering_interval to: %d.", buffering_interval_);
286+
}
252287
rcl_interfaces::msg::SetParametersResult result;
253288
result.successful = true;
254289
result.reason = "success";

0 commit comments

Comments
 (0)