From 94b681cdaa6f224df09c131b2614e0e94e09871d Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed, 28 Sep 2022 21:35:05 +0900 Subject: [PATCH] feat(behavior_path_planner): add new turn signal algorithm (#1964) * clean code Signed-off-by: yutaka * clean format Signed-off-by: yutaka * udpate Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * add test Signed-off-by: yutaka * add test * add test Signed-off-by: yutaka * fix(avoidance): use new turn signal algorithm Signed-off-by: satoshi-ota * fix(avoidance): fix desired_start_point position Signed-off-by: satoshi-ota * update Signed-off-by: yutaka * change policy Signed-off-by: yutaka * feat(behavior_path_planner): update pull_over for new blinker logic Signed-off-by: kosuke55 * feat(behavior_path_planner): update pull_out for new blinker logic * tmp: install flask via pip Signed-off-by: Takayuki Murooka * feat(lane_change): added lane change point * fix start_point and non backward driving turn signal Signed-off-by: kosuke55 * get 3 second during constructing lane change path Signed-off-by: Muhammad Zulfaqar Azmi * fix pull over desired start point Signed-off-by: kosuke55 * update Signed-off-by: yutaka * delete Signed-off-by: yutaka * Update Readme * Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Fumiya Watanabe * Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Fumiya Watanabe * fix format Signed-off-by: yutaka Signed-off-by: yutaka Signed-off-by: satoshi-ota Signed-off-by: kosuke55 Signed-off-by: Takayuki Murooka Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: satoshi-ota Co-authored-by: kosuke55 Co-authored-by: Takayuki Murooka Co-authored-by: Muhammad Zulfaqar Azmi Co-authored-by: Fumiya Watanabe --- planning/behavior_path_planner/CMakeLists.txt | 8 + planning/behavior_path_planner/README.md | 4 + .../behavior_path_planner_node.hpp | 2 +- .../lane_change/lane_change_module.hpp | 1 + .../lane_change/lane_change_path.hpp | 3 + .../scene_module/lane_change/util.hpp | 13 +- .../scene_module/pull_out/pull_out_module.hpp | 5 +- .../pull_over/pull_over_module.hpp | 5 +- .../scene_module/scene_module_interface.hpp | 18 +- .../turn_signal_decider.hpp | 53 ++- .../src/behavior_path_planner_node.cpp | 7 +- .../avoidance/avoidance_module.cpp | 45 +- .../lane_change/lane_change_module.cpp | 5 +- .../src/scene_module/lane_change/util.cpp | 50 ++- .../scene_module/pull_out/pull_out_module.cpp | 52 ++- .../pull_over/pull_over_module.cpp | 91 ++-- .../src/turn_signal_decider.cpp | 422 ++++++++++++++---- .../test/test_turn_signal.cpp | 400 +++++++++++++++++ 18 files changed, 967 insertions(+), 217 deletions(-) create mode 100644 planning/behavior_path_planner/test/test_turn_signal.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 05b4f1f9436c0..73a4bc7652408 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/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( diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 35ae0b892befe..ba8b01f27e1fb 100644 --- a/planning/behavior_path_planner/README.md +++ b/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. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 8aabe413c0c5b..e926e44cd53b8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/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); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index a331269871215..096b9d4cec3c0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/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 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp index 9d24060e60eb7..d603fc36f1d28 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp +++ b/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 @@ -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; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index 7d1664a48eec5..3928dca353b4a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -61,8 +61,10 @@ std::optional 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_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index cfbb63c02e073..41f49cf9dd43d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/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> pull_out_planners_; PullOutStatus status_; - std::vector backed_pose_candidates_; - PoseStamped backed_pose_; std::deque odometry_buffer_; std::unique_ptr last_route_received_time_; std::unique_ptr last_pull_out_start_update_time_; + std::unique_ptr last_approved_pose_; std::shared_ptr getCurrentPlanner() const; lanelet::ConstLanelets getCurrentLanes() const; @@ -103,7 +102,7 @@ class PullOutModule : public SceneModuleInterface std::shared_ptr lane_departure_checker_; // turn signal - TurnSignalInfo calcTurnSignalInfo(const Pose start_pose, const Pose end_pose) const; + TurnSignalInfo calcTurnSignalInfo() const; void incrementPathIndex(); PathWithLaneId getCurrentPath() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 36c10fde1e0e9..f85aa225d647c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/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 last_received_time_; std::unique_ptr last_approved_time_; + std::unique_ptr 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 getHazardInfo() const; - std::pair getTurnInfo() const; + TurnSignalInfo calcTurnSignalInfo() const; // debug void setDebugData(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index d7f47f5391d15..b8fdd5a01d736 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/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 +#include #include #include #include @@ -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::max()}; -}; - struct BehaviorModuleOutput { BehaviorModuleOutput() = default; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp index ff6f3aed118c2..e67c50a13587e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp @@ -18,27 +18,60 @@ #include #include +#include #include #include #include +#include #include +#include #include 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 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,9 +83,20 @@ class TurnSignalDecider std::pair getIntersectionPoseAndDistance(); private: - std::pair getIntersectionTurnSignal( + boost::optional 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")}; @@ -60,6 +104,7 @@ class TurnSignalDecider // data double intersection_search_distance_{0.0}; double base_link2front_{0.0}; + std::map desired_start_point_map_; mutable bool intersection_turn_signal_ = false; mutable bool approaching_intersection_turn_signal_ = false; mutable double intersection_distance_ = std::numeric_limits::max(); 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 1eb43822654c2..173521f051f1a 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/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(); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 92db1e8e42792..11d5661ccabc4 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/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 diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index fc1b99b05fa4d..1a31e6db4dd37 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/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 @@ -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; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 44a521734934b..056204ba5fdf6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -87,8 +87,10 @@ std::optional 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) { PathShifter path_shifter; path_shifter.setPath(target_lane_reference_path); @@ -147,6 +149,9 @@ std::optional constructCandidatePath( return std::nullopt; } + candidate_path.turn_signal_info = lane_change_utils::calc_turn_signal_info( + prepare_segment, prepare_speed, minimum_prepare_distance, prepare_duration, shift_point, + shifted_path); // check candidate path is in lanelet if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) { return std::nullopt; @@ -241,6 +246,7 @@ LaneChangePaths getLaneChangePaths( const auto candidate_path = constructCandidatePath( prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path, shift_point, original_lanelets, target_lanelets, acceleration, prepare_distance, + lane_change_prepare_duration, prepare_speed, minimum_lane_change_prepare_distance, lane_changing_distance, lane_changing_duration, minimum_lane_change_velocity); if (!candidate_path) { @@ -581,4 +587,44 @@ PathWithLaneId getLaneChangePathLaneChangingSegment( return lane_changing_segment; } +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) +{ + TurnSignalInfo turn_signal_info{}; + constexpr double turn_signal_start_duration{3.0}; + turn_signal_info.desired_start_point = + std::invoke([&prepare_path, &prepare_velocity, &min_prepare_distance, &prepare_duration]() { + if (prepare_velocity * turn_signal_start_duration > min_prepare_distance) { + const auto duration = static_cast(prepare_path.points.size()) / prepare_duration; + double time{-duration}; + for (auto itr = prepare_path.points.crbegin(); itr != prepare_path.points.crend(); ++itr) { + time += duration; + if (time >= turn_signal_start_duration) { + return itr->point.pose.position; + } + } + } + return prepare_path.points.front().point.pose.position; + }); + + turn_signal_info.required_start_point = shift_points.start.position; + turn_signal_info.required_end_point = std::invoke([&lane_changing_path]() { + const auto mid_path_idx = lane_changing_path.path.points.size() / 2; + return lane_changing_path.path.points.at(mid_path_idx).point.pose.position; + }); + turn_signal_info.desired_end_point = shift_points.end.position; + return turn_signal_info; +} + +void get_turn_signal_info( + const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info) +{ + turn_signal_info->desired_start_point = lane_change_path.turn_signal_info.desired_start_point; + turn_signal_info->required_start_point = lane_change_path.turn_signal_info.required_start_point; + turn_signal_info->required_end_point = lane_change_path.turn_signal_info.required_end_point; + turn_signal_info->desired_end_point = lane_change_path.turn_signal_info.desired_end_point; +} + } // namespace behavior_path_planner::lane_change_utils diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 901378416493c..05d0ffe532833 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -60,7 +60,6 @@ PullOutModule::PullOutModule( BehaviorModuleOutput PullOutModule::run() { - clearWaitingApproval(); current_state_ = BT::NodeStatus::RUNNING; return plan(); } @@ -155,6 +154,12 @@ BT::NodeStatus PullOutModule::updateState() BehaviorModuleOutput PullOutModule::plan() { + if (isWaitingApproval()) { + clearWaitingApproval(); + // save current_pose when approved for start_point of turn_signal for backward driving + last_approved_pose_ = std::make_unique(planner_data_->self_pose->pose); + } + BehaviorModuleOutput output; if (!status_.is_safe) { return output; @@ -175,8 +180,7 @@ BehaviorModuleOutput PullOutModule::plan() planner_data_->parameters.vehicle_length, planner_data_); output.path = std::make_shared(path); - output.turn_signal_info = - calcTurnSignalInfo(status_.pull_out_path.start_pose, status_.pull_out_path.end_pose); + output.turn_signal_info = calcTurnSignalInfo(); const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { @@ -256,6 +260,8 @@ PathWithLaneId PullOutModule::getFullPath() const BehaviorModuleOutput PullOutModule::planWaitingApproval() { + waitApproval(); + BehaviorModuleOutput output; const auto current_lanes = getCurrentLanes(); const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_); @@ -272,12 +278,9 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() } output.path = std::make_shared(stop_path); - output.turn_signal_info = - calcTurnSignalInfo(status_.pull_out_path.start_pose, status_.pull_out_path.end_pose); + output.turn_signal_info = calcTurnSignalInfo(); output.path_candidate = std::make_shared(candidate_path); - waitApproval(); - const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; @@ -603,35 +606,38 @@ bool PullOutModule::hasFinishedCurrentPath() return is_near_target && isStopped(); } -TurnSignalInfo PullOutModule::calcTurnSignalInfo(const Pose start_pose, const Pose end_pose) const +TurnSignalInfo PullOutModule::calcTurnSignalInfo() const { - TurnSignalInfo turn_signal; + TurnSignalInfo turn_signal{}; // output + const auto & current_pose = planner_data_->self_pose->pose; - // turn hazard light when backward driving + // turn on hazard light when backward driving if (!status_.back_finished) { turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; - turn_signal.signal_distance = - tier4_autoware_utils::calcDistance2d(start_pose, planner_data_->self_pose->pose); + const auto back_start_pose = isWaitingApproval() ? current_pose : *last_approved_pose_; + turn_signal.desired_start_point = back_start_pose.position; + turn_signal.required_start_point = back_start_pose.position; + // pull_out start_pose is same to backward driving end_pose + turn_signal.required_end_point = status_.pull_out_path.start_pose.position; + turn_signal.desired_end_point = status_.pull_out_path.start_pose.position; return turn_signal; } - // calculate distance to pull_out end on target lanes - const auto current_lanes = getCurrentLanes(); - const auto arc_position_current_pose = - lanelet::utils::getArcCoordinates(current_lanes, planner_data_->self_pose->pose); - const auto arc_position_pull_out_end = lanelet::utils::getArcCoordinates(current_lanes, end_pose); - const double distance_from_pull_out_end = - arc_position_current_pose.length - arc_position_pull_out_end.length; - // turn on right signal until passing pull_out end point - const double turn_signal_off_buffer = std::min(parameters_.pull_out_finish_judge_buffer, 3.0); - if (distance_from_pull_out_end < turn_signal_off_buffer) { + const auto path = getFullPath(); + // pull out path does not overlap + const double distance_from_end = motion_utils::calcSignedArcLength( + path.points, status_.pull_out_path.end_pose.position, current_pose.position); + if (distance_from_end < parameters_.pull_out_finish_judge_buffer) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; } else { turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; } - turn_signal.signal_distance = -distance_from_pull_out_end + turn_signal_off_buffer; + turn_signal.desired_start_point = status_.pull_out_path.start_pose.position; + turn_signal.required_start_point = status_.pull_out_path.start_pose.position; + turn_signal.required_end_point = status_.pull_out_path.end_pose.position; + turn_signal.desired_end_point = status_.pull_out_path.end_pose.position; return turn_signal; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 8d88a523c06f4..ea28e6c80a43c 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -427,6 +427,8 @@ bool PullOverModule::planWithCloseGoal() BehaviorModuleOutput PullOverModule::plan() { + const auto & current_pose = planner_data_->self_pose->pose; + status_.current_lanes = util::getExtendedCurrentLanes(planner_data_); status_.pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler)); status_.lanes = lanelet::ConstLanelets{}; @@ -438,8 +440,8 @@ BehaviorModuleOutput PullOverModule::plan() // Check if it needs to decide path if (status_.is_safe) { const auto dist_to_parking_start_pose = calcSignedArcLength( - getCurrentPath().points, planner_data_->self_pose->pose, - status_.pull_over_path.start_pose.position, std::numeric_limits::max(), M_PI_2); + getCurrentPath().points, current_pose, status_.pull_over_path.start_pose.position, + std::numeric_limits::max(), M_PI_2); if (*dist_to_parking_start_pose < parameters_.decide_path_distance) { status_.has_decided_path = true; @@ -460,6 +462,7 @@ BehaviorModuleOutput PullOverModule::plan() // When it is approved again after path is decided clearWaitingApproval(); last_approved_time_ = std::make_unique(clock_->now()); + last_approved_pose_ = std::make_unique(current_pose); // decide velocity to guarantee turn signal lighting time if (!status_.has_decided_velocity) { @@ -542,16 +545,7 @@ BehaviorModuleOutput PullOverModule::plan() // set hazard and turn signal if (status_.has_decided_path) { - const auto hazard_info = getHazardInfo(); - const auto turn_info = getTurnInfo(); - - if (hazard_info.first.command == HazardLightsCommand::ENABLE) { - output.turn_signal_info.hazard_signal.command = hazard_info.first.command; - output.turn_signal_info.signal_distance = hazard_info.second; - } else { - output.turn_signal_info.turn_signal.command = turn_info.first.command; - output.turn_signal_info.signal_distance = turn_info.second; - } + output.turn_signal_info = calcTurnSignalInfo(); } const auto distance_to_path_change = calcDistanceToPathChange(); @@ -568,9 +562,10 @@ BehaviorModuleOutput PullOverModule::plan() } const uint16_t steering_factor_direction = std::invoke([this]() { - if (getTurnInfo().first.command == TurnIndicatorsCommand::ENABLE_LEFT) { + const auto turn_signal = calcTurnSignalInfo(); + if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; - } else if (getTurnInfo().first.command == TurnIndicatorsCommand::ENABLE_RIGHT) { + } else if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { return SteeringFactor::RIGHT; } return SteeringFactor::STRAIGHT; @@ -608,9 +603,10 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); const uint16_t steering_factor_direction = std::invoke([this]() { - if (getTurnInfo().first.command == TurnIndicatorsCommand::ENABLE_LEFT) { + const auto turn_signal = calcTurnSignalInfo(); + if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; - } else if (getTurnInfo().first.command == TurnIndicatorsCommand::ENABLE_RIGHT) { + } else if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { return SteeringFactor::RIGHT; } return SteeringFactor::STRAIGHT; @@ -840,50 +836,37 @@ bool PullOverModule::hasFinishedPullOver() return car_is_on_goal && isStopped(); } -std::pair PullOverModule::getHazardInfo() const +TurnSignalInfo PullOverModule::calcTurnSignalInfo() const { - HazardLightsCommand hazard_signal{}; + TurnSignalInfo turn_signal{}; // output - const auto arc_position_goal_pose = - lanelet::utils::getArcCoordinates(status_.pull_over_lanes, modified_goal_pose_); - const auto arc_position_current_pose = - lanelet::utils::getArcCoordinates(status_.pull_over_lanes, planner_data_->self_pose->pose); - const double distance_to_goal = arc_position_goal_pose.length - arc_position_current_pose.length; + const auto & current_pose = planner_data_->self_pose->pose; + const auto & start_pose = status_.pull_over_path.start_pose; + const auto & end_pose = status_.pull_over_path.end_pose; + const auto & full_path = getFullPath(); - const double velocity = std::abs(planner_data_->self_odometry->twist.twist.linear.x); - if ( - (distance_to_goal < parameters_.hazard_on_threshold_distance && - velocity < parameters_.hazard_on_threshold_velocity) || - status_.planner->getPlannerType() == PullOverPlannerType::ARC_BACKWARD) { - hazard_signal.command = HazardLightsCommand::ENABLE; - const double distance_from_front_to_goal = - distance_to_goal - planner_data_->parameters.base_link2front; - return std::make_pair(hazard_signal, distance_from_front_to_goal); + // calc TurnIndicatorsCommand + { + const double distance_to_end = + calcSignedArcLength(full_path.points, current_pose.position, end_pose.position); + const bool is_before_end_pose = distance_to_end >= 0.0; + turn_signal.turn_signal.command = + is_before_end_pose ? TurnIndicatorsCommand::ENABLE_LEFT : TurnIndicatorsCommand::NO_COMMAND; } - return std::make_pair(hazard_signal, std::numeric_limits::max()); -} - -std::pair PullOverModule::getTurnInfo() const -{ - std::pair turn_info{}; - - const double distance_from_vehicle_front = std::invoke([&]() { - const auto arc_position_current_pose = - lanelet::utils::getArcCoordinates(status_.current_lanes, planner_data_->self_pose->pose); - const auto arc_position_end_pose = - lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_over_path.end_pose); - return arc_position_end_pose.length - arc_position_current_pose.length - - planner_data_->parameters.base_link2front; - }); + // calc desired/required start/end point + { + // ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds + // before starting pull_over + turn_signal.desired_start_point = last_approved_pose_ && status_.has_decided_path + ? last_approved_pose_->position + : current_pose.position; + turn_signal.desired_end_point = end_pose.position; + turn_signal.required_start_point = start_pose.position; + turn_signal.required_end_point = end_pose.position; + } - TurnIndicatorsCommand turn_signal{}; - const bool is_before_parking_end = distance_from_vehicle_front >= 0.0; - turn_signal.command = - is_before_parking_end ? TurnIndicatorsCommand::ENABLE_LEFT : TurnIndicatorsCommand::NO_COMMAND; - turn_info.first = turn_signal; - turn_info.second = distance_from_vehicle_front; - return turn_info; + return turn_signal; } void PullOverModule::setDebugData() diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index 9fbe0d5a5f997..f2e295cffbc52 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -16,6 +16,9 @@ #include "behavior_path_planner/utilities.hpp" +#include +#include +#include #include #include @@ -25,120 +28,355 @@ namespace behavior_path_planner { TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( - 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 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) { - auto turn_signal = turn_signal_plan; + // Guard + if (path.points.empty()) { + return turn_signal_info.turn_signal; + } - // If the distance to intersection is nearer than path change point, - // use turn signal for turning at the intersection - const auto intersection_result = - getIntersectionTurnSignal(path, current_pose, current_seg_idx, route_handler); - const auto intersection_turn_signal = intersection_result.first; - const auto intersection_distance = intersection_result.second; + // Get closest intersection turn signal if exists + const auto intersection_turn_signal_info = + getIntersectionTurnSignalInfo(path, current_pose, current_vel, current_seg_idx, route_handler); - if ( - intersection_distance < plan_distance || - turn_signal_plan.command == TurnIndicatorsCommand::NO_COMMAND || - turn_signal_plan.command == TurnIndicatorsCommand::DISABLE) { - turn_signal.command = intersection_turn_signal.command; + if (!intersection_turn_signal_info) { + initialize_intersection_info(); + return turn_signal_info.turn_signal; + } else if ( + turn_signal_info.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND || + turn_signal_info.turn_signal.command == TurnIndicatorsCommand::DISABLE) { + set_intersection_info(path, current_pose, current_seg_idx, *intersection_turn_signal_info); + return intersection_turn_signal_info->turn_signal; } - return turn_signal; + return resolve_turn_signal( + path, current_pose, current_seg_idx, *intersection_turn_signal_info, turn_signal_info); } -std::pair TurnSignalDecider::getIntersectionTurnSignal( - const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, - const RouteHandler & route_handler) const +std::pair TurnSignalDecider::getIntersectionTurnSignalFlag() { - TurnIndicatorsCommand turn_signal{}; - turn_signal.command = TurnIndicatorsCommand::DISABLE; - double distance = std::numeric_limits::max(); - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + return std::make_pair(intersection_turn_signal_, approaching_intersection_turn_signal_); +} - if (path.points.empty()) { - return std::make_pair(turn_signal, distance); - } - - // Get frenet coordinate of current_pose on path - const auto vehicle_pose_frenet = - util::convertToFrenetCoordinate3d(path, current_pose.position, current_seg_idx); - - // Get nearest intersection and decide turn signal - double accumulated_distance = 0; - - auto prev_point = path.points.front(); - auto lane_attribute = std::string("none"); - for (const auto & path_point : path.points) { - const double path_point_distance = - tier4_autoware_utils::calcDistance3d(prev_point.point, path_point.point); - accumulated_distance += path_point_distance; - prev_point = path_point; - const double distance_from_vehicle_front = - accumulated_distance - vehicle_pose_frenet.length - base_link2front_; - if (distance_from_vehicle_front > intersection_search_distance_) { - if (turn_signal.command == TurnIndicatorsCommand::DISABLE) { - distance = std::numeric_limits::max(); - approaching_intersection_turn_signal_ = false; - } else { - intersection_distance_ = distance; - approaching_intersection_turn_signal_ = true; +std::pair TurnSignalDecider::getIntersectionPoseAndDistance() +{ + return std::make_pair(intersection_pose_point_, intersection_distance_); +} + +boost::optional TurnSignalDecider::getIntersectionTurnSignalInfo( + const PathWithLaneId & path, const Pose & current_pose, const double current_vel, + const size_t current_seg_idx, const RouteHandler & route_handler) +{ + // search distance + const double search_distance = 3.0 * current_vel + intersection_search_distance_; + + // unique lane ids + std::vector unique_lane_ids; + for (size_t i = 0; i < path.points.size(); ++i) { + for (const auto & lane_id : path.points.at(i).lane_ids) { + if ( + std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) == + unique_lane_ids.end()) { + unique_lane_ids.push_back(lane_id); } - return std::make_pair(turn_signal, distance); } - // TODO(Horibe): Route Handler should be a library. - for (const auto & lane : route_handler.getLaneletsFromIds(path_point.lane_ids)) { - // judgement of lighting of turn_signal - bool lighting_turn_signal = false; - if (lane.attributeOr("turn_direction", std::string("none")) != lane_attribute) { - if ( - distance_from_vehicle_front >= 0.0 && - distance_from_vehicle_front < - lane.attributeOr("turn_signal_distance", intersection_search_distance_) && - path_point_distance > 0.0) { - lighting_turn_signal = true; - } - } else { - if ( - lane.hasAttribute("turn_direction") && - distance_from_vehicle_front < path_point_distance && distance_from_vehicle_front > 0) { - lighting_turn_signal = true; - } + } + + std::queue signal_queue; + for (const auto & lane_id : unique_lane_ids) { + const auto lane = route_handler.getLaneletsFromId(lane_id); + + // lane front and back point + const geometry_msgs::msg::Point lane_front_point = + lanelet::utils::conversion::toGeomMsgPt(lane.centerline3d().front()); + const geometry_msgs::msg::Point lane_terminal_point = + lanelet::utils::conversion::toGeomMsgPt(lane.centerline3d().back()); + + const size_t front_nearest_seg_idx = + motion_utils::findNearestSegmentIndex(path.points, lane_front_point); + const size_t terminal_nearest_seg_idx = + motion_utils::findNearestSegmentIndex(path.points, lane_terminal_point); + + // Distance from ego vehicle front pose to front point of the lane + const double dist_to_front_point = motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, + lane_front_point, front_nearest_seg_idx) - + base_link2front_; + + // Distance from ego vehicle base link to the terminal point of the lane + const double dist_to_terminal_point = motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, lane_terminal_point, + terminal_nearest_seg_idx); + + if (dist_to_terminal_point < 0.0) { + // Vehicle is already passed this lane + if (desired_start_point_map_.find(lane_id) != desired_start_point_map_.end()) { + desired_start_point_map_.erase(lane_id); } - lane_attribute = lane.attributeOr("turn_direction", std::string("none")); - - if (lighting_turn_signal) { - if (lane_attribute == std::string("left")) { - turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - distance = distance_from_vehicle_front; - } else if (lane_attribute == std::string("right")) { - turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - distance = distance_from_vehicle_front; - } - intersection_pose_point_ = path_point.point.pose; + continue; + } else if (search_distance < dist_to_front_point) { + break; + } + const std::string lane_attribute = lane.attributeOr("turn_direction", std::string("none")); + if ( + (lane_attribute == "right" || lane_attribute == "left") && + dist_to_front_point < lane.attributeOr("turn_signal_distance", search_distance)) { + // update map if necessary + if (desired_start_point_map_.find(lane_id) == desired_start_point_map_.end()) { + desired_start_point_map_.emplace(lane_id, current_pose.position); } + + TurnSignalInfo turn_signal_info{}; + turn_signal_info.desired_start_point = desired_start_point_map_.at(lane_id); + turn_signal_info.required_start_point = lane_front_point; + turn_signal_info.required_end_point = get_required_end_point(lane.centerline3d()); + turn_signal_info.desired_end_point = lane_terminal_point; + turn_signal_info.turn_signal.command = signal_map.at(lane_attribute); + signal_queue.push(turn_signal_info); } } - if (turn_signal.command == TurnIndicatorsCommand::DISABLE) { - distance = std::numeric_limits::max(); - intersection_turn_signal_ = false; - approaching_intersection_turn_signal_ = false; - } else { - intersection_turn_signal_ = true; + + // Resolve the conflict between several turn signal requirements + while (!signal_queue.empty()) { + if (signal_queue.size() == 1) { + return signal_queue.front(); + } + + const auto & turn_signal_info = signal_queue.front(); + const auto & required_end_point = turn_signal_info.required_end_point; + const size_t nearest_seg_idx = + motion_utils::findNearestSegmentIndex(path.points, required_end_point); + const double dist_to_end_point = motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, required_end_point, nearest_seg_idx); + + if (dist_to_end_point >= 0.0) { + // we haven't finished the current mandatory turn signal + return turn_signal_info; + } + + signal_queue.pop(); } - intersection_distance_ = distance; - return std::make_pair(turn_signal, distance); + + return {}; } -std::pair TurnSignalDecider::getIntersectionTurnSignalFlag() +TurnIndicatorsCommand TurnSignalDecider::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) { - return std::make_pair(intersection_turn_signal_, approaching_intersection_turn_signal_); + const auto get_distance = [&](const auto & input_point) { + const size_t nearest_seg_idx = motion_utils::findNearestSegmentIndex(path.points, input_point); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point, nearest_seg_idx); + }; + + const auto & inter_desired_start_point = intersection_signal_info.desired_start_point; + const auto & inter_desired_end_point = intersection_signal_info.desired_end_point; + const auto & inter_required_start_point = intersection_signal_info.required_start_point; + const auto & inter_required_end_point = intersection_signal_info.required_end_point; + const auto & behavior_desired_start_point = behavior_signal_info.desired_start_point; + const auto & behavior_desired_end_point = behavior_signal_info.desired_end_point; + const auto & behavior_required_start_point = behavior_signal_info.required_start_point; + const auto & behavior_required_end_point = behavior_signal_info.required_end_point; + + const double dist_to_intersection_desired_start = + get_distance(inter_desired_start_point) - base_link2front_; + const double dist_to_intersection_desired_end = get_distance(inter_desired_end_point); + const double dist_to_intersection_required_start = + get_distance(inter_required_start_point) - base_link2front_; + const double dist_to_intersection_required_end = get_distance(inter_required_end_point); + const double dist_to_behavior_desired_start = + get_distance(behavior_desired_start_point) - base_link2front_; + const double dist_to_behavior_desired_end = get_distance(behavior_desired_end_point); + const double dist_to_behavior_required_start = + get_distance(behavior_required_start_point) - base_link2front_; + const double dist_to_behavior_required_end = get_distance(behavior_required_end_point); + + // If we still do not reach the desired front point we ignore it + if (dist_to_intersection_desired_start > 0.0 && dist_to_behavior_desired_start > 0.0) { + TurnIndicatorsCommand empty_signal_command; + empty_signal_command.command = TurnIndicatorsCommand::DISABLE; + initialize_intersection_info(); + return empty_signal_command; + } else if (dist_to_intersection_desired_start > 0.0) { + initialize_intersection_info(); + return behavior_signal_info.turn_signal; + } else if (dist_to_behavior_desired_start > 0.0) { + set_intersection_info(path, current_pose, current_seg_idx, intersection_signal_info); + return intersection_signal_info.turn_signal; + } + + // If we already passed the desired end point, return the other signal + if (dist_to_intersection_desired_end < 0.0 && dist_to_behavior_desired_end < 0.0) { + TurnIndicatorsCommand empty_signal_command; + empty_signal_command.command = TurnIndicatorsCommand::DISABLE; + initialize_intersection_info(); + return empty_signal_command; + } else if (dist_to_intersection_desired_end < 0.0) { + initialize_intersection_info(); + return behavior_signal_info.turn_signal; + } else if (dist_to_behavior_desired_end < 0.0) { + set_intersection_info(path, current_pose, current_seg_idx, intersection_signal_info); + return intersection_signal_info.turn_signal; + } + + if (dist_to_intersection_desired_start <= dist_to_behavior_desired_start) { + // intersection signal is prior than behavior signal + const auto enable_prior = use_prior_turn_signal( + dist_to_intersection_required_start, dist_to_intersection_required_end, + dist_to_behavior_required_start, dist_to_behavior_required_end); + + if (enable_prior) { + set_intersection_info(path, current_pose, current_seg_idx, intersection_signal_info); + return intersection_signal_info.turn_signal; + } + initialize_intersection_info(); + return behavior_signal_info.turn_signal; + } + + // behavior signal is prior than intersection signal + const auto enable_prior = use_prior_turn_signal( + dist_to_behavior_required_start, dist_to_behavior_required_end, + dist_to_intersection_required_start, dist_to_intersection_required_end); + if (enable_prior) { + initialize_intersection_info(); + return behavior_signal_info.turn_signal; + } + set_intersection_info(path, current_pose, current_seg_idx, intersection_signal_info); + return intersection_signal_info.turn_signal; } -std::pair TurnSignalDecider::getIntersectionPoseAndDistance() +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) { - return std::make_pair(intersection_pose_point_, intersection_distance_); + const bool before_prior_required = dist_to_prior_required_start > 0.0; + const bool before_subsequent_required = dist_to_subsequent_required_start > 0.0; + const bool inside_prior_required = + dist_to_prior_required_start < 0.0 && 0.0 <= dist_to_prior_required_end; + + if (dist_to_prior_required_start < dist_to_subsequent_required_start) { + // subsequent signal required section is completely overlapped the prior signal required section + if (dist_to_subsequent_required_end < dist_to_prior_required_end) { + return true; + } + + // Vehicle is inside or in front of the prior required section + if (before_prior_required || inside_prior_required) { + return true; + } + + // passed prior required section but in front of the subsequent required section + if (before_subsequent_required) { + return true; + } + + // within or passed subsequent required section and completely passed prior required section + return false; + } + + // Subsequent required section starts faster than prior required starts section + + // If the prior section is inside of the subsequent required section + if (dist_to_prior_required_end < dist_to_subsequent_required_end) { + if (before_prior_required || inside_prior_required) { + return true; + } + return false; + } + + // inside or passed the intersection required + if (before_prior_required) { + return false; + } + + return true; } +geometry_msgs::msg::Point TurnSignalDecider::get_required_end_point( + const lanelet::ConstLineString3d & centerline) +{ + std::vector converted_centerline(centerline.size()); + for (size_t i = 0; i < centerline.size(); ++i) { + converted_centerline.at(i).position = lanelet::utils::conversion::toGeomMsgPt(centerline[i]); + } + motion_utils::insertOrientation(converted_centerline, true); + + const double length = motion_utils::calcArcLength(converted_centerline); + + // Create resampling intervals + const double resampling_interval = 1.0; + std::vector resampling_arclength; + for (double s = 0.0; s < length; s += resampling_interval) { + resampling_arclength.push_back(s); + } + + // Insert terminal point + if (length - resampling_arclength.back() < motion_utils::overlap_threshold) { + resampling_arclength.back() = length; + } else { + resampling_arclength.push_back(length); + } + + const auto resampled_centerline = + motion_utils::resamplePath(converted_centerline, resampling_arclength); + + const double terminal_yaw = tf2::getYaw(resampled_centerline.back().orientation); + for (size_t i = 0; i < resampled_centerline.size(); ++i) { + const double yaw = tf2::getYaw(resampled_centerline.at(i).orientation); + const double yaw_diff = tier4_autoware_utils::normalizeRadian(yaw - terminal_yaw); + if (std::fabs(yaw_diff) < tier4_autoware_utils::deg2rad(15)) { + return resampled_centerline.at(i).position; + } + } + + return resampled_centerline.back().position; +} + +void TurnSignalDecider::set_intersection_info( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & intersection_turn_signal_info) +{ + const auto get_distance = [&](const auto & input_point) { + const size_t nearest_seg_idx = motion_utils::findNearestSegmentIndex(path.points, input_point); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point, nearest_seg_idx); + }; + + const auto & inter_desired_start_point = intersection_turn_signal_info.desired_start_point; + const auto & inter_desired_end_point = intersection_turn_signal_info.desired_end_point; + const auto & inter_required_start_point = intersection_turn_signal_info.required_start_point; + + const double dist_to_intersection_desired_start = + get_distance(inter_desired_start_point) - base_link2front_; + const double dist_to_intersection_desired_end = get_distance(inter_desired_end_point); + const double dist_to_intersection_required_start = + get_distance(inter_required_start_point) - base_link2front_; + + if (dist_to_intersection_desired_start < 0.0 && dist_to_intersection_desired_end > 0.0) { + if (dist_to_intersection_required_start > 0.0) { + intersection_turn_signal_ = false; + approaching_intersection_turn_signal_ = true; + } else { + intersection_turn_signal_ = true; + approaching_intersection_turn_signal_ = false; + } + intersection_distance_ = dist_to_intersection_required_start; + const size_t required_start_nearest_seg_idx = + motion_utils::findNearestSegmentIndex(path.points, inter_required_start_point); + intersection_pose_point_ = path.points.at(required_start_nearest_seg_idx).point.pose; + } + + initialize_intersection_info(); +} + +void TurnSignalDecider::initialize_intersection_info() +{ + intersection_turn_signal_ = false; + approaching_intersection_turn_signal_ = false; + intersection_pose_point_ = Pose(); + intersection_distance_ = std::numeric_limits::max(); +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/test/test_turn_signal.cpp b/planning/behavior_path_planner/test/test_turn_signal.cpp new file mode 100644 index 0000000000000..7d9630bfe0358 --- /dev/null +++ b/planning/behavior_path_planner/test/test_turn_signal.cpp @@ -0,0 +1,400 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "behavior_path_planner/turn_signal_decider.hpp" + +#include "autoware_auto_planning_msgs/msg/path_point.hpp" + +#include +#include + +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; +using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using behavior_path_planner::PathWithLaneId; +using behavior_path_planner::Pose; +using behavior_path_planner::TurnSignalDecider; +using behavior_path_planner::TurnSignalInfo; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Twist; +using tier4_autoware_utils::createPoint; + +namespace +{ +PathWithLaneId generateStraightSamplePathWithLaneId( + float initial_pose_value, float pose_increment, size_t point_sample) +{ + PathWithLaneId path; + for (size_t idx = 0; idx < point_sample; ++idx) { + PathPoint point; + point.pose.position.x = std::exchange(initial_pose_value, initial_pose_value + pose_increment); + point.pose.position.y = 0.0; + point.pose.position.z = 0.0; + point.longitudinal_velocity_mps = 0.1; // [m/s] + point.heading_rate_rps = 0.0; // [rad/s] + point.is_final = (idx == point_sample - 1); + + PathPointWithLaneId path_point_with_lane_id; + path_point_with_lane_id.point = point; + path_point_with_lane_id.lane_ids = {}; + + path.header.frame_id = "map"; + path.points.push_back(path_point_with_lane_id); + } + + return path; +} + +Pose generateEgoSamplePose(float && p_x, float && p_y, float && p_z) +{ + Pose pose; + pose.position.x = p_x; + pose.position.y = p_y; + pose.position.z = p_z; + return pose; +} +} // namespace + +TEST(BehaviorPathPlanningTurnSignal, Condition1) +{ + PathWithLaneId path = generateStraightSamplePathWithLaneId(0.0f, 1.0f, 70u); + TurnSignalDecider turn_signal_decider; + turn_signal_decider.setParameters(1.0, 30.0); + + TurnSignalInfo intersection_signal_info; + intersection_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + intersection_signal_info.desired_start_point = createPoint(0.0, 0.0, 0.0); + intersection_signal_info.desired_end_point = createPoint(65.0, 0.0, 0.0); + intersection_signal_info.required_start_point = createPoint(35.0, 0.0, 0.0); + intersection_signal_info.required_end_point = createPoint(48.0, 0.0, 0.0); + + TurnSignalInfo behavior_signal_info; + behavior_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + behavior_signal_info.desired_start_point = createPoint(5.0, 0.0, 0.0); + behavior_signal_info.desired_end_point = createPoint(70.0, 0.0, 0.0); + behavior_signal_info.required_start_point = createPoint(45.0, 0.0, 0.0); + behavior_signal_info.required_end_point = createPoint(50.0, 0.0, 0.0); + + // current pose on the behavior desired start + { + Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right before the intersection required start + { + Pose current_pose = generateEgoSamplePose(34.99f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is within the intersection required section + { + Pose current_pose = generateEgoSamplePose(40.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is within the intersection and behavior required section + { + Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is on the intersection required end + { + Pose current_pose = generateEgoSamplePose(48.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right after the intersection required end + { + Pose current_pose = generateEgoSamplePose(48.1f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is on the behavior required end + { + Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right after the behavior required end + { + Pose current_pose = generateEgoSamplePose(50.1f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right on the intersection desired end + { + Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right right after the intersection desired end + { + Pose current_pose = generateEgoSamplePose(65.1f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } +} + +TEST(BehaviorPathPlanningTurnSignal, Condition2) +{ + PathWithLaneId path = generateStraightSamplePathWithLaneId(0.0f, 1.0f, 70u); + TurnSignalDecider turn_signal_decider; + turn_signal_decider.setParameters(1.0, 30.0); + + TurnSignalInfo intersection_signal_info; + intersection_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + intersection_signal_info.desired_start_point = createPoint(0.0, 0.0, 0.0); + intersection_signal_info.desired_end_point = createPoint(65.0, 0.0, 0.0); + intersection_signal_info.required_start_point = createPoint(35.0, 0.0, 0.0); + intersection_signal_info.required_end_point = createPoint(50.0, 0.0, 0.0); + + TurnSignalInfo behavior_signal_info; + behavior_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + behavior_signal_info.desired_start_point = createPoint(5.0, 0.0, 0.0); + behavior_signal_info.desired_end_point = createPoint(70.0, 0.0, 0.0); + behavior_signal_info.required_start_point = createPoint(40.0, 0.0, 0.0); + behavior_signal_info.required_end_point = createPoint(45.0, 0.0, 0.0); + + // current pose on the behavior desired start + { + Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right before the intersection required start + { + Pose current_pose = generateEgoSamplePose(34.99f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is on the behavior required start + { + Pose current_pose = generateEgoSamplePose(40.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is on the behavior required end + { + Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is on the intersection required end + { + Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right after the intersection required end + { + Pose current_pose = generateEgoSamplePose(50.1f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is on the intersection desired end + { + Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } +} + +TEST(BehaviorPathPlanningTurnSignal, Condition3) +{ + PathWithLaneId path = generateStraightSamplePathWithLaneId(0.0f, 1.0f, 70u); + TurnSignalDecider turn_signal_decider; + turn_signal_decider.setParameters(1.0, 30.0); + + TurnSignalInfo intersection_signal_info; + intersection_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + intersection_signal_info.desired_start_point = createPoint(0.0, 0.0, 0.0); + intersection_signal_info.desired_end_point = createPoint(65.0, 0.0, 0.0); + intersection_signal_info.required_start_point = createPoint(35.0, 0.0, 0.0); + intersection_signal_info.required_end_point = createPoint(50.0, 0.0, 0.0); + + TurnSignalInfo behavior_signal_info; + behavior_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + behavior_signal_info.desired_start_point = createPoint(5.0, 0.0, 0.0); + behavior_signal_info.desired_end_point = createPoint(70.0, 0.0, 0.0); + behavior_signal_info.required_start_point = createPoint(30.0, 0.0, 0.0); + behavior_signal_info.required_end_point = createPoint(45.0, 0.0, 0.0); + + // current pose on the behavior desired start + { + Pose current_pose = generateEgoSamplePose(5.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right before the behavior required start + { + Pose current_pose = generateEgoSamplePose(29.9f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right on the behavior required start + { + Pose current_pose = generateEgoSamplePose(30.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right before the intersection required start + { + Pose current_pose = generateEgoSamplePose(33.9f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_RIGHT); + } + + // current pose is right on the intersection required start + { + Pose current_pose = generateEgoSamplePose(35.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right before the behavior required end + { + Pose current_pose = generateEgoSamplePose(44.9f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right on the behavior required end + { + Pose current_pose = generateEgoSamplePose(45.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right before the intersection required end + { + Pose current_pose = generateEgoSamplePose(49.9f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right on the intersection required end + { + Pose current_pose = generateEgoSamplePose(50.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } + + // current pose is right on the intersection desired end + { + Pose current_pose = generateEgoSamplePose(65.0f, 0.0f, 0.0); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, 3.0, 1.0); + const auto result_signal = turn_signal_decider.resolve_turn_signal( + path, current_pose, current_seg_idx, intersection_signal_info, behavior_signal_info); + EXPECT_EQ(result_signal.command, TurnIndicatorsCommand::ENABLE_LEFT); + } +}