From 651dc5758baae3f57f963b4ca9ebda94ebd9b62d Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 14 Aug 2024 18:17:44 +0900 Subject: [PATCH 1/7] feat: rework filter parameters Signed-off-by: vividf --- .../README.md | 9 +-- .../config/filter.param.yaml | 8 +++ .../schema/filter.schema.json | 66 +++++++++++++++++++ .../src/filter.cpp | 12 ++-- 4 files changed, 81 insertions(+), 14 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/config/filter.param.yaml create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/filter.schema.json diff --git a/sensing/autoware_pointcloud_preprocessor/README.md b/sensing/autoware_pointcloud_preprocessor/README.md index 4424259a52daa..e58bb6ba1c63e 100644 --- a/sensing/autoware_pointcloud_preprocessor/README.md +++ b/sensing/autoware_pointcloud_preprocessor/README.md @@ -45,14 +45,7 @@ Detail description of each filter's algorithm is in the following links. ### Node Parameters -| Name | Type | Default Value | Description | -| ------------------ | ------ | ------------- | ------------------------------------- | -| `input_frame` | string | " " | input frame id | -| `output_frame` | string | " " | output frame id | -| `max_queue_size` | int | 5 | max queue size of input/output topics | -| `use_indices` | bool | false | flag to use pointcloud indices | -| `latched_indices` | bool | false | flag to latch pointcloud indices | -| `approximate_sync` | bool | false | flag to use approximate sync option | +{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/filter.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/config/filter.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/filter.param.yaml new file mode 100644 index 0000000000000..abd396892239e --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/filter.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + input_frame: "" + output_frame: "" + max_queue_size: 5 + use_indices: false + latched_indices: false + approximate_sync: false diff --git a/sensing/autoware_pointcloud_preprocessor/schema/filter.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/filter.schema.json new file mode 100644 index 0000000000000..8e0a6605933b8 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/filter.schema.json @@ -0,0 +1,66 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameter for filter", + "type": "object", + "definitions": { + "filter": { + "type": "object", + "properties": { + "input_frame": { + "type": "string", + "description": "input frame id", + "default": "" + }, + "output_frame": { + "type": "string", + "description": "output frame id", + "default": "" + }, + "max_queue_size": { + "type": "integer", + "minimum": 0, + "description": "max queue size of input/output topics", + "default": 5 + }, + "use_indices": { + "type": "boolean", + "description": "flag to use pointcloud indices", + "default": false + }, + "latched_indices": { + "type": "boolean", + "description": "flag to latch pointcloud indices", + "default": false + }, + "approximate_sync": { + "type": "boolean", + "description": "flag to use approximate sync option", + "default": false + } + }, + "required": [ + "input_frame", + "output_frame", + "max_queue_size", + "use_indices", + "latched_indices", + "approximate_sync" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp index 854e93ed52ebc..d49a72b2b4938 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp @@ -70,14 +70,14 @@ autoware::pointcloud_preprocessor::Filter::Filter( { // Set parameters (moved from NodeletLazy onInit) { - tf_input_frame_ = static_cast(declare_parameter("input_frame", "")); - tf_output_frame_ = static_cast(declare_parameter("output_frame", "")); - max_queue_size_ = static_cast(declare_parameter("max_queue_size", 5)); + tf_input_frame_ = declare_parameter("input_frame"); + tf_output_frame_ = declare_parameter("output_frame"); + max_queue_size_ = declare_parameter("max_queue_size"); // ---[ Optional parameters - use_indices_ = static_cast(declare_parameter("use_indices", false)); - latched_indices_ = static_cast(declare_parameter("latched_indices", false)); - approximate_sync_ = static_cast(declare_parameter("approximate_sync", false)); + use_indices_ = declare_parameter("use_indices"); + latched_indices_ = declare_parameter("latched_indices"); + approximate_sync_ = declare_parameter("approximate_sync"); RCLCPP_DEBUG_STREAM( this->get_logger(), From 504ccd7e9ec24a9299dda78434a8f11bd7ac7935 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 14 Aug 2024 18:19:48 +0900 Subject: [PATCH 2/7] chore: fix explanation Signed-off-by: vividf --- .../autoware_pointcloud_preprocessor/schema/filter.schema.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/autoware_pointcloud_preprocessor/schema/filter.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/filter.schema.json index 8e0a6605933b8..50468edd5912c 100644 --- a/sensing/autoware_pointcloud_preprocessor/schema/filter.schema.json +++ b/sensing/autoware_pointcloud_preprocessor/schema/filter.schema.json @@ -1,6 +1,6 @@ { "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameter for filter", + "title": "Parameters for Filter Class", "type": "object", "definitions": { "filter": { From 8e44da15f915d4831224c51b0269ce9d6326d701 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 15 Aug 2024 00:49:12 +0900 Subject: [PATCH 3/7] chore: fix readme Signed-off-by: vividf --- sensing/autoware_pointcloud_preprocessor/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/autoware_pointcloud_preprocessor/README.md b/sensing/autoware_pointcloud_preprocessor/README.md index e58bb6ba1c63e..e63ee14a12853 100644 --- a/sensing/autoware_pointcloud_preprocessor/README.md +++ b/sensing/autoware_pointcloud_preprocessor/README.md @@ -45,7 +45,7 @@ Detail description of each filter's algorithm is in the following links. ### Node Parameters -{{ json_to_markdown("sensing/pointcloud_preprocessor/schema/filter.schema.json") }} +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/filter.schema.json") }} ## Assumptions / Known limits From 5f3b8a5453bf232406f560aadcf77d242e8dc13b Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 15 Aug 2024 00:58:04 +0900 Subject: [PATCH 4/7] chore: fix readme Signed-off-by: vividf --- sensing/autoware_pointcloud_preprocessor/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/autoware_pointcloud_preprocessor/README.md b/sensing/autoware_pointcloud_preprocessor/README.md index e63ee14a12853..c0cda373b75bb 100644 --- a/sensing/autoware_pointcloud_preprocessor/README.md +++ b/sensing/autoware_pointcloud_preprocessor/README.md @@ -45,7 +45,7 @@ Detail description of each filter's algorithm is in the following links. ### Node Parameters -{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/filter.schema.json") }} +{{ json_to_markdown("schema/filter.schema.json") }} ## Assumptions / Known limits From c338a59d9424b6c8413a3383204ba73d3826fe83 Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 20 Aug 2024 18:31:17 +0900 Subject: [PATCH 5/7] chore: add explicit cast Signed-off-by: vividf --- sensing/autoware_pointcloud_preprocessor/src/filter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp index d49a72b2b4938..7095fce82a38b 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp @@ -72,7 +72,7 @@ autoware::pointcloud_preprocessor::Filter::Filter( { tf_input_frame_ = declare_parameter("input_frame"); tf_output_frame_ = declare_parameter("output_frame"); - max_queue_size_ = declare_parameter("max_queue_size"); + max_queue_size_ = static_cast(declare_parameter("max_queue_size")); // ---[ Optional parameters use_indices_ = declare_parameter("use_indices"); From 9819fb6273683f2963e1b0cfd0070e851dfa22ac Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 20 Aug 2024 18:37:51 +0900 Subject: [PATCH 6/7] chore: remove init value Signed-off-by: vividf --- .../include/autoware/pointcloud_preprocessor/filter.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp index 789f377e7ec95..ead862d7c4a38 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp @@ -223,7 +223,7 @@ class Filter : public rclcpp::Node * ~indices topics must be synchronised in time, either exact or within a * specified jitter. See also @ref latched_indices_ and approximate_sync. **/ - bool use_indices_ = false; + bool use_indices_; /** \brief Set to true if the indices topic is latched. * * If use_indices_ is true, the ~input and ~indices topics generally must @@ -231,14 +231,14 @@ class Filter : public rclcpp::Node * value from ~indices can be used instead of requiring a synchronised * message. **/ - bool latched_indices_ = false; + bool latched_indices_; /** \brief The maximum queue size (default: 3). */ - size_t max_queue_size_ = 3; + size_t max_queue_size_; /** \brief True if we use an approximate time synchronizer * versus an exact one (false by default). */ - bool approximate_sync_ = false; + bool approximate_sync_; std::unique_ptr static_tf_buffer_{nullptr}; From 6504d8148607b9cb10dd495205aa3a96339ecae7 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 18 Sep 2024 08:12:40 +0000 Subject: [PATCH 7/7] style(pre-commit): autofix --- sensing/autoware_pointcloud_preprocessor/src/filter.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp index ad9b2a32c2687..8fe08b465c1ec 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp @@ -70,7 +70,6 @@ autoware::pointcloud_preprocessor::Filter::Filter( { // Set parameters (moved from NodeletLazy onInit) { - tf_input_frame_ = declare_parameter("input_frame"); tf_output_frame_ = declare_parameter("output_frame"); max_queue_size_ = static_cast(declare_parameter("max_queue_size"));