diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index f637911dae903..21e3ecb7bf758 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1378,7 +1378,6 @@ void ObstacleCruisePlannerNode::checkConsistency( const auto current_closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles); - // If previous closest obstacle ptr is not set if (!prev_closest_stop_obstacle_ptr_) { if (current_closest_stop_obstacle) { prev_closest_stop_obstacle_ptr_ = @@ -1387,44 +1386,23 @@ void ObstacleCruisePlannerNode::checkConsistency( return; } - // Put previous closest target obstacle if necessary const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), [&](PredictedObject predicted_object) { return tier4_autoware_utils::toHexString(predicted_object.object_id) == prev_closest_stop_obstacle_ptr_->uuid; }); - - // If previous closest obstacle is not in the current perception lists - // just return the current target obstacles + // If previous closest obstacle disappear from the perception result, do nothing anymore. if (predicted_object_itr == predicted_objects.objects.end()) { return; } - // Previous closest obstacle is in the perception lists - const auto obstacle_itr = std::find_if( + const auto is_disappeared_from_stop_obstacle = std::none_of( stop_obstacles.begin(), stop_obstacles.end(), [&](const auto & obstacle) { return obstacle.uuid == prev_closest_stop_obstacle_ptr_->uuid; }); - - // Previous closest obstacle is both in the perception lists and target obstacles - if (obstacle_itr != stop_obstacles.end()) { - if (current_closest_stop_obstacle) { - if ((current_closest_stop_obstacle->uuid == prev_closest_stop_obstacle_ptr_->uuid)) { - // prev_closest_obstacle is current_closest_stop_obstacle just return the target - // obstacles(in target obstacles) - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } else { - // New obstacle becomes new stop obstacle - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } - } else { - // Previous closest stop obstacle becomes cruise obstacle - prev_closest_stop_obstacle_ptr_ = nullptr; - } - } else { - // prev obstacle is not in the target obstacles, but in the perception list + if (is_disappeared_from_stop_obstacle) { + // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" + // condition is satisfied const double elapsed_time = (current_time - prev_closest_stop_obstacle_ptr_->stamp).seconds(); if ( predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < @@ -1433,13 +1411,13 @@ void ObstacleCruisePlannerNode::checkConsistency( stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_); return; } + } - if (current_closest_stop_obstacle) { - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } else { - prev_closest_stop_obstacle_ptr_ = nullptr; - } + if (current_closest_stop_obstacle) { + prev_closest_stop_obstacle_ptr_ = + std::make_shared(*current_closest_stop_obstacle); + } else { + prev_closest_stop_obstacle_ptr_ = nullptr; } }