From ce1255a2e82f47c642edcb48ed5764c4431c89c0 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 9 May 2023 08:31:18 +0900 Subject: [PATCH] feat(behavior_path_planner): resolve multiple modules turn signal info (#3634) feat(behavior_path_planner): resolve multiple modules turn signal Signed-off-by: satoshi-ota --- .../behavior_path_planner_node.hpp | 6 +- .../behavior_path_planner/data_manager.hpp | 30 +++--- .../turn_signal_decider.hpp | 30 +++++- .../src/behavior_path_planner_node.cpp | 13 +-- .../src/turn_signal_decider.cpp | 98 ++++++++++++++++--- 5 files changed, 131 insertions(+), 46 deletions(-) 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 2c1ef1038554e..29ed73b9374e6 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 @@ -37,7 +37,6 @@ #endif #include "behavior_path_planner/steering_factor_interface.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" @@ -136,8 +135,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node bool has_received_map_{false}; bool has_received_route_{false}; - TurnSignalDecider turn_signal_decider_; - std::mutex mutex_pd_; // mutex for planner_data_ std::mutex mutex_manager_; // mutex for bt_manager_ or planner_manager_ std::mutex mutex_map_; // mutex for has_received_map_ and map_ptr_ @@ -220,7 +217,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief publish steering factor from intersection */ - void publish_steering_factor(const TurnIndicatorsCommand & turn_signal); + void publish_steering_factor( + const std::shared_ptr & planner_data, const TurnIndicatorsCommand & turn_signal); /** * @brief publish left and right bound diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 41981f17b4f9e..f48e82a9e2af6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__DATA_MANAGER_HPP_ #include "behavior_path_planner/parameters.hpp" +#include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include @@ -96,24 +97,6 @@ struct DrivableAreaInfo bool is_already_expanded{false}; }; -struct TurnSignalInfo -{ - TurnSignalInfo() - { - turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; - hazard_signal.command = HazardLightsCommand::NO_COMMAND; - } - - // desired turn signal - TurnIndicatorsCommand turn_signal; - HazardLightsCommand hazard_signal; - - geometry_msgs::msg::Pose desired_start_point; - geometry_msgs::msg::Pose desired_end_point; - geometry_msgs::msg::Pose required_start_point; - geometry_msgs::msg::Pose required_end_point; -}; - struct BehaviorModuleOutput { BehaviorModuleOutput() = default; @@ -160,6 +143,17 @@ struct PlannerData BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; + mutable TurnSignalDecider turn_signal_decider; + + TurnIndicatorsCommand getTurnSignal( + const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info) + { + const auto & current_pose = self_odometry->pose.pose; + const auto & current_vel = self_odometry->twist.twist.linear.x; + return turn_signal_decider.getTurnSignal( + route_handler, path, turn_signal_info, current_pose, current_vel, parameters); + } + template size_t findEgoIndex(const std::vector & points) const { 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 f702fe1b4c8e3..5691c03d31f6c 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 @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__TURN_SIGNAL_DECIDER_HPP_ #define BEHAVIOR_PATH_PLANNER__TURN_SIGNAL_DECIDER_HPP_ -#include +#include #include #include @@ -45,18 +45,42 @@ const std::map signal_map = { {"straight", TurnIndicatorsCommand::DISABLE}, {"none", TurnIndicatorsCommand::DISABLE}}; +struct TurnSignalInfo +{ + TurnSignalInfo() + { + turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + hazard_signal.command = HazardLightsCommand::NO_COMMAND; + } + + // desired turn signal + TurnIndicatorsCommand turn_signal; + HazardLightsCommand hazard_signal; + + geometry_msgs::msg::Pose desired_start_point; + geometry_msgs::msg::Pose desired_end_point; + geometry_msgs::msg::Pose required_start_point; + geometry_msgs::msg::Pose required_end_point; +}; + class TurnSignalDecider { public: TurnIndicatorsCommand getTurnSignal( - const std::shared_ptr & planner_data, const PathWithLaneId & path, - const TurnSignalInfo & turn_signal_info); + const std::shared_ptr & route_handler, const PathWithLaneId & path, + const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel, + const BehaviorPathPlannerParameters & parameters); TurnIndicatorsCommand resolve_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info, const double nearest_dist_threshold, const double nearest_yaw_threshold); + TurnSignalInfo use_prior_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold); + void setParameters( const double base_link2front, const double intersection_search_distance, const double intersection_search_time, const double intersection_angle_threshold_deg) 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 87edf64c97674..7adb135da3f61 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -313,7 +313,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const double turn_signal_intersection_angle_threshold_deg = planner_data_->parameters.turn_signal_intersection_angle_threshold_deg; const double turn_signal_search_time = planner_data_->parameters.turn_signal_search_time; - turn_signal_decider_.setParameters( + planner_data_->turn_signal_decider.setParameters( planner_data_->parameters.base_link2front, turn_signal_intersection_search_distance, turn_signal_search_time, turn_signal_intersection_angle_threshold_deg); } @@ -1249,7 +1249,7 @@ void BehaviorPathPlannerNode::computeTurnSignal( turn_signal.command = TurnIndicatorsCommand::DISABLE; hazard_signal.command = output.turn_signal_info.hazard_signal.command; } else { - turn_signal = turn_signal_decider_.getTurnSignal(planner_data, path, output.turn_signal_info); + turn_signal = planner_data->getTurnSignal(path, output.turn_signal_info); hazard_signal.command = HazardLightsCommand::DISABLE; } turn_signal.stamp = get_clock()->now(); @@ -1257,13 +1257,14 @@ void BehaviorPathPlannerNode::computeTurnSignal( turn_signal_publisher_->publish(turn_signal); hazard_signal_publisher_->publish(hazard_signal); - publish_steering_factor(turn_signal); + publish_steering_factor(planner_data, turn_signal); } -void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsCommand & turn_signal) +void BehaviorPathPlannerNode::publish_steering_factor( + const std::shared_ptr & planner_data, const TurnIndicatorsCommand & turn_signal) { const auto [intersection_flag, approaching_intersection_flag] = - turn_signal_decider_.getIntersectionTurnSignalFlag(); + planner_data->turn_signal_decider.getIntersectionTurnSignalFlag(); if (intersection_flag || approaching_intersection_flag) { const uint16_t steering_factor_direction = std::invoke([&turn_signal]() { if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { @@ -1273,7 +1274,7 @@ void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsComman }); const auto [intersection_pose, intersection_distance] = - turn_signal_decider_.getIntersectionPoseAndDistance(); + planner_data->turn_signal_decider.getIntersectionPoseAndDistance(); const uint16_t steering_factor_state = std::invoke([&intersection_flag]() { if (intersection_flag) { return SteeringFactor::TURNING; diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index a69fdea549646..9234f06bf897d 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -39,34 +39,29 @@ double calc_distance( } TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( - const std::shared_ptr & planner_data, const PathWithLaneId & path, - const TurnSignalInfo & turn_signal_info) + const std::shared_ptr & route_handler, const PathWithLaneId & path, + const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel, + const BehaviorPathPlannerParameters & parameters) { // Guard if (path.points.empty()) { return turn_signal_info.turn_signal; } - // Data - const double nearest_dist_threshold = planner_data->parameters.ego_nearest_dist_threshold; - const double nearest_yaw_threshold = planner_data->parameters.ego_nearest_yaw_threshold; - const auto & current_pose = planner_data->self_odometry->pose.pose; - const double & current_vel = planner_data->self_odometry->twist.twist.linear.x; - const auto route_handler = *(planner_data->route_handler); - // Get current lanelets - const double forward_length = planner_data->parameters.forward_path_length; + const double forward_length = parameters.forward_path_length; + const double nearest_dist_threshold = parameters.ego_nearest_dist_threshold; + const double nearest_yaw_threshold = parameters.ego_nearest_yaw_threshold; const double backward_length = 50.0; - const lanelet::ConstLanelets current_lanes = utils::calcLaneAroundPose( - planner_data->route_handler, current_pose, forward_length, backward_length); + const lanelet::ConstLanelets current_lanes = + utils::calcLaneAroundPose(route_handler, current_pose, forward_length, backward_length); if (current_lanes.empty()) { return turn_signal_info.turn_signal; } const PathWithLaneId extended_path = utils::getCenterLinePath( - route_handler, current_lanes, current_pose, backward_length, forward_length, - planner_data->parameters); + *route_handler, current_lanes, current_pose, backward_length, forward_length, parameters); if (extended_path.points.empty()) { return turn_signal_info.turn_signal; @@ -78,7 +73,7 @@ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( // Get closest intersection turn signal if exists const auto intersection_turn_signal_info = getIntersectionTurnSignalInfo( - extended_path, current_pose, current_vel, ego_seg_idx, route_handler, nearest_dist_threshold, + extended_path, current_pose, current_vel, ego_seg_idx, *route_handler, nearest_dist_threshold, nearest_yaw_threshold); if (!intersection_turn_signal_info) { @@ -333,6 +328,79 @@ TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal( return intersection_signal_info.turn_signal; } +TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold) +{ + const auto get_distance = [&](const Pose & input_point) { + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); + }; + + const auto & original_desired_start_point = original_signal.desired_start_point; + const auto & original_desired_end_point = original_signal.desired_end_point; + const auto & original_required_start_point = original_signal.required_start_point; + const auto & original_required_end_point = original_signal.required_end_point; + const auto & new_desired_start_point = new_signal.desired_start_point; + const auto & new_desired_end_point = new_signal.desired_end_point; + const auto & new_required_start_point = new_signal.required_start_point; + const auto & new_required_end_point = new_signal.required_end_point; + + const double dist_to_original_desired_start = + get_distance(original_desired_start_point) - base_link2front_; + const double dist_to_original_desired_end = get_distance(original_desired_end_point); + const double dist_to_original_required_start = + get_distance(original_required_start_point) - base_link2front_; + const double dist_to_original_required_end = get_distance(original_required_end_point); + const double dist_to_new_desired_start = get_distance(new_desired_start_point) - base_link2front_; + const double dist_to_new_desired_end = get_distance(new_desired_end_point); + const double dist_to_new_required_start = + get_distance(new_required_start_point) - base_link2front_; + const double dist_to_new_required_end = get_distance(new_required_end_point); + + // If we still do not reach the desired front point we ignore it + if (dist_to_original_desired_start > 0.0 && dist_to_new_desired_start > 0.0) { + TurnSignalInfo empty_signal_info; + return empty_signal_info; + } else if (dist_to_original_desired_start > 0.0) { + return new_signal; + } else if (dist_to_new_desired_start > 0.0) { + return original_signal; + } + + // If we already passed the desired end point, return the other signal + if (dist_to_original_desired_end < 0.0 && dist_to_new_desired_end < 0.0) { + TurnSignalInfo empty_signal_info; + return empty_signal_info; + } else if (dist_to_original_desired_end < 0.0) { + return new_signal; + } else if (dist_to_new_desired_end < 0.0) { + return original_signal; + } + + if (dist_to_original_desired_start <= dist_to_new_desired_start) { + const auto enable_prior = use_prior_turn_signal( + dist_to_original_required_start, dist_to_original_required_end, dist_to_new_required_start, + dist_to_new_required_end); + + if (enable_prior) { + return original_signal; + } + return new_signal; + } + + const auto enable_prior = use_prior_turn_signal( + dist_to_new_required_start, dist_to_new_required_end, dist_to_original_required_start, + dist_to_original_required_end); + if (enable_prior) { + return new_signal; + } + return original_signal; +} + bool TurnSignalDecider::use_prior_turn_signal( const double dist_to_prior_required_start, const double dist_to_prior_required_end, const double dist_to_subsequent_required_start, const double dist_to_subsequent_required_end)