fix(obstacle_stop_planner): fix for multithread

[h-ohta]

Description

object_ptr_ とcurrent_velocity_ptr_周りの扱いの見直し

• pathCallbackのmutexの中でだけshared_ptrの参照カウントを上げることでリソース競合を防ぐ
• 各関数でshared_ptr を直接参照するのではなく、引数で渡す形に変更
• object_ptr_を取得するcallbackでmutexが不足していたのを追加

すでにupstreamではこのノードはリファクタリングが実施されていて、その中で同じ内容が対応されていることを確認
https://github.com/tier4/autoware.universe/blob/2dde073a101e96757ef0cd189bb9ff06836934e9/planning/obstacle_stop_planner/src/node.cpp

TIERIV_INTERNAL_LINK: https://tier4.atlassian.net/browse/T4PB-24093

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

[YoheiMishina]

LGTM

[yn-mrse]

大丈夫です！ありがとうございます！

• callback groupはそれぞれ別定義なのでcallbackがマルチスレッドで動く

  autoware.universe/planning/obstacle_stop_planner/src/node.cpp

  Lines 516 to 536 in 0330459

  	obstacle_pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
  	"~/input/pointcloud", rclcpp::SensorDataQoS(),
  	std::bind(&ObstacleStopPlannerNode::obstaclePointcloudCallback, this, std::placeholders::_1),
  	createSubscriptionOptions(this));
  	path_sub_ = this->create_subscription<Trajectory>(
  	"~/input/trajectory", 1,
  	std::bind(&ObstacleStopPlannerNode::pathCallback, this, std::placeholders::_1),
  	createSubscriptionOptions(this));
  	current_velocity_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
  	"~/input/odometry", 1,
  	std::bind(&ObstacleStopPlannerNode::currentVelocityCallback, this, std::placeholders::_1),
  	createSubscriptionOptions(this));
  	dynamic_object_sub_ = this->create_subscription<PredictedObjects>(
  	"~/input/objects", 1,
  	std::bind(&ObstacleStopPlannerNode::dynamicObjectCallback, this, std::placeholders::_1),
  	createSubscriptionOptions(this));
  	expand_stop_range_sub_ = this->create_subscription<ExpandStopRange>(
  	"~/input/expand_stop_range", 1,
  	std::bind(
  	&ObstacleStopPlannerNode::externalExpandStopRangeCallback, this, std::placeholders::_1),
  	createSubscriptionOptions(this));

• 対象のcallbackは下記で全て。
  • obstaclePointCloudCallback
  • pathCallback
  • currentVelocityCallback
  • dynamicObjectCallback
  • externalExpandStopRangeCallback
• 上記Callback内の全メンバ変数/staticな変数に対して
  • 書き換え中に読み取りが発生しないか？
    • pathCallback以外で読み取り・書き換えが発生するメンバ変数のリストは
      • obstacle_ros_pointcloud_ptr_ (obstaclePointCloudCallback)
      • current_velocity_ptr_ (currentVelocityCallback)
      • prev_velocity_ptr_ (currentVelocityCallback)
      • current_acc_ (currentVelocityCallback)
      • object_ptr_ (dynamicObjectCallback)
      • stop_param_ (externalExpandStopRangeCallback)
      • vehicle_info_ (externalExpandStopRangeCallback)
    • 上記について読み取り・書き換え部に全てmutex lockがついていることを確認済み。またpathCallback以外で共通のメンバ変数を内部処理で使っている任意の2つのCallbackは存在しないことを確認済み。→pathCallback以外での排他失敗はなさそう
    • pathCallback処理内の上記メンバ変数の読み込み・書き換えが行われている全箇所について、

      autoware.universe/planning/obstacle_stop_planner/src/node.cpp

      Lines 567 to 575 in 0330459

      	mutex_.lock();
      	// NOTE: these variables must not be referenced for multithreading
      	const auto vehicle_info = vehicle_info_;
      	const auto stop_param = stop_param_;
      	const double current_acc = current_acc_;
      	const auto obstacle_ros_pointcloud_ptr = obstacle_ros_pointcloud_ptr_;
      	const auto object_ptr = object_ptr_;
      	const auto current_velocity_ptr = current_velocity_ptr_;
      	mutex_.unlock();

      の範囲に閉じており、mutex lockできている。
    • ⇒全Callbackについてメンバ変数の排他は担保されているものと思われます。

[yn-mrse]

Approveしましたが、次のbetaを準備できるまでマージをお待ちください。。。

[yn-mrse]

@h-ohta
変更内容はばっちりですが、upstreamへの反映の有無について（beta/v3.x.xに先行投入して後追いでupstreamでは対応するのか、あるいはupstreamでは実装がすでに全然違っているために取り込めないため不要など）詳細をPR description欄に記載いただけますと幸いです。（tier4/autoware.universeへのPRである根拠を明確にしておきたい）

あわせてこのPRが作成された背景としての不具合報告等もしあればリンクいただきたく、なければない旨記載していただきたいです！

[h-ohta]

@yn-mrse すみません記載しました