chore: sync upstream

perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp

@@ -41,12 +41,11 @@ class RoiDetectedObjectFusionNode : public FusionNode<DetectedObjects, DetectedO
     const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg) override;
 
   std::map<std::size_t, RegionOfInterest> generateDetectedObjectRoIs(
-    const std::vector<DetectedObject> & input_objects, const double image_width,
-    const double image_height, const Eigen::Affine3d & object2camera_affine,
-    const Eigen::Matrix4d & camera_projection);
+    const DetectedObjects & input_object_msg, const double image_width, const double image_height,
+    const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection);
 
   void fuseObjectsOnImage(
-    const std::vector<DetectedObject> & objects,
+    const DetectedObjects & input_object_msg,
     const std::vector<DetectedObjectWithFeature> & image_rois,
     const std::map<std::size_t, sensor_msgs::msg::RegionOfInterest> & object_roi_map);
 
@@ -63,7 +62,8 @@ class RoiDetectedObjectFusionNode : public FusionNode<DetectedObjects, DetectedO
     double roi_probability_threshold{};
   } fusion_params_;
 
-  std::vector<bool> passthrough_object_flags_, fused_object_flags_, ignored_object_flags_;
+  std::map<int64_t, std::vector<bool>> passthrough_object_flags_map_, fused_object_flags_map_,
+    ignored_object_flags_map_;
 };
 
 }  // namespace image_projection_based_fusion

perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp

@@ -34,19 +34,23 @@ RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptio
 
 void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg)
 {
-  passthrough_object_flags_.clear();
-  passthrough_object_flags_.resize(output_msg.objects.size());
-  fused_object_flags_.clear();
-  fused_object_flags_.resize(output_msg.objects.size());
-  ignored_object_flags_.clear();
-  ignored_object_flags_.resize(output_msg.objects.size());
+  std::vector<bool> passthrough_object_flags, fused_object_flags, ignored_object_flags;
+  passthrough_object_flags.resize(output_msg.objects.size());
+  fused_object_flags.resize(output_msg.objects.size());
+  ignored_object_flags.resize(output_msg.objects.size());
   for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) {
     if (
       output_msg.objects.at(obj_i).existence_probability >
       fusion_params_.passthrough_lower_bound_probability_threshold) {
-      passthrough_object_flags_.at(obj_i) = true;
+      passthrough_object_flags.at(obj_i) = true;
     }
   }
