From 7823ae4828cced1d828bc72aff8dedc2dfcd9a08 Mon Sep 17 00:00:00 2001 From: ktro2828 Date: Tue, 12 Sep 2023 22:15:32 +0900 Subject: [PATCH] refactor: update funsion node configuration Signed-off-by: ktro2828 --- .../config/roi_sync.param.yaml | 7 ++++ .../launch/roi_cluster_fusion.launch.xml | 15 -------- .../roi_detected_object_fusion.launch.xml | 2 -- .../schema/roi_sync.schema.json | 36 +++++++++++++++++++ .../src/fusion_node.cpp | 19 +++++----- 5 files changed, 53 insertions(+), 26 deletions(-) diff --git a/perception/image_projection_based_fusion/config/roi_sync.param.yaml b/perception/image_projection_based_fusion/config/roi_sync.param.yaml index 21ba13787f1c0..a4e96ff291a49 100644 --- a/perception/image_projection_based_fusion/config/roi_sync.param.yaml +++ b/perception/image_projection_based_fusion/config/roi_sync.param.yaml @@ -3,3 +3,10 @@ input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0] timeout_ms: 70.0 match_threshold_ms: 50.0 + image_buffer_size: 15 + filter_scope_minx: -100.0 + filter_scope_miny: -100.0 + filter_scope_minz: -100.0 + filter_scope_maxx: 100.0 + filter_scope_maxy: 100.0 + filter_scope_maxz: 100.0 diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 15ca2abd80e25..ca27e432d9049 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -1,5 +1,4 @@ - @@ -27,13 +26,6 @@ - - - - - - - @@ -78,13 +70,6 @@ - - - - - - - diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml index b6165fc7c87d2..5c77b3a1f64d0 100644 --- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -26,7 +26,6 @@ - @@ -72,7 +71,6 @@ - diff --git a/perception/image_projection_based_fusion/schema/roi_sync.schema.json b/perception/image_projection_based_fusion/schema/roi_sync.schema.json index b60717ba77d1f..4df0193c65ebf 100644 --- a/perception/image_projection_based_fusion/schema/roi_sync.schema.json +++ b/perception/image_projection_based_fusion/schema/roi_sync.schema.json @@ -24,6 +24,42 @@ "default": 50.0, "minimum": 0.0, "maximum": 100.0 + }, + "image_buffer_size": { + "type": "integer", + "description": "The number of image buffer size for debug.", + "default": 15, + "minimum": 1 + }, + "filter_scope_minx": { + "type": "number", + "description": "Minimum x position to be considered for debug [m].", + "default": -100.0 + }, + "filter_scope_miny": { + "type": "number", + "description": "Minimum y position to be considered for debug [m].", + "default": -100.0 + }, + "filter_scope_minz": { + "type": "number", + "description": "Minimum z position to be considered for debug [m].", + "default": -100.0 + }, + "filter_scope_maxx": { + "type": "number", + "description": "Maximum x position to be considered for debug [m].", + "default": 100.0 + }, + "filter_scope_maxy": { + "type": "number", + "description": "Maximum y position to be considered for debug [m].", + "default": 100.0 + }, + "filter_scope_maxz": { + "type": "number", + "description": "Maximum z position [m].", + "default": 100.0 } } } diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 1bc351b08c01b..209a096be8db7 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -46,7 +46,7 @@ FusionNode::FusionNode( : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // set rois_number - rois_number_ = static_cast(declare_parameter("rois_number", 1)); + rois_number_ = static_cast(declare_parameter("rois_number")); if (rois_number_ < 1) { RCLCPP_WARN( this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number_); @@ -80,7 +80,7 @@ FusionNode::FusionNode( "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); } - input_offset_ms_ = declare_parameter("input_offset_ms", std::vector{}); + input_offset_ms_ = declare_parameter>("input_offset_ms"); if (!input_offset_ms_.empty() && rois_number_ != input_offset_ms_.size()) { throw std::runtime_error("The number of offsets does not match the number of topics."); } @@ -122,7 +122,7 @@ FusionNode::FusionNode( // debugger if (declare_parameter("debug_mode", false)) { std::size_t image_buffer_size = - static_cast(declare_parameter("image_buffer_size", 15)); + static_cast(declare_parameter("image_buffer_size")); debugger_ = std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics_); } @@ -136,14 +136,15 @@ FusionNode::FusionNode( stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } + // cspell: ignore minx, maxx, miny, maxy, minz, maxz // FIXME: use min_x instead of minx - filter_scope_minx_ = declare_parameter("filter_scope_minx", -100); - filter_scope_maxx_ = declare_parameter("filter_scope_maxx", 100); - filter_scope_miny_ = declare_parameter("filter_scope_miny", -100); - filter_scope_maxy_ = declare_parameter("filter_scope_maxy", 100); - filter_scope_minz_ = declare_parameter("filter_scope_minz", -100); - filter_scope_maxz_ = declare_parameter("filter_scope_maxz", 100); + filter_scope_minx_ = declare_parameter("filter_scope_minx"); + filter_scope_maxx_ = declare_parameter("filter_scope_maxx"); + filter_scope_miny_ = declare_parameter("filter_scope_miny"); + filter_scope_maxy_ = declare_parameter("filter_scope_maxy"); + filter_scope_minz_ = declare_parameter("filter_scope_minz"); + filter_scope_maxz_ = declare_parameter("filter_scope_maxz"); } template