diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 73b19ef7ee65f..4da5933afa7af 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -618,7 +618,11 @@ void BehaviorPathPlannerNode::run() // update planner data planner_data_->self_pose = self_pose_listener_.getCurrentPose(); - const auto planner_data = planner_data_; + const auto planner_data = std::make_shared(*planner_data_); + + // unlock planner data + mutex_pd_.unlock(); + // run behavior planner const auto output = bt_manager_->run(planner_data); @@ -631,9 +635,6 @@ void BehaviorPathPlannerNode::run() // compute turn signal computeTurnSignal(planner_data, *path, output); - // unlock planner data - mutex_pd_.unlock(); - // publish drivable bounds publish_bounds(*path);