feat(behavior_path_planner): add new turn signal algorithm (autowaref…

…oundation#1964)

* clean code

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

* clean format

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

* udpate

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

* update

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

* update

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

* update

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

* update

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

* add test

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

* add test

* add test

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

* fix(avoidance): use new turn signal algorithm

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

* fix(avoidance): fix desired_start_point position

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

* update

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

* change policy

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

* feat(behavior_path_planner): update pull_over for new blinker logic

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* feat(behavior_path_planner): update pull_out for new blinker logic

* tmp: install flask via pip

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* feat(lane_change): added lane change point

* fix start_point and non backward driving turn signal

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* get 3 second during constructing lane change path

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix pull over desired start point

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* update

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

* delete

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

* Update Readme

* Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

* Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix format

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

Signed-off-by: yutaka <purewater0901@gmail.com>
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: satoshi-ota <satoshi.ota928@gmail.com>
Co-authored-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

planning/behavior_path_planner/CMakeLists.txt

@@ -67,6 +67,14 @@ if(BUILD_TESTING)
     behavior_path_planner_node
   )
 
+  ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_turn_signal
+    test/test_turn_signal.cpp
+  )
+
+  target_link_libraries(test_${CMAKE_PROJECT_NAME}_turn_signal
+    behavior_path_planner_node
+  )
+
 endif()
 
 ament_auto_package(

planning/behavior_path_planner/README.md

@@ -517,6 +517,10 @@ Turn on signal when the planned path crosses lanes or when a right or left turn
 
 When multiple turn signal conditions are met, the turn signal with the smaller distance is selected.
 
+#### Limitation
+
+Currently, the algorithm of turn signal has been chagned, and the document of the turn signal will be available soon.
+
 ## References / External links
 
 This module depends on the external [BehaviorTreeCpp](https://github.com/BehaviorTree/BehaviorTree.CPP) library.

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -175,7 +175,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   bool isForcedCandidatePath() const;
 
   /**
-   * @brief publish the steering factor from intersection
+   * @brief publish steering factor from intersection
    */
   void publish_steering_factor(const TurnIndicatorsCommand & turn_signal);
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp

@@ -19,6 +19,7 @@
 #include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+#include "behavior_path_planner/turn_signal_decider.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp

@@ -16,6 +16,7 @@
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_PATH_HPP_
 
 #include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
+#include "behavior_path_planner/turn_signal_decider.hpp"
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
@@ -24,6 +25,7 @@
 namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using behavior_path_planner::TurnSignalInfo;
 struct LaneChangePath
 {
   PathWithLaneId path;
@@ -32,6 +34,7 @@ struct LaneChangePath
   double acceleration{0.0};
   double preparation_length{0.0};
   double lane_change_length{0.0};
+  TurnSignalInfo turn_signal_info;
 };
 using LaneChangePaths = std::vector<LaneChangePath>;
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp

@@ -61,8 +61,10 @@ std::optional<LaneChangePath> constructCandidatePath(
   const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment,
   const PathWithLaneId & target_lane_reference_path, const ShiftPoint & shift_point,
   const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
-  const double & acceleration, const double & prepare_distance, const double & lane_change_distance,
-  const double & lane_changing_duration, const double & minimum_lane_change_velocity);
+  const double & acceleration, const double & prepare_distance, const double & prepare_duration,
+  const double & prepare_speed, const double & minimum_prepare_distance,
+  const double & lane_change_distance, const double & lane_changing_duration,
+  const double & minimum_lane_change_velocity);
 
 LaneChangePaths getLaneChangePaths(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
@@ -126,6 +128,13 @@ PathWithLaneId getLaneChangePathLaneChangingSegment(
   const double & lane_change_distance_buffer, const double & lane_changing_duration,
   const double & minimum_lane_change_velocity);
 
+TurnSignalInfo calc_turn_signal_info(
+  const PathWithLaneId & prepare_path, const double prepare_velocity,
+  const double min_prepare_distance, const double prepare_duration, const ShiftPoint & shift_points,
+  const ShiftedPath & lane_changing_path);
+
+void get_turn_signal_info(
+  const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info);
 }  // namespace behavior_path_planner::lane_change_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp

@@ -87,12 +87,11 @@ class PullOutModule : public SceneModuleInterface
   std::vector<std::shared_ptr<PullOutPlannerBase>> pull_out_planners_;
   PullOutStatus status_;
 
-  std::vector<Pose> backed_pose_candidates_;
-  PoseStamped backed_pose_;
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;
 
   std::unique_ptr<rclcpp::Time> last_route_received_time_;
   std::unique_ptr<rclcpp::Time> last_pull_out_start_update_time_;
+  std::unique_ptr<Pose> last_approved_pose_;
 
   std::shared_ptr<PullOutPlannerBase> getCurrentPlanner() const;
   lanelet::ConstLanelets getCurrentLanes() const;
@@ -103,7 +102,7 @@ class PullOutModule : public SceneModuleInterface
   std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
 
   // turn signal
-  TurnSignalInfo calcTurnSignalInfo(const Pose start_pose, const Pose end_pose) const;
+  TurnSignalInfo calcTurnSignalInfo() const;
 
   void incrementPathIndex();
   PathWithLaneId getCurrentPath() const;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp

@@ -125,6 +125,7 @@ class PullOverModule : public SceneModuleInterface
   tier4_autoware_utils::LinearRing2d vehicle_footprint_;
   std::unique_ptr<rclcpp::Time> last_received_time_;
   std::unique_ptr<rclcpp::Time> last_approved_time_;
+  std::unique_ptr<Pose> last_approved_pose_;
 
   void incrementPathIndex();
   PathWithLaneId getCurrentPath() const;
@@ -150,9 +151,7 @@ class PullOverModule : public SceneModuleInterface
     const Pose & ego_pose, const PredictedObjects & dynamic_objects) const;
   bool checkCollisionWithPose(const Pose & pose) const;
 
