Merge pull request #1199 from tier4/hotfix/shape_estimation

fix(shape estimation): 5330 + 6157 + 6598

Author: shmpwk

perception/shape_estimation/CMakeLists.txt

@@ -64,4 +64,5 @@ rclcpp_components_register_node(shape_estimation_node
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )

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
 

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

perception/shape_estimation/launch/shape_estimation.launch.xml

@@ -1,18 +1,13 @@
 <launch>
   <arg name="input/objects" default="labeled_clusters"/>
   <arg name="output/objects" default="shape_estimated_objects"/>
-  <arg name="use_filter" default="true"/>
-  <arg name="use_corrector" default="true"/>
   <arg name="node_name" default="shape_estimation"/>
-  <arg name="use_vehicle_reference_yaw" default="false"/>
-  <arg name="use_vehicle_reference_shape_size" default="false"/>
-  <arg name="use_boost_bbox_optimizer" default="false"/>
+  <!-- Parameter -->
+  <arg name="config_file" default="$(find-pkg-share shape_estimation)/config/shape_estimation.param.yaml"/>
+
   <node pkg="shape_estimation" exec="shape_estimation" name="$(var node_name)" output="screen">
     <remap from="input" to="$(var input/objects)"/>
     <remap from="objects" to="$(var output/objects)"/>
-    <param name="use_filter" value="$(var use_filter)"/>
-    <param name="use_corrector" value="$(var use_corrector)"/>
-    <param name="use_vehicle_reference_yaw" value="$(var use_vehicle_reference_yaw)"/>
-    <param name="use_boost_bbox_optimizer" value="$(var use_boost_bbox_optimizer)"/>
+    <param from="$(var config_file)"/>
   </node>
 </launch>

perception/shape_estimation/lib/shape_estimator.cpp

@@ -39,25 +39,31 @@ 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;
     }
   }
 
   // rule based corrector
   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;

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": ["/**"]
+}

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<DetectedObjectsWithFeature>("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<bool>("use_corrector");
+  bool use_filter = declare_parameter<bool>("use_filter");
+  use_vehicle_reference_yaw_ = declare_parameter<bool>("use_vehicle_reference_yaw");
+  use_vehicle_reference_shape_size_ = declare_parameter<bool>("use_vehicle_reference_shape_size");
+  bool use_boost_bbox_optimizer = declare_parameter<bool>("use_boost_bbox_optimizer");
+  fix_filtered_objects_label_to_unknown_ =
+    declare_parameter<bool>("fix_filtered_objects_label_to_unknown");
   RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer);
   estimator_ =
     std::make_unique<ShapeEstimator>(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;
   }

perception/shape_estimation/src/node.hpp

@@ -38,6 +38,7 @@ class ShapeEstimationNode : public rclcpp::Node
   std::unique_ptr<ShapeEstimator> 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);