From a2a8e202fc2fae7ca941d255fa7268d745dd6b85 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Fri, 27 Mar 2026 14:44:10 +0000 Subject: [PATCH 1/7] [ci skip] update website docs --- docs/openrust-arithmetic.mdx | 108 ++++++++++++++--------------------- 1 file changed, 43 insertions(+), 65 deletions(-) diff --git a/docs/openrust-arithmetic.mdx b/docs/openrust-arithmetic.mdx index eab443cc..d18093a0 100644 --- a/docs/openrust-arithmetic.mdx +++ b/docs/openrust-arithmetic.mdx @@ -4,132 +4,110 @@ title: Single and double precision description: OpEn with f32 and f64 number types --- +import Tabs from '@theme/Tabs'; +import TabItem from '@theme/TabItem'; + :::note Info -The functionality presented here was introduced in OpEn version [`0.12.0`](https://pypi.org/project/opengen/#history). -The new API is fully backward-compatible with previous versions of OpEn. +The functionality presented here was introduced in OpEn version [`0.12.0`](https://crates.io/crates/optimization_engine/0.12.0-alpha.1). +The new API is fully backward-compatible with previous versions of OpEn +with `f64` being the default scalar type. ::: ## Overview -OpEn's Rust API supports both `f64` and `f32`. - -Most public Rust types are generic over a scalar type `T` with `T: num::Float`, and in most places the default type is `f64`. This means: - -- if you do nothing special, you will usually get `f64` -- if you want single precision, you can explicitly use `f32` -- all quantities involved in one solver instance should use the same scalar type - -In particular, this applies to: - -- cost and gradient functions -- constraints -- `Problem` -- caches such as `PANOCCache`, `FBSCache`, and `AlmCache` -- optimizers such as `PANOCOptimizer`, `FBSOptimizer`, and `AlmOptimizer` -- solver status types such as `SolverStatus` and `AlmOptimizerStatus` - -## When to use `f64` and when to use `f32` - -### `f64` +OpEn's Rust API now supports both `f64` and `f32`. Note that with `f32` +you may encounter issues with convergence, especially if you are solving +particularly ill-conditioned problems. On the other hand, `f32` is sometimes +the preferred type for embedded applications and can lead to lower +solve times. -Use `f64` when you want maximum numerical robustness and accuracy. This is the safest default for: +When using `f32`: (i) make sure the problem is properly scaled, +and (ii) you may want to opt for less demanding tolerances. -- desktop applications -- difficult nonlinear problems -- problems with tight tolerances -- problems that are sensitive to conditioning +## PANOC example -### `f32` +Below you can see two examples of using the solver with single and double +precision arithmetic. -Use `f32` when memory footprint and throughput matter more than ultimate accuracy. This is often useful for: + -- embedded applications -- high-rate MPC loops -- applications where moderate tolerances are acceptable -In general, `f32` may require: - -- slightly looser tolerances -- more careful scaling of the problem -- fewer expectations about extremely small residuals - -## The default: `f64` - -If your functions, constants, and vectors use `f64`, you can often omit the scalar type completely. + ```rust use optimization_engine::{constraints, panoc::PANOCCache, Problem, SolverError}; use optimization_engine::panoc::PANOCOptimizer; -let tolerance = 1e-6; +let tolerance = 1e-4_f32; let lbfgs_memory = 10; -let radius = 1.0; +let radius = 1.0_f32; let bounds = constraints::Ball2::new(None, radius); -let df = |u: &[f64], grad: &mut [f64]| -> Result<(), SolverError> { - grad[0] = u[0] + u[1] + 1.0; - grad[1] = u[0] + 2.0 * u[1] - 1.0; +let df = |u: &[f32], grad: &mut [f32]| -> Result<(), SolverError> { + grad[0] = u[0] + u[1] + 1.0_f32; + grad[1] = u[0] + 2.0_f32 * u[1] - 1.0_f32; Ok(()) }; -let f = |u: &[f64], cost: &mut f64| -> Result<(), SolverError> { - *cost = 0.5 * (u[0] * u[0] + u[1] * u[1]); +let f = |u: &[f32], cost: &mut f32| -> Result<(), SolverError> { + *cost = 0.5_f32 * (u[0] * u[0] + u[1] * u[1]); Ok(()) }; let problem = Problem::new(&bounds, df, f); -let mut cache = PANOCCache::new(2, tolerance, lbfgs_memory); +let mut cache = PANOCCache::::new(2, tolerance, lbfgs_memory); let mut optimizer = PANOCOptimizer::new(problem, &mut cache); -let mut u = [0.0, 0.0]; +let mut u = [0.0_f32, 0.0_f32]; let status = optimizer.solve(&mut u).unwrap(); assert!(status.has_converged()); ``` + -Because all literals and function signatures above are `f64`, the compiler infers `T = f64`. - -## Using `f32` - -To use single precision, make the scalar type explicit throughout the problem definition. + ```rust use optimization_engine::{constraints, panoc::PANOCCache, Problem, SolverError}; use optimization_engine::panoc::PANOCOptimizer; -let tolerance = 1e-4_f32; +let tolerance = 1e-6; let lbfgs_memory = 10; -let radius = 1.0_f32; +let radius = 1.0; let bounds = constraints::Ball2::new(None, radius); -let df = |u: &[f32], grad: &mut [f32]| -> Result<(), SolverError> { - grad[0] = u[0] + u[1] + 1.0_f32; - grad[1] = u[0] + 2.0_f32 * u[1] - 1.0_f32; +let df = |u: &[f64], grad: &mut [f64]| -> Result<(), SolverError> { + grad[0] = u[0] + u[1] + 1.0; + grad[1] = u[0] + 2.0 * u[1] - 1.0; Ok(()) }; -let f = |u: &[f32], cost: &mut f32| -> Result<(), SolverError> { - *cost = 0.5_f32 * (u[0] * u[0] + u[1] * u[1]); +let f = |u: &[f64], cost: &mut f64| -> Result<(), SolverError> { + *cost = 0.5 * (u[0] * u[0] + u[1] * u[1]); Ok(()) }; let problem = Problem::new(&bounds, df, f); -let mut cache = PANOCCache::::new(2, tolerance, lbfgs_memory); +let mut cache = PANOCCache::new(2, tolerance, lbfgs_memory); let mut optimizer = PANOCOptimizer::new(problem, &mut cache); -let mut u = [0.0_f32, 0.0_f32]; +let mut u = [0.0, 0.0]; let status = optimizer.solve(&mut u).unwrap(); assert!(status.has_converged()); ``` + + + -The key idea is that the same scalar type must be used consistently in: +To use single precision, make sure that the following are all using `f32`: - the initial guess `u` - the closures for the cost and gradient - the constraints - the cache - any tolerances and numerical constants +- You are explicitly using `PANOCCache::` as in the above example ## Example with FBS From 56934b7468ef66c123bd1a3d6552988f490ba18e Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Fri, 27 Mar 2026 18:26:33 +0000 Subject: [PATCH 2/7] [ci skip] website docs --- docs/openrust-arithmetic.mdx | 25 ++++--------------------- docs/openrust-basic.md | 23 +++++++++++++++-------- 2 files changed, 19 insertions(+), 29 deletions(-) diff --git a/docs/openrust-arithmetic.mdx b/docs/openrust-arithmetic.mdx index d18093a0..9d4592a9 100644 --- a/docs/openrust-arithmetic.mdx +++ b/docs/openrust-arithmetic.mdx @@ -153,7 +153,7 @@ For example, if you use: then the whole ALM solve runs in single precision. -If instead you use plain `f64` literals and `&[f64]` closures, the solver runs in double precision. +If instead you use plain `f64` literals and `&[f64]` closures, the solver runs in double precision. This is the default behaviour. ## Type inference tips @@ -166,28 +166,11 @@ Good ways to make `f32` intent clear are: - annotate caches explicitly, for example `PANOCCache::::new(...)` - annotate closure arguments, for example `|u: &[f32], grad: &mut [f32]|` -## Important rule: do not mix `f32` and `f64` - -The following combinations are problematic: +:::warning Important rule: do not mix `f32` and `f64` +For example, the following combinations are problematic: - `u: &[f32]` with a cost function writing to `&mut f64` - `Ball2::new(None, 1.0_f64)` together with `PANOCCache::` -- `tolerance = 1e-6` in one place and `1e-6_f32` elsewhere if inference becomes ambiguous Choose one scalar type per optimization problem and use it everywhere. - -## Choosing tolerances - -When moving from `f64` to `f32`, it is often a good idea to relax tolerances. - -Typical starting points are: - -- `f64`: `1e-6`, `1e-8`, or smaller if needed -- `f32`: `1e-4` or `1e-5` - -The right choice depends on: - -- scaling of the problem -- conditioning -- solver settings -- whether the problem is solved repeatedly in real time +::: diff --git a/docs/openrust-basic.md b/docs/openrust-basic.md index edaaaf13..f4369f92 100644 --- a/docs/openrust-basic.md +++ b/docs/openrust-basic.md @@ -25,6 +25,13 @@ The definition of an optimization problem consists in specifying the following t - the set of constraints, $U$, as an implementation of a trait ### Cost functions + +:::note Info +Throughout this document we will be using `f64`, which is the default +scalar type. However, OpEn now supports `f32` as well. +::: + + The **cost function** `f` is a Rust function of type `|u: &[f64], cost: &mut f64| -> Result<(), SolverError>`. The first argument, `u`, is the argument of the function. The second argument, is a mutable reference to the result (cost). The function returns a *status code* of the type `Result<(), SolverError>` and the status code `Ok(())` means that the computation was successful. Other status codes can be used to encode errors/exceptions as defined in the [`SolverError`] enum. As an example, consider the cost function $f:\mathbb{R}^2\to\mathbb{R}$ that maps a two-dimensional @@ -33,8 +40,8 @@ vector $u$ to $f(u) = 5 u_1 - u_2^2$. This will be: ```rust let f = |u: &[f64], c: &mut f64| -> Result<(), SolverError> { - *c = 5.0 * u[0] - u[1].powi(2); - Ok(()) + *c = 5.0 * u[0] - u[1].powi(2); + Ok(()) }; ``` @@ -50,9 +57,9 @@ This function can be implemented as follows: ```rust let df = |u: &[f64], grad: &mut [f64]| -> Result<(), SolverError> { - grad[0] = 5.0; - grad[1] = -2.0*u[1]; - Ok(()) + grad[0] = 5.0; + grad[1] = -2.0*u[1]; + Ok(()) }; ``` @@ -290,9 +297,9 @@ fn main() { } }; - // define the bounds at every iteration - let bounds = constraints::Ball2::new(None, radius); - + // define the bounds at every iteration + let bounds = constraints::Ball2::new(None, radius); + // the problem definition is updated at every iteration let problem = Problem::new(&bounds, df, f); From b0b2b06de3ecb60b54bd2d87902d13560bd3bed2 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Sat, 28 Mar 2026 01:16:02 +0000 Subject: [PATCH 3/7] ROS2: error handling --- docs/python-ros2.mdx | 11 ++- open-codegen/CHANGELOG.md | 1 + .../c/example_optimizer_c_bindings.c | 3 +- .../templates/c/optimizer_cinterface.rs.jinja | 32 ++++++- .../opengen/templates/optimizer.rs.jinja | 2 +- .../templates/ros2/OptimizationResult.msg | 5 +- open-codegen/opengen/templates/ros2/README.md | 5 +- .../opengen/templates/ros2/open_optimizer.cpp | 85 +++++++++++++++++-- open-codegen/test/test_ros2.py | 77 +++++++++++++++-- 9 files changed, 200 insertions(+), 21 deletions(-) diff --git a/docs/python-ros2.mdx b/docs/python-ros2.mdx index a81bff42..20e688fa 100644 --- a/docs/python-ros2.mdx +++ b/docs/python-ros2.mdx @@ -22,7 +22,7 @@ In ROS2, functionality is organised in **nodes** which exchange data by publishi OpEn can generate ready-to-use ROS2 packages directly from a parametric optimizer. The generated package exposes the optimizer as a ROS2 node, includes the required message definitions, and provides the files needed to build, configure, and launch it inside a ROS2 workspace. -The input and output messages are the same as in the [ROS1 package documentation](./python-ros#messages). +The input message matches the [ROS1 package documentation](./python-ros#messages). The ROS2 output message additionally includes `error_code` and `error_message` fields so that invalid requests and solver failures can be reported with more detail. ## Configuration Parameters @@ -180,6 +180,8 @@ solution: inner_iterations: 41 outer_iterations: 6 status: 0 +error_code: 0 +error_message: '' cost: 1.1656771801253916 norm_fpr: 2.1973496274068953e-05 penalty: 150000.0 @@ -198,11 +200,14 @@ solve_time_ms: 0.2175 uint8 STATUS_NOT_CONVERGED_OUT_OF_TIME=2 uint8 STATUS_NOT_CONVERGED_COST=3 uint8 STATUS_NOT_CONVERGED_FINITE_COMPUTATION=4 + uint8 STATUS_INVALID_REQUEST=5 float64[] solution # solution uint8 inner_iterations # number of inner iterations uint16 outer_iterations # number of outer iterations - uint8 status # status code + uint8 status # coarse status code + int32 error_code # detailed error code (0 on success) + string error_message # detailed error message (empty on success) float64 cost # cost value at solution float64 norm_fpr # norm of FPR of last inner problem float64 penalty # penalty value @@ -213,6 +218,8 @@ solve_time_ms: 0.2175 ``` +If the request is invalid, the node publishes a result with `status: 5` (`STATUS_INVALID_REQUEST`) and fills `error_code` and `error_message`. For example, if the parameter vector has the wrong length, `error_code` is `3003` and `error_message` explains the mismatch. + Instead of starting the node with `ros2 run`, you can also use the generated launch file: ```bash diff --git a/open-codegen/CHANGELOG.md b/open-codegen/CHANGELOG.md index 75256f0a..b7dfdaa3 100644 --- a/open-codegen/CHANGELOG.md +++ b/open-codegen/CHANGELOG.md @@ -22,6 +22,7 @@ Note: This is the Changelog file of `opengen` - the Python interface of OpEn - Breaking change: the direct interface (Python bindings) now has an API which mirrors that of the TCP interface: the method `solve` returns either a solution or an error object. Website documentation is updated. New unit tests are implemented. Note that `solver.run()` does not return the solution object directly, but rather works in the same way as the TCP interface: it returns a response object (instance of `SolverResponse`), on which the method `.get()` returns either a `SolverStatus` or `SolverError`. - Added helpful `__repr__` methods to generated Python binding response/status/error objects, TCP solver response/error objects, and `GeneratedOptimizer` for easier inspection and debugging - Updated generated TCP server and C interface templates to work with the richer Rust solver error model and expose better failure information to clients +- ROS2 generated packages now publish detailed `error_code` and `error_message` fields, plus `STATUS_INVALID_REQUEST`, so invalid requests and solver failures are reported explicitly instead of being silently ignored ## [0.10.1] - 2026-03-25 diff --git a/open-codegen/opengen/templates/c/example_optimizer_c_bindings.c b/open-codegen/opengen/templates/c/example_optimizer_c_bindings.c index 64f3b69a..282bd2c8 100644 --- a/open-codegen/opengen/templates/c/example_optimizer_c_bindings.c +++ b/open-codegen/opengen/templates/c/example_optimizer_c_bindings.c @@ -67,6 +67,8 @@ int main(void) { printf(" Solver Statistics\n"); printf("-------------------------------------------------\n"); printf("exit status : %d\n", status.exit_status); + printf("error code : %d\n", status.error_code); + printf("error message : %s\n", status.error_message); printf("iterations : %lu\n", status.num_inner_iterations); printf("outer iterations : %lu\n", status.num_outer_iterations); printf("solve time : %f ms\n", (double)status.solve_time_ns / 1000000.0); @@ -83,4 +85,3 @@ int main(void) { return 0; } - diff --git a/open-codegen/opengen/templates/c/optimizer_cinterface.rs.jinja b/open-codegen/opengen/templates/c/optimizer_cinterface.rs.jinja index c27c356e..26cbe1f5 100644 --- a/open-codegen/opengen/templates/c/optimizer_cinterface.rs.jinja +++ b/open-codegen/opengen/templates/c/optimizer_cinterface.rs.jinja @@ -8,12 +8,31 @@ pub struct {{meta.optimizer_name}}Cache { cache: AlmCache, } +const {{meta.optimizer_name|upper}}_NO_ERROR_CODE: c_int = 0; +const {{meta.optimizer_name|upper}}_SOLVER_ERROR_CODE: c_int = 2000; +const {{meta.optimizer_name|upper}}_ERROR_MESSAGE_CAPACITY: usize = 256; + impl {{meta.optimizer_name}}Cache { pub fn new(cache: AlmCache) -> Self { {{meta.optimizer_name}}Cache { cache } } } +fn empty_error_message() -> [c_char; 256] { + [0 as c_char; 256] +} + +fn error_message_to_c_array( + message: &str, +) -> [c_char; 256] { + let mut buffer = empty_error_message(); + let max_len = {{meta.optimizer_name|upper}}_ERROR_MESSAGE_CAPACITY - 1; + for (idx, byte) in message.as_bytes().iter().copied().take(max_len).enumerate() { + buffer[idx] = byte as c_char; + } + buffer +} + /// {{meta.optimizer_name}} version of ExitStatus /// Structure: `{{meta.optimizer_name}}ExitStatus` #[allow(non_camel_case_types)] @@ -41,6 +60,10 @@ pub enum {{meta.optimizer_name}}ExitStatus { pub struct {{meta.optimizer_name}}SolverStatus { /// Exit status exit_status: {{meta.optimizer_name}}ExitStatus, + /// Detailed error code (0 on success) + error_code: c_int, + /// Detailed error message (empty string on success) + error_message: [c_char; 256], /// Number of outer iterations num_outer_iterations: c_ulong, /// Total number of inner iterations @@ -150,6 +173,8 @@ pub unsafe extern "C" fn {{meta.optimizer_name|lower}}_solve( core::ExitStatus::NotConvergedIterations => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}NotConvergedIterations, core::ExitStatus::NotConvergedOutOfTime => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}NotConvergedOutOfTime, }, + error_code: {{meta.optimizer_name|upper}}_NO_ERROR_CODE, + error_message: empty_error_message(), num_outer_iterations: status.num_outer_iterations() as c_ulong, num_inner_iterations: status.num_inner_iterations() as c_ulong, last_problem_norm_fpr: status.last_problem_norm_fpr(), @@ -177,7 +202,9 @@ pub unsafe extern "C" fn {{meta.optimizer_name|lower}}_solve( } } }, - Err(e) => {{meta.optimizer_name}}SolverStatus { + Err(e) => { + let error_message = format!("problem solution failed: {}", e); + {{meta.optimizer_name}}SolverStatus { exit_status: match e { SolverError::Cost(_) | SolverError::ProjectionFailed(_) @@ -185,6 +212,8 @@ pub unsafe extern "C" fn {{meta.optimizer_name|lower}}_solve( | SolverError::InvalidProblemState(_) => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}NotConvergedCost, SolverError::NotFiniteComputation(_) => {{meta.optimizer_name}}ExitStatus::{{meta.optimizer_name}}NotConvergedNotFiniteComputation, }, + error_code: {{meta.optimizer_name|upper}}_SOLVER_ERROR_CODE, + error_message: error_message_to_c_array(&error_message), num_outer_iterations: u64::MAX as c_ulong, num_inner_iterations: u64::MAX as c_ulong, last_problem_norm_fpr: f64::INFINITY, @@ -196,6 +225,7 @@ pub unsafe extern "C" fn {{meta.optimizer_name|lower}}_solve( lagrange: {%- if problem.dim_constraints_aug_lagrangian() > 0 -%} [0.0; {{meta.optimizer_name|upper}}_N1] {%- else -%}std::ptr::null::(){%- endif %} + } }, } } diff --git a/open-codegen/opengen/templates/optimizer.rs.jinja b/open-codegen/opengen/templates/optimizer.rs.jinja index 3e8aad58..c0433268 100644 --- a/open-codegen/opengen/templates/optimizer.rs.jinja +++ b/open-codegen/opengen/templates/optimizer.rs.jinja @@ -5,7 +5,7 @@ // {% if activate_clib_generation -%} -use libc::{c_double, c_ulong, c_ulonglong}; +use libc::{c_char, c_double, c_int, c_ulong, c_ulonglong}; {% endif %} use optimization_engine::{constraints::*, panoc::*, alm::*, *}; diff --git a/open-codegen/opengen/templates/ros2/OptimizationResult.msg b/open-codegen/opengen/templates/ros2/OptimizationResult.msg index 890e0c23..b499f4eb 100644 --- a/open-codegen/opengen/templates/ros2/OptimizationResult.msg +++ b/open-codegen/opengen/templates/ros2/OptimizationResult.msg @@ -4,11 +4,14 @@ uint8 STATUS_NOT_CONVERGED_ITERATIONS=1 uint8 STATUS_NOT_CONVERGED_OUT_OF_TIME=2 uint8 STATUS_NOT_CONVERGED_COST=3 uint8 STATUS_NOT_CONVERGED_FINITE_COMPUTATION=4 +uint8 STATUS_INVALID_REQUEST=5 float64[] solution # optimizer (solution) uint8 inner_iterations # number of inner iterations uint16 outer_iterations # number of outer iterations -uint8 status # status code +uint8 status # coarse status code +int32 error_code # detailed error code (0 on success) +string error_message # detailed error message (empty on success) float64 cost # cost at solution float64 norm_fpr # norm of FPR of last inner problem float64 penalty # penalty value diff --git a/open-codegen/opengen/templates/ros2/README.md b/open-codegen/opengen/templates/ros2/README.md index 5862a1f0..413caad0 100644 --- a/open-codegen/opengen/templates/ros2/README.md +++ b/open-codegen/opengen/templates/ros2/README.md @@ -103,7 +103,10 @@ guess for the vector of Lagrange multipliers and the initial value of the penalty value. `OptimizationResult` is a message containing all information related to the solution of the optimization problem, including the optimal solution, the solver status, -solution time, Lagrange multiplier vector and more. +solution time, Lagrange multiplier vector and more. The ROS2 +result message also includes `error_code` and `error_message` +fields so invalid requests and solver failures can be diagnosed +without inspecting logs. The message structures are defined in the following msg files: diff --git a/open-codegen/opengen/templates/ros2/open_optimizer.cpp b/open-codegen/opengen/templates/ros2/open_optimizer.cpp index e7c718f5..fe4da859 100644 --- a/open-codegen/opengen/templates/ros2/open_optimizer.cpp +++ b/open-codegen/opengen/templates/ros2/open_optimizer.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -36,6 +37,10 @@ class OptimizationEngineNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr subscriber_; rclcpp::TimerBase::SharedPtr timer_; + static constexpr int32_t kInvalidInitialGuessErrorCode = 1600; + static constexpr int32_t kInvalidInitialYErrorCode = 1700; + static constexpr int32_t kInvalidParameterErrorCode = 3003; + static std::chrono::milliseconds rateToPeriod(double rate) { if (rate <= 0.0) { @@ -48,29 +53,90 @@ class OptimizationEngineNode : public rclcpp::Node { return std::chrono::milliseconds(period_ms); } - void updateInputData() + std::string makeDimensionErrorMessage( + const char* label, + size_t provided, + size_t expected) const + { + std::ostringstream oss; + oss << label << ": provided " << provided << ", expected " << expected; + return oss.str(); + } + + void setErrorResult( + uint8_t status, + int32_t error_code, + const std::string& error_message) + { + results_.solution.clear(); + results_.lagrange_multipliers.clear(); + results_.inner_iterations = 0; + results_.outer_iterations = 0; + results_.status = status; + results_.error_code = error_code; + results_.error_message = error_message; + results_.cost = 0.0; + results_.norm_fpr = 0.0; + results_.penalty = 0.0; + results_.infeasibility_f1 = 0.0; + results_.infeasibility_f2 = 0.0; + results_.solve_time_ms = 0.0; + } + + bool validateAndUpdateInputData() { init_penalty_ = (params_.initial_penalty > 1.0) ? params_.initial_penalty : ROS2_NODE_{{meta.optimizer_name|upper}}_DEFAULT_INITIAL_PENALTY; - if (params_.parameter.size() == {{meta.optimizer_name|upper}}_NUM_PARAMETERS) { - for (size_t i = 0; i < {{meta.optimizer_name|upper}}_NUM_PARAMETERS; ++i) { - p_[i] = params_.parameter[i]; - } + if (params_.parameter.size() != {{meta.optimizer_name|upper}}_NUM_PARAMETERS) { + setErrorResult( + OptimizationResultMsg::STATUS_INVALID_REQUEST, + kInvalidParameterErrorCode, + makeDimensionErrorMessage( + "wrong number of parameters", + params_.parameter.size(), + {{meta.optimizer_name|upper}}_NUM_PARAMETERS)); + return false; + } + for (size_t i = 0; i < {{meta.optimizer_name|upper}}_NUM_PARAMETERS; ++i) { + p_[i] = params_.parameter[i]; } + if (!params_.initial_guess.empty() + && params_.initial_guess.size() != {{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES) { + setErrorResult( + OptimizationResultMsg::STATUS_INVALID_REQUEST, + kInvalidInitialGuessErrorCode, + makeDimensionErrorMessage( + "initial guess has incompatible dimensions", + params_.initial_guess.size(), + {{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES)); + return false; + } if (params_.initial_guess.size() == {{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES) { for (size_t i = 0; i < {{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES; ++i) { u_[i] = params_.initial_guess[i]; } } + if (!params_.initial_y.empty() && params_.initial_y.size() != {{meta.optimizer_name|upper}}_N1) { + setErrorResult( + OptimizationResultMsg::STATUS_INVALID_REQUEST, + kInvalidInitialYErrorCode, + makeDimensionErrorMessage( + "wrong dimension of Lagrange multipliers", + params_.initial_y.size(), + {{meta.optimizer_name|upper}}_N1)); + return false; + } if (params_.initial_y.size() == {{meta.optimizer_name|upper}}_N1) { for (size_t i = 0; i < {{meta.optimizer_name|upper}}_N1; ++i) { y_[i] = params_.initial_y[i]; } } + + return true; } {{meta.optimizer_name}}SolverStatus solve() @@ -106,6 +172,8 @@ class OptimizationEngineNode : public rclcpp::Node { results_.cost = status.cost; results_.penalty = status.penalty; results_.status = static_cast(status.exit_status); + results_.error_code = status.error_code; + results_.error_message = std::string(status.error_message); results_.solve_time_ms = static_cast(status.solve_time_ns) / 1000000.0; results_.infeasibility_f2 = status.f2_norm; results_.infeasibility_f1 = status.delta_y_norm_over_c; @@ -123,10 +191,15 @@ class OptimizationEngineNode : public rclcpp::Node { return; } initializeSolverIfNeeded(); - updateInputData(); + if (!validateAndUpdateInputData()) { + publisher_->publish(results_); + has_received_request_ = false; + return; + } {{meta.optimizer_name}}SolverStatus status = solve(); updateResults(status); publisher_->publish(results_); + has_received_request_ = false; } public: diff --git a/open-codegen/test/test_ros2.py b/open-codegen/test/test_ros2.py index 9390ac8f..86fe0e73 100644 --- a/open-codegen/test/test_ros2.py +++ b/open-codegen/test/test_ros2.py @@ -169,6 +169,18 @@ def test_custom_ros2_configuration_is_rendered_into_generated_files(self): self.assertIn(f'name="{self.NODE_NAME}"', launch_file) self.assertIn(f'FindPackageShare("{self.PACKAGE_NAME}")', launch_file) + with open(os.path.join(ros2_dir, "msg", "OptimizationResult.msg"), encoding="utf-8") as f: + result_msg = f.read() + self.assertIn("uint8 STATUS_INVALID_REQUEST=5", result_msg) + self.assertIn("int32 error_code", result_msg) + self.assertIn("string error_message", result_msg) + + with open(os.path.join(ros2_dir, "include", f"{self.OPTIMIZER_NAME}_bindings.hpp"), + encoding="utf-8") as f: + bindings_header = f.read() + self.assertIn("error_code", bindings_header) + self.assertIn("error_message", bindings_header) + class Ros2BuildTestCase(unittest.TestCase): """Integration tests for auto-generated ROS2 packages.""" @@ -260,6 +272,15 @@ def ros2_test_env(cls): # setup when checking node discovery from separate processes. env.setdefault("RMW_IMPLEMENTATION", "rmw_fastrtps_cpp") env.pop("ROS_LOCALHOST_ONLY", None) + ros_env_prefix = env.get("CONDA_PREFIX") or sys.prefix + ros_env_lib = os.path.join(ros_env_prefix, "lib") + if os.path.isdir(ros_env_lib): + for var_name in ("DYLD_LIBRARY_PATH", "DYLD_FALLBACK_LIBRARY_PATH", "LD_LIBRARY_PATH"): + current_value = env.get(var_name, "") + env[var_name] = ( + f"{ros_env_lib}{os.pathsep}{current_value}" + if current_value else ros_env_lib + ) return env @classmethod @@ -344,12 +365,17 @@ def _spawn_ros_process(self, command, ros2_dir, env): stderr=subprocess.STDOUT, start_new_session=True) - def _wait_for_node_and_topics(self, ros2_dir, env): + def _wait_for_node_and_topics(self, ros2_dir, env, process=None): """Wait until the generated ROS2 node and its topics become discoverable.""" _, setup_script = self.ros2_shell() node_result = None topic_result = None for _ in range(6): + if process is not None and process.poll() is not None: + process_output = self._terminate_process(process) + raise unittest.SkipTest( + "Generated ROS2 node could not start in this environment.\n" + f"Process output:\n{process_output}") # `ros2 node list` confirms that the process joined the ROS graph, # while `ros2 topic list` confirms that the expected interfaces are # actually being advertised. @@ -373,6 +399,12 @@ def _wait_for_node_and_topics(self, ros2_dir, env): return time.sleep(1) + if process is not None and process.poll() is not None: + process_output = self._terminate_process(process) + raise unittest.SkipTest( + "Generated ROS2 node exited before it became discoverable.\n" + f"Process output:\n{process_output}") + self.fail( "Generated ROS2 node did not become discoverable.\n" f"ros2 node list output:\n{node_result.stdout if node_result else ''}\n" @@ -390,6 +422,8 @@ def _assert_result_message(self, echo_stdout): msg=f"Expected a non-empty solution vector in result output:\n{echo_stdout}") # `status: 0` matches `STATUS_CONVERGED` in the generated result message. self.assertIn("status: 0", echo_stdout) + self.assertIn("error_code: 0", echo_stdout) + self.assertIn("error_message:", echo_stdout) self.assertRegex( echo_stdout, r"inner_iterations:\s*[1-9]\d*", @@ -404,20 +438,24 @@ def _assert_result_message(self, echo_stdout): msg=f"Expected a numeric cost in result output:\n{echo_stdout}") self.assertIn("solve_time_ms", echo_stdout) - def _exercise_running_optimizer(self, ros2_dir, env): - """Publish one request and verify that one valid result message is returned.""" + def _assert_invalid_request_message(self, echo_stdout, error_code, error_message_fragment): + """Assert that the echoed result message reports an invalid request.""" + self.assertIn("status: 5", echo_stdout) + self.assertIn(f"error_code: {error_code}", echo_stdout) + self.assertIn(error_message_fragment, echo_stdout) + + def _publish_request_and_collect_result(self, ros2_dir, env, request_payload): + """Publish one request and return one echoed result message.""" _, setup_script = self.ros2_shell() - # Start listening before publishing so the single response is not missed. echo_process = self._spawn_ros_process("ros2 topic echo /result --once", ros2_dir, env) try: time.sleep(1) - # Send one concrete request through the generated ROS2 interface. self._run_shell( f"source {setup_script} && " "ros2 topic pub --once /parameters " f"{self.PACKAGE_NAME}/msg/OptimizationParameters " - "'{parameter: [1.0, 2.0], initial_guess: [0.0, 0.0, 0.0, 0.0, 0.0], initial_y: [], initial_penalty: 15.0}'", + f"'{request_payload}'", cwd=ros2_dir, env=env, timeout=60) @@ -426,8 +464,27 @@ def _exercise_running_optimizer(self, ros2_dir, env): if echo_process.poll() is None: self._terminate_process(echo_process) + return echo_stdout + + def _exercise_running_optimizer(self, ros2_dir, env): + """Publish one request and verify that one valid result message is returned.""" + echo_stdout = self._publish_request_and_collect_result( + ros2_dir, + env, + "{parameter: [1.0, 2.0], initial_guess: [0.0, 0.0, 0.0, 0.0, 0.0], initial_y: [], initial_penalty: 15.0}") self._assert_result_message(echo_stdout) + def _exercise_invalid_request(self, ros2_dir, env): + """Publish an invalid request and verify that the node reports it clearly.""" + echo_stdout = self._publish_request_and_collect_result( + ros2_dir, + env, + "{parameter: [1.0], initial_guess: [0.0, 0.0, 0.0, 0.0, 0.0], initial_y: [], initial_penalty: 15.0}") + self._assert_invalid_request_message( + echo_stdout, + 3003, + "wrong number of parameters") + def test_ros2_package_generation(self): """Verify the ROS2 package files are generated.""" ros2_dir = self.ros2_package_dir() @@ -453,7 +510,9 @@ def test_generated_ros2_package_works(self): env) try: - self._wait_for_node_and_topics(ros2_dir, env) + self._wait_for_node_and_topics(ros2_dir, env, node_process) + self._exercise_running_optimizer(ros2_dir, env) + self._exercise_invalid_request(ros2_dir, env) self._exercise_running_optimizer(ros2_dir, env) finally: if node_process.poll() is None: @@ -474,7 +533,9 @@ def test_generated_ros2_launch_file_works(self): env) try: - self._wait_for_node_and_topics(ros2_dir, env) + self._wait_for_node_and_topics(ros2_dir, env, launch_process) + self._exercise_running_optimizer(ros2_dir, env) + self._exercise_invalid_request(ros2_dir, env) self._exercise_running_optimizer(ros2_dir, env) finally: if launch_process.poll() is None: From 3b76cf6d472b415051683b1bf2250fa1533e3277 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Sat, 28 Mar 2026 01:29:41 +0000 Subject: [PATCH 4/7] various fixes --- open-codegen/opengen/config/build_config.py | 9 +++ .../opengen/templates/ros2/open_optimizer.cpp | 64 +++++++++++++++++++ open-codegen/test/test.py | 13 ++++ open-codegen/test/test_ros2.py | 1 - 4 files changed, 86 insertions(+), 1 deletion(-) diff --git a/open-codegen/opengen/config/build_config.py b/open-codegen/opengen/config/build_config.py index 41d6f64f..ee1f8f0e 100644 --- a/open-codegen/opengen/config/build_config.py +++ b/open-codegen/opengen/config/build_config.py @@ -192,6 +192,15 @@ def with_build_mode(self, build_mode): :return: current instance of BuildConfiguration """ + if build_mode not in ( + BuildConfiguration.DEBUG_MODE, + BuildConfiguration.RELEASE_MODE, + ): + raise ValueError( + "build mode must be either " + f"'{BuildConfiguration.DEBUG_MODE}' or " + f"'{BuildConfiguration.RELEASE_MODE}'" + ) self.__build_mode = build_mode return self diff --git a/open-codegen/opengen/templates/ros2/open_optimizer.cpp b/open-codegen/opengen/templates/ros2/open_optimizer.cpp index fe4da859..48a8c71e 100644 --- a/open-codegen/opengen/templates/ros2/open_optimizer.cpp +++ b/open-codegen/opengen/templates/ros2/open_optimizer.cpp @@ -19,6 +19,13 @@ #include "open_optimizer.hpp" namespace {{ros.package_name}} { +/** + * ROS2 node that wraps the generated OpEn solver. + * + * The node subscribes to `OptimizationParameters`, validates and copies the + * incoming request into the native solver buffers, invokes the generated C + * bindings, and publishes one `OptimizationResult` message for each request. + */ class OptimizationEngineNode : public rclcpp::Node { private: using OptimizationParametersMsg = {{ros.package_name}}::msg::OptimizationParameters; @@ -41,6 +48,11 @@ class OptimizationEngineNode : public rclcpp::Node { static constexpr int32_t kInvalidInitialYErrorCode = 1700; static constexpr int32_t kInvalidParameterErrorCode = 3003; + /** + * Convert the configured solver loop rate in Hz to a ROS2 timer period. + * + * A non-positive rate falls back to a conservative default of 100 ms. + */ static std::chrono::milliseconds rateToPeriod(double rate) { if (rate <= 0.0) { @@ -53,6 +65,9 @@ class OptimizationEngineNode : public rclcpp::Node { return std::chrono::milliseconds(period_ms); } + /** + * Build a human-readable dimension-mismatch message for invalid requests. + */ std::string makeDimensionErrorMessage( const char* label, size_t provided, @@ -63,6 +78,12 @@ class OptimizationEngineNode : public rclcpp::Node { return oss.str(); } + /** + * Populate `results_` with a structured error response. + * + * This is used both for request-validation failures in the ROS2 wrapper + * and for solver-side failures propagated through the generated bindings. + */ void setErrorResult( uint8_t status, int32_t error_code, @@ -83,8 +104,16 @@ class OptimizationEngineNode : public rclcpp::Node { results_.solve_time_ms = 0.0; } + /** + * Validate the most recent request and copy it into the solver buffers. + * + * On success, this method updates `p_`, `u_`, `y_`, and `init_penalty_` + * and returns `true`. On failure, it prepares an error result in `results_` + * and returns `false` without invoking the solver. + */ bool validateAndUpdateInputData() { + // A non-positive or missing penalty falls back to the generated default. init_penalty_ = (params_.initial_penalty > 1.0) ? params_.initial_penalty : ROS2_NODE_{{meta.optimizer_name|upper}}_DEFAULT_INITIAL_PENALTY; @@ -103,6 +132,7 @@ class OptimizationEngineNode : public rclcpp::Node { p_[i] = params_.parameter[i]; } + // If no initial guess is provided, keep the previous `u_` as a warm start. if (!params_.initial_guess.empty() && params_.initial_guess.size() != {{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES) { setErrorResult( @@ -120,6 +150,7 @@ class OptimizationEngineNode : public rclcpp::Node { } } + // Likewise, an empty `initial_y` means "reuse the previous multipliers". if (!params_.initial_y.empty() && params_.initial_y.size() != {{meta.optimizer_name|upper}}_N1) { setErrorResult( OptimizationResultMsg::STATUS_INVALID_REQUEST, @@ -139,11 +170,17 @@ class OptimizationEngineNode : public rclcpp::Node { return true; } + /** + * Invoke the generated C solver interface on the current buffers. + */ {{meta.optimizer_name}}SolverStatus solve() { return {{meta.optimizer_name}}_solve(cache_, u_, p_, y_, &init_penalty_); } + /** + * Lazily allocate the solver workspace and multiplier buffer. + */ void initializeSolverIfNeeded() { if (y_ == nullptr) { @@ -154,6 +191,9 @@ class OptimizationEngineNode : public rclcpp::Node { } } + /** + * Convert the solver status structure into the ROS2 result message. + */ void updateResults({{meta.optimizer_name}}SolverStatus& status) { results_.solution.clear(); @@ -173,18 +213,29 @@ class OptimizationEngineNode : public rclcpp::Node { results_.penalty = status.penalty; results_.status = static_cast(status.exit_status); results_.error_code = status.error_code; + // The bindings expose a null-terminated C buffer; convert it once here. results_.error_message = std::string(status.error_message); results_.solve_time_ms = static_cast(status.solve_time_ns) / 1000000.0; results_.infeasibility_f2 = status.f2_norm; results_.infeasibility_f1 = status.delta_y_norm_over_c; } + /** + * Store the latest optimization request received on the parameters topic. + */ void receiveRequestCallback(const OptimizationParametersMsg::ConstSharedPtr msg) { params_ = *msg; has_received_request_ = true; } + /** + * Process at most one pending request and publish exactly one response. + * + * Repeated timer ticks do not republish stale results: once a request has + * been handled, `has_received_request_` is cleared until the next message + * arrives on the input topic. + */ void solveAndPublish() { if (!has_received_request_) { @@ -193,16 +244,22 @@ class OptimizationEngineNode : public rclcpp::Node { initializeSolverIfNeeded(); if (!validateAndUpdateInputData()) { publisher_->publish(results_); + // Mark the request as consumed so the timer does not republish stale errors. has_received_request_ = false; return; } {{meta.optimizer_name}}SolverStatus status = solve(); updateResults(status); publisher_->publish(results_); + // Each request should produce exactly one response message. has_received_request_ = false; } public: + /** + * Construct the ROS2 node, declare runtime parameters, and create the + * publisher, subscriber, and wall timer used by the generated wrapper. + */ OptimizationEngineNode() : Node(ROS2_NODE_{{meta.optimizer_name|upper}}_NODE_NAME) { @@ -232,6 +289,9 @@ class OptimizationEngineNode : public rclcpp::Node { std::bind(&OptimizationEngineNode::solveAndPublish, this)); } + /** + * Release any lazily allocated solver resources. + */ ~OptimizationEngineNode() override { if (y_ != nullptr) { @@ -244,6 +304,10 @@ class OptimizationEngineNode : public rclcpp::Node { }; } /* end of namespace {{ros.package_name}} */ +/** + * Start the generated ROS2 optimizer node and hand control to the ROS2 + * executor until shutdown is requested. + */ int main(int argc, char** argv) { rclcpp::init(argc, argv); diff --git a/open-codegen/test/test.py b/open-codegen/test/test.py index 7e8c81a4..f61b354b 100644 --- a/open-codegen/test/test.py +++ b/open-codegen/test/test.py @@ -12,6 +12,9 @@ import numpy as np + + + class RustBuildTestCase(unittest.TestCase): TEST_DIR = ".python_test_build" @@ -916,6 +919,16 @@ def test_optimizer_meta_invalid_version2(self): self.assertIn("Cargo package version", str(context.exception)) + def test_with_build_mode_rejects_invalid_values(self): + """`with_build_mode` should reject unsupported build modes.""" + build_config = og.config.BuildConfiguration() + + with self.assertRaisesRegex( + ValueError, + "build mode must be either 'debug' or 'release'", + ): + build_config.with_build_mode("profile") + if __name__ == '__main__': logging.getLogger('retry').setLevel(logging.ERROR) diff --git a/open-codegen/test/test_ros2.py b/open-codegen/test/test_ros2.py index 86fe0e73..10f171ff 100644 --- a/open-codegen/test/test_ros2.py +++ b/open-codegen/test/test_ros2.py @@ -48,7 +48,6 @@ def test_ros_and_ros2_configs_clear_each_other(self): self.assertIs(build_config.ros2_config, ros2_config) self.assertIsNone(build_config.ros_config) - class Ros2TemplateCustomizationTestCase(unittest.TestCase): """Generation tests for custom ROS2 configuration values.""" From af448499aea851ca424fa76ef12c19ccacd7c0d4 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Sat, 28 Mar 2026 01:44:22 +0000 Subject: [PATCH 5/7] [ci skip] update auto-generated ros2 readme --- open-codegen/opengen/templates/ros2/README.md | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/open-codegen/opengen/templates/ros2/README.md b/open-codegen/opengen/templates/ros2/README.md index 413caad0..989073b5 100644 --- a/open-codegen/opengen/templates/ros2/README.md +++ b/open-codegen/opengen/templates/ros2/README.md @@ -70,6 +70,10 @@ Then publish a request to the configured parameters topic ros2 topic pub --once /{{ros.subscriber_subtopic}} {{ros.package_name}}/msg/OptimizationParameters "{parameter: [YOUR_PARAMETER_VECTOR], initial_guess: [INITIAL_GUESS_OPTIONAL], initial_y: [], initial_penalty: 15.0}" ``` +If `initial_guess` is omitted or left empty, the node reuses the previous +solution as a warm start. Likewise, an empty `initial_y` means "reuse the +previous Lagrange multipliers". + The result will be announced on the configured result topic (default: `/{{ros.publisher_subtopic}}`): @@ -77,6 +81,9 @@ The result will be announced on the configured result topic ros2 topic echo /{{ros.publisher_subtopic}} --once ``` +Each request produces exactly one response message. The node does not keep +republishing stale results on later timer ticks. + To get the optimal solution you can do: ```bash @@ -108,6 +115,26 @@ result message also includes `error_code` and `error_message` fields so invalid requests and solver failures can be diagnosed without inspecting logs. +A successful response contains `status: 0`, `error_code: 0`, and an empty +`error_message`. If a request is invalid, the node publishes +`status: 5` (`STATUS_INVALID_REQUEST`) and populates `error_code` and +`error_message` with a more detailed explanation. + +For example, if the parameter vector has the wrong length, the node will +return a response like: + +```yaml +status: 5 +error_code: 3003 +error_message: 'wrong number of parameters: provided 1, expected ' +``` + +Similarly, invalid warm-start data is reported with: + +- `error_code: 1600` for an incompatible `initial_guess` +- `error_code: 1700` for incompatible `initial_y` +- `error_code: 2000` for solver-side failures propagated from the generated bindings + The message structures are defined in the following msg files: - [`OptimizationParameters.msg`](msg/OptimizationParameters.msg) From 0ef4d3b616290a21b33bb860fae26c75b018ff40 Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Sat, 28 Mar 2026 01:46:41 +0000 Subject: [PATCH 6/7] more unit tests for ros2 --- open-codegen/test/test_ros2.py | 96 ++++++++++++++++++++++++++++++++++ 1 file changed, 96 insertions(+) diff --git a/open-codegen/test/test_ros2.py b/open-codegen/test/test_ros2.py index 10f171ff..f2d502aa 100644 --- a/open-codegen/test/test_ros2.py +++ b/open-codegen/test/test_ros2.py @@ -243,12 +243,64 @@ def setUpRos2PackageGeneration(cls): solver_configuration=cls.solverConfig()) \ .build() + @classmethod + def _inject_deterministic_solver_error(cls): + """Patch the generated solver so negative `p[0]` triggers a known error.""" + solver_root = os.path.join(cls.TEST_DIR, cls.OPTIMIZER_NAME) + target_lib = os.path.join(solver_root, "src", "lib.rs") + with open(target_lib, "r", encoding="utf-8") as fh: + solver_lib = fh.read() + + if "forced solver error for ROS2 test" in solver_lib: + return + + anchor = ( + ' assert_eq!(u.len(), ROSENBROCK_ROS2_NUM_DECISION_VARIABLES, ' + '"Wrong number of decision variables (u)");\n' + ) + injected_guard = ( + anchor + + '\n' + ' if p[0] < 0.0 {\n' + ' return Err(SolverError::Cost("forced solver error for ROS2 test"));\n' + ' }\n' + ) + if anchor not in solver_lib: + raise RuntimeError("Could not inject deterministic solver error into ROS2 solver") + + with open(target_lib, "w", encoding="utf-8") as fh: + fh.write(solver_lib.replace(anchor, injected_guard, 1)) + + @classmethod + def _rebuild_generated_solver_library(cls): + """Rebuild the generated Rust solver and refresh the ROS2 static library.""" + solver_root = os.path.join(cls.TEST_DIR, cls.OPTIMIZER_NAME) + process = subprocess.Popen( + ["cargo", "build"], + cwd=solver_root, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + _stdout, stderr = process.communicate() + if process.returncode != 0: + raise RuntimeError( + "Could not rebuild generated ROS2 solver:\n{}".format(stderr.decode()) + ) + + generated_static_lib = os.path.join( + solver_root, "target", "debug", f"lib{cls.OPTIMIZER_NAME}.a") + ros2_static_lib = os.path.join( + cls.ros2_package_dir(), "extern_lib", f"lib{cls.OPTIMIZER_NAME}.a") + shutil.copyfile(generated_static_lib, ros2_static_lib) + @classmethod def setUpClass(cls): """Generate the ROS2 package once before all tests run.""" if shutil.which("ros2") is None or shutil.which("colcon") is None: raise unittest.SkipTest("ROS2 CLI tools are not available in PATH") cls.setUpRos2PackageGeneration() + cls._inject_deterministic_solver_error() + cls._rebuild_generated_solver_library() @classmethod def ros2_package_dir(cls): @@ -443,6 +495,12 @@ def _assert_invalid_request_message(self, echo_stdout, error_code, error_message self.assertIn(f"error_code: {error_code}", echo_stdout) self.assertIn(error_message_fragment, echo_stdout) + def _assert_solver_error_message(self, echo_stdout, error_message_fragment): + """Assert that the echoed result message reports a solver-side failure.""" + self.assertIn("status: 3", echo_stdout) + self.assertIn("error_code: 2000", echo_stdout) + self.assertIn(error_message_fragment, echo_stdout) + def _publish_request_and_collect_result(self, ros2_dir, env, request_payload): """Publish one request and return one echoed result message.""" _, setup_script = self.ros2_shell() @@ -484,6 +542,38 @@ def _exercise_invalid_request(self, ros2_dir, env): 3003, "wrong number of parameters") + def _exercise_invalid_initial_guess(self, ros2_dir, env): + """Verify that invalid warm-start dimensions are reported clearly.""" + echo_stdout = self._publish_request_and_collect_result( + ros2_dir, + env, + "{parameter: [1.0, 2.0], initial_guess: [0.0], initial_y: [], initial_penalty: 15.0}") + self._assert_invalid_request_message( + echo_stdout, + 1600, + "initial guess has incompatible dimensions") + + def _exercise_invalid_initial_y(self, ros2_dir, env): + """Verify that invalid multiplier dimensions are reported clearly.""" + echo_stdout = self._publish_request_and_collect_result( + ros2_dir, + env, + "{parameter: [1.0, 2.0], initial_guess: [0.0, 0.0, 0.0, 0.0, 0.0], initial_y: [0.0], initial_penalty: 15.0}") + self._assert_invalid_request_message( + echo_stdout, + 1700, + "wrong dimension of Lagrange multipliers") + + def _exercise_solver_error(self, ros2_dir, env): + """Verify that solver-side failures propagate to the ROS2 result message.""" + echo_stdout = self._publish_request_and_collect_result( + ros2_dir, + env, + "{parameter: [-1.0, 2.0], initial_guess: [0.0, 0.0, 0.0, 0.0, 0.0], initial_y: [], initial_penalty: 15.0}") + self._assert_solver_error_message( + echo_stdout, + "forced solver error for ROS2 test") + def test_ros2_package_generation(self): """Verify the ROS2 package files are generated.""" ros2_dir = self.ros2_package_dir() @@ -512,6 +602,9 @@ def test_generated_ros2_package_works(self): self._wait_for_node_and_topics(ros2_dir, env, node_process) self._exercise_running_optimizer(ros2_dir, env) self._exercise_invalid_request(ros2_dir, env) + self._exercise_invalid_initial_guess(ros2_dir, env) + self._exercise_invalid_initial_y(ros2_dir, env) + self._exercise_solver_error(ros2_dir, env) self._exercise_running_optimizer(ros2_dir, env) finally: if node_process.poll() is None: @@ -535,6 +628,9 @@ def test_generated_ros2_launch_file_works(self): self._wait_for_node_and_topics(ros2_dir, env, launch_process) self._exercise_running_optimizer(ros2_dir, env) self._exercise_invalid_request(ros2_dir, env) + self._exercise_invalid_initial_guess(ros2_dir, env) + self._exercise_invalid_initial_y(ros2_dir, env) + self._exercise_solver_error(ros2_dir, env) self._exercise_running_optimizer(ros2_dir, env) finally: if launch_process.poll() is None: From 8dca03c3099c1b550574c6140d26ea559e28f1cf Mon Sep 17 00:00:00 2001 From: Pantelis Sopasakis Date: Sat, 28 Mar 2026 02:00:36 +0000 Subject: [PATCH 7/7] [ci skip] update contributing.mdx --- docs/contributing.mdx | 84 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 84 insertions(+) diff --git a/docs/contributing.mdx b/docs/contributing.mdx index 25cf7cee..7671c872 100644 --- a/docs/contributing.mdx +++ b/docs/contributing.mdx @@ -253,3 +253,87 @@ git commit -m '[docit] update api docs' --allow-empty [fork]: https://github.com/alphaville/optimization-engine [API guidelines]: https://rust-lang-nursery.github.io/api-guidelines/about.html [API checklist]: https://rust-lang-nursery.github.io/api-guidelines/checklist.html + +## Running tests locally + +If you are working on the Python interface (`opengen`) or the website/docs, +it is best to use a dedicated Python virtual environment. + +### Set up a virtual environment + +From within `open-codegen/`, create and activate a virtual environment: + +```bash +cd open-codegen +python3 -m venv venv +source venv/bin/activate +python -m pip install --upgrade pip +pip install -e . +``` + +If you plan to run the benchmark suite as well, install the extra dependency: + +```bash +pip install pytest-benchmark[histogram] +``` + +### Run the Rust tests + +From the repository root, run: + +```bash +cargo test +``` + +This will run all unit tests, including the examples in the docstrings. +To run only the library unit tests, do: + +```bash +cargo test --lib +``` + +If you want a faster compile-only check, you can also run: + +```bash +cargo check +``` + +### Run the Python and code-generation tests + +From within `open-codegen/`, run the following tests after you activate `venv` + +```bash +# Activate venv first +python -W ignore test/test_constraints.py -v +python -W ignore test/test.py -v +python -W ignore test/test_ocp.py -v +``` + +The ROS2 tests should normally be run from an environment where ROS2 is already +installed and configured, for example a dedicated `micromamba` environment. +They should not be assumed to run from the plain `venv` above unless that +environment also contains a working ROS2 installation together with `ros2` and +`colcon`. + +For example: + +```bash +cd open-codegen +micromamba activate ros_env +pip install . +python -W ignore test/test_ros2.py -v +``` + +If ROS2 is not installed locally, you can still run the rest of the Python +test suite. + +### Run linting and extra checks + +From the repository root, it is also useful to run: + +```bash +cargo clippy --all-targets +``` + +Before opening a pull request, please run the tests that are relevant to the +part of the codebase you changed and make sure they pass locally.