feat(behavior_path_planner): change turn signal output timing (autowa…

…refoundation#2493)

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -627,23 +627,6 @@ void BehaviorPathPlannerNode::run()
 
   // update planner data
   planner_data_->prev_output_path = path;
-  mutex_pd_.unlock();
-
-  // publish drivable bounds
-  publish_bounds(*path);
-
-  const size_t target_idx = findEgoIndex(path->points);
-  util::clipPathLength(*path, target_idx, planner_data_->parameters);
-
-  if (!path->points.empty()) {
-    path_publisher_->publish(*path);
-  } else {
-    RCLCPP_ERROR_THROTTLE(
-      get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish.");
-  }
-
-  const auto path_candidate = getPathCandidate(output, planner_data);
-  path_candidate_publisher_->publish(util::toPath(*path_candidate));
 
   // for turn signal
   {
@@ -665,6 +648,25 @@ void BehaviorPathPlannerNode::run()
     publish_steering_factor(turn_signal);
   }
 
+  // unlock planner data
+  mutex_pd_.unlock();
+
+  // publish drivable bounds
+  publish_bounds(*path);
+
+  const size_t target_idx = findEgoIndex(path->points);
+  util::clipPathLength(*path, target_idx, planner_data_->parameters);
+
+  if (!path->points.empty()) {
+    path_publisher_->publish(*path);
+  } else {
+    RCLCPP_ERROR_THROTTLE(
+      get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish.");
+  }
+
+  const auto path_candidate = getPathCandidate(output, planner_data);
+  path_candidate_publisher_->publish(util::toPath(*path_candidate));
+
   publishSceneModuleDebugMsg();
 
   if (planner_data->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) {