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..e277d99d70edd
--- /dev/null
+++ b/perception/shape_estimation/config/shape_estimation.param.yaml
@@ -0,0 +1,8 @@
+/**:
+ ros__parameters:
+ use_corrector: true
+ use_filter: true
+ 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/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml
index 8e90e3ea57cc0..f13b5d1e5f273 100644
--- a/perception/shape_estimation/launch/shape_estimation.launch.xml
+++ b/perception/shape_estimation/launch/shape_estimation.launch.xml
@@ -1,18 +1,13 @@
-
-
-
-
-
+
+
+
-
-
-
-
+
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/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..d666ed059fe2f 100644
--- a/perception/shape_estimation/src/node.cpp
+++ b/perception/shape_estimation/src/node.cpp
@@ -42,11 +42,13 @@ 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");
+ 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);