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 22f31fffec21a..423dda764492b 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); @@ -103,12 +105,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 87df44f08f836..a1c30446605b0 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/node.hpp @@ -44,6 +44,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);