From 8c16ca1e9100fc2ce5008e3b7659eb499aa9cc67 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 12 Jan 2024 13:33:30 +0900 Subject: [PATCH] feat(crosswalk): change the TTC formula (#6033) * change params * change the TTC formula Signed-off-by: Yuki Takagi --- .../config/crosswalk.param.yaml | 6 +++--- .../src/scene_crosswalk.cpp | 7 +++++-- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 240963309e9a2..273db320f1027 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -16,7 +16,7 @@ # For the case where the crosswalk width is very wide far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object). # For the case where the stop position is determined according to the object position. - stop_distance_from_object: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin + stop_distance_from_object: 3.0 # [m] the vehicle decelerates to be able to stop in front of object with margin # params for ego's slow down velocity. These params are not used for the case of "enable_rtc: false". slow_down: @@ -38,7 +38,7 @@ # params for the pass judge logic against the crosswalk users such as pedestrians or bicycles pass_judge: ego_pass_first_margin_x: [0.0] # [[s]] time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) - ego_pass_first_margin_y: [6.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_margin_y: [3.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) ego_pass_first_additional_margin: 0.5 # [s] additional time margin for ego pass first situation to suppress chattering ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_margin_y: [10.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) @@ -62,7 +62,7 @@ # param for target object filtering object_filtering: - crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk + crosswalk_attention_range: 2.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk vehicle_object_cross_angle_threshold: 0.5 # [rad] threshold judging object is crossing walkway as a pedestrian or passing over as a vehicle target_object: unknown: true # [-] whether to look and stop by UNKNOWN objects diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 7175cd0895fc0..acb88a214b0c0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -766,7 +766,6 @@ CollisionPoint CrosswalkModule::createCollisionPoint( const std::optional object_crosswalk_passage_direction) const { constexpr double min_ego_velocity = 1.38; // [m/s] - const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto velocity = std::max(planner_param_.min_object_velocity, estimated_velocity); @@ -774,8 +773,12 @@ CollisionPoint CrosswalkModule::createCollisionPoint( CollisionPoint collision_point{}; collision_point.collision_point = nearest_collision_point; collision_point.crosswalk_passage_direction = object_crosswalk_passage_direction; + + // The decision of whether the ego vehicle or the pedestrian goes first is determined by the logic + // for ego_pass_first or yield; even the decision for ego_pass_later does not affect this sense. + // Hence, here, we use the length that would be appropriate for the ego_pass_first judge. collision_point.time_to_collision = - std::max(0.0, dist_ego2cp - planner_param_.stop_distance_from_object - base_link2front) / + std::max(0.0, dist_ego2cp - planner_data_->vehicle_info_.min_longitudinal_offset_m) / std::max(ego_vel.x, min_ego_velocity); collision_point.time_to_vehicle = std::max(0.0, dist_obj2cp) / velocity;