fix linting issues

Author: jpbusch

trajectory_optimization/include/trajectory_optimization/performance_logger.hpp

@@ -129,10 +129,10 @@ class PerformanceLogger {
    * @param[in] dims acados NLP dimensions.
    * @param[in] obstacle_circles Number of obstacle circles represented in the nonlinear constraints.
    */
-  void collectConstraintDiagnostics(PerformanceMetrics& metrics,
-                                    ocp_nlp_solver* solver,
-                                    const ocp_nlp_dims* dims,
-                                    int obstacle_circles) const;
+  static void collectConstraintDiagnostics(PerformanceMetrics& metrics,
+                                           ocp_nlp_solver* solver,
+                                           const ocp_nlp_dims* dims,
+                                           int obstacle_circles);
 
   /**
    * @brief Returns the path of the CSV performance log.

trajectory_optimization/include/trajectory_optimization/trajectory_optimization_node.hpp

@@ -157,13 +157,6 @@ class TrajectoryOptimizationNode : public rclcpp::Node {
    */
   void printSolution(const PerformanceMetrics& metrics);
 
-  /**
-   * @brief Emits one machine-readable performance record when performance logging is enabled.
-   *
-   * @param[in] metrics Performance metrics to write to the log.
-   */
-  void logPerformance(const PerformanceMetrics& metrics);
-
   /**
    * @brief Transforms the planned trajectory into the configured output frame (trajectory_frame_id_) if required.
    *

trajectory_optimization/src/performance_logger.cpp

@@ -112,7 +112,9 @@ void PerformanceLogger::collectSolverStatistics(PerformanceMetrics& metrics,
 void PerformanceLogger::collectConstraintDiagnostics(PerformanceMetrics& metrics,
                                                      ocp_nlp_solver* solver,
                                                      const ocp_nlp_dims* dims,
-                                                     int obstacle_circles) const {
+                                                     int obstacle_circles) {
+  // The acados C API exposes stage-dependent dimensions as raw arrays.
+  // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   for (int stage = 0; stage <= dims->N; ++stage) {
     std::vector<double> residuals(2 * dims->ni[stage]);
     ocp_nlp_get_at_stage(solver, stage, "ineq_fun", residuals.data());
@@ -177,6 +179,7 @@ void PerformanceLogger::collectConstraintDiagnostics(PerformanceMetrics& metrics
     metrics.max_ineq_type = "slack";
     metrics.max_ineq_index = index - nb - ng - nh;
   }
+  // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
 }
 
 void PerformanceLogger::write(const PerformanceMetrics& metrics) {

trajectory_optimization/src/trajectory_optimization_node.cpp

@@ -436,7 +436,9 @@ void TrajectoryOptimizationNode::planningCycle() {
   auto logCompletedCycle = [&]() {
     metrics.cycle_ms = elapsedMilliseconds(cycle_start);
     metrics.postprocessing_ms = metrics.cycle_ms - metrics.preprocessing_ms - metrics.solve_wall_ms;
-    logPerformance(metrics);
+    if (performance_logger_) {
+      performance_logger_->write(metrics);
+    }
   };
 
   // set initial state
@@ -504,8 +506,8 @@ void TrajectoryOptimizationNode::planningCycle() {
                 "Rejecting solver output: status=%d finite=%d primal residuals=[eq=%e, ineq=%e] tolerances=[eq=%e, ineq=%e].",
                 metrics.status, finite_solution, metrics.res_eq, metrics.res_ineq, solver_opts->tol_eq, solver_opts->tol_ineq);
     if (finite_solution && performance_logger_) {
-      performance_logger_->collectConstraintDiagnostics(metrics, nlp_solver_, nlp_dims_,
-                                                        static_cast<int>(p_obstacle_circles_shape_[0]));
+      PerformanceLogger::collectConstraintDiagnostics(metrics, nlp_solver_, nlp_dims_,
+                                                      static_cast<int>(p_obstacle_circles_shape_[0]));
     }
     if (finite_solution && (metrics.status == ACADOS_MAXITER || metrics.status == ACADOS_TIMEOUT)) {
       // Preserve progress from a recoverable solve; the states are rolled out again from the next x_init.

trajectory_optimization/src/utils.cpp

@@ -481,10 +481,4 @@ void TrajectoryOptimizationNode::printSolution(const PerformanceMetrics& metrics
   }
 }
 
-void TrajectoryOptimizationNode::logPerformance(const PerformanceMetrics& metrics) {
-  if (performance_logger_) {
-    performance_logger_->write(metrics);
-  }
-}
-
 }  // namespace trajectory_optimization