various fixes

Author: alphaville

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
 

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<uint8_t>(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<double>(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);

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)

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."""