From 3ea87ec25e98d48354f708c9d8df28018a1e1d12 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Mon, 22 Jan 2024 21:59:19 +0900 Subject: [PATCH 1/4] improve algorithm Signed-off-by: Yuki Takagi --- .../src/manager.cpp | 4 +- .../src/scene_crosswalk.cpp | 46 +++++++++---------- .../src/scene_crosswalk.hpp | 2 +- 3 files changed, 26 insertions(+), 26 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index dc3ce32be8505..094369b0a7a3c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -68,8 +68,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity"); cp.max_stuck_vehicle_lateral_offset = getOrDeclareParameter(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); - cp.stuck_vehicle_attention_range = - getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_attention_range"); + cp.required_clearance = + getOrDeclareParameter(node, ns + ".stuck_vehicle.required_clearance"); cp.min_acc_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.min_acc"); cp.max_jerk_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.max_jerk"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 48ae5ff03b8bd..6f2f042c5f5e6 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -896,6 +896,8 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( } } + const auto & ego_pos = planner_data_->current_odometry->pose.position; + for (const auto & object : objects) { if (!isVehicle(object)) { continue; @@ -906,34 +908,35 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( continue; } - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pos); + const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pose.position); if (p.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) { continue; } - const auto & ego_pos = planner_data_->current_odometry->pose.position; - const auto ego_vel = planner_data_->current_velocity->twist.linear.x; - const auto ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; - - const double near_attention_range = - calcSignedArcLength(ego_path.points, ego_pos, path_intersects.back()); - const double far_attention_range = near_attention_range + p.stuck_vehicle_attention_range; - - const auto dist_ego2obj = calcSignedArcLength(ego_path.points, ego_pos, obj_pos); - - if (near_attention_range < dist_ego2obj && dist_ego2obj < far_attention_range) { + // check if STOP is required + const double crosswalk_front_to_obj_rear = + calcSignedArcLength(ego_path.points, path_intersects.front(), obj_pose.position) - + object.shape.dimensions.x / 2.0; + const double crosswalk_back_to_obj_rear = + calcSignedArcLength(ego_path.points, path_intersects.back(), obj_pose.position) - + object.shape.dimensions.x / 2.0; + const double required_space_length = + planner_data_->vehicle_info_.vehicle_length_m + planner_param_.required_clearance; + + if (crosswalk_front_to_obj_rear > 0.0 && crosswalk_back_to_obj_rear < required_space_length) { // Plan STOP considering min_acc, max_jerk and min_jerk. - const auto min_feasible_dist_ego2stop = calcDecelDistWithJerkAndAccConstraints( - ego_vel, 0.0, ego_acc, p.min_acc_for_stuck_vehicle, p.max_jerk_for_stuck_vehicle, - p.min_jerk_for_stuck_vehicle); - if (!min_feasible_dist_ego2stop) { + const auto braking_distance = calcDecelDistWithJerkAndAccConstraints( + planner_data_->current_velocity->twist.linear.x, 0.0, + planner_data_->current_acceleration->accel.accel.linear.x, p.min_acc_for_stuck_vehicle, + p.max_jerk_for_stuck_vehicle, p.min_jerk_for_stuck_vehicle); + if (!braking_distance) { continue; } const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, stop_pose->position); - const double feasible_dist_ego2stop = std::max(*min_feasible_dist_ego2stop, dist_ego2stop); + const double feasible_dist_ego2stop = std::max(*braking_distance, dist_ego2stop); const double dist_to_ego = calcSignedArcLength(ego_path.points, ego_path.points.front().point.pose.position, ego_pos); @@ -943,11 +946,8 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( continue; } - setObjectsOfInterestData( - object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); - - // early return may not appropriate because the nearest in range object should be handled - return createStopFactor(*feasible_stop_pose, {obj_pos}); + setObjectsOfInterestData(obj_pose, object.shape, ColorName::RED); + return createStopFactor(*feasible_stop_pose, {obj_pose.position}); } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 247907ad2dc80..ed1e342df9f7a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -124,7 +124,7 @@ class CrosswalkModule : public SceneModuleInterface bool enable_stuck_check_in_intersection{false}; double stuck_vehicle_velocity; double max_stuck_vehicle_lateral_offset; - double stuck_vehicle_attention_range; + double required_clearance; double min_acc_for_stuck_vehicle; double max_jerk_for_stuck_vehicle; double min_jerk_for_stuck_vehicle; From 252b93e48bcbb03a4dd403c856b815ea630e4c04 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 23 Jan 2024 15:13:45 +0900 Subject: [PATCH 2/4] refactor Signed-off-by: Yuki Takagi --- .../src/scene_crosswalk.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 6f2f042c5f5e6..099ab88c00a1a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -896,8 +896,6 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( } } - const auto & ego_pos = planner_data_->current_odometry->pose.position; - for (const auto & object : objects) { if (!isVehicle(object)) { continue; @@ -925,25 +923,27 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( planner_data_->vehicle_info_.vehicle_length_m + planner_param_.required_clearance; if (crosswalk_front_to_obj_rear > 0.0 && crosswalk_back_to_obj_rear < required_space_length) { - // Plan STOP considering min_acc, max_jerk and min_jerk. + // If there exists at least one vehicle ahead, plan STOP considering min_acc, max_jerk and + // min_jerk. Note that nearest search is not required because the stop pose independends to + // the vehicles. const auto braking_distance = calcDecelDistWithJerkAndAccConstraints( planner_data_->current_velocity->twist.linear.x, 0.0, planner_data_->current_acceleration->accel.accel.linear.x, p.min_acc_for_stuck_vehicle, p.max_jerk_for_stuck_vehicle, p.min_jerk_for_stuck_vehicle); if (!braking_distance) { - continue; + return {}; } + const auto & ego_pos = planner_data_->current_odometry->pose.position; const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, stop_pose->position); const double feasible_dist_ego2stop = std::max(*braking_distance, dist_ego2stop); const double dist_to_ego = calcSignedArcLength(ego_path.points, ego_path.points.front().point.pose.position, ego_pos); - const auto feasible_stop_pose = calcLongitudinalOffsetPose(ego_path.points, 0, dist_to_ego + feasible_dist_ego2stop); if (!feasible_stop_pose) { - continue; + return {}; } setObjectsOfInterestData(obj_pose, object.shape, ColorName::RED); From 3082354cda1f79051c0632af13a0d86a361ace92 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Tue, 23 Jan 2024 15:24:02 +0900 Subject: [PATCH 3/4] sync param Signed-off-by: Yuki Takagi --- .../config/crosswalk.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index aa5dfed1e4bc5..29079b99b6718 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -30,7 +30,7 @@ enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection stuck_vehicle_velocity: 1.0 # [m/s] threshold velocity whether other vehicles are assumed to be stuck or not. max_stuck_vehicle_lateral_offset: 2.0 # [m] The feature does not handle the vehicles farther than this value to the ego's path. - stuck_vehicle_attention_range: 10.0 # [m] Ego does not enter the crosswalk area if a stuck vehicle exists within this distance from the end of the crosswalk on the ego's path. + required_clearance: 6.0 # [m] clearance to be secured between the ego and the ahead vehicle min_acc: -1.0 # min acceleration [m/ss] min_jerk: -1.0 # min jerk [m/sss] max_jerk: 1.0 # max jerk [m/sss] From 2ce6841a7b1b10a659278f76f4b96dd63ecd3175 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 25 Jan 2024 12:50:09 +0900 Subject: [PATCH 4/4] fix spell Signed-off-by: Yuki Takagi --- .../behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 099ab88c00a1a..cee6975df3155 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -924,7 +924,7 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( if (crosswalk_front_to_obj_rear > 0.0 && crosswalk_back_to_obj_rear < required_space_length) { // If there exists at least one vehicle ahead, plan STOP considering min_acc, max_jerk and - // min_jerk. Note that nearest search is not required because the stop pose independends to + // min_jerk. Note that nearest search is not required because the stop pose independents to // the vehicles. const auto braking_distance = calcDecelDistWithJerkAndAccConstraints( planner_data_->current_velocity->twist.linear.x, 0.0,