-  // turn signal
-  std::pair<HazardLightsCommand, double> getHazardInfo() const;
-  std::pair<TurnIndicatorsCommand, double> getTurnInfo() const;
+  TurnSignalInfo calcTurnSignalInfo() const;
 
   // debug
   void setDebugData();

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -19,6 +19,7 @@
 #include "behavior_path_planner/utilities.hpp"
 
 #include <behavior_path_planner/steering_factor_interface.hpp>
+#include <behavior_path_planner/turn_signal_decider.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <route_handler/route_handler.hpp>
 #include <rtc_interface/rtc_interface.hpp>
@@ -53,23 +54,6 @@ using unique_identifier_msgs::msg::UUID;
 using visualization_msgs::msg::MarkerArray;
 using PlanResult = PathWithLaneId::SharedPtr;
 
-struct TurnSignalInfo
-{
-  TurnSignalInfo()
-  {
-    turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
-    hazard_signal.command = HazardLightsCommand::NO_COMMAND;
-  }
-
-  // desired turn signal
-  TurnIndicatorsCommand turn_signal;
-  HazardLightsCommand hazard_signal;
-
-  // TODO(Horibe) replace with point. Distance should be calculates in turn_signal_decider.
-  // distance to the turn signal trigger point (to choose nearest signal for multiple requests)
-  double signal_distance{std::numeric_limits<double>::max()};
-};
-
 struct BehaviorModuleOutput
 {
   BehaviorModuleOutput() = default;

planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp

@@ -18,27 +18,60 @@
 #include <route_handler/route_handler.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 
 #include <limits>
+#include <map>
 #include <memory>
+#include <string>
 #include <utility>
 
 namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
 using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
 using geometry_msgs::msg::Pose;
 using route_handler::RouteHandler;
+
+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::Point desired_start_point;
+  geometry_msgs::msg::Point desired_end_point;
+  geometry_msgs::msg::Point required_start_point;
+  geometry_msgs::msg::Point required_end_point;
+};
+
+const std::map<std::string, uint8_t> signal_map = {
+  {"left", TurnIndicatorsCommand::ENABLE_LEFT},
+  {"right", TurnIndicatorsCommand::ENABLE_RIGHT},
+  {"straight", TurnIndicatorsCommand::DISABLE},
+  {"none", TurnIndicatorsCommand::DISABLE}};
+
 class TurnSignalDecider
 {
 public:
   TurnIndicatorsCommand getTurnSignal(
+    const PathWithLaneId & path, const Pose & current_pose, const double current_vel,
+    const size_t current_seg_idx, const RouteHandler & route_handler,
+    const TurnSignalInfo & turn_signal_info);
+
+  TurnIndicatorsCommand resolve_turn_signal(
     const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
-    const RouteHandler & route_handler, const TurnIndicatorsCommand & turn_signal_plan,
-    const double plan_distance) const;
+    const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info);
 
   void setParameters(const double base_link2front, const double intersection_search_distance)
   {
@@ -50,16 +83,28 @@ class TurnSignalDecider
   std::pair<Pose, double> getIntersectionPoseAndDistance();
 
 private:
-  std::pair<TurnIndicatorsCommand, double> getIntersectionTurnSignal(
+  boost::optional<TurnSignalInfo> getIntersectionTurnSignalInfo(
+    const PathWithLaneId & path, const Pose & current_pose, const double current_vel,
+    const size_t current_seg_idx, const RouteHandler & route_handler);
+
+  geometry_msgs::msg::Point get_required_end_point(const lanelet::ConstLineString3d & centerline);
+
+  bool 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);
+
+  void set_intersection_info(
     const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
-    const RouteHandler & route_handler) const;
+    const TurnSignalInfo & intersection_turn_signal_info);
+  void initialize_intersection_info();
 
   rclcpp::Logger logger_{
     rclcpp::get_logger("behavior_path_planner").get_child("turn_signal_decider")};
 
   // data
   double intersection_search_distance_{0.0};
   double base_link2front_{0.0};
