behavior_path_planner, behavior_velocity_planner node should pass planner_data_ as reference to value, not as shared pointer

[soblin]

behavior_path_planner and behavior_velocity_planner takes several sensor/map/system topics as input and publish path / path_with_lane_id as output. To obtain the output it needs information of its input, namely the PlannerData, that stores all of the input messages in it (as shared pointer).

Since behavior_path_planner and behavior_velocity_planner are using independent callback groups for each input and output, the access to PlannerData will come from multiple threads (callbacks). To avoid racy situation each node has a mutex to PlannerData. (behavior_path_planner example). And we write boilerplate code like this:

void onFooCallback(foo_msgs::msg::ConstSharedPtr msg) {
   auto lock = std::lock_guard<std::mutex>(planner_data_mutex_); // common mutex object to planner_data_
   planner_data_->foo = msg;
}

void onOutputTimer() {
   auto lock = std::lock_guard<std::mutex>(planner_data_mutex_); // common mutex object to planner_data_
   output = do_stuff(planner_data);
   publish(output);
}

I believe most of other packages/developers also follow this boilerplate style if they do not use default callback groups. As long as we follow this manner we can rely on the rclcpp framework without tacking care of thread safety.

behavior_path_planner and behavior_velocity_planner add some complexity because they take plugin style. For implementation-wise each plugin is given the shared pointer of PlannerData and returns its status/solution availability through interfaces.

struct PlannerData
{
  Odometry::ConstSharedPtr self_odometry{};
  AccelWithCovarianceStamped::ConstSharedPtr self_acceleration{};
  PredictedObjects::ConstSharedPtr dynamic_object{};
  OccupancyGrid::ConstSharedPtr occupancy_grid{};
  OccupancyGrid::ConstSharedPtr costmap{};
}

While I'm working on this issue , I found out that the following operation

/// copy planner_data_ (plugin's base class member variable, also shared_ptr)
void setData(const std::shared_ptr<const PlannerData> & data) { planner_data_ = data; }

increments the reference count of planner_data_, but the reference count of its internal members does not change. This is problematic if a submodule uses another thread, because planner_data_->foo of submodule A is identical to planner_data_->foo of behavior_path_planner node in the sense that the reference count is 1, not 2. Therefore if the submodule thread accesses to planner_data_->foo of submodule A at the same time when planner_data_->foo = msg happens in behavior_path_planner::onFooCallback, data race happens. Please be noted that if the reference count is 2, these operations are thread safe because they are different variables and the mutable change is just the re-pointing of the shared pointer to another.
Unfortunately we have no way to avoid this so far because the mutex is on behavior_path_planner side and the submodule cannot synchronize the access. Also it is not intuitive that the reference count of member variables remains the same.

So I suggest passing the PlannerData as const PlannerData & planner_data to all interface functions. We have two pros:

• each submodule does not need to check planner_data_ != nullptr anymore because the behavior_path_planner ensures it is not null by definition (otherwise it cannot pass it as const PlannerData & planner_data). It gets clear "who is responsible for planner_data_ != nullptr".
• if the submodule wants to run a custom thread, then it has to copy PlannerData value as its member variable. This forces the shared pointer members to be copied (the reference count is incremented) and they becomes safe from planner_data_->foo = msg operation.

I think we have two different approaches actually:

• setData() does planner_data_ = make_shared<PlannerData>(data);
• also pass the reference/pointer to planner_data_mutex_ to submodule
  but they does not provide the merits of suggested approach.