From 4a28a3c7fbb4c18915f89b1ad34dbac314204007 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 24 Oct 2023 11:52:24 +0900 Subject: [PATCH] feat(behavior_velocity_run_out): ignore momentary detection caused by false positive (#5359) * feat(behavior_velocity_run_out): ignore momentary detection caused by false positive Signed-off-by: Tomohito Ando * style(pre-commit): autofix --------- Signed-off-by: Tomohito Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../README.md | 5 ++++ .../config/run_out.param.yaml | 5 ++++ .../src/manager.cpp | 7 ++++++ .../src/scene.cpp | 25 ++++++++++++++++++- .../src/scene.hpp | 6 +++-- .../src/utils.hpp | 7 ++++++ 6 files changed, 52 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/behavior_velocity_run_out_module/README.md index ee762653245c6..a36cfdf6485c6 100644 --- a/planning/behavior_velocity_run_out_module/README.md +++ b/planning/behavior_velocity_run_out_module/README.md @@ -217,6 +217,11 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab | `max_jerk` | double | [m/s^3] minimum jerk deceleration for safe brake. | | `max_acc` | double | [m/s^2] minimum accel deceleration for safe brake. | +| Parameter /ignore_momentary_detection | Type | Description | +| ------------------------------------- | ------ | ----------------------------------------------------------------- | +| `enable` | bool | [-] whether to ignore momentary detection | +| `time_threshold` | double | [sec] ignores detections that persist for less than this duration | + ### Future extensions / Unimplemented parts - Calculate obstacle's min velocity and max velocity from covariance diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml index f9668549f2fb2..2641214ac5ad4 100644 --- a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml +++ b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml @@ -48,3 +48,8 @@ enable: true max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. + + # prevent abrupt stops caused by false positives in perception + ignore_momentary_detection: + enable: true + time_threshold: 0.15 # [sec] ignores detections that persist for less than this duration diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index b262bf00cb57a..a5253f59b60f9 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -119,6 +119,13 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) p.max_acc = getOrDeclareParameter(node, ns_m + ".max_acc"); } + { + auto & p = planner_param_.ignore_momentary_detection; + const std::string ns_param = ns + ".ignore_momentary_detection"; + p.enable = getOrDeclareParameter(node, ns_param + ".enable"); + p.time_threshold = getOrDeclareParameter(node, ns_param + ".time_threshold"); + } + debug_ptr_ = std::make_shared(node); setDynamicObstacleCreator(node, debug_ptr_); } diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index 3b2ed6a78a96c..a943e37d666db 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -132,7 +132,7 @@ bool RunOutModule::modifyPathVelocity( } boost::optional RunOutModule::detectCollision( - const std::vector & dynamic_obstacles, const PathWithLaneId & path) const + const std::vector & dynamic_obstacles, const PathWithLaneId & path) { if (path.points.size() < 2) { RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points."); @@ -176,6 +176,11 @@ boost::optional RunOutModule::detectCollision( continue; } + // ignore momentary obstacle detection to avoid sudden stopping by false positive + if (isMomentaryDetection()) { + return {}; + } + debug_ptr_->pushCollisionPoints(obstacle_selected->collision_points); debug_ptr_->pushNearestCollisionPoint(obstacle_selected->nearest_collision_point); @@ -183,6 +188,7 @@ boost::optional RunOutModule::detectCollision( } // no collision detected + first_detected_time_.reset(); return {}; } @@ -807,4 +813,21 @@ void RunOutModule::publishDebugValue( debug_ptr_->publishDebugValue(); } +bool RunOutModule::isMomentaryDetection() +{ + if (!planner_param_.ignore_momentary_detection.enable) { + return false; + } + + if (!first_detected_time_) { + first_detected_time_ = std::make_shared(clock_->now()); + return true; + } + + const auto now = clock_->now(); + const double elapsed_time_since_detection = (now - *first_detected_time_).seconds(); + RCLCPP_DEBUG_STREAM(logger_, "elapsed_time_since_detection: " << elapsed_time_since_detection); + + return elapsed_time_since_detection < planner_param_.ignore_momentary_detection.time_threshold; +} } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index 20bd8ffc799ff..925d8ea8b234c 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -62,13 +62,13 @@ class RunOutModule : public SceneModuleInterface std::unique_ptr dynamic_obstacle_creator_; std::shared_ptr debug_ptr_; std::unique_ptr state_machine_; + std::shared_ptr first_detected_time_; // Function Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const; boost::optional detectCollision( - const std::vector & dynamic_obstacles, - const PathWithLaneId & path_points) const; + const std::vector & dynamic_obstacles, const PathWithLaneId & path_points); float calcCollisionPositionOfVehicleSide( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const; @@ -141,6 +141,8 @@ class RunOutModule : public SceneModuleInterface const PathWithLaneId & path, const std::vector extracted_obstacles, const boost::optional & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose) const; + + bool isMomentaryDetection(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 1f725291a38b7..9b746589d3f93 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -121,6 +121,12 @@ struct DynamicObstacleParam float points_interval; // [m] }; +struct IgnoreMomentaryDetection +{ + bool enable; + double time_threshold; +}; + struct PlannerParam { CommonParam common; @@ -133,6 +139,7 @@ struct PlannerParam DynamicObstacleParam dynamic_obstacle; SlowDownLimit slow_down_limit; Smoother smoother; + IgnoreMomentaryDetection ignore_momentary_detection; }; enum class DetectionMethod {