+  std::map<lanelet::Id, geometry_msgs::msg::Point> desired_start_point_map_;
   mutable bool intersection_turn_signal_ = false;
   mutable bool approaching_intersection_turn_signal_ = false;
   mutable double intersection_distance_ = std::numeric_limits<double>::max();

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -606,10 +606,13 @@ void BehaviorPathPlannerNode::run()
       turn_signal.command = TurnIndicatorsCommand::DISABLE;
       hazard_signal.command = output.turn_signal_info.hazard_signal.command;
     } else {
+      const auto & current_pose = planner_data->self_pose->pose;
+      const double & current_vel = planner_data->self_odometry->twist.twist.linear.x;
       const size_t ego_seg_idx = findEgoSegmentIndex(path->points);
+
       turn_signal = turn_signal_decider_.getTurnSignal(
-        *path, planner_data->self_pose->pose, ego_seg_idx, *(planner_data->route_handler),
-        output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance);
+        *path, current_pose, current_vel, ego_seg_idx, *(planner_data->route_handler),
+        output.turn_signal_info);
       hazard_signal.command = HazardLightsCommand::DISABLE;
     }
     turn_signal.stamp = get_clock()->now();

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2546,25 +2546,44 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con
     return {};
   }
 
-  const auto latest_shift_point = shift_points.front();  // assuming it is sorted.
+  const auto getRelativeLength = [this](const ShiftPoint & sp) {
+    const auto current_shift = getCurrentShift();
+    return sp.length - current_shift;
+  };
 
-  const auto turn_info = util::getPathTurnSignal(
-    avoidance_data_.current_lanelets, path, latest_shift_point, planner_data_->self_pose->pose,
-    planner_data_->self_odometry->twist.twist.linear.x, planner_data_->parameters);
+  const auto front_shift_point = shift_points.front();
 
-  // Set turn signal if the vehicle across the lane.
-  if (!path.shift_length.empty()) {
-    if (isAvoidancePlanRunning()) {
-      turn_signal.turn_signal.command = turn_info.first.command;
-    }
+  TurnSignalInfo turn_signal_info{};
+
+  if (std::abs(getRelativeLength(front_shift_point)) < 0.1) {
+    return turn_signal_info;
   }
 
-  // calc distance from ego to latest_shift_point end point.
-  if (turn_info.second >= 0.0) {
-    turn_signal.signal_distance = turn_info.second;
+  const auto signal_prepare_distance = std::max(getEgoSpeed() * 3.0, 10.0);
+  const auto ego_to_shift_start =
+    calcSignedArcLength(path.path.points, getEgoPosition(), front_shift_point.start.position);
+
+  if (signal_prepare_distance < ego_to_shift_start) {
+    return turn_signal_info;
   }
 
-  return turn_signal;
+  if (getRelativeLength(front_shift_point) > 0.0) {
+    turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
+  } else {
+    turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
+  }
+
+  if (ego_to_shift_start > 0.0) {
+    turn_signal_info.desired_start_point = getEgoPosition();
+  } else {
+    turn_signal_info.desired_start_point = front_shift_point.start.position;
+  }
+
+  turn_signal_info.desired_end_point = front_shift_point.end.position;
+  turn_signal_info.required_start_point = front_shift_point.start.position;
+  turn_signal_info.required_end_point = front_shift_point.end.position;
+
+  return turn_signal_info;
 }
 
 double AvoidanceModule::getCurrentShift() const

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -17,6 +17,7 @@
 #include "behavior_path_planner/path_utilities.hpp"
 #include "behavior_path_planner/scene_module/lane_change/util.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+#include "behavior_path_planner/turn_signal_decider.hpp"
 #include "behavior_path_planner/utilities.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
@@ -190,7 +191,9 @@ BehaviorModuleOutput LaneChangeModule::plan()
     status_.lane_change_path.shift_point, planner_data_->self_pose->pose,
     planner_data_->self_odometry->twist.twist.linear.x, planner_data_->parameters);
   output.turn_signal_info.turn_signal.command = turn_signal_info.first.command;
-  output.turn_signal_info.signal_distance = turn_signal_info.second;
+
+  lane_change_utils::get_turn_signal_info(status_.lane_change_path, &output.turn_signal_info);
+  // output.turn_signal_info.signal_distance = turn_signal_info.second;
   return output;
 }