From 6be818ae36b00d7373c7f2ec707f539103c7f50e Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Mon, 9 Sep 2024 18:37:56 +0900 Subject: [PATCH] add params Signed-off-by: Yuki Takagi --- .../src/manager.cpp | 7 ++ .../src/scene_crosswalk.cpp | 64 ++++++++----------- .../src/scene_crosswalk.hpp | 5 ++ 3 files changed, 38 insertions(+), 38 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index 5ea2d42782317..92015fb6308ca 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -49,10 +49,17 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_crosswalk"); cp.stop_distance_from_object = getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object"); + cp.stop_distance_from_object_preferd = + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object_preferd"); + cp.stop_distance_from_object_limit = + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object_limit"); cp.far_object_threshold = getOrDeclareParameter(node, ns + ".stop_position.far_object_threshold"); cp.stop_position_threshold = getOrDeclareParameter(node, ns + ".stop_position.stop_position_threshold"); + cp.min_acc_prefered = getOrDeclareParameter(node, ns + ".stop_position.min_acc_preferd"); + cp.min_jerk_prefered = + getOrDeclareParameter(node, ns + ".stop_position.min_jerk_preferd"); // param for ego velocity cp.min_slow_down_velocity = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 666f5076fc883..a54864739234c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -396,37 +396,29 @@ std::optional CrosswalkModule::calcStopPose( const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; const auto default_stop_opt = [&]() -> std::optional { - if (!default_stop_pose_opt.has_value()) { - return std::nullopt; - } + if (!default_stop_pose_opt.has_value()) return std::nullopt; + return StopCandidate{ + default_stop_pose_opt.value(), + calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position)}; + }(); + + const auto ped_stop_pref_opt = [&]() -> std::optional { const double dist = - calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position); - return StopCandidate{default_stop_pose_opt.value(), dist}; + dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object_preferd; + const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist); + if (!pose_opt.has_value()) return std::nullopt; + return StopCandidate{pose_opt.value(), dist}; }(); - const auto ped_stop_limit = [&]() -> std::optional { + const auto ped_stop_limit_opt = [&]() -> std::optional { const double dist = - dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object; + dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object_limit; const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist); - if (!pose_opt.has_value()) { - return std::nullopt; - } + if (!pose_opt.has_value()) return std::nullopt; return StopCandidate{pose_opt.value(), dist}; }(); - if (!ped_stop_limit.has_value()) { - return std::nullopt; - } const auto without_acc_stop = [&]() -> std::optional { - const auto ped_stop_pref_opt = [&]() -> std::optional { - const double dist = - dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object; - const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist); - if (!pose_opt.has_value()) { - return std::nullopt; - } - return StopCandidate{pose_opt.value(), dist}; - }(); if (!ped_stop_pref_opt.has_value()) { // RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin."); return std::nullopt; @@ -438,33 +430,28 @@ std::optional CrosswalkModule::calcStopPose( return ped_stop_pref_opt; } }(); - if (!without_acc_stop.has_value()) { - return std::nullopt; - } + if (!without_acc_stop.has_value()) return std::nullopt; const auto weak_brk_stop = [&]() -> std::optional { const auto dist_opt = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( - ego_vel, 0.0, ego_acc, p.min_acc_for_no_stop_decision, p.max_jerk_for_no_stop_decision, - p.min_jerk_for_no_stop_decision); - if (!dist_opt.has_value()) { - return std::nullopt; - } + ego_vel, 0.0, ego_acc, p.min_acc_prefered, 10.0, p.min_jerk_prefered); + if (!dist_opt.has_value()) return std::nullopt; const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist_opt.value()); - if (!pose_opt.has_value()) { - return std::nullopt; - } + if (!pose_opt.has_value()) return std::nullopt; return StopCandidate{pose_opt.value(), dist_opt.value()}; }(); if (!weak_brk_stop.has_value()) { + // RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin."); return std::nullopt; } - const auto decided_stop_pos = [&]() { + + const auto selected_stop = [&]() { if (weak_brk_stop->dist < without_acc_stop->dist) { return without_acc_stop.value(); - } else if (weak_brk_stop->dist < ped_stop_limit->dist) { + } else if (weak_brk_stop->dist < ped_stop_limit_opt->dist) { return weak_brk_stop.value(); } else { - return ped_stop_limit.value(); + return ped_stop_limit_opt.value(); } }(); @@ -474,11 +461,12 @@ std::optional CrosswalkModule::calcStopPose( p.min_jerk_for_no_stop_decision); return strong_brk_dist_opt ? strong_brk_dist_opt.value() : 0.0; }(); - if (decided_stop_pos.dist < strong_brk_dist) { + if (selected_stop.dist < strong_brk_dist) { + // RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin."); return std::nullopt; } - return std::make_optional(decided_stop_pos.pose); + return std::make_optional(selected_stop.pose); } std::pair CrosswalkModule::getAttentionRange( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index b0801645df6c3..65ef7a64b622f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -112,9 +112,14 @@ class CrosswalkModule : public SceneModuleInterface bool show_processing_time; // param for stop position double stop_distance_from_object; + double stop_distance_from_object_preferd; + double stop_distance_from_object_limit; double stop_distance_from_crosswalk; double far_object_threshold; double stop_position_threshold; + double min_acc_prefered; + double min_jerk_prefered; + // param for ego velocity float min_slow_down_velocity; double max_slow_down_jerk;