trajectory_optimization v1.4.0
Loading...
Searching...
No Matches
Public Member Functions | Static Public Member Functions | Private Attributes | Static Private Attributes | List of all members
trajectory_optimization::PerformanceLogger Class Reference

#include <performance_logger.hpp>

Public Member Functions

 PerformanceLogger (const std::string &node_name)
 Creates a CSV performance log for the given node.
 
 ~PerformanceLogger ()
 Flushes and closes the performance log.
 
 PerformanceLogger (const PerformanceLogger &)=delete
 Copy construction is disabled because the logger owns a file stream.
 
PerformanceLogger & operator= (const PerformanceLogger &)=delete
 Copy assignment is disabled because the logger owns a file stream.
 
 PerformanceLogger (PerformanceLogger &&)=delete
 Move construction is disabled to keep the log stream bound to one logger.
 
PerformanceLogger & operator= (PerformanceLogger &&)=delete
 Move assignment is disabled to keep the log stream bound to one logger.
 
void write (const PerformanceMetrics &metrics)
 Appends one set of performance metrics to the CSV log.
 
const std::filesystem::path & path () const
 Returns the path of the CSV performance log.
 

Static Public Member Functions

static void collectSolverStatistics (PerformanceMetrics &metrics, ocp_nlp_solver *solver, ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *input, ocp_nlp_out *output, bool collect_details)
 Reads timing, iteration, cost, and residual statistics from acados.
 
static void collectConstraintDiagnostics (PerformanceMetrics &metrics, ocp_nlp_solver *solver, const ocp_nlp_dims *dims, int obstacle_circles)
 Collects the largest equality and inequality constraint violations from acados.
 

Private Attributes

std::filesystem::path path_
 
std::ofstream stream_
 
uint64_t records_since_flush_ = 0
 

Static Private Attributes

static constexpr uint64_t FLUSH_INTERVAL = 100
 

Detailed Description

Definition at line 61 of file performance_logger.hpp.

Constructor & Destructor Documentation

◆ PerformanceLogger() [1/3]

trajectory_optimization::PerformanceLogger::PerformanceLogger ( const std::string & node_name)
explicit

Creates a CSV performance log for the given node.

Parameters
[in]node_nameNode name used as part of the log file name.

Definition at line 38 of file performance_logger.cpp.

38 {
39 const char* configured_directory = std::getenv("TRAJECTORY_OPTIMIZATION_BENCHMARK_DIR");
40 const std::filesystem::path directory = configured_directory != nullptr && *configured_directory != '\0'
41 ? std::filesystem::path(configured_directory)
42 : std::filesystem::temp_directory_path() / "trajectory_optimization_benchmarks";
43 path_ = directory / (node_name + '_' + timestamp() + ".csv");
44
45 std::error_code error;
46 std::filesystem::create_directories(directory, error);
47 if (error) {
48 throw std::runtime_error("could not create directory '" + directory.string() + "': " + error.message());
49 }
50
51 stream_.open(path_, std::ios::out | std::ios::trunc);
52 if (!stream_) {
53 throw std::runtime_error("could not open '" + path_.string() + "'");
54 }
55 stream_ << "schema_version,source,run_id,cycle,record_stamp_ns,ego_stamp_ns,reference_stamp_ns,route_stamp_ns,status,"
56 "published,ref_points,objects,sqp_iter,qp_iter,"
57 "qp_status,cycle_ms,preprocessing_ms,solve_wall_ms,postprocessing_ms,acados_total_ms,acados_lin_ms,"
58 "acados_sim_ms,acados_qp_ms,"
59 "acados_qp_solver_ms,acados_qp_xcond_ms,acados_reg_ms,acados_glob_ms,acados_preparation_ms,"
60 "acados_feedback_ms,cost,kkt,nlp_res,res_stat,res_eq,res_ineq,res_comp,"
61 "max_ineq_violation,max_ineq_stage,max_ineq_type,max_ineq_index,max_ineq_side,"
62 "max_eq_violation,max_eq_stage,max_eq_state\n";
63 stream_.flush();
64}
trajectory_optimization::PerformanceLogger::path_
std::filesystem::path path_
Definition performance_logger.hpp:147
trajectory_optimization::PerformanceLogger::stream_
std::ofstream stream_
Definition performance_logger.hpp:148

