From 3c3aac26735a051c5e14b9953935645fcc8e4528 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue, 13 Dec 2022 18:19:41 +0900 Subject: [PATCH] feat(behavior_path_planner): change turn signal output timing (#2493) Signed-off-by: yutaka Signed-off-by: yutaka --- .../src/behavior_path_planner_node.cpp | 36 ++++++++++--------- 1 file changed, 19 insertions(+), 17 deletions(-) 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 b536689ed7b18..4ed25862940fb 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/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) {