Skip to content

Commit 74c52f3

Browse files
authored
fix(shape_estimation): segfault when the classification was empty (#7039)
fix(shape_estimation): fix segfault when the classification is empty Signed-off-by: Grzegorz Głowacki <gglowacki@autonomous-systems.pl>
1 parent e42fd71 commit 74c52f3

File tree

1 file changed

+18
-4
lines changed

1 file changed

+18
-4
lines changed

perception/shape_estimation/src/node.cpp

+18-4
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,22 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option
6161
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
6262
}
6363

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+
6480
void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg)
6581
{
6682
stop_watch_ptr_->toc("processing_time", true);
@@ -76,11 +92,9 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared
7692
// Estimate shape for each object and pack msg
7793
for (const auto & feature_object : input_msg->feature_objects) {
7894
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);
8097
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-
8498
// convert ros to pcl
8599
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
86100
pcl::fromROSMsg(feature.cluster, *cluster);

0 commit comments

Comments
 (0)