From 386e1a6951e24cb3f3631e6aaeb3261d1e461696 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Wed, 31 Jan 2024 19:10:56 +0900 Subject: [PATCH 1/2] add guard Signed-off-by: Yuki Takagi --- planning/obstacle_stop_planner/src/node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 050c4af55c9bf..6acc7400f52d8 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -1549,6 +1549,7 @@ void ObstacleStopPlannerNode::filterObstacles( const double dist_threshold, PredictedObjects & filtered_objects) { filtered_objects.header = input_objects.header; + if (traj.size() < 2) return; for (auto & object : input_objects.objects) { // Check is it in front of ego vehicle From b08e7457e622a03477fb4a49763f2f46e37b5984 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Wed, 31 Jan 2024 19:13:10 +0900 Subject: [PATCH 2/2] refactor Signed-off-by: Yuki Takagi --- planning/obstacle_stop_planner/src/node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 6acc7400f52d8..3bdfd67a7bec4 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -1548,9 +1548,10 @@ void ObstacleStopPlannerNode::filterObstacles( const PredictedObjects & input_objects, const Pose & ego_pose, const TrajectoryPoints & traj, const double dist_threshold, PredictedObjects & filtered_objects) { - filtered_objects.header = input_objects.header; if (traj.size() < 2) return; + filtered_objects.header = input_objects.header; + for (auto & object : input_objects.objects) { // Check is it in front of ego vehicle if (!isFrontObstacle(ego_pose, object.kinematics.initial_pose_with_covariance.pose.position)) {