From b0aba4c1a98f66ea74dd6f3a961dd1e962c73643 Mon Sep 17 00:00:00 2001 From: PhoebeWu21 Date: Fri, 18 Aug 2023 13:52:44 +0800 Subject: [PATCH 1/4] refactor(radar_fusion_to_detected_object): rework parameters Signed-off-by: PhoebeWu21 --- .../radar_fusion_to_detected_object/README.md | 10 +- ...adar_fusion_to_detected_object.schema.json | 114 ++++++++++++++++++ ..._object_fusion_to_detected_object_node.cpp | 27 ++--- 3 files changed, 131 insertions(+), 20 deletions(-) create mode 100644 perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md index b22938cbf0e5a..763b9fa14884b 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/radar_fusion_to_detected_object/README.md @@ -13,11 +13,11 @@ The document of core algorithm is [here](docs/algorithm.md) ### Parameters for sensor fusion -| Name | Type | Description | Default value | -| :----------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | -| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | -| threshold_yaw_diff | double | The yaw orientation threshold. If $ \vert \theta _{ob} - \theta_ {ra} \vert < threshold*yaw_diff $ attached to radar information include estimated velocity, where $ \theta*{ob} $ is yaw angle from 3d detected object, $ \theta\_ {ra} $ is yaw angle from radar object. [rad] | 0.35 | +| Name | Type | Description | Default value | +| :----------------------- | :----- |:---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| :------------ | +| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | +| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | +| threshold_yaw_diff | double | The yaw orientation threshold. If ∣*θob* − *θra*∣ < threshold × yaw_diff attached to radar information include estimated velocity, where *θob* is yaw angle from 3d detected object, *θra* is yaw angle from radar object. [rad] | 0.35 | ### Weight parameters for velocity estimation diff --git a/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json b/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json new file mode 100644 index 0000000000000..688414df56c1e --- /dev/null +++ b/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json @@ -0,0 +1,114 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Radar Fusion to Detected Object Node", + "type": "object", + "definitions": { + "radar_fusion_to_detected_object": { + "type": "object", + "properties": { + "node_params": { + "type": "object", + "properties": { + "update_rate_hz": { + "type": "number", + "description": "Update rate for parameters. [hz]", + "default": "10.0", + "minimum": 0.0 + } + }, + "required": ["update_rate_hz"] + }, + "core_params": { + "type": "object", + "properties": { + "bounding_box_margin": { + "type": "number", + "description": "The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m]", + "default": "2.0", + "minimum": 0.0 + }, + "split_threshold_velocity": { + "type": "number", + "description": "The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s]", + "default": "5.0", + "minimum": 0.0 + }, + "threshold_yaw_diff": { + "type": "number", + "description": "The yaw orientation threshold. If ∣θob−θra∣("node_params.update_rate_hz", 10.0); + node_param_.update_rate_hz = declare_parameter("node_params.update_rate_hz"); // Core Parameter - core_param_.bounding_box_margin = - declare_parameter("core_params.bounding_box_margin", 0.5); + core_param_.bounding_box_margin = declare_parameter("core_params.bounding_box_margin"); core_param_.split_threshold_velocity = - declare_parameter("core_params.split_threshold_velocity", 0.0); - core_param_.threshold_yaw_diff = - declare_parameter("core_params.threshold_yaw_diff", 0.35); + declare_parameter("core_params.split_threshold_velocity"); + core_param_.threshold_yaw_diff = declare_parameter("core_params.threshold_yaw_diff"); core_param_.velocity_weight_min_distance = - declare_parameter("core_params.velocity_weight_min_distance", 1.0); + declare_parameter("core_params.velocity_weight_min_distance"); core_param_.velocity_weight_average = - declare_parameter("core_params.velocity_weight_average", 0.0); + declare_parameter("core_params.velocity_weight_average"); core_param_.velocity_weight_median = - declare_parameter("core_params.velocity_weight_median", 0.0); + declare_parameter("core_params.velocity_weight_median"); core_param_.velocity_weight_target_value_average = - declare_parameter("core_params.velocity_weight_target_value_average", 0.0); + declare_parameter("core_params.velocity_weight_target_value_average"); core_param_.velocity_weight_target_value_top = - declare_parameter("core_params.velocity_weight_target_value_top", 1.0); + declare_parameter("core_params.velocity_weight_target_value_top"); core_param_.convert_doppler_to_twist = - declare_parameter("core_params.convert_doppler_to_twist", false); - core_param_.threshold_probability = - declare_parameter("core_params.threshold_probability", 0.0); + declare_parameter("core_params.convert_doppler_to_twist"); + core_param_.threshold_probability = declare_parameter("core_params.threshold_probability"); core_param_.compensate_probability = - declare_parameter("core_params.compensate_probability", false); + declare_parameter("core_params.compensate_probability"); // Core radar_fusion_to_detected_object_ = std::make_unique(get_logger()); From 0665c2c64affe123b1c033fa7eead77dc6ef75f8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 18 Aug 2023 05:56:54 +0000 Subject: [PATCH 2/4] style(pre-commit): autofix --- perception/radar_fusion_to_detected_object/README.md | 4 ++-- .../radar_object_fusion_to_detected_object_node.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md index 763b9fa14884b..8e91c7069fd9b 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/radar_fusion_to_detected_object/README.md @@ -14,10 +14,10 @@ The document of core algorithm is [here](docs/algorithm.md) ### Parameters for sensor fusion | Name | Type | Description | Default value | -| :----------------------- | :----- |:---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| :------------ | +| :----------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | | bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | | split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | -| threshold_yaw_diff | double | The yaw orientation threshold. If ∣*θob* − *θra*∣ < threshold × yaw_diff attached to radar information include estimated velocity, where *θob* is yaw angle from 3d detected object, *θra* is yaw angle from radar object. [rad] | 0.35 | +| threshold_yaw_diff | double | The yaw orientation threshold. If ∣*θob* − *θra*∣ < threshold × yaw_diff attached to radar information include estimated velocity, where _θob_ is yaw angle from 3d detected object, _θra_ is yaw angle from radar object. [rad] | 0.35 | ### Weight parameters for velocity estimation diff --git a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp index 34ce8928df653..3c5b86402f36c 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp @@ -80,7 +80,8 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode( declare_parameter("core_params.velocity_weight_target_value_top"); core_param_.convert_doppler_to_twist = declare_parameter("core_params.convert_doppler_to_twist"); - core_param_.threshold_probability = declare_parameter("core_params.threshold_probability"); + core_param_.threshold_probability = + declare_parameter("core_params.threshold_probability"); core_param_.compensate_probability = declare_parameter("core_params.compensate_probability"); From 9e179907dda42290b04f1e724938e74ef1980038 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 29 Aug 2023 12:15:11 +0000 Subject: [PATCH 3/4] style(pre-commit): autofix --- perception/radar_fusion_to_detected_object/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md index 8e91c7069fd9b..8bbaab62541e2 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/radar_fusion_to_detected_object/README.md @@ -13,11 +13,11 @@ The document of core algorithm is [here](docs/algorithm.md) ### Parameters for sensor fusion -| Name | Type | Description | Default value | -| :----------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | -| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | -| threshold_yaw_diff | double | The yaw orientation threshold. If ∣*θob* − *θra*∣ < threshold × yaw_diff attached to radar information include estimated velocity, where _θob_ is yaw angle from 3d detected object, _θra_ is yaw angle from radar object. [rad] | 0.35 | +| Name | Type | Description | Default value | +| :----------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | +| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | +| threshold_yaw_diff | double | The yaw orientation threshold. If ∣*θob* − *θra*∣ < threshold × yaw*diff attached to radar information include estimated velocity, where *θob* is yaw angle from 3d detected object, *θra\_ is yaw angle from radar object. [rad] | 0.35 | ### Weight parameters for velocity estimation From 11636b8193251ce1911f85e81abfa6264d6c5445 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 30 Aug 2023 03:25:07 +0000 Subject: [PATCH 4/4] style(pre-commit): autofix --- perception/radar_fusion_to_detected_object/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md index 8bbaab62541e2..24eb4eb0e93c1 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/radar_fusion_to_detected_object/README.md @@ -13,11 +13,11 @@ The document of core algorithm is [here](docs/algorithm.md) ### Parameters for sensor fusion -| Name | Type | Description | Default value | -| :----------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | -| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | -| threshold_yaw_diff | double | The yaw orientation threshold. If ∣*θob* − *θra*∣ < threshold × yaw*diff attached to radar information include estimated velocity, where *θob* is yaw angle from 3d detected object, *θra\_ is yaw angle from radar object. [rad] | 0.35 | +| Name | Type | Description | Default value | +| :----------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | +| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | +| threshold_yaw_diff | double | The yaw orientation threshold. If ∣*θob* − *θra*∣ < threshold × yaw*diff attached to radar information include estimated velocity, where*θob*is yaw angle from 3d detected object,*θra\_ is yaw angle from radar object. [rad] | 0.35 | ### Weight parameters for velocity estimation