◆ ~PerformanceLogger()

trajectory_optimization::PerformanceLogger::~PerformanceLogger ( )

Flushes and closes the performance log.

Definition at line 66 of file performance_logger.cpp.

66 {
67 stream_.flush();
68 stream_.close();
69}

◆ PerformanceLogger() [2/3]

trajectory_optimization::PerformanceLogger::PerformanceLogger ( const PerformanceLogger & )
delete

Copy construction is disabled because the logger owns a file stream.

◆ PerformanceLogger() [3/3]

trajectory_optimization::PerformanceLogger::PerformanceLogger ( PerformanceLogger && )
delete

Move construction is disabled to keep the log stream bound to one logger.

Member Function Documentation

◆ collectConstraintDiagnostics()

void trajectory_optimization::PerformanceLogger::collectConstraintDiagnostics ( PerformanceMetrics & metrics,
ocp_nlp_solver * solver,
const ocp_nlp_dims * dims,
int obstacle_circles )
static

Collects the largest equality and inequality constraint violations from acados.

Parameters
[in,out]metricsMetrics structure populated with constraint diagnostics.
[in]solveracados NLP solver instance.
[in]dimsacados NLP dimensions.
[in]obstacle_circlesNumber of obstacle circles represented in the nonlinear constraints.

Definition at line 112 of file performance_logger.cpp.

115 {
116 // The acados C API exposes stage-dependent dimensions as raw arrays.
117 // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
118 for (int stage = 0; stage <= dims->N; ++stage) {
119 std::vector<double> residuals(2 * dims->ni[stage]);
120 ocp_nlp_get_at_stage(solver, stage, "ineq_fun", residuals.data());
121 for (size_t index = 0; index < residuals.size(); ++index) {
122 if (residuals[index] > metrics.max_ineq_violation) {
123 metrics.max_ineq_violation = residuals[index];
124 metrics.max_ineq_stage = stage;
125 metrics.max_ineq_index = static_cast<int>(index);
126 }
127 }
128 }
129
130 for (int stage = 0; stage < dims->N; ++stage) {
131 std::vector<double> residuals(dims->nx[stage + 1]);
132 ocp_nlp_get_at_stage(solver, stage, "res_eq", residuals.data());
133 for (size_t state = 0; state < residuals.size(); ++state) {
134 if (std::abs(residuals[state]) > metrics.max_eq_violation) {
135 metrics.max_eq_violation = std::abs(residuals[state]);
136 metrics.max_eq_stage = stage;
137 metrics.max_eq_state = static_cast<int>(state);
138 }
139 }
140 }
141
142 if (metrics.max_ineq_stage < 0) return;
143
144 const int ni = dims->ni[metrics.max_ineq_stage];
145 const int nb = dims->nb[metrics.max_ineq_stage];
146 const int ng = dims->ng[metrics.max_ineq_stage];
147 const int nh = ni - nb - ng - dims->ns[metrics.max_ineq_stage];
148 const int index = metrics.max_ineq_index % ni;
149 metrics.max_ineq_side = metrics.max_ineq_index < ni ? "lower" : "upper";
150 metrics.max_ineq_index = index;
151 if (index < nb) {
152 std::vector<int> bound_indices(nb);
153 ocp_nlp_get_at_stage(solver, metrics.max_ineq_stage, "idxb", bound_indices.data());
154 const int variable_index = bound_indices[index];
155 if (variable_index < dims->nu[metrics.max_ineq_stage]) {
156 metrics.max_ineq_type = "control";
157 metrics.max_ineq_index = variable_index;
158 } else {
159 metrics.max_ineq_type = "state";
160 metrics.max_ineq_index = variable_index - dims->nu[metrics.max_ineq_stage];
161 }
162 } else if (index < nb + ng) {
163 metrics.max_ineq_type = "linear";
164 } else if (index < nb + ng + nh) {
165 const int h_index = index - nb - ng;
166 const int ego_circles = nh / (obstacle_circles + 2);
167 if (h_index < 2 * ego_circles) {
168 metrics.max_ineq_type = h_index % 2 == 0 ? "boundary_left" : "boundary_right";
169 metrics.max_ineq_index = h_index / 2;
170 } else if (h_index < (obstacle_circles + 2) * ego_circles) {
171 const int obstacle_index = h_index - 2 * ego_circles;
172 metrics.max_ineq_type = "obstacle_" + std::to_string(obstacle_index / ego_circles);
173 metrics.max_ineq_index = obstacle_index % ego_circles;
174 } else {
175 metrics.max_ineq_type = "vehicle";
176 metrics.max_ineq_index = h_index - (obstacle_circles + 2) * ego_circles;
177 }
178 } else {
179 metrics.max_ineq_type = "slack";
180 metrics.max_ineq_index = index - nb - ng - nh;
181 }
182 // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
183}