+
+  int64_t timestamp_nsec =
+    output_msg.header.stamp.sec * (int64_t)1e9 + output_msg.header.stamp.nanosec;
+  passthrough_object_flags_map_.insert(std::make_pair(timestamp_nsec, passthrough_object_flags));
+  fused_object_flags_map_.insert(std::make_pair(timestamp_nsec, fused_object_flags));
+  ignored_object_flags_map_.insert(std::make_pair(timestamp_nsec, ignored_object_flags));
 }
 
 void RoiDetectedObjectFusionNode::fuseOnSingleImage(
@@ -58,8 +62,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage(
   Eigen::Affine3d object2camera_affine;
   {
     const auto transform_stamped_optional = getTransformStamped(
-      tf_buffer_, /*target*/ camera_info.header.frame_id,
-      /*source*/ input_object_msg.header.frame_id, input_object_msg.header.stamp);
+      tf_buffer_, /*target*/ input_roi_msg.header.frame_id,
+      /*source*/ input_object_msg.header.frame_id, input_roi_msg.header.stamp);
     if (!transform_stamped_optional) {
       return;
     }
@@ -73,9 +77,9 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage(
     camera_info.p.at(11);
 
   const auto object_roi_map = generateDetectedObjectRoIs(
-    input_object_msg.objects, static_cast<double>(camera_info.width),
+    input_object_msg, static_cast<double>(camera_info.width),
     static_cast<double>(camera_info.height), object2camera_affine, camera_projection);
-  fuseObjectsOnImage(input_object_msg.objects, input_roi_msg.feature_objects, object_roi_map);
+  fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map);
 
   if (debugger_) {
     debugger_->image_rois_.reserve(input_roi_msg.feature_objects.size());
@@ -87,17 +91,25 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage(
 }
 
 std::map<std::size_t, RegionOfInterest> RoiDetectedObjectFusionNode::generateDetectedObjectRoIs(
-  const std::vector<DetectedObject> & input_objects, const double image_width,
-  const double image_height, const Eigen::Affine3d & object2camera_affine,
-  const Eigen::Matrix4d & camera_projection)
+  const DetectedObjects & input_object_msg, const double image_width, const double image_height,
+  const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection)
 {
   std::map<std::size_t, RegionOfInterest> object_roi_map;
-  for (std::size_t obj_i = 0; obj_i < input_objects.size(); ++obj_i) {
+  int64_t timestamp_nsec =
+    input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec;
+  if (passthrough_object_flags_map_.size() == 0) {
+    return object_roi_map;
+  }
+  if (passthrough_object_flags_map_.count(timestamp_nsec) == 0) {
+    return object_roi_map;
+  }
+  const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec);
+  for (std::size_t obj_i = 0; obj_i < input_object_msg.objects.size(); ++obj_i) {
     std::vector<Eigen::Vector3d> vertices_camera_coord;
     {
-      const auto & object = input_objects.at(obj_i);
+      const auto & object = input_object_msg.objects.at(obj_i);
 
-      if (passthrough_object_flags_.at(obj_i)) {
+      if (passthrough_object_flags.at(obj_i)) {
         continue;
       }
 
@@ -142,7 +154,7 @@ std::map<std::size_t, RegionOfInterest> RoiDetectedObjectFusionNode::generateDet
         }
       }
     }
-    if (point_on_image_cnt == 0) {
+    if (point_on_image_cnt == 3) {
       continue;
     }
 
@@ -168,13 +180,26 @@ std::map<std::size_t, RegionOfInterest> RoiDetectedObjectFusionNode::generateDet
 }
 
 void RoiDetectedObjectFusionNode::fuseObjectsOnImage(
-  const std::vector<DetectedObject> & objects __attribute__((unused)),
+  const DetectedObjects & input_object_msg,
   const std::vector<DetectedObjectWithFeature> & image_rois,
   const std::map<std::size_t, sensor_msgs::msg::RegionOfInterest> & object_roi_map)
 {
+  int64_t timestamp_nsec =
+    input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec;
+  if (fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) {
+    return;
+  }
+  if (
+    fused_object_flags_map_.count(timestamp_nsec) == 0 ||
+    ignored_object_flags_map_.count(timestamp_nsec) == 0) {
+    return;
+  }
+  auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec);
+  auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec);
+
   for (const auto & object_pair : object_roi_map) {
     const auto & obj_i = object_pair.first;
-    if (fused_object_flags_.at(obj_i)) {
+    if (fused_object_flags.at(obj_i)) {
       continue;
     }
 
@@ -192,15 +217,15 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage(
     if (max_iou > fusion_params_.min_iou_threshold) {
       if (fusion_params_.use_roi_probability) {
         if (roi_prob > fusion_params_.roi_probability_threshold) {
-          fused_object_flags_.at(obj_i) = true;
+          fused_object_flags.at(obj_i) = true;
         } else {
-          ignored_object_flags_.at(obj_i) = true;
+          ignored_object_flags.at(obj_i) = true;
         }
       } else {
-        fused_object_flags_.at(obj_i) = true;
+        fused_object_flags.at(obj_i) = true;
       }
     } else {
-      ignored_object_flags_.at(obj_i) = true;
+      ignored_object_flags.at(obj_i) = true;
     }
   }
 }
@@ -234,19 +259,37 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg)
     return;
   }
 
+  int64_t timestamp_nsec =
+    output_msg.header.stamp.sec * (int64_t)1e9 + output_msg.header.stamp.nanosec;
+  if (
+    passthrough_object_flags_map_.size() == 0 || fused_object_flags_map_.size() == 0 ||
+    ignored_object_flags_map_.size() == 0) {
+    return;
+  }
+  if (
+    passthrough_object_flags_map_.count(timestamp_nsec) == 0 ||
+    fused_object_flags_map_.count(timestamp_nsec) == 0 ||
+    ignored_object_flags_map_.count(timestamp_nsec) == 0) {
+    return;
+  }
+  auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec);
+  auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec);
+  auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec);
+
   DetectedObjects output_objects_msg, debug_fused_objects_msg, debug_ignored_objects_msg;
   output_objects_msg.header = output_msg.header;
   debug_fused_objects_msg.header = output_msg.header;
   debug_ignored_objects_msg.header = output_msg.header;
   for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) {
     const auto & obj = output_msg.objects.at(obj_i);
-    if (passthrough_object_flags_.at(obj_i)) {
+    if (passthrough_object_flags.at(obj_i)) {
       output_objects_msg.objects.emplace_back(obj);
     }
-    if (fused_object_flags_.at(obj_i)) {
+    if (fused_object_flags.at(obj_i)) {
       output_objects_msg.objects.emplace_back(obj);
       debug_fused_objects_msg.objects.emplace_back(obj);
-    } else if (ignored_object_flags_.at(obj_i)) {
+    }
+    if (ignored_object_flags.at(obj_i)) {
       debug_ignored_objects_msg.objects.emplace_back(obj);
     }
   }
@@ -255,6 +298,10 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg)
 
   debug_publisher_->publish<DetectedObjects>("debug/fused_objects", debug_fused_objects_msg);
   debug_publisher_->publish<DetectedObjects>("debug/ignored_objects", debug_ignored_objects_msg);
+
+  passthrough_object_flags_map_.erase(timestamp_nsec);
+  fused_object_flags_map_.erase(timestamp_nsec);
+  fused_object_flags_map_.erase(timestamp_nsec);
 }
 
 }  // namespace image_projection_based_fusion

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -11,10 +11,10 @@
 
       # avoidance module common setting
       enable_bound_clipping: false
