From 9e02bab0e85fb6a2d302597ae03ec0c5a31f1e34 Mon Sep 17 00:00:00 2001 From: Phoebe Wu Date: Fri, 12 Jan 2024 17:05:03 +0800 Subject: [PATCH 1/3] refactor(shape_estimation): rework parameters (#5330) * refactor(shape_estimation): rework parameters Signed-off-by: PhoebeWu21 * style(pre-commit): autofix * refactor(shape_estimation): rework parameters Signed-off-by: PhoebeWu21 * refactor(shape_estimation): rework parameters Signed-off-by: PhoebeWu21 --------- Signed-off-by: PhoebeWu21 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- perception/shape_estimation/CMakeLists.txt | 1 + perception/shape_estimation/README.md | 6 +- .../config/shape_estimation.param.yaml | 7 +++ .../launch/shape_estimation.launch.xml | 10 ++-- .../schema/shape_estimation.schema.json | 56 +++++++++++++++++++ perception/shape_estimation/src/node.cpp | 10 ++-- 6 files changed, 74 insertions(+), 16 deletions(-) create mode 100644 perception/shape_estimation/config/shape_estimation.param.yaml create mode 100644 perception/shape_estimation/schema/shape_estimation.schema.json diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/shape_estimation/CMakeLists.txt index 5ae6002cd7c3b..8eb7a15b5a885 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/shape_estimation/CMakeLists.txt @@ -64,4 +64,5 @@ rclcpp_components_register_node(shape_estimation_node ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/shape_estimation/README.md b/perception/shape_estimation/README.md index c50d66b546213..b635631381cc3 100644 --- a/perception/shape_estimation/README.md +++ b/perception/shape_estimation/README.md @@ -36,11 +36,7 @@ This node calculates a refined object shape (bounding box, cylinder, convex hull ## Parameters -| Name | Type | Default Value | Description | -| --------------------------- | ---- | ------------- | --------------------------------------------------- | -| `use_corrector` | bool | true | The flag to apply rule-based filter | -| `use_filter` | bool | true | The flag to apply rule-based corrector | -| `use_vehicle_reference_yaw` | bool | true | The flag to use vehicle reference yaw for corrector | +{{ json_to_markdown("perception/shape_estimation/schema/shape_estimation.schema.json") }} ## Assumptions / Known limits diff --git a/perception/shape_estimation/config/shape_estimation.param.yaml b/perception/shape_estimation/config/shape_estimation.param.yaml new file mode 100644 index 0000000000000..253516fffe0d4 --- /dev/null +++ b/perception/shape_estimation/config/shape_estimation.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + use_corrector: true + use_filter: true + use_vehicle_reference_yaw: false + use_vehicle_reference_shape_size: false + use_boost_bbox_optimizer: false diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml index 8e90e3ea57cc0..65d1944417cc0 100644 --- a/perception/shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/shape_estimation/launch/shape_estimation.launch.xml @@ -1,18 +1,16 @@ - - + + + - - - - + diff --git a/perception/shape_estimation/schema/shape_estimation.schema.json b/perception/shape_estimation/schema/shape_estimation.schema.json new file mode 100644 index 0000000000000..d81bfa636a923 --- /dev/null +++ b/perception/shape_estimation/schema/shape_estimation.schema.json @@ -0,0 +1,56 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Shape Estimation Node", + "type": "object", + "definitions": { + "shape_estimation": { + "type": "object", + "properties": { + "use_corrector": { + "type": "boolean", + "description": "The flag to apply rule-based corrector.", + "default": "true" + }, + "use_filter": { + "type": "boolean", + "description": "The flag to apply rule-based filter", + "default": "true" + }, + "use_vehicle_reference_yaw": { + "type": "boolean", + "description": "The flag to use vehicle reference yaw for corrector", + "default": "false" + }, + "use_vehicle_reference_shape_size": { + "type": "boolean", + "description": "The flag to use vehicle reference shape size", + "default": "false" + }, + "use_boost_bbox_optimizer": { + "type": "boolean", + "description": "The flag to use boost bbox optimizer", + "default": "false" + } + }, + "required": [ + "use_corrector", + "use_filter", + "use_vehicle_reference_yaw", + "use_vehicle_reference_shape_size", + "use_boost_bbox_optimizer" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/shape_estimation" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 987cf8106c99e..624ca604d8fbf 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -42,11 +42,11 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option "input", rclcpp::QoS{1}, std::bind(&ShapeEstimationNode::callback, this, _1)); pub_ = create_publisher("objects", rclcpp::QoS{1}); - bool use_corrector = declare_parameter("use_corrector", true); - bool use_filter = declare_parameter("use_filter", true); - use_vehicle_reference_yaw_ = declare_parameter("use_vehicle_reference_yaw", true); - use_vehicle_reference_shape_size_ = declare_parameter("use_vehicle_reference_shape_size", true); - bool use_boost_bbox_optimizer = declare_parameter("use_boost_bbox_optimizer", false); + bool use_corrector = declare_parameter("use_corrector"); + bool use_filter = declare_parameter("use_filter"); + use_vehicle_reference_yaw_ = declare_parameter("use_vehicle_reference_yaw"); + use_vehicle_reference_shape_size_ = declare_parameter("use_vehicle_reference_shape_size"); + bool use_boost_bbox_optimizer = declare_parameter("use_boost_bbox_optimizer"); RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer); estimator_ = std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); From 0ac0d9cac01a01a15a8a3f271826dbf63838a186 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 24 Jan 2024 22:33:47 +0900 Subject: [PATCH 2/3] chore(shape_estimation): remove unnecessary param (#6157) --- perception/shape_estimation/launch/shape_estimation.launch.xml | 3 --- 1 file changed, 3 deletions(-) diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml index 65d1944417cc0..f13b5d1e5f273 100644 --- a/perception/shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/shape_estimation/launch/shape_estimation.launch.xml @@ -4,9 +4,6 @@ - - - From 290b42bc3fb929ba6e36a32106e067a6a28e2e1f Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Wed, 13 Mar 2024 18:43:59 +0900 Subject: [PATCH 3/3] fix(shape_estimation): preserve irregular large size vehicle detected by camera_lidar_fusion as unknown (#6598) * fix(shape_estimation): keep excluded objects as unknown Signed-off-by: badai-nguyen * chore: add optional param Signed-off-by: badai-nguyen * chore: rename param Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen Co-authored-by: Yoshi Ri --- .../config/shape_estimation.param.yaml | 1 + .../shape_estimation/lib/shape_estimator.cpp | 14 ++++++++++---- perception/shape_estimation/src/node.cpp | 11 ++++++++--- perception/shape_estimation/src/node.hpp | 1 + 4 files changed, 20 insertions(+), 7 deletions(-) diff --git a/perception/shape_estimation/config/shape_estimation.param.yaml b/perception/shape_estimation/config/shape_estimation.param.yaml index 253516fffe0d4..e277d99d70edd 100644 --- a/perception/shape_estimation/config/shape_estimation.param.yaml +++ b/perception/shape_estimation/config/shape_estimation.param.yaml @@ -5,3 +5,4 @@ use_vehicle_reference_yaw: false use_vehicle_reference_shape_size: false use_boost_bbox_optimizer: false + fix_filtered_objects_label_to_unknown: true diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/shape_estimation/lib/shape_estimator.cpp index e89eaac0a6db0..e0be98edf926a 100644 --- a/perception/shape_estimation/lib/shape_estimator.cpp +++ b/perception/shape_estimation/lib/shape_estimator.cpp @@ -39,14 +39,15 @@ bool ShapeEstimator::estimateShapeAndPose( autoware_auto_perception_msgs::msg::Shape shape; geometry_msgs::msg::Pose pose; // estimate shape + bool reverse_to_unknown = false; if (!estimateOriginalShapeAndPose(label, cluster, ref_yaw_info, shape, pose)) { - return false; + reverse_to_unknown = true; } // rule based filter if (use_filter_) { if (!applyFilter(label, shape, pose)) { - return false; + reverse_to_unknown = true; } } @@ -54,10 +55,15 @@ bool ShapeEstimator::estimateShapeAndPose( if (use_corrector_) { bool use_reference_yaw = ref_yaw_info ? true : false; if (!applyCorrector(label, use_reference_yaw, ref_shape_size_info, shape, pose)) { - return false; + reverse_to_unknown = true; } } - + if (reverse_to_unknown) { + estimateOriginalShapeAndPose(Label::UNKNOWN, cluster, ref_yaw_info, shape, pose); + shape_output = shape; + pose_output = pose; + return false; + } shape_output = shape; pose_output = pose; return true; diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 624ca604d8fbf..d666ed059fe2f 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -47,6 +47,8 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option use_vehicle_reference_yaw_ = declare_parameter("use_vehicle_reference_yaw"); use_vehicle_reference_shape_size_ = declare_parameter("use_vehicle_reference_shape_size"); bool use_boost_bbox_optimizer = declare_parameter("use_boost_bbox_optimizer"); + fix_filtered_objects_label_to_unknown_ = + declare_parameter("fix_filtered_objects_label_to_unknown"); RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer); estimator_ = std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); @@ -96,12 +98,15 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared const bool estimated_success = estimator_->estimateShapeAndPose( label, *cluster, ref_yaw_info, ref_shape_size_info, shape, pose); - // If the shape estimation fails, ignore it. - if (!estimated_success) { + // If the shape estimation fails, change to Unknown object. + if (!fix_filtered_objects_label_to_unknown_ && !estimated_success) { continue; } - output_msg.feature_objects.push_back(feature_object); + if (!estimated_success) { + output_msg.feature_objects.back().object.classification.front().label = Label::UNKNOWN; + } + output_msg.feature_objects.back().object.shape = shape; output_msg.feature_objects.back().object.kinematics.pose_with_covariance.pose = pose; } diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/node.hpp index 95ee4afe8d9e8..6befa1f49d333 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/node.hpp @@ -38,6 +38,7 @@ class ShapeEstimationNode : public rclcpp::Node std::unique_ptr estimator_; bool use_vehicle_reference_yaw_; bool use_vehicle_reference_shape_size_; + bool fix_filtered_objects_label_to_unknown_; public: explicit ShapeEstimationNode(const rclcpp::NodeOptions & node_options);