◆ collectSolverStatistics()

void trajectory_optimization::PerformanceLogger::collectSolverStatistics ( PerformanceMetrics & metrics,
ocp_nlp_solver * solver,
ocp_nlp_config * config,
ocp_nlp_dims * dims,
ocp_nlp_in * input,
ocp_nlp_out * output,
bool collect_details )
static

Reads timing, iteration, cost, and residual statistics from acados.

Parameters
[out]metricsMetrics structure populated with the solver statistics.
[in]solveracados NLP solver instance.
[in]configacados NLP configuration.
[in]dimsacados NLP dimensions.
[in]inputacados NLP input.
[in]outputacados NLP output.

Definition at line 71 of file performance_logger.cpp.

77 {
78 ocp_nlp_eval_cost(solver, input, output);
79 ocp_nlp_eval_residuals(solver, input, output);
80 ocp_nlp_get(solver, "cost_value", &metrics.cost_value);
81 ocp_nlp_get(solver, "res_eq", &metrics.res_eq);
82 ocp_nlp_get(solver, "res_ineq", &metrics.res_ineq);
83
84 if (!collect_details) return;
85
86 ocp_nlp_get(solver, "res_stat", &metrics.res_stat);
87 ocp_nlp_get(solver, "res_comp", &metrics.res_comp);
88 metrics.nlp_res = std::max({metrics.res_stat, metrics.res_eq, metrics.res_ineq, metrics.res_comp});
89
90 auto readTime = [&](const char* field, double& destination_ms) {
91 double time_seconds = 0.0;
92 ocp_nlp_get(solver, field, &time_seconds);
93 destination_ms = time_seconds * 1000.0;
94 };
95
96 readTime("time_tot", metrics.acados_total_ms);
97 readTime("time_lin", metrics.acados_lin_ms);
98 readTime("time_sim", metrics.acados_sim_ms);
99 readTime("time_qp", metrics.acados_qp_ms);
100 readTime("time_qp_solver_call", metrics.acados_qp_solver_ms);
101 readTime("time_qp_xcond", metrics.acados_qp_xcond_ms);
102 readTime("time_reg", metrics.acados_reg_ms);
103 readTime("time_glob", metrics.acados_glob_ms);
104 readTime("time_preparation", metrics.acados_preparation_ms);
105 readTime("time_feedback", metrics.acados_feedback_ms);
106 ocp_nlp_get(solver, "sqp_iter", &metrics.sqp_iter);
107 ocp_nlp_get(solver, "qp_iter", &metrics.qp_iter);
108 ocp_nlp_get(solver, "qp_status", &metrics.qp_status);
109 ocp_nlp_out_get(config, dims, output, 0, "kkt_norm_inf", &metrics.kkt_norm_inf);
110}

◆ operator=() [1/2]

