From c286f7ca14bde7fc4634574af318720fbef79f3b Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Tue, 28 Jun 2022 18:57:32 +0900 Subject: [PATCH] fix(intersection_module): remove decel state (#1178) * fix(intersection_module): remove decel mode Signed-off-by: 1222-takeshi * Update planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp Co-authored-by: Yukihiro Saito Co-authored-by: Yukihiro Saito --- .../intersection/scene_intersection.hpp | 1 - .../src/scene_module/intersection/manager.cpp | 2 -- .../intersection/scene_intersection.cpp | 13 +------------ .../intersection/scene_merge_from_private_road.cpp | 14 +++++--------- 4 files changed, 6 insertions(+), 24 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 04352513f927a..0868bd3712d7a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -103,7 +103,6 @@ class IntersectionModule : public SceneModuleInterface struct PlannerParam { double state_transit_margin_time; - double decel_velocity; //! used when in straight and traffic_light lane double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check double diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 602194866a78d..05cc130c3b0f1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -32,7 +32,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) auto & ip = intersection_param_; const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.state_transit_margin_time = node.declare_parameter(ns + ".state_transit_margin_time", 2.0); - ip.decel_velocity = node.declare_parameter(ns + ".decel_velocity", 30.0 / 3.6); ip.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0); ip.stuck_vehicle_detect_dist = node.declare_parameter(ns + ".stuck_vehicle_detect_dist", 3.0); ip.stuck_vehicle_ignore_dist = node.declare_parameter(ns + ".stuck_vehicle_ignore_dist", 5.0) + @@ -60,7 +59,6 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node auto & mp = merge_from_private_area_param_; mp.stop_duration_sec = node.declare_parameter(ns + ".merge_from_private_area.stop_duration_sec", 1.0); - mp.decel_velocity = node.get_parameter("intersection.decel_velocity").as_double(); mp.detection_area_length = node.get_parameter("intersection.detection_area_length").as_double(); mp.stop_line_margin = node.get_parameter("intersection.stop_line_margin").as_double(); } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 905b9601aa1dc..f2774ac9f524f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -181,11 +181,9 @@ bool IntersectionModule::modifyPathVelocity( state_machine_.setStateWithMarginTime( isActivated() ? State::GO : State::STOP, logger_.get_child("state_machine"), *clock_); - /* set stop speed : TODO behavior on straight lane should be improved*/ - const bool is_stop_required = is_stuck || !has_traffic_light_ || turn_direction_ != "straight"; const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - setSafe(!(is_stop_required && is_entry_prohibited)); + setSafe(!is_entry_prohibited); setDistance(tier4_autoware_utils::calcSignedArcLength( input_path.points, planner_data_->current_pose.pose.position, input_path.points.at(stop_line_idx).point.pose.position)); @@ -213,15 +211,6 @@ bool IntersectionModule::modifyPathVelocity( return true; } - if (state_machine_.getState() == State::STOP && is_entry_prohibited) { - const double v = planner_param_.decel_velocity; - util::setVelocityFrom(stop_line_idx, v, path); - setSafe(true); - - debug_data_.stop_required = false; - debug_data_.slow_wall_pose = util::getAheadPose(stop_line_idx, base_link2front, *path); - } - RCLCPP_DEBUG(logger_, "===== plan end ====="); return true; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index 9d8374d25c14d..eb4984eecc49d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -110,18 +110,14 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( /* set stop speed */ if (state_machine_.getState() == State::STOP) { - constexpr double stop_vel = 0.0; - const double decel_vel = planner_param_.decel_velocity; - double v = (has_traffic_light_ && turn_direction_ == "straight") ? decel_vel : stop_vel; + constexpr double v = 0.0; util::setVelocityFrom(stop_line_idx, v, path); /* get stop point and stop factor */ - if (v == stop_vel) { - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = debug_data_.stop_point_pose; - stop_factor.stop_factor_points.emplace_back(debug_data_.first_collision_point); - planning_utils::appendStopReason(stop_factor, stop_reason); - } + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = debug_data_.stop_point_pose; + stop_factor.stop_factor_points.emplace_back(debug_data_.first_collision_point); + planning_utils::appendStopReason(stop_factor, stop_reason); const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength( path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position);