From 103651dc9f0a6760e3f2b146bdc1504170b0f0e2 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 15 Mar 2024 22:42:03 +0900 Subject: [PATCH 1/8] feat(behavior_path_planner_common): add general method for calculating turn signal for bpp modules (#6625) add general method for calculating turn signal for bpp modules Signed-off-by: Daniel Sanchez --- ...ehavior_path_planner_turn_signal_design.md | 74 ++++----- .../data_manager.hpp | 60 ++++++- .../turn_signal_decider.hpp | 146 +++++++++++++++++ .../src/turn_signal_decider.cpp | 148 ++++++++++++++++++ 4 files changed, 390 insertions(+), 38 deletions(-) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md index 8b64db997aec6..d3f72c03c6e54 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md @@ -4,7 +4,7 @@ Turn Signal decider determines necessary blinkers. ## Purpose / Role -This module is responsible for activating a necessary blinker during driving. It uses rule-based algorithm to determine blinkers, and the details of this algorithm are described in the following sections. Note that this algorithm is strictly based on the Japanese Road Traffic Raw. +This module is responsible for activating a necessary blinker during driving. It uses rule-based algorithm to determine blinkers, and the details of this algorithm are described in the following sections. Note that this algorithm is strictly based on the Japanese Road Traffic Law. ### Assumptions @@ -16,7 +16,7 @@ Autoware has following order of priorities for turn signals. ### Limitations -Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in a complicated situations. This is because it tries to follow the road traffic raw and cannot solve `blinker conflicts` clearly in that environment. +Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in complicated situations. This is because it tries to follow the road traffic law and cannot solve `blinker conflicts` clearly in that environment. ## Parameters for turn signal decider @@ -59,16 +59,16 @@ For left turn, right turn, avoidance, lane change, goal planner and pull out, we Turn signal decider checks each lanelet on the map if it has `turn_direction` information. If a lanelet has this information, it activates necessary blinker based on this information. -- desired start point - The `search_distance` for blinkers at intersections is `v * turn_signal_search_time + turn_signal_intersection_search_distance`. Then the start point becomes `search_distance` meters before the start point of the intersection lanelet(depicted in gree in the following picture), where `v` is the velocity of the ego vehicle. However, if we set `turn_signal_distance` in the lanelet, we use that length as search distance. +- desired start point + The `search_distance` for blinkers at intersections is `v * turn_signal_search_time + turn_signal_intersection_search_distance`. Then the start point becomes `search_distance` meters before the start point of the intersection lanelet(depicted in green in the following picture), where `v` is the velocity of the ego vehicle. However, if we set `turn_signal_distance` in the lanelet, we use that length as search distance. -- desired end point +- desired end point Terminal point of the intersection lanelet. -- required start point +- required start point Initial point of the intersection lanelet. -- required end point +- required end point The earliest point that satisfies the following condition. $\theta - \theta_{\textrm{end}} < \delta$, where $\theta_{\textrm{end}}$ is yaw angle of the terminal point of the lanelet, $\theta$ is the angle of a required end point and $\delta$ is the threshold defined by the user. ![section_turn_signal](../image/turn_signal_decider/left_right_turn.drawio.svg) @@ -79,100 +79,100 @@ Avoidance can be separated into two sections, first section and second section. First section -- desired start point +- desired start point `v * turn_signal_search_time` meters before the start point of the avoidance shift path. -- desired end point +- desired end point Shift complete point where the path shift is completed. -- required start point +- required start point Avoidance start point. -- required end point +- required end point Shift complete point same as the desired end point. ![section_first_avoidance](../image/turn_signal_decider/avoidance_first_section.drawio.svg) Second section -- desired start point +- desired start point Shift complete point. -- desired end point +- desired end point Avoidance terminal point -- required start point +- required start point Shift complete point. -- required end point +- required end point Avoidance terminal point. ![section_second_avoidance](../image/turn_signal_decider/avoidance_second_section.drawio.svg) #### 3. Lane Change -- desired start point +- desired start point `v * turn_signal_search_time` meters before the start point of the lane change path. -- desired end point +- desired end point Terminal point of the lane change path. -- required start point +- required start point Initial point of the lane change path. -- required end point +- required end point Cross point with lane change path and boundary line of the adjacent lane. ![section_lane_change](../image/turn_signal_decider/lane_change.drawio.svg) #### 4. Pull out -- desired start point +- desired start point Start point of the path of pull out. -- desired end point +- desired end point Terminal point of the path of pull out. -- required start point +- required start point Start point of the path pull out. -- required end point +- required end point Terminal point of the path of pull out. ![section_pull_out](../image/turn_signal_decider/pull_out.drawio.svg) #### 5. Goal Planner -- desired start point +- desired start point `v * turn_signal_search_time` meters before the start point of the pull over path. -- desired end point +- desired end point Terminal point of the path of pull over. -- required start point +- required start point Start point of the path of pull over. -- required end point +- required end point Terminal point of the path of pull over. ![section_pull_over](../image/turn_signal_decider/pull_over.drawio.svg) When it comes to handle several blinkers, it gives priority to the first blinker that comes first. However, this rule sometimes activate unnatural blinkers, so turn signal decider uses the following five rules to decide the necessary turn signal. -- pattern1 - ![pattern1](../image/turn_signal_decider/pattern1.drawio.svg) +- pattern1 + ![pattern1](../images/turn_signal_decider/pattern1.drawio.svg) -- pattern2 - ![pattern2](../image/turn_signal_decider/pattern2.drawio.svg) +- pattern2 + ![pattern2](../images/turn_signal_decider/pattern2.drawio.svg) -- pattern3 - ![pattern3](../image/turn_signal_decider/pattern3.drawio.svg) +- pattern3 + ![pattern3](../images/turn_signal_decider/pattern3.drawio.svg) -- pattern4 - ![pattern4](../image/turn_signal_decider/pattern4.drawio.svg) +- pattern4 + ![pattern4](../images/turn_signal_decider/pattern4.drawio.svg) -- pattern5 - ![pattern5](../image/turn_signal_decider/pattern5.drawio.svg) +- pattern5 + ![pattern5](../images/turn_signal_decider/pattern5.drawio.svg) Based on these five rules, turn signal decider can solve `blinker conflicts`. The following pictures show some examples of this kind of conflicts. diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index 9c879ec08d9fd..a479a920524b1 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -21,7 +21,9 @@ #include "motion_utils/trajectory/trajectory.hpp" #include -#include +#include +#include +#include #include #include @@ -165,6 +167,62 @@ struct PlannerData mutable std::vector drivable_area_expansion_prev_curvatures{}; mutable TurnSignalDecider turn_signal_decider; + std::pair getBehaviorTurnSignalInfo( + const PathWithLaneId & path, const size_t shift_start_idx, const size_t shift_end_idx, + const lanelet::ConstLanelets & current_lanelets, const double current_shift_length, + const bool is_driving_forward, const bool egos_lane_is_shifted, + const bool override_ego_stopped_check = false, const bool is_pull_out = false) const + { + if (shift_start_idx + 1 > path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + if (shift_end_idx + 1 > path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + std::vector lengths(path.points.size(), 0.0); + ShiftedPath shifted_path{path, lengths}; + ShiftLine shift_line; + + { + const auto start_pose = path.points.at(shift_start_idx).point.pose; + const auto start_shift_length = + lanelet::utils::getArcCoordinates(current_lanelets, start_pose).distance; + const auto end_pose = path.points.at(shift_end_idx).point.pose; + const auto end_shift_length = + lanelet::utils::getArcCoordinates(current_lanelets, end_pose).distance; + shifted_path.shift_length.at(shift_start_idx) = start_shift_length; + shifted_path.shift_length.at(shift_end_idx) = end_shift_length; + + shift_line.start = start_pose; + shift_line.end = end_pose; + shift_line.start_shift_length = start_shift_length; + shift_line.end_shift_length = end_shift_length; + shift_line.start_idx = shift_start_idx; + shift_line.end_idx = shift_end_idx; + } + + return turn_signal_decider.getBehaviorTurnSignalInfo( + shifted_path, shift_line, current_lanelets, route_handler, parameters, self_odometry, + current_shift_length, is_driving_forward, egos_lane_is_shifted, override_ego_stopped_check, + is_pull_out); + } + + std::pair getBehaviorTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, + const lanelet::ConstLanelets & current_lanelets, const double current_shift_length, + const bool is_driving_forward, const bool egos_lane_is_shifted, + const bool override_ego_stopped_check = false, const bool is_pull_out = false) const + { + return turn_signal_decider.getBehaviorTurnSignalInfo( + path, shift_line, current_lanelets, route_handler, parameters, self_odometry, + current_shift_length, is_driving_forward, egos_lane_is_shifted, override_ego_stopped_check, + is_pull_out); + } + TurnIndicatorsCommand getTurnSignal( const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info, TurnSignalDebugData & debug_data) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp index 372041c7fb586..18afecf5feb4f 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp @@ -15,14 +15,24 @@ #ifndef BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ #define BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" + #include +#include +#include #include +#include +#include #include #include #include +#include + +#include #include +#include #include #include @@ -37,6 +47,7 @@ using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; using route_handler::RouteHandler; const std::map signal_map = { @@ -100,6 +111,15 @@ class TurnSignalDecider std::pair getIntersectionTurnSignalFlag(); std::pair getIntersectionPoseAndDistance(); + std::pair getBehaviorTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, + const lanelet::ConstLanelets & current_lanelets, + const std::shared_ptr route_handler, + const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry, + const double current_shift_length, const bool is_driving_forward, + const bool egos_lane_is_shifted, const bool override_ego_stopped_check = false, + const bool is_pull_out = false) const; + private: std::optional getIntersectionTurnSignalInfo( const PathWithLaneId & path, const Pose & current_pose, const double current_vel, @@ -118,6 +138,132 @@ class TurnSignalDecider const double nearest_yaw_threshold); void initialize_intersection_info(); + inline bool isAvoidShift( + const double start_shift_length, const double end_shift_length, const double threshold) const + { + return std::abs(start_shift_length) < threshold && std::abs(end_shift_length) > threshold; + }; + + inline bool isReturnShift( + const double start_shift_length, const double end_shift_length, const double threshold) const + { + return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold; + }; + + inline bool isLeftMiddleShift( + const double start_shift_length, const double end_shift_length, const double threshold) const + { + return start_shift_length > threshold && end_shift_length > threshold; + }; + + inline bool isRightMiddleShift( + const double start_shift_length, const double end_shift_length, const double threshold) const + { + return start_shift_length < threshold && end_shift_length < threshold; + }; + + inline bool existShiftSideLane( + const double start_shift_length, const double end_shift_length, const bool no_left_lanes, + const bool no_right_lanes, const double threshold) const + { + const auto relative_shift_length = end_shift_length - start_shift_length; + if (isAvoidShift(start_shift_length, end_shift_length, threshold)) { + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_left_lanes) { + return false; + } + + // Right avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_right_lanes) { + return false; + } + } + + if (isReturnShift(start_shift_length, end_shift_length, threshold)) { + // Right return. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_right_lanes) { + return false; + } + + // Left return. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_left_lanes) { + return false; + } + } + + if (isLeftMiddleShift(start_shift_length, end_shift_length, threshold)) { + // Left avoid. But there is no adjacent lane. No need blinker. + + if (relative_shift_length > 0.0 && no_left_lanes) { + return false; + } + + // Left return. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_left_lanes) { + return false; + } + } + + if (isRightMiddleShift(start_shift_length, end_shift_length, threshold)) { + // Right avoid. But there is no adjacent lane. No need blinker. + + if (relative_shift_length < 0.0 && no_right_lanes) { + return false; + } + + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_right_lanes) { + return false; + } + } + return true; + }; + + inline bool straddleRoadBound( + const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, + const vehicle_info_util::VehicleInfo & vehicle_info) const + { + using boost::geometry::intersects; + using tier4_autoware_utils::pose2transform; + using tier4_autoware_utils::transformVector; + + const auto footprint = vehicle_info.createFootprint(); + + for (const auto & lane : lanes) { + for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) { + const auto transform = pose2transform(path.path.points.at(i).point.pose); + const auto shifted_vehicle_footprint = transformVector(footprint, transform); + + if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + + if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + } + } + + return false; + }; + + inline bool isNearEndOfShift( + const double start_shift_length, const double end_shift_length, const Point & ego_pos, + const lanelet::ConstLanelets & original_lanes, const double threshold) const + { + using boost::geometry::within; + using lanelet::utils::to2D; + using lanelet::utils::conversion::toLaneletPoint; + + if (!isReturnShift(start_shift_length, end_shift_length, threshold)) { + return false; + } + + return std::any_of(original_lanes.begin(), original_lanes.end(), [&ego_pos](const auto & lane) { + return within(to2D(toLaneletPoint(ego_pos)), lane.polygon2d().basicPolygon()); + }); + }; + geometry_msgs::msg::Quaternion calc_orientation(const Point & src_point, const Point & dst_point); rclcpp::Logger logger_{ diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index 0c105e804693a..53e521ee24788 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -33,6 +33,8 @@ namespace behavior_path_planner { +using motion_utils::calcSignedArcLength; + double calc_distance( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const Pose & input_point, const double nearest_dist_threshold, const double nearest_yaw_threshold) @@ -43,6 +45,11 @@ double calc_distance( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); } +/*** + * @brief: + * Gets the turn signal info after comparing the turn signal info output from the behavior path + * module and comparing it to turn signal info obtained from intersections. + */ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( const std::shared_ptr & route_handler, const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel, @@ -606,4 +613,145 @@ geometry_msgs::msg::Quaternion TurnSignalDecider::calc_orientation( const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); return tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); } + +std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, + const lanelet::ConstLanelets & current_lanelets, + const std::shared_ptr route_handler, + const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry, + const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, + const bool override_ego_stopped_check, const bool is_pull_out) const +{ + constexpr double THRESHOLD = 0.1; + const auto & p = parameters; + const auto & rh = route_handler; + const auto & ego_pose = self_odometry->pose.pose; + const auto & ego_speed = self_odometry->twist.twist.linear.x; + + if (!is_driving_forward) { + TurnSignalInfo turn_signal_info{}; + turn_signal_info.hazard_signal.command = HazardLightsCommand::ENABLE; + const auto back_start_pose = rh->getOriginalStartPose(); + const Pose & start_pose = self_odometry->pose.pose; + + turn_signal_info.desired_start_point = back_start_pose; + turn_signal_info.required_start_point = back_start_pose; + // pull_out start_pose is same to backward driving end_pose + turn_signal_info.required_end_point = start_pose; + turn_signal_info.desired_end_point = start_pose; + return std::make_pair(turn_signal_info, false); + } + + if (shift_line.start_idx + 1 > path.shift_length.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + if (shift_line.start_idx + 1 > path.path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + if (shift_line.end_idx + 1 > path.shift_length.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + if (shift_line.end_idx + 1 > path.path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + const auto [start_shift_length, end_shift_length] = + std::invoke([&path, &shift_line, &egos_lane_is_shifted]() -> std::pair { + const auto temp_start_shift_length = path.shift_length.at(shift_line.start_idx); + const auto temp_end_shift_length = path.shift_length.at(shift_line.end_idx); + // Shift is done using the target lane and not current ego's lane + if (!egos_lane_is_shifted) { + return std::make_pair(temp_end_shift_length, -temp_start_shift_length); + } + return std::make_pair(temp_start_shift_length, temp_end_shift_length); + }); + + const auto relative_shift_length = end_shift_length - start_shift_length; + + // If shift length is shorter than the threshold, it does not need to turn on blinkers + if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { + return std::make_pair(TurnSignalInfo{}, true); + } + + // If the vehicle does not shift anymore, we turn off the blinker + if (std::fabs(end_shift_length - current_shift_length) < THRESHOLD) { + return std::make_pair(TurnSignalInfo{}, true); + } + + const auto get_command = [](const auto & shift_length) { + return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT + : TurnIndicatorsCommand::ENABLE_RIGHT; + }; + + const auto signal_prepare_distance = + std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance); + const auto ego_front_to_shift_start = + calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) - + p.vehicle_info.max_longitudinal_offset_m; + + if (signal_prepare_distance < ego_front_to_shift_start) { + return std::make_pair(TurnSignalInfo{}, false); + } + + const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; + const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose; + const auto get_start_pose = [&](const auto & ego_to_shift_start) { + return ego_to_shift_start > 0.0 ? ego_pose : blinker_start_pose; + }; + + TurnSignalInfo turn_signal_info{}; + turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start); + turn_signal_info.desired_end_point = blinker_end_pose; + turn_signal_info.required_start_point = blinker_start_pose; + turn_signal_info.required_end_point = blinker_end_pose; + turn_signal_info.turn_signal.command = get_command(relative_shift_length); + + if (!p.turn_signal_on_swerving) { + return std::make_pair(turn_signal_info, false); + } + + lanelet::ConstLanelet lanelet; + const auto query_pose = (egos_lane_is_shifted) ? shift_line.end : shift_line.start; + if (!rh->getClosestLaneletWithinRoute(query_pose, &lanelet)) { + return std::make_pair(TurnSignalInfo{}, true); + } + + const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); + const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet); + const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true); + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet); + const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty(); + const auto has_right_lane = + right_same_direction_lane.has_value() || !right_opposite_lanes.empty(); + + if ( + !is_pull_out && !existShiftSideLane( + start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, + p.turn_signal_shift_length_threshold)) { + return std::make_pair(TurnSignalInfo{}, true); + } + + if (!straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) { + return std::make_pair(TurnSignalInfo{}, true); + } + + constexpr double STOPPED_THRESHOLD = 0.1; // [m/s] + if (ego_speed < STOPPED_THRESHOLD && !override_ego_stopped_check) { + if (isNearEndOfShift( + start_shift_length, end_shift_length, ego_pose.position, current_lanelets, + p.turn_signal_shift_length_threshold)) { + return std::make_pair(TurnSignalInfo{}, true); + } + } + + return std::make_pair(turn_signal_info, false); +} + } // namespace behavior_path_planner From ea2d70136f3519163606fedc2f5448783701839a Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Mon, 18 Mar 2024 13:32:06 +0900 Subject: [PATCH 2/8] feat(behavior_path_avoidance_module): use the general calc turn signal method (#6626) use the general calc turn signal method Signed-off-by: Daniel Sanchez --- .../behavior_path_avoidance_module/utils.hpp | 3 - .../src/scene.cpp | 10 +- .../src/utils.cpp | 192 ------------------ 3 files changed, 7 insertions(+), 198 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index 14d3bd1800dad..3f1d3495e51cb 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -172,9 +172,6 @@ double calcDistanceToAvoidStartLine( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); -TurnSignalInfo calcTurnSignalInfo( - const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, - const AvoidancePlanningData & data, const std::shared_ptr & planner_data); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 877302c4bcd91..16effece0c7aa 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -950,9 +950,13 @@ BehaviorModuleOutput AvoidanceModule::plan() output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info; - const auto new_signal = utils::avoidance::calcTurnSignalInfo( - linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_, - planner_data_); + + constexpr bool is_driving_forward = true; + constexpr bool egos_lane_is_shifted = true; + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + linear_shift_path, path_shifter_.getShiftLines().front(), avoid_data_.current_lanelets, + helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index ee1d2dd8726fd..7c3941d4b95da 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -251,98 +251,6 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); } -bool existShiftSideLane( - const double start_shift_length, const double end_shift_length, const bool no_left_lanes, - const bool no_right_lanes) -{ - constexpr double THRESHOLD = 0.1; - const auto relative_shift_length = end_shift_length - start_shift_length; - - const auto avoid_shift = - std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD; - if (avoid_shift) { - // Left avoid. But there is no adjacent lane. No need blinker. - if (relative_shift_length > 0.0 && no_left_lanes) { - return false; - } - - // Right avoid. But there is no adjacent lane. No need blinker. - if (relative_shift_length < 0.0 && no_right_lanes) { - return false; - } - } - - const auto return_shift = - std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD; - if (return_shift) { - // Right return. But there is no adjacent lane. No need blinker. - if (relative_shift_length > 0.0 && no_right_lanes) { - return false; - } - - // Left return. But there is no adjacent lane. No need blinker. - if (relative_shift_length < 0.0 && no_left_lanes) { - return false; - } - } - - const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD; - if (left_middle_shift) { - // Left avoid. But there is no adjacent lane. No need blinker. - if (relative_shift_length > 0.0 && no_left_lanes) { - return false; - } - - // Left return. But there is no adjacent lane. No need blinker. - if (relative_shift_length < 0.0 && no_left_lanes) { - return false; - } - } - - const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD; - if (right_middle_shift) { - // Right avoid. But there is no adjacent lane. No need blinker. - if (relative_shift_length < 0.0 && no_right_lanes) { - return false; - } - - // Left avoid. But there is no adjacent lane. No need blinker. - if (relative_shift_length > 0.0 && no_right_lanes) { - return false; - } - } - - return true; -} - -bool straddleRoadBound( - const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, - const vehicle_info_util::VehicleInfo & vehicle_info) -{ - using boost::geometry::intersects; - using tier4_autoware_utils::pose2transform; - using tier4_autoware_utils::transformVector; - - const auto footprint = vehicle_info.createFootprint(); - - for (const auto & lane : lanes) { - for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) { - const auto transform = pose2transform(path.path.points.at(i).point.pose); - const auto shifted_vehicle_footprint = transformVector(footprint, transform); - - if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) { - return true; - } - - if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) { - return true; - } - } - } - - return false; -} - } // namespace namespace filtering_utils @@ -2363,104 +2271,4 @@ double calcDistanceToReturnDeadLine( return distance_to_return_dead_line; } - -TurnSignalInfo calcTurnSignalInfo( - const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, - const AvoidancePlanningData & data, const std::shared_ptr & planner_data) -{ - constexpr double THRESHOLD = 0.1; - const auto & p = planner_data->parameters; - const auto & rh = planner_data->route_handler; - const auto & ego_pose = planner_data->self_odometry->pose.pose; - const auto & ego_speed = planner_data->self_odometry->twist.twist.linear.x; - - if (shift_line.start_idx + 1 > path.shift_length.size()) { - RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - return {}; - } - - if (shift_line.start_idx + 1 > path.path.points.size()) { - RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - return {}; - } - - if (shift_line.end_idx + 1 > path.shift_length.size()) { - RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - return {}; - } - - if (shift_line.end_idx + 1 > path.path.points.size()) { - RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - return {}; - } - - const auto start_shift_length = path.shift_length.at(shift_line.start_idx); - const auto end_shift_length = path.shift_length.at(shift_line.end_idx); - const auto relative_shift_length = end_shift_length - start_shift_length; - - // If shift length is shorter than the threshold, it does not need to turn on blinkers - if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { - return {}; - } - - // If the vehicle does not shift anymore, we turn off the blinker - if (std::fabs(path.shift_length.at(shift_line.end_idx) - current_shift_length) < THRESHOLD) { - return {}; - } - - const auto get_command = [](const auto & shift_length) { - return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT - : TurnIndicatorsCommand::ENABLE_RIGHT; - }; - - const auto signal_prepare_distance = - std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance); - const auto ego_front_to_shift_start = - calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) - - p.vehicle_info.max_longitudinal_offset_m; - - if (signal_prepare_distance < ego_front_to_shift_start) { - return {}; - } - - const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; - const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose; - const auto get_start_pose = [&](const auto & ego_to_shift_start) { - return ego_to_shift_start ? ego_pose : blinker_start_pose; - }; - - TurnSignalInfo turn_signal_info{}; - turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start); - turn_signal_info.desired_end_point = blinker_end_pose; - turn_signal_info.required_start_point = blinker_start_pose; - turn_signal_info.required_end_point = blinker_end_pose; - turn_signal_info.turn_signal.command = get_command(relative_shift_length); - - if (!p.turn_signal_on_swerving) { - return turn_signal_info; - } - - lanelet::ConstLanelet lanelet; - if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) { - return {}; - } - - const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); - const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet); - const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true); - const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet); - const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty(); - const auto has_right_lane = - right_same_direction_lane.has_value() || !right_opposite_lanes.empty(); - - if (!existShiftSideLane(start_shift_length, end_shift_length, !has_left_lane, !has_right_lane)) { - return {}; - } - - if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) { - return {}; - } - - return turn_signal_info; -} } // namespace behavior_path_planner::utils::avoidance From 47bcea45cc187143d74d0a3e4fb5b539176061bf Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Mon, 18 Mar 2024 16:27:35 +0900 Subject: [PATCH 3/8] feat(lane_change_module): add general method to get turn signal for LC module (#6627) add general method to get turn signal for LC module Signed-off-by: Daniel Sanchez --- .../scene.hpp | 4 +- .../utils/base_class.hpp | 2 - .../src/scene.cpp | 85 +++++++------------ 3 files changed, 35 insertions(+), 56 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 9c396c74301c4..bd4f19848fbdc 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -141,7 +141,9 @@ class NormalLaneChange : public LaneChangeBase const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const override; - TurnSignalInfo calcTurnSignalInfo() override; + std::optional calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; bool isValidPath(const PathWithLaneId & path) const override; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index aa3ee0dc4c98b..d7a559bb54cf6 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -225,8 +225,6 @@ class LaneChangeBase LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety) const = 0; - virtual TurnSignalInfo calcTurnSignalInfo() = 0; - virtual bool isValidPath(const PathWithLaneId & path) const = 0; virtual bool isAbleToStopSafely() const = 0; diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index bd767191884ad..51be691770abf 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -426,14 +426,20 @@ void NormalLaneChange::resetParameters() TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() { - TurnSignalInfo turn_signal_info = calcTurnSignalInfo(); - const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal( - status_.current_lanes, status_.lane_change_path.shifted_path, - status_.lane_change_path.info.shift_line, getEgoPose(), getEgoTwist().linear.x, - planner_data_->parameters); - turn_signal_info.turn_signal.command = turn_signal_command.command; - - return turn_signal_info; + const auto & pose = getEgoPose(); + const auto & current_lanes = status_.current_lanes; + const auto & shift_line = status_.lane_change_path.info.shift_line; + const auto & shift_path = status_.lane_change_path.shifted_path; + const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; + constexpr bool is_driving_forward = true; + // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's + // current lane, lane change is different, so we set this flag to false. + constexpr bool egos_lane_is_shifted = false; + + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + shift_path, shift_line, current_lanes, current_shift_length, is_driving_forward, + egos_lane_is_shifted); + return new_signal; } lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const @@ -1445,53 +1451,26 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return safety_status; } -TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() +PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis( + PathSafetyStatus approved_path_safety_status) { - const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) { - double accumulated_length = 0.0; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - accumulated_length += - tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (accumulated_length > length) { - return path.points.at(i).point.pose; - } - } - - return path.points.front().point.pose; - }; - - const auto & path = status_.lane_change_path; - const auto & shifted_path = path.shifted_path.path; - - TurnSignalInfo turn_signal_info{}; - - // desired start pose = prepare start pose - turn_signal_info.desired_start_point = std::invoke([&]() { - const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; - const auto diff_time = path.info.duration.prepare - blinker_start_duration; - if (diff_time < 1e-5) { - return path.path.points.front().point.pose; + if (!approved_path_safety_status.is_safe) { + ++unsafe_hysteresis_count_; + RCLCPP_DEBUG( + logger_, "%s: Increasing hysteresis count to %d.", __func__, unsafe_hysteresis_count_); + } else { + if (unsafe_hysteresis_count_ > 0) { + RCLCPP_DEBUG(logger_, "%s: Lane change is now SAFE. Resetting hysteresis count.", __func__); } - - const auto current_twist = getEgoTwist(); - const auto diff_length = std::abs(current_twist.linear.x) * diff_time; - return get_blinker_pose(path.path, diff_length); - }); - - // desired end pose - const auto length_ratio = - std::clamp(lane_change_parameters_->length_ratio_for_turn_signal_deactivation, 0.0, 1.0); - const auto desired_end_length = path.info.length.lane_changing * length_ratio; - turn_signal_info.desired_end_point = get_blinker_pose(shifted_path, desired_end_length); - - // required start pose = lane changing start pose - turn_signal_info.required_start_point = path.info.shift_line.start; - - // required end pose = in the middle of the lane change - const auto mid_lane_change_length = path.info.length.lane_changing / 2; - turn_signal_info.required_end_point = get_blinker_pose(shifted_path, mid_lane_change_length); - - return turn_signal_info; + unsafe_hysteresis_count_ = 0; + } + if (unsafe_hysteresis_count_ > lane_change_parameters_->cancel.unsafe_hysteresis_threshold) { + RCLCPP_DEBUG( + logger_, "%s: hysteresis count exceed threshold. lane change is now %s", __func__, + (approved_path_safety_status.is_safe ? "safe" : "UNSAFE")); + return approved_path_safety_status; + } + return {}; } bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const From 5bcd590ca17072b87bec98f76e954e641f92df88 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Mon, 18 Mar 2024 10:16:23 +0900 Subject: [PATCH 4/8] feat(start_planner): add general turn signal method to start planner (#6628) * Add general turnSignal method to start planner Signed-off-by: Daniel Sanchez * add description to ignore signal Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../start_planner_module.hpp | 7 +- .../src/start_planner_module.cpp | 141 +++++++----------- 2 files changed, 58 insertions(+), 90 deletions(-) diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 8be439422c91c..a45afe560968b 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -33,6 +33,7 @@ #include +#include #include #include @@ -196,6 +197,10 @@ class StartPlannerModule : public SceneModuleInterface PullOutStatus status_; mutable StartGoalPlannerData start_planner_data_; + // Keeps track of lanelets that should be ignored when calculating the turnSignalInfo for this + // module's output. If the ego vehicle is in this lanelet, the calculation is skipped. + std::optional ignore_signal_{std::nullopt}; + std::deque odometry_buffer_; std::unique_ptr last_route_received_time_; @@ -218,7 +223,7 @@ class StartPlannerModule : public SceneModuleInterface std::shared_ptr lane_departure_checker_; // turn signal - TurnSignalInfo calcTurnSignalInfo() const; + TurnSignalInfo calcTurnSignalInfo(); void incrementPathIndex(); PathWithLaneId getCurrentPath() const; diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 68be0171e1520..74d260fdcabb8 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -1008,107 +1008,70 @@ bool StartPlannerModule::hasFinishedCurrentPath() return is_near_target && isStopped(); } -TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const +TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() { - TurnSignalInfo turn_signal{}; // output + const auto path = getFullPath(); + if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const Pose & start_pose = status_.pull_out_path.start_pose; - const Pose & end_pose = status_.pull_out_path.end_pose; - - // turn on hazard light when backward driving - if (!status_.driving_forward) { - turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; - const auto back_start_pose = planner_data_->route_handler->getOriginalStartPose(); - turn_signal.desired_start_point = back_start_pose; - turn_signal.required_start_point = back_start_pose; - // pull_out start_pose is same to backward driving end_pose - turn_signal.required_end_point = start_pose; - turn_signal.desired_end_point = start_pose; - return turn_signal; - } + const auto shift_start_idx = + motion_utils::findNearestIndex(path.points, status_.pull_out_path.start_pose.position); + const auto shift_end_idx = + motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position); + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); - // turn on right signal until passing pull_out end point - const auto path = getFullPath(); - // pull out path does not overlap - const double distance_from_end = - motion_utils::calcSignedArcLength(path.points, end_pose.position, current_pose.position); + const auto is_ignore_signal = [this](const lanelet::Id & id) { + if (!ignore_signal_.has_value()) { + return false; + } + return ignore_signal_.value() == id; + }; - if (path.points.empty()) { - return {}; - } + const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + return is_ignore ? std::make_optional(id) : std::nullopt; + }; - // calculate lateral offset from pull out target lane center line - lanelet::ConstLanelet closest_road_lane; - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); - lanelet::utils::query::getClosestLanelet(road_lanes, start_pose, &closest_road_lane); - const double lateral_offset = - lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose); - - if (distance_from_end < 0.0 && lateral_offset > parameters_->th_turn_signal_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } else if ( - distance_from_end < 0.0 && lateral_offset < -parameters_->th_turn_signal_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; + lanelet::Lanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &closest_lanelet); + + if (is_ignore_signal(closest_lanelet.id())) { + return getPreviousModuleOutput().turn_signal_info; } - turn_signal.desired_start_point = start_pose; - turn_signal.required_start_point = start_pose; - turn_signal.desired_end_point = end_pose; - - // check if intersection exists within search length - const bool is_near_intersection = std::invoke([&]() { - const double check_length = parameters_->intersection_search_length; - double accumulated_length = 0.0; - const size_t current_idx = motion_utils::findNearestIndex(path.points, current_pose.position); - for (size_t i = current_idx; i < path.points.size() - 1; ++i) { - const auto & p = path.points.at(i); - for (const auto & lane : planner_data_->route_handler->getLaneletsFromIds(p.lane_ids)) { - const std::string turn_direction = lane.attributeOr("turn_direction", "else"); - if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { - return true; - } - } - accumulated_length += tier4_autoware_utils::calcDistance2d(p, path.points.at(i + 1)); - if (accumulated_length > check_length) { - return false; - } + const double current_shift_length = + lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance; + + constexpr bool egos_lane_is_shifted = true; + constexpr bool is_pull_out = true; + + // In Geometric pull out, the ego stops once and then steers the wheels to the opposite direction. + // This sometimes causes the getBehaviorTurnSignalInfo method to detect the ego as stopped and + // close to complete its shift, so it wrongly turns off the blinkers, this override helps avoid + // this issue. + const bool override_ego_stopped_check = std::invoke([&]() { + if (status_.planner_type != PlannerType::GEOMETRIC) { + return false; } - return false; + constexpr double distance_threshold = 1.0; + const auto stop_point = status_.pull_out_path.partial_paths.front().points.back(); + const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength( + path.points, stop_point.point.pose.position, current_pose.position)); + return distance_from_ego_to_stop_point < distance_threshold; }); - if (is_near_intersection) { - // offset required end pose with ration to activate turn signal for intersection - turn_signal.required_end_point = std::invoke([&]() { - const double length_start_to_end = - motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position); - const auto ratio = std::clamp( - parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0); - - const double required_end_length = length_start_to_end * ratio; - double accumulated_length = 0.0; - const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position); - for (size_t i = start_idx; i < path.points.size() - 1; ++i) { - accumulated_length += - tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (accumulated_length > required_end_length) { - return path.points.at(i).point.pose; - } - } - // not found required end point - return end_pose; - }); - } else { - turn_signal.required_end_point = end_pose; - } + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length, + status_.driving_forward, egos_lane_is_shifted, override_ego_stopped_check, is_pull_out); + ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore); + + const auto original_signal = getPreviousModuleOutput().turn_signal_info; + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points); + const auto output_turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + path, current_pose, current_seg_idx, original_signal, new_signal, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); - return turn_signal; + return output_turn_signal_info; } bool StartPlannerModule::isSafePath() const From 2c765a93f6dd2497cf38bb950b41e1745bfd3d39 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 26 Mar 2024 18:45:39 +0900 Subject: [PATCH 5/8] feat(goal_planner): add general turnsignalinfo method for goal planner (#6629) add general turnsignalinfo method for goal planner Signed-off-by: Daniel Sanchez --- .../goal_planner_module.hpp | 13 +- .../src/goal_planner_module.cpp | 111 +++++++++++++----- 2 files changed, 91 insertions(+), 33 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 9fb9c17f1ca70..134db83172673 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -37,6 +37,9 @@ #include #include +#include + +#include #include #include #include @@ -452,9 +455,8 @@ class GoalPlannerModule : public SceneModuleInterface void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // output setter - void setOutput(BehaviorModuleOutput & output) const; - void setStopPath(BehaviorModuleOutput & output) const; - void updatePreviousData(const BehaviorModuleOutput & output); + void setOutput(BehaviorModuleOutput & output); + void updatePreviousData(); /** * @brief Sets a stop path in the current path based on safety conditions and previous paths. @@ -467,10 +469,11 @@ class GoalPlannerModule : public SceneModuleInterface */ void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const; void setModifiedGoal(BehaviorModuleOutput & output) const; - void setTurnSignalInfo(BehaviorModuleOutput & output) const; + void setTurnSignalInfo(BehaviorModuleOutput & output); // new turn signal - TurnSignalInfo calcTurnSignalInfo() const; + TurnSignalInfo calcTurnSignalInfo(); + std::optional ignore_signal_{std::nullopt}; std::optional last_previous_module_output_{}; bool hasPreviousModulePathShapeChanged() const; diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 894577f51b795..7564df5da48bd 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -796,7 +796,7 @@ std::vector GoalPlannerModule::generateDrivableLanes() const return utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_over_lanes); } -void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const +void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { output.reference_path = getPreviousModuleOutput().reference_path; @@ -907,7 +907,7 @@ void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const } } -void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const +void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) { const auto original_signal = getPreviousModuleOutput().turn_signal_info; const auto new_signal = calcTurnSignalInfo(); @@ -1433,43 +1433,98 @@ bool GoalPlannerModule::isOnModifiedGoal() const parameters_->th_arrived_distance; } -TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const +TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() { - TurnSignalInfo turn_signal{}; // output + const auto path = thread_safe_data_.get_pull_over_path()->getFullPath(); + if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose; const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose; - const auto full_path = thread_safe_data_.get_pull_over_path()->getFullPath(); - // 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; - if (is_before_end_pose) { - if (left_side_parking_) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } - } else { - turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + const auto shift_start_idx = motion_utils::findNearestIndex(path.points, start_pose.position); + const auto shift_end_idx = motion_utils::findNearestIndex(path.points, end_pose.position); + + const auto is_ignore_signal = [this](const lanelet::Id & id) { + if (!ignore_signal_.has_value()) { + return false; } + return ignore_signal_.value() == id; + }; + + const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + return is_ignore ? std::make_optional(id) : std::nullopt; + }; + + const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length, + /*forward_only_in_route*/ false); + + if (current_lanes.empty()) { + return {}; } - // 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_approval_data_ && hasDecidedPath() ? last_approval_data_->pose : current_pose; - turn_signal.desired_end_point = end_pose; - turn_signal.required_start_point = start_pose; - turn_signal.required_end_point = end_pose; + lanelet::Lanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &closest_lanelet); + + if (is_ignore_signal(closest_lanelet.id())) { + return getPreviousModuleOutput().turn_signal_info; } - return turn_signal; + const double current_shift_length = + lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance; + + constexpr bool egos_lane_is_shifted = true; + constexpr bool is_driving_forward = true; + + constexpr bool is_pull_out = false; + const bool override_ego_stopped_check = std::invoke([&]() { + if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::SHIFT) { + return false; + } + constexpr double distance_threshold = 1.0; + const auto stop_point = + thread_safe_data_.get_pull_over_path()->partial_paths.front().points.back(); + const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength( + path.points, stop_point.point.pose.position, current_pose.position)); + return distance_from_ego_to_stop_point < distance_threshold; + }); + + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length, is_driving_forward, + egos_lane_is_shifted, override_ego_stopped_check, is_pull_out); + ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore); + + return new_signal; + // // 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; + // if (is_before_end_pose) { + // if (left_side_parking_) { + // turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + // } else { + // turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + // } + // } else { + // turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + // } + // } + + // // 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_approval_data_ && hasDecidedPath() ? last_approval_data_->pose : current_pose; + // turn_signal.desired_end_point = end_pose; + // turn_signal.required_start_point = start_pose; + // turn_signal.required_end_point = end_pose; + // } + + // return turn_signal; } bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) const From c7d8f2c204a232bad71f4c0b1a14cf7c5981f659 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Fri, 24 May 2024 21:20:43 +0900 Subject: [PATCH 6/8] resolve merge conflict --- .../goal_planner_module.hpp | 3 ++- .../src/scene.cpp | 22 ------------------- 2 files changed, 2 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 134db83172673..12f8b7075bb14 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -456,7 +456,8 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(BehaviorModuleOutput & output); - void updatePreviousData(); + void setStopPath(BehaviorModuleOutput & output) const; + void updatePreviousData(const BehaviorModuleOutput & output); /** * @brief Sets a stop path in the current path based on safety conditions and previous paths. diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 51be691770abf..43acb159c1799 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1451,28 +1451,6 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return safety_status; } -PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis( - PathSafetyStatus approved_path_safety_status) -{ - if (!approved_path_safety_status.is_safe) { - ++unsafe_hysteresis_count_; - RCLCPP_DEBUG( - logger_, "%s: Increasing hysteresis count to %d.", __func__, unsafe_hysteresis_count_); - } else { - if (unsafe_hysteresis_count_ > 0) { - RCLCPP_DEBUG(logger_, "%s: Lane change is now SAFE. Resetting hysteresis count.", __func__); - } - unsafe_hysteresis_count_ = 0; - } - if (unsafe_hysteresis_count_ > lane_change_parameters_->cancel.unsafe_hysteresis_threshold) { - RCLCPP_DEBUG( - logger_, "%s: hysteresis count exceed threshold. lane change is now %s", __func__, - (approved_path_safety_status.is_safe ? "safe" : "UNSAFE")); - return approved_path_safety_status; - } - return {}; -} - bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const { const auto & route_handler = planner_data_->route_handler; From 947960a9fa8dd8d51760da7157c37a41503b80c9 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Wed, 29 May 2024 17:18:56 +0900 Subject: [PATCH 7/8] add turn_signal_remaining_shift_length_threshold Signed-off-by: Daniel Sanchez --- .../config/behavior_path_planner.param.yaml | 1 + .../behavior_path_planner_turn_signal_design.md | 17 +++++++++-------- .../src/behavior_path_planner_node.cpp | 2 ++ .../behavior_path_planner_common/parameters.hpp | 1 + .../src/turn_signal_decider.cpp | 5 +++-- 5 files changed, 16 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index b4d323de45cde..2bdff139673f0 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -18,6 +18,7 @@ turn_signal_minimum_search_distance: 10.0 turn_signal_search_time: 3.0 turn_signal_shift_length_threshold: 0.3 + turn_signal_remaining_shift_length_threshold: 0.1 turn_signal_on_swerving: true enable_akima_spline_first: false diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md index d3f72c03c6e54..f0c8f8d8cbb63 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md @@ -20,14 +20,15 @@ Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in c ## Parameters for turn signal decider -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------- | :--- | :----- | :--------------------------------------------------------------------------- | :------------ | -| turn_signal_intersection_search_distance | [m] | double | constant search distance to decide activation of blinkers at intersections | 30 | -| turn_signal_intersection_angle_threshold_degree | deg | double | angle threshold to determined the end point of intersection required section | 15 | -| turn_signal_minimum_search_distance | [m] | double | minimum search distance for avoidance and lane change | 10 | -| turn_signal_search_time | [s] | double | search time for to decide activation of blinkers | 3.0 | -| turn_signal_shift_length_threshold | [m] | double | shift length threshold to decide activation of blinkers | 0.3 | -| turn_signal_on_swerving | [-] | bool | flag to activate blinkers when swerving | true | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------ | :------------ | +| turn_signal_intersection_search_distance | [m] | double | constant search distance to decide activation of blinkers at intersections | 30 | +| turn_signal_intersection_angle_threshold_degree | deg | double | angle threshold to determined the end point of intersection required section | 15 | +| turn_signal_minimum_search_distance | [m] | double | minimum search distance for avoidance and lane change | 10 | +| turn_signal_search_time | [s] | double | search time for to decide activation of blinkers | 3.0 | +| turn_signal_shift_length_threshold | [m] | double | shift length threshold to decide activation of blinkers | 0.3 | +| turn_signal_remaining_shift_length_threshold | [m] | double | When the ego's current shift length minus its end shift length is less than this threshold, the turn signal will be turned off. | 0.1 | +| turn_signal_on_swerving | [-] | bool | flag to activate blinkers when swerving | true | Note that the default values for `turn_signal_intersection_search_distance` and `turn_signal_search_time` is strictly followed by [Japanese Road Traffic Laws](https://www.japaneselawtranslation.go.jp/ja/laws/view/2962). So if your country does not allow to use these default values, you should change these values in configuration files. 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 298111e1c27b2..3c6c6a883715f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -259,6 +259,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.turn_signal_search_time = declare_parameter("turn_signal_search_time"); p.turn_signal_shift_length_threshold = declare_parameter("turn_signal_shift_length_threshold"); + p.turn_signal_remaining_shift_length_threshold = + declare_parameter("turn_signal_remaining_shift_length_threshold"); p.turn_signal_on_swerving = declare_parameter("turn_signal_on_swerving"); p.enable_akima_spline_first = declare_parameter("enable_akima_spline_first"); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 6decd8fd2a131..c9ebfe18362f9 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -58,6 +58,7 @@ struct BehaviorPathPlannerParameters double turn_signal_search_time; double turn_signal_minimum_search_distance; double turn_signal_shift_length_threshold; + double turn_signal_remaining_shift_length_threshold; bool turn_signal_on_swerving; bool enable_akima_spline_first; diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index 53e521ee24788..f808aee2bde2e 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -622,7 +622,6 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, const bool override_ego_stopped_check, const bool is_pull_out) const { - constexpr double THRESHOLD = 0.1; const auto & p = parameters; const auto & rh = route_handler; const auto & ego_pose = self_odometry->pose.pose; @@ -681,7 +680,9 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( } // If the vehicle does not shift anymore, we turn off the blinker - if (std::fabs(end_shift_length - current_shift_length) < THRESHOLD) { + if ( + std::fabs(end_shift_length - current_shift_length) < + p.turn_signal_remaining_shift_length_threshold) { return std::make_pair(TurnSignalInfo{}, true); } From d845ecd0e93a22db489ae9cdaa8d0a2e43d03a4e Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Tue, 4 Jun 2024 09:44:25 +0900 Subject: [PATCH 8/8] include utility header to use std::pair --- .../include/behavior_path_planner_common/data_manager.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index a479a920524b1..6821b07fb3f4e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -46,6 +46,7 @@ #include #include #include +#include #include namespace behavior_path_planner