From e31c4f64e20bc1f003e0747c4482db79e41d4337 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Fri, 20 Oct 2023 09:57:19 +0900 Subject: [PATCH 1/2] feat(behavior_velocity_run_out): ignore momentary detection caused by false positive Signed-off-by: Tomohito Ando --- .../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..e831d228c0c1c 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 \ No newline at end of file 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 b91d1d1fe1ae7..54902f52c47da 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -137,7 +137,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."); @@ -181,6 +181,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); @@ -188,6 +193,7 @@ boost::optional RunOutModule::detectCollision( } // no collision detected + first_detected_time_.reset(); return {}; } @@ -812,4 +818,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 8fc3d48de4858..b25a8fc94bff3 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -126,6 +126,12 @@ struct DynamicObstacleParam float points_interval; // [m] }; +struct IgnoreMomentaryDetection +{ + bool enable; + double time_threshold; +}; + struct PlannerParam { CommonParam common; @@ -138,6 +144,7 @@ struct PlannerParam DynamicObstacleParam dynamic_obstacle; SlowDownLimit slow_down_limit; Smoother smoother; + IgnoreMomentaryDetection ignore_momentary_detection; }; enum class DetectionMethod { From db5d0da39a9f6484f0592ddd8089e10a61473097 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 20 Oct 2023 01:52:11 +0000 Subject: [PATCH 2/2] style(pre-commit): autofix --- .../behavior_velocity_run_out_module/config/run_out.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e831d228c0c1c..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 @@ -52,4 +52,4 @@ # 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 \ No newline at end of file + time_threshold: 0.15 # [sec] ignores detections that persist for less than this duration