fix(ndt_scan_matcher): ndt_scan_matcher crash when input cloud is emp…

…ty. (autowarefoundation#3354) * Fixed NDTScanMatcher crash when input cloud is empty. * style(pre-commit): autofix --------- Co-authored-by: Zygfryd Wieszok <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: M(-_-)Sakamoto <[email protected]>
esteve · May 12, 2023 · 5e5c4dd · 5e5c4dd
1 parent 8400a86
commit 5e5c4dd
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -329,6 +329,11 @@ void NDTScanMatcher::callback_regularization_pose(
 void NDTScanMatcher::callback_sensor_points(
   sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_sensorTF_msg_ptr)
 {
+  if (sensor_points_sensorTF_msg_ptr->data.empty()) {
+    RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "Empty sensor points!");
+    return;
+  }
+
   // mutex ndt_ptr_
   std::lock_guard<std::mutex> lock(ndt_ptr_mtx_);