From d8e84255c49c72f1c825e995b16ee304b2a1127d Mon Sep 17 00:00:00 2001 From: beyza Date: Mon, 9 Sep 2024 10:58:22 +0300 Subject: [PATCH] shift path according to traffic flow if lanes is bidirectional Signed-off-by: beyza --- .../config/behavior_path_planner.param.yaml | 2 + .../behavior_path_planner_node.hpp | 2 + .../src/behavior_path_planner_node.cpp | 66 ++++++++++++++++++- .../data_manager.hpp | 8 +++ .../parameters.hpp | 4 ++ 5 files changed, 81 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml index 41f8f53a8f17c..ac84ec49627ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml @@ -19,6 +19,8 @@ turn_signal_shift_length_threshold: 0.3 turn_signal_remaining_shift_length_threshold: 0.1 turn_signal_on_swerving: true + check_bidirectional_lane: true + traffic_flow: "left_side" # right or left enable_akima_spline_first: false enable_cog_on_centerline: false diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index 1965a0f909927..90636d751cb13 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -235,6 +235,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node const std::shared_ptr & path_candidate_ptr, const bool is_ready, const std::shared_ptr & planner_data); + PathWithLaneId shiftPath(const PathWithLaneId & ref_paths, const double shift_distance); + bool isLaneBidirectional(const lanelet::ConstLanelets & current_lanelets); std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 2f9c82f46d3bc..592d82dfe1461 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -383,6 +383,9 @@ void BehaviorPathPlannerNode::run() // NOTE: In order to keep backward_path_length at least, resampling interval is added to the // backward. const auto current_pose = planner_data_->self_odometry->pose.pose; + + const auto current_lanelets = planner_data_->route_handler->getRoadLaneletsAtPose(current_pose); + const auto is_bidirectional = isLaneBidirectional(current_lanelets); if (!path->points.empty()) { const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(path->points); path->points = autoware::motion_utils::cropPoints( @@ -390,7 +393,19 @@ void BehaviorPathPlannerNode::run() planner_data_->parameters.forward_path_length, planner_data_->parameters.backward_path_length + planner_data_->parameters.input_path_interval); - + if (planner_data_->parameters.check_bidirectional_lane && is_bidirectional) { + double bound_to_centerline = 0.0; + for (const auto & current_lanelet : current_lanelets) { + bound_to_centerline = lanelet::geometry::distance2d( + lanelet::utils::to2D(current_lanelet.leftBound().basicLineString()), + lanelet::utils::to2D(current_lanelet.centerline().basicLineString())); + } + if (planner_data_->parameters.traffic_flow == "right_side") { + path->points = shiftPath(*path, bound_to_centerline * 0.5).points; + } else if (planner_data_->parameters.traffic_flow == "left_side") { + path->points = shiftPath(*path, -bound_to_centerline * 0.5).points; + } + } if (!path->points.empty()) { path_publisher_->publish(*path); published_time_publisher_->publish_if_subscribed(path_publisher_, path->header.stamp); @@ -434,6 +449,55 @@ void BehaviorPathPlannerNode::run() RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n"); } +PathWithLaneId BehaviorPathPlannerNode::shiftPath( + const PathWithLaneId & path, const double shift_distance) +{ + PathWithLaneId shifted_path; + shifted_path.header.stamp = path.header.stamp; + shifted_path.points.reserve(path.points.size()); + for (const auto & pose : path.points) { + tier4_planning_msgs::msg::PathPointWithLaneId shifted_pose{}; + shifted_pose = pose; + // Get yaw from quaternion + tf2::Quaternion quat( + pose.point.pose.orientation.x, pose.point.pose.orientation.y, pose.point.pose.orientation.z, + pose.point.pose.orientation.w); + tf2::Matrix3x3 mat(quat); + double roll, pitch, yaw; + mat.getRPY(roll, pitch, yaw); + + // Calculate the lateral shift + double delta_x = shift_distance * cos(yaw + M_PI_2); + double delta_y = shift_distance * sin(yaw + M_PI_2); + + // Apply the shift + shifted_pose.point.pose.position.x += delta_x; + shifted_pose.point.pose.position.y += delta_y; + + shifted_path.points.push_back(shifted_pose); + } + + return shifted_path; +} + +bool BehaviorPathPlannerNode::isLaneBidirectional(const lanelet::ConstLanelets & current_lanelets) +{ + for (const auto & current_lanelet : current_lanelets) { + const auto left_bound_id = current_lanelet.leftBound().id(); + const auto right_bound_id = current_lanelet.rightBound().id(); + for (const auto & other_lanelet : current_lanelets) { + if (current_lanelet != other_lanelet) { + if ( + left_bound_id == other_lanelet.rightBound().id() || + right_bound_id == other_lanelet.leftBound().id()) { + return true; + } + } + } + } + return false; +} + void BehaviorPathPlannerNode::computeTurnSignal( const std::shared_ptr planner_data, const PathWithLaneId & path, const BehaviorModuleOutput & output) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp index 27f0dc4e58178..10e368f99f692 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp @@ -88,6 +88,12 @@ struct ModuleNameStamped rclcpp::Time stamp{0, 0, RCL_ROS_TIME}; }; +struct LaneletData +{ + lanelet::Lanelet lanelet; + bool is_bidirectional{false}; +}; + struct DrivableLanes { lanelet::ConstLanelet right_lane; @@ -240,7 +246,9 @@ struct PlannerData node.declare_parameter("ego_nearest_dist_threshold"); parameters.ego_nearest_yaw_threshold = node.declare_parameter("ego_nearest_yaw_threshold"); + parameters.check_bidirectional_lane = node.declare_parameter("check_bidirectional_lane"); + parameters.traffic_flow = node.declare_parameter("traffic_flow"); drivable_area_expansion_parameters.init(node); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp index ecbe9b5208347..0a7d81312ff88 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp @@ -17,6 +17,8 @@ #include +#include + struct ModuleConfigParameters { bool enable_module{false}; @@ -52,6 +54,8 @@ struct BehaviorPathPlannerParameters double turn_signal_shift_length_threshold; double turn_signal_remaining_shift_length_threshold; bool turn_signal_on_swerving; + bool check_bidirectional_lane; + std::string traffic_flow{"left_side"}; bool enable_akima_spline_first; bool enable_cog_on_centerline;