feat(dynamic_avoidance): consider a wider range of objects (autowaref…

…oundation#4381) * feat(dynamic_avoidance): consider a wider range of objects Signed-off-by: Takayuki Murooka <[email protected]> * fix Signed-off-by: Takayuki Murooka <[email protected]> * update config Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
tier4 · Jul 25, 2023 · 36fecf2 · 36fecf2
1 parent 559f873
commit 36fecf2
Show file tree

Hide file tree

Showing 4 changed files with 302 additions and 153 deletions.
diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml
@@ -16,21 +16,29 @@
 
         successive_num_to_entry_dynamic_avoidance_condition: 5
 
-        min_obj_lat_offset_to_ego_path: 0.3 # [m]
+        min_obj_lat_offset_to_ego_path: 0.0 # [m]
+        max_obj_lat_offset_to_ego_path: 1.0 # [m]
+
+        crossing_object:
+          min_object_vel: 1.0
+          max_object_angle: 1.05
+
+        front_object:
+          max_object_angle: 0.785
 
       drivable_area_generation:
-        lat_offset_from_obstacle: 0.8 # [m]
+        lat_offset_from_obstacle: 1.0 # [m]
         max_lat_offset_to_avoid: 0.5 # [m]
 
         # for same directional object
         overtaking_object:
-          max_time_to_collision: 3.0 # [s]
-          start_duration_to_avoid: 4.0  # [s]
-          end_duration_to_avoid: 8.0  # [s]
+          max_time_to_collision: 10.0 # [s]
+          start_duration_to_avoid: 2.0  # [s]
+          end_duration_to_avoid: 4.0  # [s]
           duration_to_hold_avoidance: 3.0 # [s]
 
         # for opposite directional object
         oncoming_object:
-          max_time_to_collision: 3.0 # [s]
+          max_time_to_collision: 15.0 # [s]
           start_duration_to_avoid: 12.0  # [s]
           end_duration_to_avoid: 0.0  # [s]
diff --git a/...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
@@ -48,7 +48,13 @@ struct DynamicAvoidanceParameters
   bool avoid_pedestrian{false};
   double min_obstacle_vel{0.0};
   int successive_num_to_entry_dynamic_avoidance_condition{0};
+
   double min_obj_lat_offset_to_ego_path{0.0};
+  double max_obj_lat_offset_to_ego_path{0.0};
+
+  double max_front_object_angle{0.0};
+  double min_crossing_object_vel{0.0};
+  double max_crossing_object_angle{0.0};
 
   // drivable area generation
   double lat_offset_from_obstacle{0.0};
@@ -70,12 +76,15 @@ class DynamicAvoidanceModule : public SceneModuleInterface
   struct DynamicAvoidanceObject
   {
     DynamicAvoidanceObject(
-      const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel)
+      const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel,
+      const bool arg_is_left, const double arg_time_to_collision)
     : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)),
       pose(predicted_object.kinematics.initial_pose_with_covariance.pose),
+      shape(predicted_object.shape),
       vel(arg_vel),
       lat_vel(arg_lat_vel),
-      shape(predicted_object.shape)
+      is_left(arg_is_left),
+      time_to_collision(arg_time_to_collision)
     {
       for (const auto & path : predicted_object.kinematics.predicted_paths) {
         predicted_paths.push_back(path);
@@ -84,12 +93,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface
 
     std::string uuid;
     geometry_msgs::msg::Pose pose;
+    autoware_auto_perception_msgs::msg::Shape shape;
     double vel;
     double lat_vel;
-    autoware_auto_perception_msgs::msg::Shape shape;
-    std::vector<autoware_auto_perception_msgs::msg::PredictedPath> predicted_paths{};
-
     bool is_left;
+    double time_to_collision;
+    std::vector<autoware_auto_perception_msgs::msg::PredictedPath> predicted_paths{};
   };
   struct DynamicAvoidanceObjectCandidate
   {
@@ -113,7 +122,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
   DynamicAvoidanceModule(
     const std::string & name, rclcpp::Node & node,
     std::shared_ptr<DynamicAvoidanceParameters> parameters,
-    const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map);
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
 
   void updateModuleParams(const std::shared_ptr<DynamicAvoidanceParameters> & parameters)
   {
@@ -135,7 +144,20 @@ class DynamicAvoidanceModule : public SceneModuleInterface
 
 private:
   bool isLabelTargetObstacle(const uint8_t label) const;
-  std::vector<DynamicAvoidanceObjectCandidate> calcTargetObjectsCandidate() const;
+  std::vector<DynamicAvoidanceObjectCandidate> calcTargetObjectsCandidate();
+  bool willObjectCutIn(
+    const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & predicted_path,
+    const double obj_tangent_vel) const;
+  bool willObjectCutOut(
+    const double obj_tangent_vel, const double obj_normal_vel, const bool is_left) const;
+  bool isObjectFarFromPath(
+    const PredictedObject & predicted_object, const double obj_dist_to_path) const;
+  double calcTimeToCollision(
+    const std::vector<PathPointWithLaneId> & ego_path, const geometry_msgs::msg::Pose & obj_pose,
+    const double obj_tangent_vel) const;
+  std::optional<std::pair<size_t, size_t>> calcCollisionSection(
+    const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & obj_path) const;
+
   std::pair<lanelet::ConstLanelets, lanelet::ConstLanelets> getAdjacentLanes(
     const double forward_distance, const double backward_distance) const;
   std::optional<tier4_autoware_utils::Polygon2d> calcDynamicObstaclePolygon(