hotfix: murooka's change for beta v0.42.1.0 (#615)

* feat(dynamic_avoidance): apply LPF to shift length, and positive relative velocity (autowarefoundation#4047) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * feat(dynamic_avoidance): suppress flickering of dynamic avoidance launching (autowarefoundation#4048) * feat(dynamic_avoidance): suppress flickering of dynamic avoidance launching Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update config Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * feat(dynamic_avoidance): ignore cut-out vehicle Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update cut out decision policy Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix(obstacle_avoidance_planner): smooth path connection after optimization (autowarefoundation#3754) * fix(obstacle_avoidance_planner): smooth path connection after optimization Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix(obstacle_avoidance_planner): fix optimization constraint for narrow-road driving (autowarefoundation#3701) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix(lane_change): copy drivable area info (autowarefoundation#3738) Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(map_based_prediction): suppress lane change decision flickering (autowarefoundation#3788) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * feat(map_based_prediction): disable time delay compensation (autowarefoundation#3806) * disable time delay compensation Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix unused parameter Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix lane change decision not working problem due to empty future_lanenet in unconnected lanelets Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> --------- Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix(behavior_path_planner): boundary's nearest search (autowarefoundation#3716) * fix(behavior_path_planner): boundary's nearest search Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix(behavior_path_planner): extract backward obstacles correctly (autowarefoundation#3719) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix(behavior_path_planner): extract obstacles from drivable area without intersection (autowarefoundation#3741) * fix(behavior_path_planner): extract obstacles from drivable area without intersection Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update map_based_prediction method Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix(avoidance): fix invalid cherry pick Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Yoshi Ri <yoshiyoshidetteiu@gmail.com> Co-authored-by: satoshi-ota <satoshi.ota928@gmail.com>
tier4 · Jun 26, 2023 · 08fc2c9 · 08fc2c9
1 parent caf77f4
commit 08fc2c9
Show file tree

Hide file tree

Showing 14 changed files with 367 additions and 108 deletions.
diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml
@@ -13,7 +13,7 @@
     history_time_length: 1.0 #[s]
 
     lane_change_detection:
-      method: "time_to_change_lane" # choose from "lat_diff_distance" or "time_to_change_lane"
+      method: "lat_diff_distance" # choose from "lat_diff_distance" or "time_to_change_lane"
       time_to_change_lane:
         dist_threshold_for_lane_change_detection: 1.0 #[m]
         time_threshold_for_lane_change_detection: 5.0 #[s]
@@ -23,5 +23,6 @@
         dist_ratio_threshold_to_right_bound: 0.5 #[ratio
         diff_dist_threshold_to_left_bound: 0.29 #[m]
         diff_dist_threshold_to_right_bound: -0.29 #[m]
+      num_continuous_state_transition: 3
 
     reference_path_resolution: 0.5 #[m]
diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -59,6 +59,13 @@ struct LateralKinematicsToLanelet
   double filtered_right_lateral_velocity;
 };
 
+enum class Maneuver {
+  UNINITIALIZED = 0,
+  LANE_FOLLOW = 1,
+  LEFT_LANE_CHANGE = 2,
+  RIGHT_LANE_CHANGE = 3,
+};
+
 struct ObjectData
 {
   std_msgs::msg::Header header;
@@ -69,12 +76,9 @@ struct ObjectData
   double time_delay;
   // for lane change prediction
   std::unordered_map<lanelet::ConstLanelet, LateralKinematicsToLanelet> lateral_kinematics_set;
-};
-
-enum class Maneuver {
-  LANE_FOLLOW = 0,
-  LEFT_LANE_CHANGE = 1,
-  RIGHT_LANE_CHANGE = 2,
+  Maneuver one_shot_maneuver{Maneuver::UNINITIALIZED};
+  Maneuver output_maneuver{
+    Maneuver::UNINITIALIZED};  // output maneuver considering previous one shot maneuvers
 };
 
 struct LaneletData
@@ -153,6 +157,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   double dist_ratio_threshold_to_right_bound_;
   double diff_dist_threshold_to_left_bound_;
   double diff_dist_threshold_to_right_bound_;
+  int num_continuous_state_transition_;
   double reference_path_resolution_;
 
   // Stop watch

diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -517,6 +517,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
       "lane_change_detection.lat_diff_distance.diff_dist_threshold_to_left_bound");
     diff_dist_threshold_to_right_bound_ = declare_parameter<double>(
       "lane_change_detection.lat_diff_distance.diff_dist_threshold_to_right_bound");
+
+    num_continuous_state_transition_ =
+      declare_parameter<int>("lane_change_detection.num_continuous_state_transition");
   }
   reference_path_resolution_ = declare_parameter("reference_path_resolution", 0.5);
 
