fix(avoidance): fix bug in the logic to check object moving distance (a…

…utowarefoundation#6766)

fix(avoidance): check object moving distance

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -424,9 +424,6 @@ class AvoidanceModule : public SceneModuleInterface
   // TODO(Satoshi OTA) create detected object manager.
   ObjectDataArray registered_objects_;
 
-  // TODO(Satoshi OTA) remove mutable.
-  mutable ObjectDataArray detected_objects_;
-
   // TODO(Satoshi OTA) remove this variable.
   mutable ObjectDataArray ego_stopped_objects_;
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

@@ -112,8 +112,6 @@ void fillObjectStoppableJudge(
   ObjectData & object_data, const ObjectDataArray & registered_objects,
   const double feasible_stop_distance, const std::shared_ptr<AvoidanceParameters> & parameters);
 
-void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects);
-
 void updateRegisteredObject(
   ObjectDataArray & registered_objects, const ObjectDataArray & now_objects,
   const std::shared_ptr<AvoidanceParameters> & parameters);

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -412,9 +412,6 @@ ObjectData AvoidanceModule::createObjectData(
   // Calc moving time.
   utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_);
 
-  // Fill init pose.
-  utils::avoidance::fillInitialPose(object_data, detected_objects_);
-
   // Calc lateral deviation from path to target object.
   object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0
                             ? Direction::LEFT

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -659,15 +659,6 @@ bool isNeverAvoidanceTarget(
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {
-  const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
-  const auto is_moving_distance_longer_than_threshold =
-    tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) >
-    parameters->distance_threshold_for_ambiguous_vehicle;
-  if (is_moving_distance_longer_than_threshold) {
-    object.reason = AvoidanceDebugFactor::MOVING_OBJECT;
-    return true;
-  }
-
   if (object.is_within_intersection) {
     if (object.behavior == ObjectData::Behavior::NONE) {
       object.reason = "ParallelToEgoLane";
@@ -856,6 +847,15 @@ bool isSatisfiedWithVehicleCondition(
     return false;
   }
 
+  const auto & current_pose = object.object.kinematics.initial_pose_with_covariance.pose;
+  const auto is_moving_distance_longer_than_threshold =
+    calcDistance2d(object.init_pose, current_pose) >
+    parameters->distance_threshold_for_ambiguous_vehicle;
+  if (is_moving_distance_longer_than_threshold) {
+    object.reason = "AmbiguousStoppedVehicle";
+    return false;
+  }
+
   if (object.is_within_intersection) {
     if (object.behavior == ObjectData::Behavior::DEVIATING) {
       object.reason = "AmbiguousStoppedVehicle(wait-and-see)";
@@ -1488,6 +1488,7 @@ void fillObjectMovingTime(
     object_data.last_stop = now;
     object_data.move_time = 0.0;
     if (is_new_object) {
+      object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose;
       object_data.stop_time = 0.0;
       object_data.last_move = now;
       stopped_objects.push_back(object_data);
@@ -1496,11 +1497,13 @@ void fillObjectMovingTime(
       same_id_obj->last_stop = now;
       same_id_obj->move_time = 0.0;
       object_data.stop_time = same_id_obj->stop_time;
+      object_data.init_pose = same_id_obj->init_pose;
     }
     return;
   }
 
   if (is_new_object) {
+    object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose;
     object_data.move_time = std::numeric_limits<double>::infinity();
     object_data.stop_time = 0.0;
     object_data.last_move = now;
@@ -1510,6 +1513,7 @@ void fillObjectMovingTime(
   object_data.last_stop = same_id_obj->last_stop;
   object_data.move_time = (now - same_id_obj->last_stop).seconds();
   object_data.stop_time = 0.0;
+  object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose;
 
   if (object_data.move_time > object_parameter.moving_time_threshold) {
     stopped_objects.erase(same_id_obj);
@@ -1556,22 +1560,6 @@ void fillAvoidanceNecessity(
   object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate);
 }
 
-void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects)
-{
-  const auto id = object_data.object.object_id;
-  const auto same_id_obj = std::find_if(
-    detected_objects.begin(), detected_objects.end(),
-    [&id](const auto & o) { return o.object.object_id == id; });
-
-  if (same_id_obj != detected_objects.end()) {
-    object_data.init_pose = same_id_obj->init_pose;
-    return;
-  }
-
-  object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose;
-  detected_objects.push_back(object_data);
-}
-
 void fillObjectStoppableJudge(
   ObjectData & object_data, const ObjectDataArray & registered_objects,
   const double feasible_stop_distance, const std::shared_ptr<AvoidanceParameters> & parameters)