From bb1800eeb5939d1adab970c56e4880861eeb4d24 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 31 Jul 2023 13:20:37 +0900 Subject: [PATCH 1/4] feat(crosswalk): disable cancel yield at no traffic signals intersection Signed-off-by: Takayuki Murooka --- .../src/scene_crosswalk.cpp | 8 +++++++- .../src/scene_crosswalk.hpp | 10 ++++++---- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 120f8ec8581be..98f699a6578d6 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -832,6 +832,11 @@ void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) { const auto & objects_ptr = planner_data_->predicted_objects; + const auto traffic_lights_reg_elems = + crosswalk_.regulatoryElementsAs(); + const bool has_traffic_light = !traffic_lights_reg_elems.empty(); + std::cerr << has_traffic_light << std::endl; + // Check if ego is yielding const bool is_ego_yielding = [&]() { const auto has_reached_stop_point = dist_ego_to_stop < planner_param_.stop_position_threshold; @@ -846,7 +851,8 @@ void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) const auto obj_uuid = toHexString(object.object_id); const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; object_info_manager_.update( - obj_uuid, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, planner_param_); + obj_uuid, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, has_traffic_light, + planner_param_); } object_info_manager_.finalize(); } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 228be0d62ae29..d948cf15f3bc5 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -103,7 +103,7 @@ class CrosswalkModule : public SceneModuleInterface void updateState( const rclcpp::Time & now, const double obj_vel, const bool is_ego_yielding, - const PlannerParam & planner_param) + const bool has_traffic_light, const PlannerParam & planner_param) { const bool is_stopped = obj_vel < planner_param.stop_object_velocity; @@ -117,7 +117,9 @@ class CrosswalkModule : public SceneModuleInterface } const bool intent_to_cross = (now - *time_to_start_stopped).seconds() < planner_param.timeout_no_intention_to_walk; - if ((is_ego_yielding || planner_param.disable_stop_for_yield_cancel) && !intent_to_cross) { + if ( + (is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) && + !intent_to_cross) { state = State::FULLY_STOPPED; } else { // NOTE: Object may start moving @@ -134,7 +136,7 @@ class CrosswalkModule : public SceneModuleInterface void init() { current_uuids_.clear(); } void update( const std::string & uuid, const double obj_vel, const rclcpp::Time & now, - const bool is_ego_yielding, const PlannerParam & planner_param) + const bool is_ego_yielding, const bool has_traffic_light, const PlannerParam & planner_param) { // update current uuids current_uuids_.push_back(uuid); @@ -145,7 +147,7 @@ class CrosswalkModule : public SceneModuleInterface } // update object state - objects.at(uuid).updateState(now, obj_vel, is_ego_yielding, planner_param); + objects.at(uuid).updateState(now, obj_vel, is_ego_yielding, has_traffic_light, planner_param); } void finalize() { From 97d5dd1ecf6ff17c5327051defb415f127280be9 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 31 Jul 2023 14:26:27 +0900 Subject: [PATCH 2/4] update Signed-off-by: Takayuki Murooka --- .../behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp | 1 - 1 file changed, 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 98f699a6578d6..b2fc57b7e6420 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -835,7 +835,6 @@ void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) const auto traffic_lights_reg_elems = crosswalk_.regulatoryElementsAs(); const bool has_traffic_light = !traffic_lights_reg_elems.empty(); - std::cerr << has_traffic_light << std::endl; // Check if ego is yielding const bool is_ego_yielding = [&]() { From 536fedd9c7b3e9fecaf6a2bf91b899d1a466621a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 31 Jul 2023 15:37:12 +0900 Subject: [PATCH 3/4] feat(crosswalk): add option to disable yield for new stopped object Signed-off-by: Takayuki Murooka --- .../src/manager.cpp | 2 ++ .../src/scene_crosswalk.hpp | 11 +++++++++-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index c774c8876c850..2a478c3a5ff06 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -77,6 +77,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.min_object_velocity = node.declare_parameter(ns + ".pass_judge.min_object_velocity"); cp.disable_stop_for_yield_cancel = node.declare_parameter(ns + ".pass_judge.disable_stop_for_yield_cancel"); + cp.disable_yield_for_new_stopped_object = + node.declare_parameter(ns + ".pass_judge.disable_yield_for_new_stopped_object"); cp.timeout_no_intention_to_walk = node.declare_parameter(ns + ".pass_judge.timeout_no_intention_to_walk"); cp.timeout_ego_stop_for_yield = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index d948cf15f3bc5..63fd7bdf3ba28 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -82,6 +82,7 @@ class CrosswalkModule : public SceneModuleInterface double stop_object_velocity; double min_object_velocity; bool disable_stop_for_yield_cancel; + bool disable_yield_for_new_stopped_object; double timeout_no_intention_to_walk; double timeout_ego_stop_for_yield; // param for input data @@ -98,7 +99,7 @@ class CrosswalkModule : public SceneModuleInterface { // NOTE: FULLY_STOPPED means stopped object which can be ignored. enum class State { STOPPED = 0, FULLY_STOPPED, OTHER }; - State state{State::OTHER}; + State state; std::optional time_to_start_stopped{std::nullopt}; void updateState( @@ -143,7 +144,13 @@ class CrosswalkModule : public SceneModuleInterface // add new object if (objects.count(uuid) == 0) { - objects.emplace(uuid, ObjectInfo{}); + if ( + has_traffic_light && planner_param.disable_stop_for_yield_cancel && + planner_param.disable_yield_for_new_stopped_object) { + objects.emplace(uuid, ObjectInfo{ObjectInfo::State::FULLY_STOPPED}); + } else { + objects.emplace(uuid, ObjectInfo{ObjectInfo::State::OTHER}); + } } // update object state From 974afc0a6c261bdf4da5df183c7e90bf4806a141 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 31 Jul 2023 15:38:58 +0900 Subject: [PATCH 4/4] update config Signed-off-by: Takayuki Murooka --- .../config/crosswalk.param.yaml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 6ff7104bfa992..3d2c5c428adef 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -4,8 +4,8 @@ common: show_processing_time: false # [-] whether to show processing time # param for input data - 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 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. # param for stop position stop_position: @@ -38,7 +38,8 @@ stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding - disable_stop_for_yield_cancel: false + disable_stop_for_yield_cancel: false # for the crosswalk where there is a traffic signal + disable_yield_for_new_stopped_object: false # for the crosswalk where there is a traffic signal timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not