@@ -1170,14 +1173,50 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver(
   const TrackedObject & object, const LaneletData & current_lanelet_data,
   const double object_detected_time)
 {
-  if (lane_change_detection_method_ == "time_to_change_lane") {
-    return predictObjectManeuverByTimeToLaneChange(
-      object, current_lanelet_data, object_detected_time);
-  } else if (lane_change_detection_method_ == "lat_diff_distance") {
-    return predictObjectManeuverByLatDiffDistance(
-      object, current_lanelet_data, object_detected_time);
-  }
-  throw std::logic_error("Lane change detection method is invalid.");
+  // calculate maneuver
+  const auto current_maneuver = [&]() {
+    if (lane_change_detection_method_ == "time_to_change_lane") {
+      return predictObjectManeuverByTimeToLaneChange(
+        object, current_lanelet_data, object_detected_time);
+    } else if (lane_change_detection_method_ == "lat_diff_distance") {
+      return predictObjectManeuverByLatDiffDistance(
+        object, current_lanelet_data, object_detected_time);
+    }
+    throw std::logic_error("Lane change detection method is invalid.");
+  }();
+
+  const std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
+  if (objects_history_.count(object_id) == 0) {
+    return current_maneuver;
+  }
+  auto & object_info = objects_history_.at(object_id);
+
+  // update maneuver in object history
+  if (!object_info.empty()) {
+    object_info.back().one_shot_maneuver = current_maneuver;
+  }
+
+  // decide maneuver considering previous results
+  if (object_info.size() < 2) {
+    object_info.back().output_maneuver = current_maneuver;
+    return current_maneuver;
+  }
+  // NOTE: The index of previous maneuver is not object_info.size() - 1
+  const auto prev_output_maneuver =
+    object_info.at(static_cast<int>(object_info.size()) - 2).output_maneuver;
+
+  for (int i = 0;
+       i < std::min(num_continuous_state_transition_, static_cast<int>(object_info.size())); ++i) {
+    const auto & tmp_maneuver =
+      object_info.at(static_cast<int>(object_info.size()) - 1 - i).one_shot_maneuver;
+    if (tmp_maneuver != current_maneuver) {
+      object_info.back().output_maneuver = prev_output_maneuver;
+      return prev_output_maneuver;
+    }
+  }
+
+  object_info.back().output_maneuver = current_maneuver;
+  return current_maneuver;
 }
 
 Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange(
@@ -1253,7 +1292,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange(
 
 Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance(
   const TrackedObject & object, const LaneletData & current_lanelet_data,
-  const double object_detected_time)
+  const double /*object_detected_time*/)
 {
   // Step1. Check if we have the object in the buffer
   const std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
@@ -1283,7 +1322,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance(
 
   // Step3. Get closest previous lanelet ID
   const auto & prev_info = object_info.at(static_cast<size_t>(prev_id));
-  const auto prev_pose = compensateTimeDelay(prev_info.pose, prev_info.twist, prev_info.time_delay);
+  const auto prev_pose = prev_info.pose;
   const lanelet::ConstLanelets prev_lanelets =
     object_info.at(static_cast<size_t>(prev_id)).current_lanelets;
   if (prev_lanelets.empty()) {
@@ -1303,19 +1342,20 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance(
 
   // Step4. Check if the vehicle has changed lane
   const auto current_lanelet = current_lanelet_data.lanelet;
-  const double current_time_delay = std::max(current_time - object_detected_time, 0.0);
-  const auto current_pose = compensateTimeDelay(
-    object.kinematics.pose_with_covariance.pose, object.kinematics.twist_with_covariance.twist,
-    current_time_delay);
+  const auto current_pose = object.kinematics.pose_with_covariance.pose;
   const double dist = tier4_autoware_utils::calcDistance2d(prev_pose, current_pose);
   lanelet::routing::LaneletPaths possible_paths =
     routing_graph_ptr_->possiblePaths(prev_lanelet, dist + 2.0, 0, false);
   bool has_lane_changed = true;
-  for (const auto & path : possible_paths) {
-    for (const auto & lanelet : path) {
-      if (lanelet == current_lanelet) {
-        has_lane_changed = false;
-        break;
+  if (prev_lanelet == current_lanelet) {
+    has_lane_changed = false;
+  } else {
+    for (const auto & path : possible_paths) {
+      for (const auto & lanelet : path) {
+        if (lanelet == current_lanelet) {
+          has_lane_changed = false;
+          break;
+        }
       }
     }
   }

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
@@ -12,21 +12,23 @@
         motorcycle: true
         pedestrian: false
 
-        min_obstacle_vel: 1.0 # [m/s]
+        min_obstacle_vel: 0.0 # [m/s]
+
+        successive_num_to_entry_dynamic_avoidance_condition: 5
 
       drivable_area_generation:
-        lat_offset_from_obstacle: 1.3 # [m]
+        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: 3.0 # [s]
           start_duration_to_avoid: 4.0  # [s]
-          end_duration_to_avoid: 5.0  # [s]
+          end_duration_to_avoid: 8.0  # [s]
           duration_to_hold_avoidance: 3.0 # [s]
 
         # for opposite directional object
         oncoming_object:
           max_time_to_collision: 3.0 # [s]
-          start_duration_to_avoid: 9.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
@@ -47,6 +47,7 @@ struct DynamicAvoidanceParameters
   bool avoid_motorcycle{false};
   bool avoid_pedestrian{false};
   double min_obstacle_vel{0.0};
+  int successive_num_to_entry_dynamic_avoidance_condition{0};
 
   // drivable area generation
   double lat_offset_from_obstacle{0.0};
@@ -67,24 +68,46 @@ class DynamicAvoidanceModule : public SceneModuleInterface
 public:
   struct DynamicAvoidanceObject
   {
-    explicit DynamicAvoidanceObject(
-      const PredictedObject & predicted_object, const double arg_path_projected_vel)
-    : pose(predicted_object.kinematics.initial_pose_with_covariance.pose),
-      path_projected_vel(arg_path_projected_vel),
+    DynamicAvoidanceObject(
+      const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel)
+    : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)),
+      pose(predicted_object.kinematics.initial_pose_with_covariance.pose),
+      vel(arg_vel),
+      lat_vel(arg_lat_vel),
       shape(predicted_object.shape)
     {
       for (const auto & path : predicted_object.kinematics.predicted_paths) {
         predicted_paths.push_back(path);
       }
     }
 
-    const geometry_msgs::msg::Pose pose;
-    const double path_projected_vel;
-    const autoware_auto_perception_msgs::msg::Shape shape;
+    std::string uuid;
+    geometry_msgs::msg::Pose pose;
+    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;
   };
+  struct DynamicAvoidanceObjectCandidate
+  {
+    DynamicAvoidanceObject object;
+    int alive_counter;
+
+    static std::optional<DynamicAvoidanceObjectCandidate> getObjectFromUuid(
+      const std::vector<DynamicAvoidanceObjectCandidate> & objects, const std::string & target_uuid)
+    {
+      const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) {
+        return object.object.uuid == target_uuid;
+      });
+
+      if (itr == objects.end()) {
+        return std::nullopt;
+      }
+      return *itr;
+    }
+  };
 
 #ifdef USE_OLD_ARCHITECTURE
   DynamicAvoidanceModule(
@@ -117,14 +140,54 @@ class DynamicAvoidanceModule : public SceneModuleInterface
 
 private:
   bool isLabelTargetObstacle(const uint8_t label) const;
-  std::vector<DynamicAvoidanceObject> calcTargetObjects() const;
+  std::vector<DynamicAvoidanceObjectCandidate> calcTargetObjectsCandidate() const;
   std::pair<lanelet::ConstLanelets, lanelet::ConstLanelets> getAdjacentLanes(
     const double forward_distance, const double backward_distance) const;
   std::optional<tier4_autoware_utils::Polygon2d> calcDynamicObstaclePolygon(
     const DynamicAvoidanceObject & object) const;
 
+  std::vector<DynamicAvoidanceModule::DynamicAvoidanceObjectCandidate>
+    prev_target_objects_candidate_;
   std::vector<DynamicAvoidanceModule::DynamicAvoidanceObject> target_objects_;
+  // std::vector<DynamicAvoidanceModule::DynamicAvoidanceObject> prev_target_objects_;
   std::shared_ptr<DynamicAvoidanceParameters> parameters_;
+
+  struct ObjectsVariable
+  {
+    void resetCurrentUuids() { current_uuids_.clear(); }
+    void addCurrentUuid(const std::string & uuid) { current_uuids_.push_back(uuid); }
+    void removeCounterUnlessUpdated()
+    {
+      std::vector<std::string> obsolete_uuids;
+      for (const auto & key_and_value : variable_) {
+        if (
+          std::find(current_uuids_.begin(), current_uuids_.end(), key_and_value.first) ==
+          current_uuids_.end()) {
+          obsolete_uuids.push_back(key_and_value.first);
+        }
+      }
+
+      for (const auto & obsolete_uuid : obsolete_uuids) {
+        variable_.erase(obsolete_uuid);
+      }
+    }
+
+    std::optional<double> get(const std::string & uuid) const
+    {
+      if (variable_.count(uuid) != 0) {
+        return variable_.at(uuid);
+      }
+      return std::nullopt;
+    }
+    void update(const std::string & uuid, const double new_variable)
+    {
+      variable_.emplace(uuid, new_variable);
+    }
+
+    std::unordered_map<std::string, double> variable_;
+    std::vector<std::string> current_uuids_;
+  };
+  mutable ObjectsVariable prev_objects_min_bound_lat_offset_;
 };
 }  // namespace behavior_path_planner
 

diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml
@@ -62,6 +62,7 @@
   <depend>route_handler</depend>
   <depend>rtc_interface</depend>
   <depend>sensor_msgs</depend>
+  <depend>signal_processing</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -721,6 +721,8 @@ DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam()
     p.avoid_motorcycle = declare_parameter<bool>(ns + "motorcycle");
     p.avoid_pedestrian = declare_parameter<bool>(ns + "pedestrian");
     p.min_obstacle_vel = declare_parameter<double>(ns + "min_obstacle_vel");
+    p.successive_num_to_entry_dynamic_avoidance_condition =
+      declare_parameter<int>(ns + "successive_num_to_entry_dynamic_avoidance_condition");
   }
 
   {  // drivable_area_generation