feat(obstacle_cruise_planner): add velocity_threshold to outside obst…

…acle (#1646) * feat(obstacle_cruise_planner): add velocity_threshold to outside obstacle Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * add parameter to config Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * update readme Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
autowarefoundation · Aug 22, 2022 · 27f9216 · 27f9216
1 parent 6f7db46
commit 27f9216
Show file tree

Hide file tree

Showing 5 changed files with 20 additions and 4 deletions.
diff --git a/...g/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/...g/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
@@ -50,6 +50,7 @@
       collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]
 
       outside_rough_detection_area_expand_width : 0.5 # rough lateral margin for rough detection area expansion for obstacles outside the trajectory [m]
+      outside_obstacle_min_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
       ego_obstacle_overlap_time_threshold : 1.0 #  time threshold to decide cut-in obstacle for cruise or stop [s]
       max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego
 

diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md
@@ -141,10 +141,11 @@ Near Cut-in vehicles are defined as vehicle objects
 
 In the `obstacle_filtering` namespace,
 
-| Parameter                                 | Type   | Description                                                     |
-| ----------------------------------------- | ------ | --------------------------------------------------------------- |
-| `ego_obstacle_overlap_time_threshold`     | double | time threshold to decide cut-in obstacle for cruise or stop [s] |
-| `max_prediction_time_for_collision_check` | double | prediction time to check collision between obstacle and ego [s] |
+| Parameter                                 | Type   | Description                                                              |
+| ----------------------------------------- | ------ | ------------------------------------------------------------------------ |
+| `ego_obstacle_overlap_time_threshold`     | double | time threshold to decide cut-in obstacle for cruise or stop [s]          |
+| `max_prediction_time_for_collision_check` | double | prediction time to check collision between obstacle and ego [s]          |
+| `outside_obstacle_min_velocity_threshold` | double | minimum velocity threshold of target obstacle for cut-in detection [m/s] |
 
 ### Stop planning
 

diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
@@ -50,6 +50,7 @@
       collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]
 
       outside_rough_detection_area_expand_width : 0.5 # rough lateral margin for rough detection area expansion for obstacles outside the trajectory [m]
+      outside_obstacle_min_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
       ego_obstacle_overlap_time_threshold : 1.0 #  time threshold to decide cut-in obstacle for cruise or stop [s]
       max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego
 

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -177,6 +177,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     double collision_time_margin;
     // outside
     double outside_rough_detection_area_expand_width;
+    double outside_obstacle_min_velocity_threshold;
     double ego_obstacle_overlap_time_threshold;
     double max_prediction_time_for_collision_check;
     double crossing_obstacle_traj_angle_threshold;

diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
@@ -242,6 +242,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
       declare_parameter<double>("obstacle_filtering.collision_time_margin");
     obstacle_filtering_param_.outside_rough_detection_area_expand_width =
       declare_parameter<double>("obstacle_filtering.outside_rough_detection_area_expand_width");
+    obstacle_filtering_param_.outside_obstacle_min_velocity_threshold =
+      declare_parameter<double>("obstacle_filtering.outside_obstacle_min_velocity_threshold");
     obstacle_filtering_param_.ego_obstacle_overlap_time_threshold =
       declare_parameter<double>("obstacle_filtering.ego_obstacle_overlap_time_threshold");
     obstacle_filtering_param_.max_prediction_time_for_collision_check =
@@ -744,6 +746,16 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
         continue;
       }
 
+      const double object_vel =
+        predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x;
+      if (
+        std::fabs(object_vel) < obstacle_filtering_param_.outside_obstacle_min_velocity_threshold) {
+        RCLCPP_INFO_EXPRESSION(
+          get_logger(), is_showing_debug_info_,
+          "Ignore outside obstacle (%s) since the obstacle velocity is low.", object_id.c_str());
+        continue;
+      }
+
       const auto predicted_path = getHighestConfidencePredictedPath(predicted_object);
       const auto resampled_predicted_path = perception_utils::resamplePredictedPath(
         predicted_path, obstacle_filtering_param_.prediction_resampling_time_interval,