-      enable_update_path_when_object_is_gone: false
       enable_force_avoidance_for_stopped_vehicle: false
       enable_yield_maneuver: true
       enable_yield_maneuver_during_shifting: false
+      enable_cancel_maneuver: false
       disable_path_update: false
 
       # drivable area setting

planning/behavior_path_planner/config/dynamic_avoidance.param.yaml

@@ -0,0 +1,58 @@
+/**:
+  ros__parameters:
+    dynamic_avoidance:
+      common:
+        enable_debug_info: true
+
+      # avoidance is performed for the object type with true
+      target_object:
+        car: true
+        truck: true
+        bus: true
+        trailer: true
+        unknown: false
+        bicycle: false
+        motorcycle: true
+        pedestrian: false
+
+        min_obstacle_vel: 0.0 # [m/s]
+
+        successive_num_to_entry_dynamic_avoidance_condition: 5
+        successive_num_to_exit_dynamic_avoidance_condition: 1
+
+        min_obj_lat_offset_to_ego_path: 0.0 # [m]
+        max_obj_lat_offset_to_ego_path: 1.0 # [m]
+
+        cut_in_object:
+          min_time_to_start_cut_in: 1.0 # [s]
+          min_lon_offset_ego_to_object: 0.0 # [m]
+
+        cut_out_object:
+          max_time_from_outside_ego_path: 2.0 # [s]
+          min_object_lat_vel: 0.3 # [m/s]
+
+        crossing_object:
+          min_overtaking_object_vel: 1.0
+          max_overtaking_object_angle: 1.05
+          min_oncoming_object_vel: 0.0
+          max_oncoming_object_angle: 0.523
+
+        front_object:
+          max_object_angle: 0.785
+
+      drivable_area_generation:
+        lat_offset_from_obstacle: 0.8 # [m]
+        max_lat_offset_to_avoid: 0.5 # [m]
+
+        # for same directional object
+        overtaking_object:
+          max_time_to_collision: 40.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: 15.0 # [s]
+          start_duration_to_avoid: 12.0  # [s]
+          end_duration_to_avoid: 0.0  # [s]

planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml

@@ -3,6 +3,7 @@
     dynamic_avoidance:
       common:
         enable_debug_info: true
+        use_hatched_road_markings: true
 
       # avoidance is performed for the object type with true
       target_object:
@@ -18,6 +19,7 @@
         min_obstacle_vel: 0.0 # [m/s]
 
         successive_num_to_entry_dynamic_avoidance_condition: 5
+        successive_num_to_exit_dynamic_avoidance_condition: 1
 
         min_obj_lat_offset_to_ego_path: 0.0 # [m]
         max_obj_lat_offset_to_ego_path: 1.0 # [m]
@@ -26,26 +28,34 @@
           min_time_to_start_cut_in: 1.0 # [s]
           min_lon_offset_ego_to_object: 0.0 # [m]
 
+        cut_out_object:
+          max_time_from_outside_ego_path: 2.0 # [s]
+          min_object_lat_vel: 0.3 # [m/s]
+
         crossing_object:
-          min_object_vel: 1.0
-          max_object_angle: 1.05
+          min_overtaking_object_vel: 1.0
+          max_overtaking_object_angle: 1.05
+          min_oncoming_object_vel: 0.0
+          max_oncoming_object_angle: 0.523
 
         front_object:
           max_object_angle: 0.785
 
       drivable_area_generation:
-        lat_offset_from_obstacle: 1.0 # [m]
+        lat_offset_from_obstacle: 0.8 # [m]
         max_lat_offset_to_avoid: 0.5 # [m]
+        max_time_for_object_lat_shift: 2.0 # [s]
+        lpf_gain_for_lat_avoid_to_offset: 0.9 # [-]
 
         # for same directional object
         overtaking_object:
-          max_time_to_collision: 10.0 # [s]
+          max_time_to_collision: 40.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: 15.0 # [s]
+          max_time_to_collision: 40.0 # [s] This value should be the same as overtaking_object's one to suppress chattering of this value for parked vehicles
           start_duration_to_avoid: 12.0  # [s]
           end_duration_to_avoid: 0.0  # [s]

planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md

@@ -601,13 +601,13 @@ $$
 
 ### Avoidance cancelling maneuver
 
-If `enable_update_path_when_object_is_gone` parameter is true, Avoidance Module takes different actions according to the situations as follows:
+If `enable_cancel_maneuver` parameter is true, Avoidance Module takes different actions according to the situations as follows:
 
 - If vehicle stops: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is stopping, the avoidance path will cancelled.
 - If vehicle is in motion, but avoidance maneuver doesn't started: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is not started avoidance maneuver, the avoidance path will cancelled.
 - If vehicle is in motion, avoidance maneuver started: If there is any object in the path of the vehicle, the avoidance path is generated,but if this object goes away while the vehicle is started avoidance maneuver, the avoidance path will not cancelled.
 
-If `enable_update_path_when_object_is_gone` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone.
+If `enable_cancel_maneuver` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone.
 
 ## How to keep the consistency of the optimize-base path generation logic
 
@@ -621,15 +621,15 @@ The avoidance specific parameter configuration file can be located at `src/autow
 
 namespace: `avoidance.`
 
-| Name                                   | Unit | Type   | Description                                                                                                                         | Default value |
-| :------------------------------------- | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| resample_interval_for_planning         | [m]  | double | Path resample interval for avoidance planning path.                                                                                 | 0.3           |
-| resample_interval_for_output           | [m]  | double | Path resample interval for output path. Too short interval increases computational cost for latter modules.                         | 4.0           |
-| detection_area_right_expand_dist       | [m]  | double | Lanelet expand length for right side to find avoidance target vehicles.                                                             | 0.0           |
-| detection_area_left_expand_dist        | [m]  | double | Lanelet expand length for left side to find avoidance target vehicles.                                                              | 1.0           |
-| enable_update_path_when_object_is_gone | [-]  | bool   | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false         |
-| enable_yield_maneuver                  | [-]  | bool   | Flag to enable yield maneuver.                                                                                                      | false         |
-| enable_yield_maneuver_during_shifting  | [-]  | bool   | Flag to enable yield maneuver during shifting.                                                                                      | false         |
+| Name                                  | Unit | Type   | Description                                                                                                                         | Default value |
+| :------------------------------------ | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| resample_interval_for_planning        | [m]  | double | Path resample interval for avoidance planning path.                                                                                 | 0.3           |
+| resample_interval_for_output          | [m]  | double | Path resample interval for output path. Too short interval increases computational cost for latter modules.                         | 4.0           |
+| detection_area_right_expand_dist      | [m]  | double | Lanelet expand length for right side to find avoidance target vehicles.                                                             | 0.0           |
+| detection_area_left_expand_dist       | [m]  | double | Lanelet expand length for left side to find avoidance target vehicles.                                                              | 1.0           |
+| enable_cancel_maneuver                | [-]  | bool   | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false         |
+| enable_yield_maneuver                 | [-]  | bool   | Flag to enable yield maneuver.                                                                                                      | false         |
+| enable_yield_maneuver_during_shifting | [-]  | bool   | Flag to enable yield maneuver during shifting.                                                                                      | false         |
 
 | Name                      | Unit | Type | Description                                                                                                             | Default value |
 | :------------------------ | ---- | ---- | ----------------------------------------------------------------------------------------------------------------------- | ------------- |