PerformanceLogger & trajectory_optimization::PerformanceLogger::operator= ( const PerformanceLogger & )
delete

Copy assignment is disabled because the logger owns a file stream.

Returns
Reference to this logger. The operator is deleted.

◆ operator=() [2/2]

PerformanceLogger & trajectory_optimization::PerformanceLogger::operator= ( PerformanceLogger && )
delete

Move assignment is disabled to keep the log stream bound to one logger.

Returns
Reference to this logger. The operator is deleted.

◆ path()

const std::filesystem::path & trajectory_optimization::PerformanceLogger::path ( ) const
inline

Returns the path of the CSV performance log.

Returns
Path of the active performance log file.

Definition at line 142 of file performance_logger.hpp.

142{ return path_; }

◆ write()

void trajectory_optimization::PerformanceLogger::write ( const PerformanceMetrics & metrics)

Appends one set of performance metrics to the CSV log.

Parameters
[in]metricsMetrics collected for one optimization cycle.

Definition at line 185 of file performance_logger.cpp.

185 {
186 stream_ << std::setprecision(17) << 5 << ",runtime,," << metrics.cycle << ',' << nowNanoseconds() << ',' << metrics.ego_stamp_ns
187 << ',' << metrics.reference_stamp_ns << ',' << metrics.route_stamp_ns << ',' << metrics.status << ','
188 << (metrics.published ? 1 : 0) << ',' << metrics.reference_points << ',' << metrics.objects << ',' << metrics.sqp_iter
189 << ',' << metrics.qp_iter << ',' << metrics.qp_status << ',' << metrics.cycle_ms << ',' << metrics.preprocessing_ms
190 << ',' << metrics.solve_wall_ms << ',' << metrics.postprocessing_ms << ',' << metrics.acados_total_ms << ','
191 << metrics.acados_lin_ms << ',' << metrics.acados_sim_ms << ',' << metrics.acados_qp_ms << ','
192 << metrics.acados_qp_solver_ms << ',' << metrics.acados_qp_xcond_ms << ',' << metrics.acados_reg_ms << ','
193 << metrics.acados_glob_ms << ',' << metrics.acados_preparation_ms << ',' << metrics.acados_feedback_ms << ','
194 << metrics.cost_value << ',' << metrics.kkt_norm_inf << ',' << metrics.nlp_res << ',' << metrics.res_stat << ','
195 << metrics.res_eq << ',' << metrics.res_ineq << ',' << metrics.res_comp << ',' << metrics.max_ineq_violation << ','
196 << metrics.max_ineq_stage << ',' << metrics.max_ineq_type << ',' << metrics.max_ineq_index << ','
197 << metrics.max_ineq_side << ',' << metrics.max_eq_violation << ',' << metrics.max_eq_stage << ','
198 << metrics.max_eq_state << '\n';
199
200 if (++records_since_flush_ >= FLUSH_INTERVAL) {
201 stream_.flush();
202 records_since_flush_ = 0;
203 }
204}
trajectory_optimization::PerformanceLogger::FLUSH_INTERVAL
static constexpr uint64_t FLUSH_INTERVAL
Definition performance_logger.hpp:145
trajectory_optimization::PerformanceLogger::records_since_flush_
uint64_t records_since_flush_
Definition performance_logger.hpp:149

Member Data Documentation

◆ FLUSH_INTERVAL

uint64_t trajectory_optimization::PerformanceLogger::FLUSH_INTERVAL = 100
staticconstexprprivate

Definition at line 145 of file performance_logger.hpp.

◆ path_

std::filesystem::path trajectory_optimization::PerformanceLogger::path_
private

Definition at line 147 of file performance_logger.hpp.

◆ records_since_flush_

uint64_t trajectory_optimization::PerformanceLogger::records_since_flush_ = 0
private

Definition at line 149 of file performance_logger.hpp.

◆ stream_

std::ofstream trajectory_optimization::PerformanceLogger::stream_
private

Definition at line 148 of file performance_logger.hpp.

The documentation for this class was generated from the following files: