From c51ccc6575a6c16d29c1d3b750ec90b5e3bafa41 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 13 Dec 2024 15:00:09 +0900 Subject: [PATCH 1/2] feat(obstacle_cruise_planner): independant decision making Signed-off-by: Takayuki Murooka --- .../src/node.cpp | 21 ++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 6931ff58e8457..86a140cbb0551 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -1120,19 +1120,16 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPredictedObjectObstacles( odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); if (cruise_obstacle) { cruise_obstacles.push_back(*cruise_obstacle); - continue; } const auto stop_obstacle = createStopObstacleForPredictedObject( odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); if (stop_obstacle) { stop_obstacles.push_back(*stop_obstacle); - continue; } const auto slow_down_obstacle = createSlowDownObstacleForPredictedObject( odometry, decimated_traj_points, obstacle, precise_lat_dist); if (slow_down_obstacle) { slow_down_obstacles.push_back(*slow_down_obstacle); - continue; } } const auto & p = behavior_determination_param_; @@ -1484,6 +1481,7 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( return std::nullopt; } + /* { // consider hysteresis // const bool is_prev_obstacle_stop = getObstacleFromUuid(prev_stop_obstacles_, // obstacle.uuid).has_value(); @@ -1504,6 +1502,7 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( // NOTE: else is cruise from stop } } + */ // Get highest confidence predicted path const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths( @@ -1666,6 +1665,22 @@ std::optional ObstacleCruisePlannerNode::createStopObstacleForPred if (!isStopObstacle(obstacle.classification.label)) { return std::nullopt; } + + const bool is_prev_obstacle_stop = + getObstacleFromUuid(prev_stop_object_obstacles_, obstacle.uuid).has_value(); + + if (is_prev_obstacle_stop) { + if (p.obstacle_velocity_threshold_from_stop_to_cruise < obstacle.longitudinal_velocity) { + return std::nullopt; + } + // keep obstacle stop + } else { + if (p.obstacle_velocity_threshold_from_cruise_to_stop < obstacle.longitudinal_velocity) { + return std::nullopt; + } + // NOTE: else is stop from cruise + } + const double max_lat_margin_for_stop = (obstacle.classification.label == ObjectClassification::UNKNOWN) ? p.max_lat_margin_for_stop_against_unknown From 1b516d60c860c1c4981931bb7036e0ce0fff933a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 13 Dec 2024 18:54:18 +0900 Subject: [PATCH 2/2] fix Signed-off-by: Takayuki Murooka --- planning/autoware_obstacle_cruise_planner/src/node.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 86a140cbb0551..4feee696370ef 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -1481,7 +1481,6 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( return std::nullopt; } - /* { // consider hysteresis // const bool is_prev_obstacle_stop = getObstacleFromUuid(prev_stop_obstacles_, // obstacle.uuid).has_value(); @@ -1502,7 +1501,6 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( // NOTE: else is cruise from stop } } - */ // Get highest confidence predicted path const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths(