From c32e8d39124d477ec0d2d1cb661f731e8af74d8a Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 19 May 2023 13:03:49 +0900 Subject: [PATCH 1/2] feat(intersection): denoise occlusion by morphology open process (#3763) * apply morpholgoyEx to occlusion_mask_raw to remove noisy occlusion cell Signed-off-by: Mamoru Sobue * parameterize kernel size Signed-off-by: Mamoru Sobue * remove debug Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../src/manager.cpp | 1 + .../src/scene_intersection.cpp | 10 ++++++++-- .../src/scene_intersection.hpp | 1 + .../config/intersection.param.yaml | 1 + 4 files changed, 11 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 02293c5634342..6a1590b4197ce 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -99,6 +99,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".occlusion.min_vehicle_brake_for_rss"); ip.occlusion.max_vehicle_velocity_for_rss = node.declare_parameter(ns + ".occlusion.max_vehicle_velocity_for_rss"); + ip.occlusion.denoise_kernel = node.declare_parameter(ns + ".occlusion.denoise_kernel"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 05050b294c0d9..4c538b303e136 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1100,8 +1100,14 @@ bool IntersectionModule::isOcclusionCleared( } // (3) occlusion mask - cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); - cv::bitwise_and(detection_mask, unknown_mask, occlusion_mask); + cv::Mat occlusion_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); + cv::bitwise_and(detection_mask, unknown_mask, occlusion_mask_raw); + // (3.1) apply morphologyEx + cv::Mat occlusion_mask; + const int morph_size = std::ceil(planner_param_.occlusion.denoise_kernel / reso); + cv::morphologyEx( + occlusion_mask_raw, occlusion_mask, cv::MORPH_OPEN, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); // (4) create distance grid // value: 0 - 254: signed distance representing [distamce_min, distance_max] diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index c2ddec10ba165..7543c5ec13322 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -122,6 +122,7 @@ class IntersectionModule : public SceneModuleInterface double before_creep_stop_time; double min_vehicle_brake_for_rss; double max_vehicle_velocity_for_rss; + double denoise_kernel; } occlusion; }; diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 1781c3617726b..d10d1093fc789 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -41,6 +41,7 @@ before_creep_stop_time: 1.0 # [s] min_vehicle_brake_for_rss: -2.5 # [m/s^2] max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph + denoise_kernel: 1.0 # [m] merge_from_private: stop_duration_sec: 1.0 From 43c84cd25f86289287d3bb68abda0d0319793eb5 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 26 May 2023 21:33:54 +0900 Subject: [PATCH 2/2] fix(intersection): add the flag for intersection_occlusion grid publication (#3844) Signed-off-by: tomoya.kimura --- .../src/manager.cpp | 1 + .../src/scene_intersection.cpp | 43 ++++++++++--------- .../src/scene_intersection.hpp | 1 + .../config/intersection.param.yaml | 1 + 4 files changed, 26 insertions(+), 20 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 6a1590b4197ce..c476b6de12332 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -100,6 +100,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.occlusion.max_vehicle_velocity_for_rss = node.declare_parameter(ns + ".occlusion.max_vehicle_velocity_for_rss"); ip.occlusion.denoise_kernel = node.declare_parameter(ns + ".occlusion.denoise_kernel"); + ip.occlusion.pub_debug_grid = node.declare_parameter(ns + ".occlusion.pub_debug_grid"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 4c538b303e136..bc9538e70e4a5 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1242,26 +1242,29 @@ bool IntersectionModule::isOcclusionCleared( } debug_data_.nearest_occlusion_point = nearest_occlusion_point; - cv::Mat distance_grid_heatmap; - cv::applyColorMap(distance_grid, distance_grid_heatmap, cv::COLORMAP_JET); - /* - cv::namedWindow("distance_grid_viz" + std::to_string(lane_id_), cv::WINDOW_NORMAL); - cv::imshow("distance_grid_viz" + std::to_string(lane_id_), distance_grid_heatmap); - cv::waitKey(1); - */ - grid_map::GridMap occlusion_grid({"elevation"}); - occlusion_grid.setFrameId("map"); - occlusion_grid.setGeometry( - grid_map::Length(width * reso, height * reso), reso, - grid_map::Position(origin.x + width * reso / 2, origin.y + height * reso / 2)); - cv::rotate(distance_grid, distance_grid, cv::ROTATE_90_COUNTERCLOCKWISE); - cv::rotate(distance_grid_heatmap, distance_grid_heatmap, cv::ROTATE_90_COUNTERCLOCKWISE); - grid_map::GridMapCvConverter::addLayerFromImage( - distance_grid, "elevation", occlusion_grid, origin.z /* elevation for 0 */, - origin.z + distance_max /* elevation for 255 */); - grid_map::GridMapCvConverter::addColorLayerFromImage( - distance_grid_heatmap, "color", occlusion_grid); - occlusion_grid_pub_->publish(grid_map::GridMapRosConverter::toMessage(occlusion_grid)); + if (planner_param_.occlusion.pub_debug_grid) { + cv::Mat distance_grid_heatmap; + cv::applyColorMap(distance_grid, distance_grid_heatmap, cv::COLORMAP_JET); + /* + cv::namedWindow("distance_grid_viz" + std::to_string(lane_id_), cv::WINDOW_NORMAL); + cv::imshow("distance_grid_viz" + std::to_string(lane_id_), distance_grid_heatmap); + cv::waitKey(1); + */ + grid_map::GridMap occlusion_grid({"elevation"}); + occlusion_grid.setFrameId("map"); + occlusion_grid.setGeometry( + grid_map::Length(width * reso, height * reso), reso, + grid_map::Position(origin.x + width * reso / 2, origin.y + height * reso / 2)); + cv::rotate(distance_grid, distance_grid, cv::ROTATE_90_COUNTERCLOCKWISE); + cv::rotate(distance_grid_heatmap, distance_grid_heatmap, cv::ROTATE_90_COUNTERCLOCKWISE); + grid_map::GridMapCvConverter::addLayerFromImage( + distance_grid, "elevation", occlusion_grid, origin.z /* elevation for 0 */, + origin.z + distance_max /* elevation for 255 */); + grid_map::GridMapCvConverter::addColorLayerFromImage( + distance_grid_heatmap, "color", occlusion_grid); + occlusion_grid_pub_->publish(grid_map::GridMapRosConverter::toMessage(occlusion_grid)); + } + if (min_cost > min_cost_thr || !min_cost_projection_ind.has_value()) { return true; } else { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 7543c5ec13322..b89e8e3997fd7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -123,6 +123,7 @@ class IntersectionModule : public SceneModuleInterface double min_vehicle_brake_for_rss; double max_vehicle_velocity_for_rss; double denoise_kernel; + bool pub_debug_grid; } occlusion; }; diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index d10d1093fc789..c86e95ae7e168 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -42,6 +42,7 @@ min_vehicle_brake_for_rss: -2.5 # [m/s^2] max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph denoise_kernel: 1.0 # [m] + pub_debug_grid: false merge_from_private: stop_duration_sec: 1.0