feat(behavior_path_planner): resolve multiple modules turn signal info (

#3634)

feat(behavior_path_planner): resolve multiple modules turn signal

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -37,7 +37,6 @@
 #endif
 
 #include "behavior_path_planner/steering_factor_interface.hpp"
-#include "behavior_path_planner/turn_signal_decider.hpp"
 #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
 #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp"
 #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
@@ -136,8 +135,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   bool has_received_map_{false};
   bool has_received_route_{false};
 
-  TurnSignalDecider turn_signal_decider_;
-
   std::mutex mutex_pd_;       // mutex for planner_data_
   std::mutex mutex_manager_;  // mutex for bt_manager_ or planner_manager_
   std::mutex mutex_map_;      // mutex for has_received_map_ and map_ptr_
@@ -220,7 +217,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   /**
    * @brief publish steering factor from intersection
    */
-  void publish_steering_factor(const TurnIndicatorsCommand & turn_signal);
+  void publish_steering_factor(
+    const std::shared_ptr<PlannerData> & planner_data, const TurnIndicatorsCommand & turn_signal);
 
   /**
    * @brief publish left and right bound

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -16,6 +16,7 @@
 #define BEHAVIOR_PATH_PLANNER__DATA_MANAGER_HPP_
 
 #include "behavior_path_planner/parameters.hpp"
+#include "behavior_path_planner/turn_signal_decider.hpp"
 #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
 
 #include <rclcpp/rclcpp.hpp>
@@ -96,24 +97,6 @@ struct DrivableAreaInfo
   bool is_already_expanded{false};
 };
 
-struct TurnSignalInfo
-{
-  TurnSignalInfo()
-  {
-    turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
-    hazard_signal.command = HazardLightsCommand::NO_COMMAND;
-  }
-
-  // desired turn signal
-  TurnIndicatorsCommand turn_signal;
-  HazardLightsCommand hazard_signal;
-
-  geometry_msgs::msg::Pose desired_start_point;
-  geometry_msgs::msg::Pose desired_end_point;
-  geometry_msgs::msg::Pose required_start_point;
-  geometry_msgs::msg::Pose required_end_point;
-};
-
 struct BehaviorModuleOutput
 {
   BehaviorModuleOutput() = default;
@@ -160,6 +143,17 @@ struct PlannerData
   BehaviorPathPlannerParameters parameters{};
   drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{};
 
+  mutable TurnSignalDecider turn_signal_decider;
+
+  TurnIndicatorsCommand getTurnSignal(
+    const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info)
+  {
+    const auto & current_pose = self_odometry->pose.pose;
+    const auto & current_vel = self_odometry->twist.twist.linear.x;
+    return turn_signal_decider.getTurnSignal(
+      route_handler, path, turn_signal_info, current_pose, current_vel, parameters);
+  }
+
   template <class T>
   size_t findEgoIndex(const std::vector<T> & points) const
   {

planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp

@@ -15,7 +15,7 @@
 #ifndef BEHAVIOR_PATH_PLANNER__TURN_SIGNAL_DECIDER_HPP_
 #define BEHAVIOR_PATH_PLANNER__TURN_SIGNAL_DECIDER_HPP_
 
-#include <behavior_path_planner/data_manager.hpp>
+#include <behavior_path_planner/parameters.hpp>
 #include <route_handler/route_handler.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -45,18 +45,42 @@ const std::map<std::string, uint8_t> signal_map = {
   {"straight", TurnIndicatorsCommand::DISABLE},
   {"none", TurnIndicatorsCommand::DISABLE}};
 
+struct TurnSignalInfo
+{
+  TurnSignalInfo()
+  {
+    turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
+    hazard_signal.command = HazardLightsCommand::NO_COMMAND;
+  }
+
+  // desired turn signal
+  TurnIndicatorsCommand turn_signal;
+  HazardLightsCommand hazard_signal;
+
+  geometry_msgs::msg::Pose desired_start_point;
+  geometry_msgs::msg::Pose desired_end_point;
+  geometry_msgs::msg::Pose required_start_point;
+  geometry_msgs::msg::Pose required_end_point;
+};
+
 class TurnSignalDecider
 {
 public:
   TurnIndicatorsCommand getTurnSignal(
-    const std::shared_ptr<const PlannerData> & planner_data, const PathWithLaneId & path,
-    const TurnSignalInfo & turn_signal_info);
+    const std::shared_ptr<RouteHandler> & route_handler, const PathWithLaneId & path,
+    const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel,
+    const BehaviorPathPlannerParameters & parameters);
 
   TurnIndicatorsCommand resolve_turn_signal(
     const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
     const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info,
     const double nearest_dist_threshold, const double nearest_yaw_threshold);
 
+  TurnSignalInfo use_prior_turn_signal(
+    const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
+    const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal,
+    const double nearest_dist_threshold, const double nearest_yaw_threshold);
+
   void setParameters(
     const double base_link2front, const double intersection_search_distance,
     const double intersection_search_time, const double intersection_angle_threshold_deg)

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -313,7 +313,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     const double turn_signal_intersection_angle_threshold_deg =
       planner_data_->parameters.turn_signal_intersection_angle_threshold_deg;
     const double turn_signal_search_time = planner_data_->parameters.turn_signal_search_time;
-    turn_signal_decider_.setParameters(
+    planner_data_->turn_signal_decider.setParameters(
       planner_data_->parameters.base_link2front, turn_signal_intersection_search_distance,
       turn_signal_search_time, turn_signal_intersection_angle_threshold_deg);
   }
@@ -1249,21 +1249,22 @@ void BehaviorPathPlannerNode::computeTurnSignal(
     turn_signal.command = TurnIndicatorsCommand::DISABLE;
     hazard_signal.command = output.turn_signal_info.hazard_signal.command;
   } else {
-    turn_signal = turn_signal_decider_.getTurnSignal(planner_data, path, output.turn_signal_info);
+    turn_signal = planner_data->getTurnSignal(path, output.turn_signal_info);
     hazard_signal.command = HazardLightsCommand::DISABLE;
   }
   turn_signal.stamp = get_clock()->now();
   hazard_signal.stamp = get_clock()->now();
   turn_signal_publisher_->publish(turn_signal);
   hazard_signal_publisher_->publish(hazard_signal);
 
-  publish_steering_factor(turn_signal);
+  publish_steering_factor(planner_data, turn_signal);
 }
 
-void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsCommand & turn_signal)
+void BehaviorPathPlannerNode::publish_steering_factor(
+  const std::shared_ptr<PlannerData> & planner_data, const TurnIndicatorsCommand & turn_signal)
 {
   const auto [intersection_flag, approaching_intersection_flag] =
-    turn_signal_decider_.getIntersectionTurnSignalFlag();
+    planner_data->turn_signal_decider.getIntersectionTurnSignalFlag();
   if (intersection_flag || approaching_intersection_flag) {
     const uint16_t steering_factor_direction = std::invoke([&turn_signal]() {
       if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
@@ -1273,7 +1274,7 @@ void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsComman
     });
 
     const auto [intersection_pose, intersection_distance] =
-      turn_signal_decider_.getIntersectionPoseAndDistance();
+      planner_data->turn_signal_decider.getIntersectionPoseAndDistance();
     const uint16_t steering_factor_state = std::invoke([&intersection_flag]() {
       if (intersection_flag) {
         return SteeringFactor::TURNING;

planning/behavior_path_planner/src/turn_signal_decider.cpp

@@ -39,34 +39,29 @@ double calc_distance(
 }
 
 TurnIndicatorsCommand TurnSignalDecider::getTurnSignal(
-  const std::shared_ptr<const PlannerData> & planner_data, const PathWithLaneId & path,
-  const TurnSignalInfo & turn_signal_info)
+  const std::shared_ptr<RouteHandler> & route_handler, const PathWithLaneId & path,
+  const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel,
+  const BehaviorPathPlannerParameters & parameters)
 {
   // Guard
   if (path.points.empty()) {
     return turn_signal_info.turn_signal;
   }
 
-  // Data
-  const double nearest_dist_threshold = planner_data->parameters.ego_nearest_dist_threshold;
-  const double nearest_yaw_threshold = planner_data->parameters.ego_nearest_yaw_threshold;
-  const auto & current_pose = planner_data->self_odometry->pose.pose;
-  const double & current_vel = planner_data->self_odometry->twist.twist.linear.x;
-  const auto route_handler = *(planner_data->route_handler);
-
   // Get current lanelets
-  const double forward_length = planner_data->parameters.forward_path_length;
+  const double forward_length = parameters.forward_path_length;
+  const double nearest_dist_threshold = parameters.ego_nearest_dist_threshold;
+  const double nearest_yaw_threshold = parameters.ego_nearest_yaw_threshold;
   const double backward_length = 50.0;
-  const lanelet::ConstLanelets current_lanes = utils::calcLaneAroundPose(
-    planner_data->route_handler, current_pose, forward_length, backward_length);
+  const lanelet::ConstLanelets current_lanes =
+    utils::calcLaneAroundPose(route_handler, current_pose, forward_length, backward_length);
 
   if (current_lanes.empty()) {
     return turn_signal_info.turn_signal;
   }
 
   const PathWithLaneId extended_path = utils::getCenterLinePath(
-    route_handler, current_lanes, current_pose, backward_length, forward_length,
-    planner_data->parameters);
+    *route_handler, current_lanes, current_pose, backward_length, forward_length, parameters);
 
   if (extended_path.points.empty()) {
     return turn_signal_info.turn_signal;
@@ -78,7 +73,7 @@ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal(
 
   // Get closest intersection turn signal if exists
   const auto intersection_turn_signal_info = getIntersectionTurnSignalInfo(
-    extended_path, current_pose, current_vel, ego_seg_idx, route_handler, nearest_dist_threshold,
+    extended_path, current_pose, current_vel, ego_seg_idx, *route_handler, nearest_dist_threshold,
     nearest_yaw_threshold);
 
   if (!intersection_turn_signal_info) {
@@ -333,6 +328,79 @@ TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal(
   return intersection_signal_info.turn_signal;
 }
 
+TurnSignalInfo TurnSignalDecider::use_prior_turn_signal(
+  const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
+  const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal,
+  const double nearest_dist_threshold, const double nearest_yaw_threshold)
+{
+  const auto get_distance = [&](const Pose & input_point) {
+    const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+      path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold);
+    return motion_utils::calcSignedArcLength(
+      path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx);
+  };
+
+  const auto & original_desired_start_point = original_signal.desired_start_point;
+  const auto & original_desired_end_point = original_signal.desired_end_point;
+  const auto & original_required_start_point = original_signal.required_start_point;
+  const auto & original_required_end_point = original_signal.required_end_point;
+  const auto & new_desired_start_point = new_signal.desired_start_point;
+  const auto & new_desired_end_point = new_signal.desired_end_point;
+  const auto & new_required_start_point = new_signal.required_start_point;
+  const auto & new_required_end_point = new_signal.required_end_point;
+
+  const double dist_to_original_desired_start =
+    get_distance(original_desired_start_point) - base_link2front_;
+  const double dist_to_original_desired_end = get_distance(original_desired_end_point);
+  const double dist_to_original_required_start =
+    get_distance(original_required_start_point) - base_link2front_;
+  const double dist_to_original_required_end = get_distance(original_required_end_point);
+  const double dist_to_new_desired_start = get_distance(new_desired_start_point) - base_link2front_;
+  const double dist_to_new_desired_end = get_distance(new_desired_end_point);
+  const double dist_to_new_required_start =
+    get_distance(new_required_start_point) - base_link2front_;
+  const double dist_to_new_required_end = get_distance(new_required_end_point);
+
+  // If we still do not reach the desired front point we ignore it
+  if (dist_to_original_desired_start > 0.0 && dist_to_new_desired_start > 0.0) {
+    TurnSignalInfo empty_signal_info;
+    return empty_signal_info;
+  } else if (dist_to_original_desired_start > 0.0) {
+    return new_signal;
+  } else if (dist_to_new_desired_start > 0.0) {
+    return original_signal;
+  }
+
+  // If we already passed the desired end point, return the other signal
+  if (dist_to_original_desired_end < 0.0 && dist_to_new_desired_end < 0.0) {
+    TurnSignalInfo empty_signal_info;
+    return empty_signal_info;
+  } else if (dist_to_original_desired_end < 0.0) {
+    return new_signal;
+  } else if (dist_to_new_desired_end < 0.0) {
+    return original_signal;
+  }
+
+  if (dist_to_original_desired_start <= dist_to_new_desired_start) {
+    const auto enable_prior = use_prior_turn_signal(
+      dist_to_original_required_start, dist_to_original_required_end, dist_to_new_required_start,
+      dist_to_new_required_end);
+
+    if (enable_prior) {
+      return original_signal;
+    }
+    return new_signal;
+  }
+
+  const auto enable_prior = use_prior_turn_signal(
+    dist_to_new_required_start, dist_to_new_required_end, dist_to_original_required_start,
+    dist_to_original_required_end);
+  if (enable_prior) {
+    return new_signal;
+  }
+  return original_signal;
+}
+
 bool TurnSignalDecider::use_prior_turn_signal(
   const double dist_to_prior_required_start, const double dist_to_prior_required_end,
   const double dist_to_subsequent_required_start, const double dist_to_subsequent_required_end)