Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(radar_fusion_to_detected_object): rework parameters #4663

10 changes: 5 additions & 5 deletions perception/radar_fusion_to_detected_object/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
Original file line number Diff line number Diff line change
@@ -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∣<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]",
"default": "0.35"
},
"velocity_weight_min_distance": {
"type": "number",
"description": "The twist coefficient of radar data nearest to the center of bounding box in velocity estimation.",
"default": "1.0"
},
"velocity_weight_average": {
"type": "number",
"description": "The twist coefficient of average twist of radar data in velocity estimation.",
"default": "0.0"
},
"velocity_weight_median": {
"type": "number",
"description": "The twist coefficient of median twist of radar data in velocity estimation.",
"default": "0.0"
},
"velocity_weight_target_value_average": {
"type": "number",
"description": "The twist coefficient of target value weighted average in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects.",
"default": "0.0"
},
"velocity_weight_target_value_top": {
"type": "number",
"description": "The twist coefficient of top target value radar data in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects.",
"default": "0.0"
},
"convert_doppler_to_twist": {
"type": "boolean",
"description": "Convert doppler velocity to twist using the yaw information of a detected object.",
"default": "false"
},
"threshold_probability": {
"type": "number",
"description": "If the probability of an output object is lower than this parameter, and the output object does not have radar points/objects, then delete the object.",
"default": "0.4",
"minimum": 0.0,
"maximum": 1.0
},
"compensate_probability": {
"type": "boolean",
"description": "If this parameter is true, compensate probability of objects to threshold probability.",
"default": "false"
}
},
"required": [
"bounding_box_margin",
"split_threshold_velocity",
"threshold_yaw_diff",
"velocity_weight_min_distance",
"velocity_weight_average",
"velocity_weight_median",
"velocity_weight_target_value_average",
"velocity_weight_target_value_top",
"convert_doppler_to_twist",
"threshold_probability",
"compensate_probability"
]
}
},
"required": ["node_params", "core_params"]
}
},
"properties": {
"/**": {
"type": "object",
"properties": {
"ros__parameters": {
"$ref": "#/definitions/radar_fusion_to_detected_object"
}
},
"required": ["ros__parameters"]
}
},
"required": ["/**"]
}
Original file line number Diff line number Diff line change
Expand Up @@ -61,31 +61,29 @@
std::bind(&RadarObjectFusionToDetectedObjectNode::onSetParam, this, _1));

// Node Parameter
node_param_.update_rate_hz = declare_parameter<double>("node_params.update_rate_hz", 10.0);
node_param_.update_rate_hz = declare_parameter<double>("node_params.update_rate_hz");

Check warning on line 64 in perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp#L64

Added line #L64 was not covered by tests

// Core Parameter
core_param_.bounding_box_margin =
declare_parameter<double>("core_params.bounding_box_margin", 0.5);
core_param_.bounding_box_margin = declare_parameter<double>("core_params.bounding_box_margin");

Check warning on line 67 in perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp#L67

Added line #L67 was not covered by tests
core_param_.split_threshold_velocity =
declare_parameter<double>("core_params.split_threshold_velocity", 0.0);
core_param_.threshold_yaw_diff =
declare_parameter<double>("core_params.threshold_yaw_diff", 0.35);
declare_parameter<double>("core_params.split_threshold_velocity");
core_param_.threshold_yaw_diff = declare_parameter<double>("core_params.threshold_yaw_diff");

Check warning on line 70 in perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp#L69-L70

Added lines #L69 - L70 were not covered by tests
core_param_.velocity_weight_min_distance =
declare_parameter<double>("core_params.velocity_weight_min_distance", 1.0);
declare_parameter<double>("core_params.velocity_weight_min_distance");

Check warning on line 72 in perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp#L72

Added line #L72 was not covered by tests
core_param_.velocity_weight_average =
declare_parameter<double>("core_params.velocity_weight_average", 0.0);
declare_parameter<double>("core_params.velocity_weight_average");

Check warning on line 74 in perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp#L74

Added line #L74 was not covered by tests
core_param_.velocity_weight_median =
declare_parameter<double>("core_params.velocity_weight_median", 0.0);
declare_parameter<double>("core_params.velocity_weight_median");

Check warning on line 76 in perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp#L76

Added line #L76 was not covered by tests
core_param_.velocity_weight_target_value_average =
declare_parameter<double>("core_params.velocity_weight_target_value_average", 0.0);
declare_parameter<double>("core_params.velocity_weight_target_value_average");

Check warning on line 78 in perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp#L78

Added line #L78 was not covered by tests
core_param_.velocity_weight_target_value_top =
declare_parameter<double>("core_params.velocity_weight_target_value_top", 1.0);
declare_parameter<double>("core_params.velocity_weight_target_value_top");

Check warning on line 80 in perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp#L80

Added line #L80 was not covered by tests
core_param_.convert_doppler_to_twist =
declare_parameter<bool>("core_params.convert_doppler_to_twist", false);
declare_parameter<bool>("core_params.convert_doppler_to_twist");

Check warning on line 82 in perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp#L82

Added line #L82 was not covered by tests
core_param_.threshold_probability =
declare_parameter<float>("core_params.threshold_probability", 0.0);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[MUST] Probability value is defined by float in Autoware message. Please revert to float.

Copy link
Member Author

@PhoebeWu21 PhoebeWu21 Aug 28, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @scepter914 , it was "float" in my previous commit, but @ambroise-arm asked me to change to "double" because of the new Declare Parameter Function. ( see in #4537 (comment))

Please let me know which to follow. Thanks!

Copy link
Contributor

@ambroise-arm ambroise-arm Aug 29, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@scepter914 Yes, as I wrote in the comment linked above, the return type of declare_parameter<float>() is actually double. So for clarity we should write declare_parameter<double>().

Copy link
Contributor

@scepter914 scepter914 Aug 29, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@PhoebeWu21 @ambroise-arm
Thank you for comment.
I agree with the reason changing from "float" to "double", but pre-commit.ci failed.
So I cannot approve until you debug this build fail.

I think two way to solve this problem.

  • Debug it in this PR.
  • Once revert the changing the parameter type and merge this PR for main branch, and make new PR for the commit of it.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am not sure how the change from float to double relates to the pre-commit.ci failure. It is not a build job but a linting job, and the failures it reports look unrelated to the changes in this PR.
Let's see what the build-and-test* jobs result in for building the code.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There are no pre-commit.ci failures in other PR. So I guess build-and-test* jobs don't have problem and your change cause pre-commit.ci failures.
I recommend that you construct local environment and debug it.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @scepter914,
Finally, pre-commit.ci check has passed.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@PhoebeWu21
[imo] Since core_param_.threshold_probability is defined as float, this is implicit type conversion here.
Is this meant?

declare_parameter<double>("core_params.threshold_probability");

Check warning on line 84 in perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp#L84

Added line #L84 was not covered by tests
core_param_.compensate_probability =
declare_parameter<bool>("core_params.compensate_probability", false);
declare_parameter<bool>("core_params.compensate_probability");

Check warning on line 86 in perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp#L86

Added line #L86 was not covered by tests

// Core
radar_fusion_to_detected_object_ = std::make_unique<RadarFusionToDetectedObject>(get_logger());
Expand Down
Loading