From 91db66161166fae88dda261666a3652bae86e058 Mon Sep 17 00:00:00 2001
From: Tomohito ANDO <tomohito.ando@tier4.jp>
Date: Mon, 20 Jun 2022 16:41:53 +0900
Subject: [PATCH] fix(behavior_velocity): add mutex for points subscription
 (#1125)

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
---
 .../include/scene_module/run_out/dynamic_obstacle.hpp | 11 +++++++----
 .../src/scene_module/run_out/dynamic_obstacle.cpp     |  7 ++++---
 2 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp
index e1360eb36e53b..27309703378f5 100644
--- a/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp
+++ b/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp
@@ -96,7 +96,7 @@ class DynamicObstacleCreator
 public:
   explicit DynamicObstacleCreator(rclcpp::Node & node) : node_(node) {}
   virtual ~DynamicObstacleCreator() = default;
-  virtual std::vector<DynamicObstacle> createDynamicObstacles() const = 0;
+  virtual std::vector<DynamicObstacle> createDynamicObstacles() = 0;
   void setParam(const DynamicObstacleParam & param) { param_ = param; }
   void setData(const PlannerData & planner_data, const PathWithLaneId & path)
   {
@@ -118,7 +118,7 @@ class DynamicObstacleCreatorForObject : public DynamicObstacleCreator
 {
 public:
   explicit DynamicObstacleCreatorForObject(rclcpp::Node & node);
-  std::vector<DynamicObstacle> createDynamicObstacles() const override;
+  std::vector<DynamicObstacle> createDynamicObstacles() override;
 };
 
 /**
@@ -129,7 +129,7 @@ class DynamicObstacleCreatorForObjectWithoutPath : public DynamicObstacleCreator
 {
 public:
   explicit DynamicObstacleCreatorForObjectWithoutPath(rclcpp::Node & node);
-  std::vector<DynamicObstacle> createDynamicObstacles() const override;
+  std::vector<DynamicObstacle> createDynamicObstacles() override;
 };
 
 /**
@@ -140,7 +140,7 @@ class DynamicObstacleCreatorForPoints : public DynamicObstacleCreator
 {
 public:
   explicit DynamicObstacleCreatorForPoints(rclcpp::Node & node);
-  std::vector<DynamicObstacle> createDynamicObstacles() const override;
+  std::vector<DynamicObstacle> createDynamicObstacles() override;
 
 private:
   void onCompareMapFilteredPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
@@ -150,6 +150,9 @@ class DynamicObstacleCreatorForPoints : public DynamicObstacleCreator
   // tf
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
+
+  // mutex for compare_map_filtered_pointcloud
+  std::mutex mutex_;
 };
 
 }  // namespace behavior_velocity_planner
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 afe2994ed4cf5..5427381814860 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
@@ -78,7 +78,7 @@ DynamicObstacleCreatorForObject::DynamicObstacleCreatorForObject(rclcpp::Node &
 {
 }
 
-std::vector<DynamicObstacle> DynamicObstacleCreatorForObject::createDynamicObstacles() const
+std::vector<DynamicObstacle> DynamicObstacleCreatorForObject::createDynamicObstacles()
 {
   // create dynamic obstacles from predicted objects
   std::vector<DynamicObstacle> dynamic_obstacles;
@@ -115,7 +115,6 @@ DynamicObstacleCreatorForObjectWithoutPath::DynamicObstacleCreatorForObjectWitho
 }
 
 std::vector<DynamicObstacle> DynamicObstacleCreatorForObjectWithoutPath::createDynamicObstacles()
-  const
 {
   std::vector<DynamicObstacle> dynamic_obstacles;
 
@@ -154,8 +153,9 @@ DynamicObstacleCreatorForPoints::DynamicObstacleCreatorForPoints(rclcpp::Node &
     std::bind(&DynamicObstacleCreatorForPoints::onCompareMapFilteredPointCloud, this, _1));
 }
 
-std::vector<DynamicObstacle> DynamicObstacleCreatorForPoints::createDynamicObstacles() const
+std::vector<DynamicObstacle> DynamicObstacleCreatorForPoints::createDynamicObstacles()
 {
+  std::lock_guard<std::mutex> lock(mutex_);
   std::vector<DynamicObstacle> dynamic_obstacles;
   for (const auto & point : dynamic_obstacle_data_.compare_map_filtered_pointcloud) {
     DynamicObstacle dynamic_obstacle;
@@ -213,6 +213,7 @@ void DynamicObstacleCreatorForPoints::onCompareMapFilteredPointCloud(
   pcl::PointCloud<pcl::PointXYZ>::Ptr pc_transformed(new pcl::PointCloud<pcl::PointXYZ>);
   pcl::transformPointCloud(pc, *pc_transformed, affine);
 
+  std::lock_guard<std::mutex> lock(mutex_);
   dynamic_obstacle_data_.compare_map_filtered_pointcloud = applyVoxelGridFilter(pc_transformed);
 }
 }  // namespace behavior_velocity_planner