@@ -61,6 +61,22 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option
61
61
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this );
62
62
}
63
63
64
+ static autoware_auto_perception_msgs::msg::ObjectClassification::_label_type get_label (
65
+ const autoware_auto_perception_msgs::msg::DetectedObject::_classification_type & classification)
66
+ {
67
+ if (classification.empty ()) {
68
+ return Label::UNKNOWN;
69
+ }
70
+ return classification.front ().label ;
71
+ }
72
+
73
+ static bool label_is_vehicle (
74
+ const autoware_auto_perception_msgs::msg::ObjectClassification::_label_type & label)
75
+ {
76
+ return Label::CAR == label || Label::TRUCK == label || Label::BUS == label ||
77
+ Label::TRAILER == label;
78
+ }
79
+
64
80
void ShapeEstimationNode::callback (const DetectedObjectsWithFeature::ConstSharedPtr input_msg)
65
81
{
66
82
stop_watch_ptr_->toc (" processing_time" , true );
@@ -76,11 +92,9 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared
76
92
// Estimate shape for each object and pack msg
77
93
for (const auto & feature_object : input_msg->feature_objects ) {
78
94
const auto & object = feature_object.object ;
79
- const auto & label = object.classification .front ().label ;
95
+ const auto label = get_label (object.classification );
96
+ const auto is_vehicle = label_is_vehicle (label);
80
97
const auto & feature = feature_object.feature ;
81
- const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label ||
82
- Label::TRAILER == label;
83
-
84
98
// convert ros to pcl
85
99
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZ>);
86
100
pcl::fromROSMsg (feature.cluster , *cluster);
0 commit comments