Skip to content

Commit

Permalink
feat(blockage_diag): time series blockage diag (tier4#1004)
Browse files Browse the repository at this point in the history 
* 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>
  • Loading branch information
@swiftfile @pre-commit-ci @boyali
2 people authored and boyali committed Oct 3, 2022
1 parent 06105fc commit 18a450a
Show file tree
Hide file tree
Showing 4 changed files with 45 additions and 6 deletions.
2 changes: 2 additions & 0 deletions sensing/pointcloud_preprocessor/docs/blockage_diag.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,8 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
| `angle_range` | vector | The effective range of LiDAR |
| `vertical_bins` | int | The LiDAR channel number |
| `model` | string | The LiDAR model |
| `buffering_frames` | uint | The number of buffering [range:1-200] |
| `buffering_interval` | uint | The interval of buffering |

## Assumptions / Known limits

Expand Down
2 changes: 1 addition & 1 deletion sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
2 changes: 2 additions & 0 deletions ...loud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,8 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter
uint sky_blockage_count_ = 0;
uint blockage_count_threshold_;
std::string lidar_model_;
uint buffering_frames_ = 100;
uint buffering_interval_ = 5;

public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
Expand Down
45 changes: 40 additions & 5 deletions sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@

#include "autoware_point_types/types.hpp"

#include <boost/thread/detail/platform_time.hpp>
#include <boost/circular_buffer.hpp>

#include <algorithm>

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

updater_.setHardwareID("blockage_diag");
Expand Down Expand Up @@ -73,7 +75,6 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat)
stat.add(
"sky_blockage_range_deg", "[" + std::to_string(sky_blockage_range_deg_[0]) + "," +
std::to_string(sky_blockage_range_deg_[1]) + "]");

// TODO(badai-nguyen): consider sky_blockage_ratio_ for DiagnosticsStatus." [todo]

auto level = DiagnosticStatus::OK;
Expand Down Expand Up @@ -151,16 +152,44 @@ void BlockageDiagComponent::filter(
lidar_depth_map.convertTo(lidar_depth_map, CV_8UC1, 1.0 / 100.0);
cv::Mat no_return_mask;
cv::inRange(lidar_depth_map_8u, 0, 1, no_return_mask);

cv::Mat erosion_dst;
cv::Mat element = cv::getStructuringElement(
cv::MORPH_RECT, cv::Size(2 * erode_kernel_ + 1, 2 * erode_kernel_ + 1),
cv::Point(erode_kernel_, erode_kernel_));
cv::erode(no_return_mask, erosion_dst, element);
cv::dilate(erosion_dst, no_return_mask, element);

static boost::circular_buffer<cv::Mat> no_return_mask_buffer(buffering_frames_);

cv::Mat no_return_mask_result(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
cv::Mat time_series_blockage_mask(
cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
cv::Mat no_return_mask_binarized(
cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));

static uint frame_count;
frame_count++;
if (buffering_interval_ != 0) {
no_return_mask_binarized = no_return_mask / 255;
if (frame_count == buffering_interval_) {
no_return_mask_buffer.push_back(no_return_mask_binarized);
frame_count = 0;
}
for (const auto & binary_mask : no_return_mask_buffer) {
time_series_blockage_mask += binary_mask;
}
cv::inRange(
time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(),
no_return_mask_result);
} else {
no_return_mask.copyTo(no_return_mask_result);
}
cv::Mat ground_no_return_mask;
cv::Mat sky_no_return_mask;
no_return_mask(cv::Rect(0, 0, horizontal_bins, horizontal_ring_id_)).copyTo(sky_no_return_mask);
no_return_mask(
no_return_mask_result(cv::Rect(0, 0, horizontal_bins, horizontal_ring_id_))
.copyTo(sky_no_return_mask);
no_return_mask_result(
cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_))
.copyTo(ground_no_return_mask);
ground_blockage_ratio_ =
Expand Down Expand Up @@ -203,7 +232,7 @@ void BlockageDiagComponent::filter(
lidar_depth_map_pub_.publish(lidar_depth_msg);

cv::Mat blockage_mask_colorized;
cv::applyColorMap(no_return_mask, blockage_mask_colorized, cv::COLORMAP_JET);
cv::applyColorMap(no_return_mask_result, blockage_mask_colorized, cv::COLORMAP_JET);
sensor_msgs::msg::Image::SharedPtr blockage_mask_msg =
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg();
blockage_mask_msg->header = input->header;
Expand Down Expand Up @@ -249,6 +278,12 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback(
get_logger(), " Setting new angle_range to: [%f , %f].", angle_range_deg_[0],
angle_range_deg_[1]);
}
if (get_param(p, "buffering_frames", buffering_frames_)) {
RCLCPP_DEBUG(get_logger(), "Setting new buffering_frames to: %d.", buffering_frames_);
}
if (get_param(p, "buffering_interval", buffering_interval_)) {
RCLCPP_DEBUG(get_logger(), "Setting new buffering_interval to: %d.", buffering_interval_);
}
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
result.reason = "success";
Expand Down

0 comments on commit 18a450a

Please sign in to comment.