From 8cf1fd5225e101281823c9e48641d55970efceff Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 10 Aug 2023 10:53:24 +0900 Subject: [PATCH] renamed to common.stop_velocity_threshold Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 2 +- .../behavior_velocity_intersection_module/src/manager.cpp | 4 ++-- .../src/scene_intersection.cpp | 3 ++- .../src/scene_intersection.hpp | 2 +- 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index cc9c5c3ba9a17..2ad53467be7db 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -12,6 +12,7 @@ use_intersection_area: false path_interpolation_ds: 0.1 # [m] consider_wrong_direction_vehicle: false + stop_velocity_threshold: 0.1388 # [m/s] stuck_vehicle: use_stuck_stopline: true # stopline generated before the first conflicting area stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. @@ -48,7 +49,6 @@ denoise_kernel: 1.0 # [m] possible_object_bbox: [1.0, 2.0] # [m x m] ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h - first_stop_velocity_threshold: 0.2777 # [m/s] enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval intersection: true diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 7255b3502e710..d64af3c3b293d 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -60,6 +60,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".common.path_interpolation_ds"); ip.common.consider_wrong_direction_vehicle = node.declare_parameter(ns + ".common.consider_wrong_direction_vehicle"); + ip.common.stop_velocity_threshold = + node.declare_parameter(ns + ".common.stop_velocity_threshold"); ip.stuck_vehicle.use_stuck_stopline = node.declare_parameter(ns + ".stuck_vehicle.use_stuck_stopline"); @@ -117,8 +119,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node.declare_parameter>(ns + ".occlusion.possible_object_bbox"); ip.occlusion.ignore_parked_vehicle_speed_threshold = node.declare_parameter(ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); - ip.occlusion.first_stop_velocity_threshold = - node.declare_parameter(ns + ".occlusion.first_stop_velocity_threshold"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 5a5ec77842fa7..ad55d495dff92 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -768,6 +768,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool approached_stop_line = (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); const bool is_stopped = planner_data_->isVehicleStopped(); + // TODO(Mamoru Sobue): fix isVehicleStopped() if (is_stopped && approached_stop_line) { stuck_private_area_timeout_.setStateWithMarginTime( StateMachine::State::GO, logger_.get_child("stuck_private_area_timeout"), *clock_); @@ -891,7 +892,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); const bool over_stop_line = (dist_stopline < 0.0); const double vel = std::fabs(planner_data_->current_velocity->twist.linear.x); - const bool is_stopped = (vel < planner_param_.occlusion.first_stop_vel_thr); + const bool is_stopped = (vel < planner_param_.common.stop_velocity_threshold); if (over_stop_line) { before_creep_state_machine_.setState(StateMachine::State::GO); } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 5e672ee6ea7f4..d6059ae909f31 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -60,6 +60,7 @@ class IntersectionModule : public SceneModuleInterface bool use_intersection_area; bool consider_wrong_direction_vehicle; double path_interpolation_ds; + double stop_velocity_threshold; } common; struct StuckVehicle { @@ -109,7 +110,6 @@ class IntersectionModule : public SceneModuleInterface double denoise_kernel; std::vector possible_object_bbox; double ignore_parked_vehicle_speed_threshold; - double first_stop_velocity_threshold; } occlusion; };