Commit f0041d3 Shigekazu Fukuta
committed
1 parent 153a115 commit f0041d3 Copy full SHA for f0041d3
File tree 1 file changed +2
-2
lines changed
planning/obstacle_avoidance_planner/src
1 file changed +2
-2
lines changed Original file line number Diff line number Diff line change @@ -343,6 +343,7 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
343
343
const auto monotonic_base_yaw = convertEulerAngleToMonotonic (base_yaw);
344
344
345
345
// spline interpolation
346
+ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
346
347
try {
347
348
const auto interpolated_x = interpolation::slerp (base_s, base_x, new_s);
348
349
const auto interpolated_y = interpolation::slerp (base_s, base_y, new_s);
@@ -354,7 +355,6 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
354
355
}
355
356
}
356
357
357
- std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
358
358
for (size_t i = 0 ; i < interpolated_x.size (); i++) {
359
359
autoware_auto_planning_msgs::msg::TrajectoryPoint point;
360
360
point.pose .position .x = interpolated_x[i];
@@ -364,10 +364,10 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
364
364
interpolated_points.push_back (point);
365
365
}
366
366
367
- return interpolated_points;
368
367
} catch (const std::invalid_argument & e) {
369
368
return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
370
369
}
370
+ return interpolated_points;
371
371
}
372
372
373
373
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> getInterpolatedTrajectoryPoints (
You can’t perform that action at this time.
0 commit comments