From 73544552b73c5c93428ab856070cef0fcfe11252 Mon Sep 17 00:00:00 2001
From: Tomohito ANDO <tomohito.ando@tier4.jp>
Date: Wed, 29 Jun 2022 12:12:08 +0900
Subject: [PATCH] fix(behavior_velocity): avoid zero division in pcl (#1183)

* fix(behavior_velocity): avoid zero division in pcl

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* remove unnecessary comments

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
---
 .../src/scene_module/run_out/dynamic_obstacle.cpp          | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp
index 5427381814860..a98ce2c6bb479 100644
--- a/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp
@@ -61,7 +61,6 @@ pcl::PointCloud<pcl::PointXYZ> applyVoxelGridFilter(
     p.z = 0.0;
   }
 
-  // use boost::makeshared instead of std beacause filter.setInputCloud requires boost shared ptr
   pcl::VoxelGrid<pcl::PointXYZ> filter;
   filter.setInputCloud(pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(no_height_points));
   filter.setLeafSize(0.05f, 0.05f, 100000.0f);
@@ -197,6 +196,12 @@ std::vector<DynamicObstacle> DynamicObstacleCreatorForPoints::createDynamicObsta
 void DynamicObstacleCreatorForPoints::onCompareMapFilteredPointCloud(
   const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
 {
+  if (msg->data.empty()) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    dynamic_obstacle_data_.compare_map_filtered_pointcloud.clear();
+    return;
+  }
+
   geometry_msgs::msg::TransformStamped transform;
   try {
     transform = tf_buffer_.lookupTransform(