From 057d3d4259f69e0c83e7897a954ddac3f4818fcc Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 6 Oct 2023 20:16:42 +0900 Subject: [PATCH 1/3] feat(lane_change): separate execution and cancel safety check param (#5246) Signed-off-by: Zulfaqar Azmi --- .../config/lane_change/lane_change.param.yaml | 26 ++++--- .../lane_change/lane_change_module_data.hpp | 1 + .../src/scene_module/lane_change/manager.cpp | 70 +++++++++++++------ 3 files changed, 66 insertions(+), 31 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index fda7390abdb9e..e7f3b51bd2d26 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -28,15 +28,23 @@ # safety check safety_check: - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.8 + allow_loose_check_for_cancel: true + execution: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + cancel: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -2.0 + rear_vehicle_reaction_time: 1.5 + rear_vehicle_safety_time_margin: 0.8 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 2.5 + longitudinal_velocity_delta_time: 0.6 # lane expansion for object filtering lane_expansion: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 0ba35635a9143..24a507d1f8695 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -82,6 +82,7 @@ struct LaneChangeParameters utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; // safety check + bool allow_loose_check_for_cancel{true}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 9c45f79d00263..e33a6c4b05ee3 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -92,35 +92,41 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.stop_time_threshold = getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); + // safety check + p.allow_loose_check_for_cancel = + getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_distance_min_threshold")); + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_velocity_delta_time")); - p.rss_params.front_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.expected_front_deceleration")); - p.rss_params.rear_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.expected_rear_deceleration")); - p.rss_params.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); - p.rss_params.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); - p.rss_params.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); + *node, parameter("safety_check.execution.longitudinal_velocity_delta_time")); + p.rss_params.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_front_deceleration")); + p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_rear_deceleration")); + p.rss_params.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.execution.rear_vehicle_reaction_time")); + p.rss_params.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.execution.rear_vehicle_safety_time_margin")); + p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.lateral_distance_max_threshold")); p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_distance_min_threshold")); + *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_velocity_delta_time")); + *node, parameter("safety_check.cancel.longitudinal_velocity_delta_time")); p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.expected_front_deceleration_for_abort")); + *node, parameter("safety_check.cancel.expected_front_deceleration")); p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.expected_rear_deceleration_for_abort")); - p.rss_params_for_abort.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); - p.rss_params_for_abort.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); - p.rss_params_for_abort.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); + *node, parameter("safety_check.cancel.expected_rear_deceleration")); + p.rss_params_for_abort.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.cancel.rear_vehicle_reaction_time")); + p.rss_params_for_abort.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin")); + p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.cancel.lateral_distance_max_threshold")); // target object { @@ -165,6 +171,26 @@ LaneChangeModuleManager::LaneChangeModuleManager( exit(EXIT_FAILURE); } + // validation of safety check parameters + // if loosely check is not allowed, lane change module will keep on chattering and canceling, and + // false positive situation might occur + if (!p.allow_loose_check_for_cancel) { + if ( + p.rss_params.front_vehicle_deceleration > p.rss_params_for_abort.front_vehicle_deceleration || + p.rss_params.rear_vehicle_deceleration > p.rss_params_for_abort.rear_vehicle_deceleration || + p.rss_params.rear_vehicle_reaction_time > p.rss_params_for_abort.rear_vehicle_reaction_time || + p.rss_params.rear_vehicle_safety_time_margin > + p.rss_params_for_abort.rear_vehicle_safety_time_margin || + p.rss_params.lateral_distance_max_threshold > + p.rss_params_for_abort.lateral_distance_max_threshold || + p.rss_params.longitudinal_distance_min_threshold > + p.rss_params_for_abort.longitudinal_distance_min_threshold || + p.rss_params.longitudinal_velocity_delta_time > + p.rss_params_for_abort.longitudinal_velocity_delta_time) { + RCLCPP_FATAL_STREAM(logger_, "abort parameter might be loose... Terminating the program..."); + exit(EXIT_FAILURE); + } + } if (p.cancel.delta_time < 1.0) { RCLCPP_WARN_STREAM( logger_, "cancel.delta_time: " << p.cancel.delta_time From 0c48944c343cd74a2acb26cf428f113eb536a7f6 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Sat, 7 Oct 2023 01:26:59 +0900 Subject: [PATCH 2/3] feat(intersection): yield initially on green light (#5234) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 4 + .../src/manager.cpp | 8 ++ .../src/scene_intersection.cpp | 76 ++++++++++++++++++- .../src/scene_intersection.hpp | 29 ++++--- 4 files changed, 103 insertions(+), 14 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index e210d2fb21b88..972f572890d16 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -37,6 +37,10 @@ keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity + yield_on_green_traffic_light: + distance_to_assigned_lanelet_start: 5.0 + duration: 2.0 + range: 30.0 # [m] occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 3c96a171516bd..4137090a5b6ae 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -109,6 +109,14 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".collision_detection.use_upstream_velocity"); ip.collision_detection.minimum_upstream_velocity = getOrDeclareParameter(node, ns + ".collision_detection.minimum_upstream_velocity"); + ip.collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start = + getOrDeclareParameter( + node, + ns + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start"); + ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); + ip.collision_detection.yield_on_green_traffic_light.range = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.range"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index b30a57f9e8dad..75bb4e861a60f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -30,14 +30,15 @@ #include #include +#include #include #include #include +#include #include #include #include - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -807,6 +808,36 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } +static bool isGreenSolidOn( + lanelet::ConstLanelet lane, const std::map & tl_infos) +{ + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + + std::optional tl_id = std::nullopt; + for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { + tl_id = tl_reg_elem->id(); + break; + } + if (!tl_id) { + // this lane has no traffic light + return false; + } + const auto tl_info_it = tl_infos.find(tl_id.value()); + if (tl_info_it == tl_infos.end()) { + // the info of this traffic light is not available + return false; + } + const auto & tl_info = tl_info_it->second; + for (auto && tl_light : tl_info.signal.elements) { + if ( + tl_light.color == TrafficSignalElement::GREEN && + tl_light.shape == TrafficSignalElement::CIRCLE) { + return true; + } + } + return false; +} + IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { @@ -994,13 +1025,52 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } + const auto target_objects = + filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); + + // If there are any vehicles on the attention area when ego entered the intersection on green + // light, do pseudo collision detection because the vehicles are very slow and no collisions may + // be detected. check if ego vehicle entered assigned lanelet + const bool is_green_solid_on = + isGreenSolidOn(assigned_lanelet, planner_data_->traffic_light_id_map); + if (is_green_solid_on) { + if (!initial_green_light_observed_time_) { + const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path->points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } + } + if (initial_green_light_observed_time_) { + const auto now = clock_->now(); + const bool exist_close_vehicles = std::any_of( + target_objects.objects.begin(), target_objects.objects.end(), [&](const auto & object) { + return tier4_autoware_utils::calcDistance3d( + object.kinematics.initial_pose_with_covariance.pose, current_pose) < + planner_param_.collision_detection.yield_on_green_traffic_light.range; + }); + if ( + exist_close_vehicles && + rclcpp::Duration((now - initial_green_light_observed_time_.value())).seconds() < + planner_param_.collision_detection.yield_on_green_traffic_light.duration) { + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + } + } + // calculate dynamic collision around attention area const double time_to_restart = (is_go_out_ || is_prioritized) ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - collision_state_machine_.getDuration()); - const auto target_objects = - filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); const bool has_collision = checkCollision( *path, target_objects, path_lanelets, closest_idx, diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index b5cf3ec04cdc3..de7d97a50c133 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -96,6 +96,12 @@ class IntersectionModule : public SceneModuleInterface double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr bool use_upstream_velocity; double minimum_upstream_velocity; + struct YieldOnGreeTrafficLight + { + double distance_to_assigned_lanelet_start; + double duration; + double range; + } yield_on_green_traffic_light; } collision_detection; struct Occlusion { @@ -234,36 +240,37 @@ class IntersectionModule : public SceneModuleInterface const std::string turn_direction_; const bool has_traffic_light_; - bool is_go_out_ = false; - bool is_permanent_go_ = false; - DecisionResult prev_decision_result_; + bool is_go_out_{false}; + bool is_permanent_go_{false}; + DecisionResult prev_decision_result_{Indecisive{""}}; // Parameter PlannerParam planner_param_; - std::optional intersection_lanelets_; + std::optional intersection_lanelets_{std::nullopt}; // for occlusion detection const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_; - // OcclusionState prev_occlusion_state_ = OcclusionState::NONE; + std::optional> occlusion_attention_divisions_{std::nullopt}; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; StateMachine temporal_stop_before_attention_state_machine_; - // NOTE: uuid_ is base member + // for pseudo-collision detection when ego just entered intersection on green light and upcoming + // vehicles are very slow + std::optional initial_green_light_observed_time_{std::nullopt}; // for stuck vehicle detection const bool is_private_area_; // for RTC const UUID occlusion_uuid_; - bool occlusion_safety_ = true; - double occlusion_stop_distance_; - bool occlusion_activated_ = true; + bool occlusion_safety_{true}; + double occlusion_stop_distance_{0.0}; + bool occlusion_activated_{true}; // for first stop in two-phase stop - bool occlusion_first_stop_required_ = false; + bool occlusion_first_stop_required_{false}; void initializeRTCStatus(); void prepareRTCStatus( From 83821d4818cf83ea3bcb14511fc64482859ed7f1 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Sat, 7 Oct 2023 17:20:47 +0900 Subject: [PATCH 3/3] feat(rtc_auto_mode_manager): eliminate rtc auto mode manager (#5235) * change namespace of auto_mode Signed-off-by: kyoichi-sugahara * delete RTC auto mode manager package Signed-off-by: kyoichi-sugahara * delete rtc_replayer.param Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * fix typo Signed-off-by: kyoichi-sugahara * fix typo Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../launch/planning.launch.xml | 2 - .../scenario_planning/lane_driving.launch.xml | 5 - launch/tier4_planning_launch/package.xml | 1 - .../config/scene_module_manager.param.yaml | 20 ++-- .../config/blind_spot.param.yaml | 2 +- .../config/crosswalk.param.yaml | 2 +- .../config/detection_area.param.yaml | 2 +- .../config/intersection.param.yaml | 4 +- .../config/no_stopping_area.param.yaml | 2 +- .../config/traffic_light.param.yaml | 2 +- planning/rtc_auto_mode_manager/CMakeLists.txt | 29 ----- planning/rtc_auto_mode_manager/README.md | 47 -------- .../config/rtc_auto_mode_manager.param.yaml | 31 ----- .../include/rtc_auto_mode_manager/node.hpp | 44 ------- .../rtc_auto_mode_manager_interface.hpp | 53 --------- .../launch/rtc_auto_mode_manager.launch.xml | 7 -- planning/rtc_auto_mode_manager/package.xml | 28 ----- planning/rtc_auto_mode_manager/src/node.cpp | 51 -------- .../src/rtc_auto_mode_manager_interface.cpp | 109 ------------------ .../include/rtc_interface/rtc_interface.hpp | 2 +- .../launch/rtc_replayer.launch.xml | 6 +- 21 files changed, 19 insertions(+), 430 deletions(-) delete mode 100644 planning/rtc_auto_mode_manager/CMakeLists.txt delete mode 100644 planning/rtc_auto_mode_manager/README.md delete mode 100644 planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml delete mode 100644 planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp delete mode 100644 planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp delete mode 100644 planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml delete mode 100644 planning/rtc_auto_mode_manager/package.xml delete mode 100644 planning/rtc_auto_mode_manager/src/node.cpp delete mode 100644 planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 7e78c8842f7df..6ace3f423d1bc 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -15,8 +15,6 @@ - - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 24a33b9d3f3df..5f17256df6b5c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -14,11 +14,6 @@ - - - - - diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 39b95286bb6cc..5988d34cded88 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -68,7 +68,6 @@ obstacle_stop_planner planning_evaluator planning_validator - rtc_auto_mode_manager scenario_selector surround_obstacle_checker diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index 9895d9b5e473f..5797704e8a0ca 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -5,7 +5,7 @@ ros__parameters: external_request_lane_change_left: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -14,7 +14,7 @@ external_request_lane_change_right: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -23,7 +23,7 @@ lane_change_left: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -32,7 +32,7 @@ lane_change_right: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -41,7 +41,7 @@ start_planner: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -50,7 +50,7 @@ side_shift: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -59,7 +59,7 @@ goal_planner: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: true @@ -68,7 +68,7 @@ avoidance: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -77,7 +77,7 @@ avoidance_by_lc: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -86,7 +86,7 @@ dynamic_avoidance: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true keep_last: false diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml index 3aa8c82a5e556..f9ddd3ce61099 100644 --- a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml @@ -8,4 +8,4 @@ max_future_movement_time: 10.0 # [second] threshold_yaw_diff: 0.523 # [rad] adjacent_extend_width: 1.5 # [m] - enable_rtc: true # 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 + enable_rtc: false # 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 diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index e7accc14096e0..a4f9e9d7ce23d 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -5,7 +5,7 @@ show_processing_time: false # [-] whether to show processing time # param for input data traffic_light_state_timeout: 3.0 # [s] timeout threshold for traffic light signal - enable_rtc: true # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. + enable_rtc: false # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. # param for stop position stop_position: diff --git a/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml b/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml index b5d075a4d6493..62b5f2458336f 100644 --- a/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml +++ b/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml @@ -8,4 +8,4 @@ state_clear_time: 2.0 hold_stop_margin_distance: 0.0 distance_to_judge_over_stop_line: 0.5 - enable_rtc: true # 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 + enable_rtc: false # 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 diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 972f572890d16..30fefcaeee035 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -66,8 +66,8 @@ attention_lane_curvature_calculation_ds: 0.5 enable_rtc: - intersection: true # 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_to_occlusion: true + intersection: false # 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_to_occlusion: false merge_from_private: stop_line_margin: 3.0 diff --git a/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml b/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml index f550188d4f2c1..c84848f8cc31d 100644 --- a/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml +++ b/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml @@ -8,4 +8,4 @@ stop_line_margin: 1.0 # [m] margin to auto-gen stop line at no stopping area detection_area_length: 200.0 # [m] used to create detection area polygon stuck_vehicle_front_margin: 6.0 # [m] obstacle stop max distance(5.0m) - enable_rtc: true # 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 + enable_rtc: false # 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 diff --git a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index e8e0357daa4a1..a837ae1b46b9b 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -5,4 +5,4 @@ tl_state_timeout: 1.0 yellow_lamp_period: 2.75 enable_pass_judge: true - enable_rtc: true # 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 + enable_rtc: false # 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 diff --git a/planning/rtc_auto_mode_manager/CMakeLists.txt b/planning/rtc_auto_mode_manager/CMakeLists.txt deleted file mode 100644 index 52c0c68384a57..0000000000000 --- a/planning/rtc_auto_mode_manager/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(rtc_auto_mode_manager) - -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/rtc_auto_mode_manager_interface.cpp - src/node.cpp -) - -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "rtc_auto_mode_manager::RTCAutoModeManagerNode" - EXECUTABLE ${PROJECT_NAME}_node -) - -ament_auto_package( - INSTALL_TO_SHARE - config - launch -) diff --git a/planning/rtc_auto_mode_manager/README.md b/planning/rtc_auto_mode_manager/README.md deleted file mode 100644 index 0e2614baaf1c8..0000000000000 --- a/planning/rtc_auto_mode_manager/README.md +++ /dev/null @@ -1,47 +0,0 @@ -# RTC Auto Mode Manager - -## Purpose - -RTC Auto Mode Manager is a node to approve request to cooperate from behavior planning modules automatically. - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ------------------------------- | --------------------------- | ------------------------------------------ | -| `/planning/enable_auto_mode/**` | tier4_rtc_msgs/srv/AutoMode | Service to enable auto mode for the module | - -### Output - -| Name | Type | Description | -| ---------------------------------------- | --------------------------- | ------------------------------------------ | -| `/planning/enable_auto_mode/internal/**` | tier4_rtc_msgs/srv/AutoMode | Service to enable auto mode for the module | - -## Parameters - -| Name | Type | Description | -| :-------------------- | :--------------- | :----------------------------------------------- | -| `module_list` | List of `string` | Module names managing in `rtc_auto_mode_manager` | -| `default_enable_list` | List of `string` | Module names enabled auto mode at initialization | - -## Inner-workings / Algorithms - -```plantuml - -start -:Read parameters; -:Send enable auto mode service to the module listed in `default_enable_list`; -repeat - if (Enable auto mode command received?) then (yes) - :Send enable auto mode command to rtc_interface; - else (no) - endif -repeat while (Is node running?) is (yes) not (no) -end - -``` - -## Assumptions / Known limits - -## Future extensions / Unimplemented parts diff --git a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml b/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml deleted file mode 100644 index 0aa3cbd49e8e9..0000000000000 --- a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml +++ /dev/null @@ -1,31 +0,0 @@ -/**: - ros__parameters: - module_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "goal_planner" - - "start_planner" - - "intersection_occlusion" - - default_enable_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "goal_planner" - - "start_planner" - - "intersection_occlusion" diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp deleted file mode 100644 index d36974508d2df..0000000000000 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RTC_AUTO_MODE_MANAGER__NODE_HPP_ -#define RTC_AUTO_MODE_MANAGER__NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp" - -#include "tier4_rtc_msgs/msg/auto_mode_status_array.hpp" - -#include -#include -#include - -namespace rtc_auto_mode_manager -{ -using tier4_rtc_msgs::msg::AutoModeStatusArray; -class RTCAutoModeManagerNode : public rclcpp::Node -{ -public: - explicit RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_options); - -private: - rclcpp::TimerBase::SharedPtr timer_; - AutoModeStatusArray auto_mode_statuses_; - rclcpp::Publisher::SharedPtr statuses_pub_; - std::vector> managers_; -}; - -} // namespace rtc_auto_mode_manager - -#endif // RTC_AUTO_MODE_MANAGER__NODE_HPP_ diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp deleted file mode 100644 index 098852c397161..0000000000000 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ -#define RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "tier4_rtc_msgs/msg/auto_mode_status.hpp" -#include "tier4_rtc_msgs/msg/module.hpp" -#include "tier4_rtc_msgs/srv/auto_mode.hpp" - -#include -#include -#include - -namespace rtc_auto_mode_manager -{ -using tier4_rtc_msgs::msg::AutoModeStatus; -using tier4_rtc_msgs::msg::Module; -using tier4_rtc_msgs::srv::AutoMode; - -class RTCAutoModeManagerInterface -{ -public: - RTCAutoModeManagerInterface( - rclcpp::Node * node, const std::string & module_name, const bool default_enable); - AutoModeStatus getAutoModeStatus() const { return auto_mode_status_; } - -private: - void onEnableService( - const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response); - AutoMode::Request createRequest(const AutoMode::Request::SharedPtr request) const; - AutoModeStatus auto_mode_status_; - rclcpp::Client::SharedPtr enable_cli_; - rclcpp::Service::SharedPtr enable_srv_; - - std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; -}; -} // namespace rtc_auto_mode_manager - -#endif // RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ diff --git a/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml b/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml deleted file mode 100644 index 85d4b3446fb89..0000000000000 --- a/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/planning/rtc_auto_mode_manager/package.xml b/planning/rtc_auto_mode_manager/package.xml deleted file mode 100644 index e9c16aac1bb91..0000000000000 --- a/planning/rtc_auto_mode_manager/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - rtc_auto_mode_manager - 0.1.0 - The rtc_auto_mode_manager package - - Fumiya Watanabe - Taiki Tanaka - - Apache License 2.0 - - Fumiya Watanabe - - ament_cmake_auto - autoware_cmake - - rclcpp - rclcpp_components - tier4_rtc_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/planning/rtc_auto_mode_manager/src/node.cpp b/planning/rtc_auto_mode_manager/src/node.cpp deleted file mode 100644 index 6092d2741d909..0000000000000 --- a/planning/rtc_auto_mode_manager/src/node.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "rtc_auto_mode_manager/node.hpp" - -#include - -namespace rtc_auto_mode_manager -{ - -RTCAutoModeManagerNode::RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_options) -: Node("rtc_auto_mode_manager_node", node_options) -{ - const std::vector module_list = - declare_parameter>("module_list"); - const std::vector default_enable_list = - declare_parameter>("default_enable_list"); - - for (const auto & module_name : module_list) { - const bool enabled = - std::count(default_enable_list.begin(), default_enable_list.end(), module_name) != 0; - managers_.push_back(std::make_shared(this, module_name, enabled)); - } - statuses_pub_ = create_publisher("output/auto_mode_statuses", 1); - using std::chrono_literals::operator""ms; - timer_ = rclcpp::create_timer(this, get_clock(), 100ms, [this] { - AutoModeStatusArray auto_mode_statuses; - for (const auto & m : managers_) { - auto_mode_statuses.stamp = get_clock()->now(); - auto_mode_statuses.statuses.emplace_back(m->getAutoModeStatus()); - } - statuses_pub_->publish(auto_mode_statuses); - auto_mode_statuses.statuses.clear(); - }); -} - -} // namespace rtc_auto_mode_manager - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(rtc_auto_mode_manager::RTCAutoModeManagerNode) diff --git a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp deleted file mode 100644 index e28957cee161b..0000000000000 --- a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp" - -namespace rtc_auto_mode_manager -{ -Module getModuleType(const std::string & module_name) -{ - Module module; - if (module_name == "blind_spot") { - module.type = Module::BLIND_SPOT; - } else if (module_name == "crosswalk") { - module.type = Module::CROSSWALK; - } else if (module_name == "detection_area") { - module.type = Module::DETECTION_AREA; - } else if (module_name == "intersection") { - module.type = Module::INTERSECTION; - } else if (module_name == "no_stopping_area") { - module.type = Module::NO_STOPPING_AREA; - } else if (module_name == "occlusion_spot") { - module.type = Module::OCCLUSION_SPOT; - } else if (module_name == "traffic_light") { - module.type = Module::TRAFFIC_LIGHT; - } else if (module_name == "virtual_traffic_light") { - module.type = Module::TRAFFIC_LIGHT; - } else if (module_name == "external_request_lane_change_left") { - module.type = Module::EXT_REQUEST_LANE_CHANGE_LEFT; - } else if (module_name == "external_request_lane_change_right") { - module.type = Module::EXT_REQUEST_LANE_CHANGE_RIGHT; - } else if (module_name == "lane_change_left") { - module.type = Module::LANE_CHANGE_LEFT; - } else if (module_name == "lane_change_right") { - module.type = Module::LANE_CHANGE_RIGHT; - } else if (module_name == "avoidance_by_lane_change_left") { - module.type = Module::AVOIDANCE_BY_LC_LEFT; - } else if (module_name == "avoidance_by_lane_change_right") { - module.type = Module::AVOIDANCE_BY_LC_RIGHT; - } else if (module_name == "avoidance_left") { - module.type = Module::AVOIDANCE_LEFT; - } else if (module_name == "avoidance_right") { - module.type = Module::AVOIDANCE_RIGHT; - } else if (module_name == "goal_planner") { - module.type = Module::GOAL_PLANNER; - } else if (module_name == "start_planner") { - module.type = Module::START_PLANNER; - } else if (module_name == "intersection_occlusion") { - module.type = Module::INTERSECTION_OCCLUSION; - } else { - module.type = Module::NONE; - } - return module; -} - -RTCAutoModeManagerInterface::RTCAutoModeManagerInterface( - rclcpp::Node * node, const std::string & module_name, const bool default_enable) -{ - using std::chrono_literals::operator""s; - using std::placeholders::_1; - using std::placeholders::_2; - - // Service client - enable_cli_ = - node->create_client(enable_auto_mode_namespace_ + "/internal/" + module_name); - - while (!enable_cli_->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for service."); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO_STREAM(node->get_logger(), "Waiting for service... [" << module_name << "]"); - } - - // Service - enable_srv_ = node->create_service( - enable_auto_mode_namespace_ + "/" + module_name, - std::bind(&RTCAutoModeManagerInterface::onEnableService, this, _1, _2)); - - auto_mode_status_.module = getModuleType(module_name); - // Send enable auto mode request - if (default_enable) { - auto_mode_status_.is_auto_mode = true; - AutoMode::Request::SharedPtr request = std::make_shared(); - request->enable = true; - enable_cli_->async_send_request(request); - } -} - -void RTCAutoModeManagerInterface::onEnableService( - const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) -{ - auto_mode_status_.is_auto_mode = request->enable; - enable_cli_->async_send_request(request); - response->success = true; -} - -} // namespace rtc_auto_mode_manager diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 4929d0f8e27f3..d6e083421c0b7 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -85,7 +85,7 @@ class RTCInterface std::string cooperate_status_namespace_ = "/planning/cooperate_status"; std::string cooperate_commands_namespace_ = "/planning/cooperate_commands"; - std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode/internal"; + std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; mutable std::mutex mutex_; }; diff --git a/planning/rtc_replayer/launch/rtc_replayer.launch.xml b/planning/rtc_replayer/launch/rtc_replayer.launch.xml index 9b832b369c77c..70d7baf2dce4a 100644 --- a/planning/rtc_replayer/launch/rtc_replayer.launch.xml +++ b/planning/rtc_replayer/launch/rtc_replayer.launch.xml @@ -1,7 +1,3 @@ - - - - - +