refactor(autoware_trajectory_optimizer): move functions and change parameter names to accommodate standard

planning/autoware_trajectory_optimizer/CMakeLists.txt

@@ -24,8 +24,11 @@ ament_auto_add_library(autoware_trajectory_optimizer_plugins SHARED
   src/trajectory_optimizer_plugins/trajectory_qp_smoother.cpp
   src/trajectory_optimizer_plugins/trajectory_spline_smoother.cpp
   src/trajectory_optimizer_plugins/trajectory_velocity_optimizer.cpp
+  src/trajectory_optimizer_plugins/plugin_utils/trajectory_extender_utils.cpp
   src/trajectory_optimizer_plugins/plugin_utils/trajectory_mpt_optimizer_utils.cpp
   src/trajectory_optimizer_plugins/plugin_utils/trajectory_point_fixer_utils.cpp
+  src/trajectory_optimizer_plugins/plugin_utils/trajectory_spline_smoother_utils.cpp
+  src/trajectory_optimizer_plugins/plugin_utils/trajectory_velocity_optimizer_utils.cpp
 )
 
 if(BUILD_TESTING)

planning/autoware_trajectory_optimizer/README.md

@@ -52,7 +52,7 @@ Each plugin can be enabled/disabled at runtime via activation flags (e.g., `use_
 
 - **QP Smoother must run before EB/Akima smoothers**: The QP solver relies on constant time intervals (Δt) between trajectory points (default: 0.1s). Both Elastic Band and Akima spline smoothers resample trajectories without preserving the time domain structure, which breaks the QP solver's assumptions. Therefore, when using multiple smoothers together, the QP smoother must execute first.
 
-- **Trajectory Extender positioning**: The trajectory extender has known discontinuity issues when placed early in the pipeline. It negatively affects the QP solver results and introduces artifacts. For this reason, it has been moved to near the end of the pipeline and is **disabled by default** (`extend_trajectory_backward: false`). Fixing the extender's discontinuity issues is future work.
+- **Trajectory Extender positioning**: The trajectory extender has known discontinuity issues when placed early in the pipeline. It negatively affects the QP solver results and introduces artifacts. For this reason, it has been moved to near the end of the pipeline and is **disabled by default** (`use_trajectory_extender: false`). Fixing the extender's discontinuity issues is future work.
 
 ## QP Smoother
 

planning/autoware_trajectory_optimizer/config/plugins/trajectory_point_fixer.param.yaml

@@ -2,6 +2,7 @@
   ros__parameters:
     trajectory_point_fixer:
       # Distance thresholds for point processing
+      remove_close_points: true    # Enable removal of close proximity points
       resample_close_points: true     # Enable resampling of close proximity points
       min_dist_to_remove_m: 0.01  # [m] Minimum distance to remove close proximity points
-      min_dist_to_merge_m: 0.05   # [m] Minimum distance to merge close proximity points
+      min_dist_to_resample_m: 0.05   # [m] Minimum distance to merge close proximity points

planning/autoware_trajectory_optimizer/config/trajectory_optimizer.param.yaml

@@ -16,8 +16,8 @@
     use_akima_spline_interpolation: true
     use_eb_smoother: false
     use_qp_smoother: true
-    fix_invalid_points: true
-    optimize_velocity: true
-    extend_trajectory_backward: false
+    use_trajectory_point_fixer: true
+    use_velocity_optimizer: true
+    use_trajectory_extender: false
     use_kinematic_feasibility_enforcer: true
     use_mpt_optimizer: false

planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/plugin_utils/trajectory_extender_utils.hpp

@@ -0,0 +1,67 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTNEXTLINE
+#ifndef AUTOWARE__TRAJECTORY_OPTIMIZER__TRAJECTORY_OPTIMIZER_PLUGINS__PLUGIN_UTILS__TRAJECTORY_EXTENDER_UTILS_HPP_
+// NOLINTNEXTLINE
+#define AUTOWARE__TRAJECTORY_OPTIMIZER__TRAJECTORY_OPTIMIZER_PLUGINS__PLUGIN_UTILS__TRAJECTORY_EXTENDER_UTILS_HPP_
+
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <vector>
+
+namespace autoware::trajectory_optimizer::plugin::trajectory_extender_utils
+{
+using autoware_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+using nav_msgs::msg::Odometry;
+
+/**
+ * @brief Adds the current ego vehicle state to the trajectory history.
+ *
+ * This function adds the current ego state to a history trajectory. It includes
+ * checks to avoid adding duplicate states and limits the trajectory length
+ * based on the backward extension parameter.
+ *
+ * @param traj_points The trajectory points to be updated (modified in place)
+ * @param current_odometry The current vehicle odometry data
+ * @param nearest_dist_threshold_m Distance threshold for considering states different
+ * @param nearest_yaw_threshold_rad Yaw threshold for considering states different
+ * @param backward_trajectory_extension_m Maximum length of backward trajectory to maintain
+ */
+void add_ego_state_to_trajectory(
+  TrajectoryPoints & traj_points, const Odometry & current_odometry,
+  const double nearest_dist_threshold_m, const double nearest_yaw_threshold_rad,
+  const double backward_trajectory_extension_m);
+
+/**
+ * @brief Extends the forward trajectory with the ego history points.
+ *
+ * This function prepends historical ego states to the beginning of the trajectory
+ * to extend it backward in time. It filters out history points that are already
+ * present in the trajectory or are ahead of the first trajectory point.
+ *
+ * @param traj_points The forward trajectory points to be extended (modified in place)
+ * @param ego_history_points The historical ego state points to prepend
+ * @param current_odometry The current vehicle odometry data
+ */
+void expand_trajectory_with_ego_history(
+  TrajectoryPoints & traj_points, const TrajectoryPoints & ego_history_points,
+  const Odometry & current_odometry);
+
+}  // namespace autoware::trajectory_optimizer::plugin::trajectory_extender_utils
+
+// NOLINTNEXTLINE
+#endif  // AUTOWARE__TRAJECTORY_OPTIMIZER__TRAJECTORY_OPTIMIZER_PLUGINS__PLUGIN_UTILS__TRAJECTORY_EXTENDER_UTILS_HPP_

planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/plugin_utils/trajectory_point_fixer_utils.hpp

@@ -109,6 +109,28 @@ void resample_single_cluster(
 void resample_close_proximity_points(
   TrajectoryPoints & traj_points, const Odometry & current_odometry, const double min_dist_m);
 
+/**
+ * @brief Removes invalid points from the trajectory.
+ *
+ * Removes points with NaN or Inf values from the trajectory. Validates each point
+ * using validate_point() from common utils.
+ *
+ * @param input_trajectory The trajectory points to be cleaned (modified in place)
+ */
+void remove_invalid_points(TrajectoryPoints & input_trajectory);
+
+/**
+ * @brief Removes points from the trajectory that are too close to each other.
+ *
+ * Iterates through the trajectory and removes consecutive points that are closer
+ * than the minimum distance threshold. Always keeps the first point.
+ *
+ * @param input_trajectory_array The trajectory points to be cleaned (modified in place)
+ * @param min_dist The minimum distance between points (default: 1E-2)
+ */
+void remove_close_proximity_points(
+  TrajectoryPoints & input_trajectory_array, const double min_dist = 1e-2);
+
 }  // namespace autoware::trajectory_optimizer::plugin::trajectory_point_fixer_utils
 
 // NOLINTNEXTLINE

planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/plugin_utils/trajectory_spline_smoother_utils.hpp

@@ -0,0 +1,48 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTNEXTLINE
+#ifndef AUTOWARE__TRAJECTORY_OPTIMIZER__TRAJECTORY_OPTIMIZER_PLUGINS__PLUGIN_UTILS__TRAJECTORY_SPLINE_SMOOTHER_UTILS_HPP_
+// NOLINTNEXTLINE
+#define AUTOWARE__TRAJECTORY_OPTIMIZER__TRAJECTORY_OPTIMIZER_PLUGINS__PLUGIN_UTILS__TRAJECTORY_SPLINE_SMOOTHER_UTILS_HPP_
+
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
+
+#include <vector>
+
+namespace autoware::trajectory_optimizer::plugin::trajectory_spline_smoother_utils
+{
+using autoware_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+
+/**
+ * @brief Interpolates trajectory using Akima spline interpolation.
+ *
+ * Applies Akima spline interpolation to smooth the trajectory path with configurable
+ * resolution. Optionally preserves the original trajectory's orientation by copying
+ * from nearest points using copy_trajectory_orientation from common utils.
+ *
+ * @param traj_points The trajectory points to be interpolated (modified in place)
+ * @param interpolation_resolution_m Interpolation resolution for Akima spline
+ * @param max_distance_discrepancy_m Maximum position deviation for orientation copying
+ * @param preserve_original_orientation If true, copies orientation from original trajectory
+ */
+void apply_spline(
+  TrajectoryPoints & traj_points, double interpolation_resolution_m,
+  double max_distance_discrepancy_m, bool preserve_original_orientation);
+
+}  // namespace autoware::trajectory_optimizer::plugin::trajectory_spline_smoother_utils
+
+// NOLINTNEXTLINE
+#endif  // AUTOWARE__TRAJECTORY_OPTIMIZER__TRAJECTORY_OPTIMIZER_PLUGINS__PLUGIN_UTILS__TRAJECTORY_SPLINE_SMOOTHER_UTILS_HPP_

planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/plugin_utils/trajectory_velocity_optimizer_utils.hpp

@@ -0,0 +1,99 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// NOLINTNEXTLINE
+#ifndef AUTOWARE__TRAJECTORY_OPTIMIZER__TRAJECTORY_OPTIMIZER_PLUGINS__PLUGIN_UTILS__TRAJECTORY_VELOCITY_OPTIMIZER_UTILS_HPP_
+// NOLINTNEXTLINE
+#define AUTOWARE__TRAJECTORY_OPTIMIZER__TRAJECTORY_OPTIMIZER_PLUGINS__PLUGIN_UTILS__TRAJECTORY_VELOCITY_OPTIMIZER_UTILS_HPP_
+
+#include "autoware/trajectory_optimizer/trajectory_optimizer_structs.hpp"
+#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp"
+
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <memory>
+#include <vector>
+
+namespace autoware::trajectory_optimizer::plugin::trajectory_velocity_optimizer_utils
+{
+using autoware::trajectory_optimizer::InitialMotion;
+using autoware::velocity_smoother::JerkFilteredSmoother;
+using autoware_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+using nav_msgs::msg::Odometry;
+
+/**
+ * @brief Clamps the velocities of trajectory points to specified minimum values.
+ *
+ * Ensures all points have at least the minimum velocity and acceleration.
+ *
+ * @param input_trajectory_array The trajectory points to be clamped (modified in place)
+ * @param min_velocity The minimum velocity to enforce
+ * @param min_acceleration The minimum acceleration to enforce
+ */
+void clamp_velocities(
+  TrajectoryPoints & input_trajectory_array, const float min_velocity,
+  const float min_acceleration);
+
+/**
+ * @brief Sets the maximum velocity for trajectory points while preserving dynamics.
+ *
+ * This function identifies segments where velocity exceeds max_velocity and caps them
+ * while preserving time intervals computed from velocity/acceleration relationships.
+ * Interior accelerations in capped segments are set to 0 (constant velocity), and
+ * boundary accelerations are recalculated for smooth transitions.
+ *
+ * @param input_trajectory_array The trajectory points to be updated (modified in place)
+ * @param max_velocity The maximum velocity to enforce
+ */
+void set_max_velocity(TrajectoryPoints & input_trajectory_array, const float max_velocity);
+
+/**
+ * @brief Limits lateral acceleration by reducing velocity at high curvature points.
+ *
+ * Calculates lateral acceleration from yaw rate and velocity, then reduces velocity
+ * where lateral acceleration exceeds the limit. Recalculates longitudinal acceleration
+ * after velocity adjustments.
+ *
+ * @param input_trajectory_array The trajectory points to be updated (modified in place)
+ * @param max_lateral_accel_mps2 Maximum allowed lateral acceleration
+ * @param current_odometry Current vehicle odometry for time calculation
+ */
+void limit_lateral_acceleration(
+  TrajectoryPoints & input_trajectory_array, const double max_lateral_accel_mps2,
+  const Odometry & current_odometry);
+
+/**
+ * @brief Filters velocity profile using jerk-constrained smoothing.
+ *
+ * Applies lateral acceleration filtering, steering rate limiting, trajectory resampling,
+ * and jerk-filtered velocity optimization using the JerkFilteredSmoother.
+ *
+ * @param input_trajectory The trajectory points to be filtered (modified in place)
+ * @param initial_motion The initial speed and acceleration for motion
+ * @param nearest_dist_threshold_m Distance threshold for trajectory matching
+ * @param nearest_yaw_threshold_rad Yaw threshold for trajectory matching
+ * @param smoother The jerk filtered smoother instance
+ * @param current_odometry The current vehicle odometry data
+ */
+void filter_velocity(
+  TrajectoryPoints & input_trajectory, const InitialMotion & initial_motion,
+  const double nearest_dist_threshold_m, const double nearest_yaw_threshold_rad,
+  const std::shared_ptr<JerkFilteredSmoother> & smoother, const Odometry & current_odometry);
+
+}  // namespace autoware::trajectory_optimizer::plugin::trajectory_velocity_optimizer_utils
+
+// NOLINTNEXTLINE
+#endif  // AUTOWARE__TRAJECTORY_OPTIMIZER__TRAJECTORY_OPTIMIZER_PLUGINS__PLUGIN_UTILS__TRAJECTORY_VELOCITY_OPTIMIZER_UTILS_HPP_

planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_plugins/trajectory_point_fixer.hpp

@@ -36,9 +36,10 @@ using TrajectoryPoints = std::vector<TrajectoryPoint>;
  */
 struct TrajectoryPointFixerParams
 {
-  bool resample_close_points{true};   // Whether to resample close proximity points
-  double min_dist_to_remove_m{0.01};  // Minimum distance to remove close proximity points [m]
-  double min_dist_to_merge_m{0.05};   // Minimum distance to merge close proximity points [m]
+  bool remove_close_points{true};       // Whether to remove close proximity points
+  bool resample_close_points{true};     // Whether to resample close proximity points
+  double min_dist_to_remove_m{0.01};    // Minimum distance to remove close proximity points [m]
+  double min_dist_to_resample_m{0.05};  // Minimum distance to merge close proximity points [m]
 };
 
 class TrajectoryPointFixer : public TrajectoryOptimizerPluginBase

planning/autoware_trajectory_optimizer/include/autoware/trajectory_optimizer/trajectory_optimizer_structs.hpp

@@ -43,9 +43,9 @@ struct TrajectoryOptimizerParams
   bool use_akima_spline_interpolation{false};
   bool use_eb_smoother{false};
   bool use_qp_smoother{false};
-  bool fix_invalid_points{false};
-  bool optimize_velocity{false};
-  bool extend_trajectory_backward{false};
+  bool use_trajectory_point_fixer{false};
+  bool use_velocity_optimizer{false};
+  bool use_trajectory_extender{false};
   bool use_kinematic_feasibility_enforcer{false};
   bool use_mpt_optimizer{false};
 };