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 92015fb6308ca..9740500031772 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 @@ -24,6 +24,8 @@ #include #include + + namespace autoware::behavior_velocity_planner { @@ -57,9 +59,9 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) 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_acc_prefered = getOrDeclareParameter(node, ns + ".stop_position.min_acc_prefered"); cp.min_jerk_prefered = - getOrDeclareParameter(node, ns + ".stop_position.min_jerk_preferd"); + getOrDeclareParameter(node, ns + ".stop_position.min_jerk_prefered"); // 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 a54864739234c..63d7381e31f6f 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 @@ -38,6 +38,36 @@ #include #include +#define debug(var) \ + do { \ + std::cerr << __func__ << ": " << __LINE__ << ", " << #var << " : "; \ + view(var); \ + } while (0) +template +void view(T e) +{ + std::cerr << e << std::endl; +} +template +void view(const std::vector & v) +{ + for (const auto & e : v) { + std::cerr << e << " "; + } + std::cerr << std::endl; +} +template +void view(const std::vector> & vv) +{ + for (const auto & v : vv) { + view(v); + } +} +#define line() \ + { \ + std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \ + } + namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -410,18 +440,11 @@ std::optional CrosswalkModule::calcStopPose( return StopCandidate{pose_opt.value(), dist}; }(); - const auto ped_stop_limit_opt = [&]() -> std::optional { - const double dist = - 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; - return StopCandidate{pose_opt.value(), dist}; - }(); - - const auto without_acc_stop = [&]() -> std::optional { + const auto without_acc_stop_opt = [&]() -> std::optional { if (!ped_stop_pref_opt.has_value()) { - // RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin."); return std::nullopt; + // RCLCPP_DEBUG(logger_, "Failure to calculate stop_pose agaist a pedestrian with margin."); + // return weak_brk_stop; だめ limitへのアプローチからオーバーランする } else if ( default_stop_opt.has_value() && ped_stop_pref_opt->dist > default_stop_opt->dist && ped_stop_pref_opt->dist < default_stop_opt->dist + planner_param_.far_object_threshold) { @@ -430,28 +453,37 @@ std::optional CrosswalkModule::calcStopPose( return ped_stop_pref_opt; } }(); - if (!without_acc_stop.has_value()) return std::nullopt; + // if (!without_acc_stop_opt.has_value()) return std::nullopt; + + const auto ped_stop_limit = [&]() -> std::optional { + const double dist = + 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; + return StopCandidate{pose_opt.value(), dist}; + }(); + if (!ped_stop_limit.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_prefered, 10.0, p.min_jerk_prefered); + ego_vel, 0.0, ego_acc, p.min_acc_prefered, 1.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; 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; - } + if (!weak_brk_stop.has_value()) return std::nullopt; 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_opt->dist) { + // debug(weak_brk_stop->dist); + // debug(without_acc_stop_opt->dist); + // debug(ped_stop_limit->dist); + if (without_acc_stop_opt.has_value() && weak_brk_stop->dist < without_acc_stop_opt->dist) { + return without_acc_stop_opt.value(); + } else if (weak_brk_stop->dist < ped_stop_limit->dist) { return weak_brk_stop.value(); } else { - return ped_stop_limit_opt.value(); + return ped_stop_limit.value(); } }();