Fix ROS/ROS2 generated interface issues

alphaville · alphaville · commit 15164e8e4ff0 · 2026-03-28T15:44:07.000Z
- allow small positive initial penalties in ROS/ROS2
- ROS/ROS2 messages: using uint64 to avoid overflow
- update ROS2 test helpers to work reliably in micromamba/conda
- update the related docs and MATLAB examples
diff --git a/docs/content/example_navigation_ros_codegen.md b/docs/content/example_navigation_ros_codegen.md
@@ -326,7 +326,7 @@ private:
      */
     void updateInputData()
     {
-        init_penalty = (params.initial_penalty > 1.0)
+        init_penalty = (params.initial_penalty > std::numeric_limits<double>::epsilon())
             ? params.initial_penalty
             : ROS_NODE_MPC_CONTROLLER_DEFAULT_INITIAL_PENALTY;
 
diff --git a/docs/content/matlab-api.mdx b/docs/content/matlab-api.mdx
@@ -115,7 +115,7 @@ optimizer directory, MATLAB can read the TCP endpoint automatically:
 ```matlab
 manifestPath = fullfile( ...
     pwd, ...
-    'open-codegen', ...
+    'python', ...
     '.python_test_build_ocp', ...
     'ocp_single_tcp', ...
     'optimizer_manifest.json');
@@ -155,7 +155,7 @@ provided:
 ```matlab
 manifestPath = fullfile( ...
     pwd, ...
-    'open-codegen', ...
+    'python', ...
     '.python_test_build_ocp', ...
     'ocp_manifest_bindings', ...
     'optimizer_manifest.json');
@@ -172,7 +172,7 @@ state trajectory:
 ```matlab
 manifestPath = fullfile( ...
     pwd, ...
-    'open-codegen', ...
+    'python', ...
     '.python_test_build_ocp', ...
     'ocp_multiple_tcp', ...
     'optimizer_manifest.json');
diff --git a/docs/content/python-ros.md b/docs/content/python-ros.md
@@ -29,7 +29,7 @@ The input parameters message follows the following specification:
 float64[]      parameter         # parameter p (mandatory)
 float64[]      initial_guess     # u0 (optional/recommended)
 float64[]      initial_y         # y0 (optional)
-float64        initial_penalty   # initial penalty (optional)
+float64        initial_penalty   # positive initial penalty (optional)
 ```
 
 An example of such a message is
@@ -41,6 +41,10 @@ initial_penalty: 1000,
 initial_y: []
 ```
 
+`initial_penalty` is applied whenever it is strictly greater than a small
+positive epsilon. If it is omitted, zero, or too close to zero, the generated
+default initial penalty is used instead.
+
 
 #### Result
 
@@ -74,8 +78,8 @@ uint8 STATUS_NOT_CONVERGED_COST=3
 uint8 STATUS_NOT_CONVERGED_FINITE_COMPUTATION=4
 
 float64[]    solution              # optimizer (solution)
-uint8        inner_iterations      # number of inner iterations
-uint16       outer_iterations      # number of outer iterations
+uint64       inner_iterations      # number of inner iterations
+uint64       outer_iterations      # number of outer iterations
 uint8        status                # status code
 float64      cost                  # cost value at solution
 float64      norm_fpr              # norm of FPR of last inner problem
@@ -182,4 +186,4 @@ OpEn generates a README file that you will find in `my_optimizers/rosenbrock/par
 - Compile with `catkin_make`
 - Run using the auto-generated launch file
 
-Check out the auto-generated README.md file for details.
+Check out the auto-generated README.md file for details.
diff --git a/docs/content/python-ros2.mdx b/docs/content/python-ros2.mdx
@@ -24,6 +24,10 @@ OpEn can generate ready-to-use ROS2 packages directly from a parametric optimize
 
 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.
 
+For scalar warm-starting, `initial_penalty` is used whenever it is strictly
+greater than a small positive epsilon. If it is omitted, zero, or too close to
+zero, the generated default initial penalty is used instead.
+
 ## Configuration Parameters
 
 The configuration parameters are the same as in the [ROS1 package documentation](./python-ros#configuration-parameters): you can configure the node rate, the input topic name, and the output topic name.
@@ -203,10 +207,10 @@ solve_time_ms: 0.2175
   uint8 STATUS_INVALID_REQUEST=5
 
   float64[]    solution              # solution
-  uint8        inner_iterations      # number of inner iterations
-  uint16       outer_iterations      # number of outer iterations
+  uint64       inner_iterations      # number of inner iterations
+  uint64       outer_iterations      # number of outer iterations
   uint8        status                # coarse status code
-  int32        error_code            # detailed error code (0 on success)
+  uint16       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
diff --git a/docs/website/publish.sh b/docs/website/publish.sh
@@ -1,5 +1,5 @@
 #!/bin/bash
 GIT_USER=alphaville \
-  CURRENT_BRANCH=master \
+  CURRENT_BRANCH=dev/release \
   USE_SSH=true \
   yarn deploy
diff --git a/python/opengen/templates/ros/OptimizationParameters.msg b/python/opengen/templates/ros/OptimizationParameters.msg
@@ -1,4 +1,4 @@
 float64[]      parameter         # parameter p (mandatory)
 float64[]      initial_guess     # u0 (optional/recommended)
 float64[]      initial_y         # y0 (optional)
-float64        initial_penalty   # initial penalty (optional)
+float64        initial_penalty   # positive initial penalty (optional)
diff --git a/python/opengen/templates/ros/OptimizationResult.msg b/python/opengen/templates/ros/OptimizationResult.msg
@@ -6,8 +6,8 @@ uint8 STATUS_NOT_CONVERGED_COST=3
 uint8 STATUS_NOT_CONVERGED_FINITE_COMPUTATION=4
 
 float64[]    solution              # optimizer (solution)
-uint8        inner_iterations      # number of inner iterations
-uint16       outer_iterations      # number of outer iterations
+uint64       inner_iterations      # number of inner iterations
+uint64       outer_iterations      # number of outer iterations
 uint8        status                # status code
 float64      cost                  # cost at solution
 float64      norm_fpr              # norm of FPR of last inner problem
@@ -16,4 +16,3 @@ float64[]    lagrange_multipliers  # vector of Lagrange multipliers
 float64      infeasibility_f1      # infeasibility wrt F1
 float64      infeasibility_f2      # infeasibility wrt F2
 float64      solve_time_ms         # solution time in ms
-
diff --git a/python/opengen/templates/ros/open_optimizer.cpp b/python/opengen/templates/ros/open_optimizer.cpp
@@ -5,6 +5,8 @@
  * dually licensed under the MIT and Apache v2 licences.
  *
  */
+#include <limits>
+
 #include "ros/ros.h"
 #include "{{ros.package_name}}/OptimizationResult.h"
 #include "{{ros.package_name}}/OptimizationParameters.h"
@@ -70,7 +72,7 @@ class OptimizationEngineManager {
      */
     void updateInputData()
     {
-        init_penalty = (params.initial_penalty > 1.0)
+        init_penalty = (params.initial_penalty > std::numeric_limits<double>::epsilon())
             ? params.initial_penalty
             : ROS_NODE_{{meta.optimizer_name|upper}}_DEFAULT_INITIAL_PENALTY;
 
diff --git a/python/opengen/templates/ros2/OptimizationParameters.msg b/python/opengen/templates/ros2/OptimizationParameters.msg
@@ -1,4 +1,4 @@
 float64[]      parameter         # parameter p (mandatory)
 float64[]      initial_guess     # u0 (optional/recommended)
 float64[]      initial_y         # y0 (optional)
-float64        initial_penalty   # initial penalty (optional)
+float64        initial_penalty   # positive initial penalty (optional)
diff --git a/python/opengen/templates/ros2/OptimizationResult.msg b/python/opengen/templates/ros2/OptimizationResult.msg
@@ -7,10 +7,10 @@ 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
+uint64       inner_iterations      # number of inner iterations
+uint64       outer_iterations      # number of outer iterations
 uint8        status                # coarse status code
-int32        error_code            # detailed error code (0 on success)
+uint16       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
diff --git a/python/opengen/templates/ros2/README.md b/python/opengen/templates/ros2/README.md
@@ -72,7 +72,9 @@ ros2 topic pub --once /{{ros.subscriber_subtopic}} {{ros.package_name}}/msg/Opti
 
 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".
+previous Lagrange multipliers". `initial_penalty` is applied whenever it is
+strictly greater than a small positive epsilon; otherwise the generated default
+penalty is used.
 
 The result will be announced on the configured result topic
 (default: `/{{ros.publisher_subtopic}}`):
diff --git a/python/opengen/templates/ros2/open_optimizer.cpp b/python/opengen/templates/ros2/open_optimizer.cpp
@@ -7,6 +7,7 @@
 #include <chrono>
 #include <cstdint>
 #include <functional>
+#include <limits>
 #include <memory>
 #include <sstream>
 #include <string>
@@ -44,9 +45,10 @@ class OptimizationEngineNode : public rclcpp::Node {
     rclcpp::Subscription<OptimizationParametersMsg>::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 constexpr uint16_t kInvalidInitialGuessErrorCode = 1600;
+    static constexpr uint16_t kInvalidInitialYErrorCode = 1700;
+    static constexpr uint16_t kInvalidParameterErrorCode = 3003;
+    static constexpr double kInitialPenaltyEpsilon = std::numeric_limits<double>::epsilon();
 
     /**
      * Convert the configured solver loop rate in Hz to a ROS2 timer period.
@@ -86,7 +88,7 @@ class OptimizationEngineNode : public rclcpp::Node {
      */
     void setErrorResult(
         uint8_t status,
-        int32_t error_code,
+        uint16_t error_code,
         const std::string& error_message)
     {
         results_.solution.clear();
@@ -113,8 +115,8 @@ class OptimizationEngineNode : public rclcpp::Node {
      */
     bool validateAndUpdateInputData()
     {
-        // A non-positive or missing penalty falls back to the generated default.
-        init_penalty_ = (params_.initial_penalty > 1.0)
+        // A missing or too-small penalty falls back to the generated default.
+        init_penalty_ = (params_.initial_penalty > kInitialPenaltyEpsilon)
             ? params_.initial_penalty
             : ROS2_NODE_{{meta.optimizer_name|upper}}_DEFAULT_INITIAL_PENALTY;
 
@@ -212,7 +214,7 @@ class OptimizationEngineNode : public rclcpp::Node {
         results_.cost = status.cost;
         results_.penalty = status.penalty;
         results_.status = static_cast<uint8_t>(status.exit_status);
-        results_.error_code = status.error_code;
+        results_.error_code = static_cast<uint16_t>(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;
diff --git a/python/test/test.py b/python/test/test.py
@@ -413,6 +413,33 @@ def test_python_bindings(self):
         self.assertEqual("Converged", status.exit_status)
         self.assertIsNotNone(status.solution)
 
+    def test_generated_ros_templates_allow_small_positive_initial_penalty(self):
+        ros_dir = os.path.join(
+            RustBuildTestCase.TEST_DIR,
+            "rosenbrock_ros",
+            "parametric_optimizer"
+        )
+        with open(os.path.join(ros_dir, "src", "open_optimizer.cpp"), "r", encoding="utf-8") as fh:
+            optimizer_cpp = fh.read()
+
+        self.assertIn("std::numeric_limits<double>::epsilon()", optimizer_cpp)
+        self.assertIn(
+            "params.initial_penalty > std::numeric_limits<double>::epsilon()",
+            optimizer_cpp
+        )
+
+    def test_generated_ros_result_message_uses_wide_iteration_counters(self):
+        ros_dir = os.path.join(
+            RustBuildTestCase.TEST_DIR,
+            "rosenbrock_ros",
+            "parametric_optimizer"
+        )
+        with open(os.path.join(ros_dir, "msg", "OptimizationResult.msg"), "r", encoding="utf-8") as fh:
+            result_msg = fh.read()
+
+        self.assertIn("uint64       inner_iterations", result_msg)
+        self.assertIn("uint64       outer_iterations", result_msg)
+
     def test_python_bindings_error_details(self):
         python_bindings = RustBuildTestCase.import_generated_module(
             "python_bindings", "python_bindings")
diff --git a/python/test/test_ros2.py b/python/test/test_ros2.py
@@ -170,7 +170,9 @@ def test_custom_ros2_configuration_is_rendered_into_generated_files(self):
         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("uint64       inner_iterations", result_msg)
+        self.assertIn("uint64       outer_iterations", result_msg)
+        self.assertIn("uint16       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"),
@@ -179,6 +181,11 @@ def test_custom_ros2_configuration_is_rendered_into_generated_files(self):
         self.assertIn("error_code", bindings_header)
         self.assertIn("error_message", bindings_header)
 
+        with open(os.path.join(ros2_dir, "src", "open_optimizer.cpp"), encoding="utf-8") as f:
+            optimizer_cpp = f.read()
+        self.assertIn("kInitialPenaltyEpsilon", optimizer_cpp)
+        self.assertIn("params_.initial_penalty > kInitialPenaltyEpsilon", optimizer_cpp)
+
 
 class Ros2BuildTestCase(unittest.TestCase):
     """Integration tests for auto-generated ROS2 packages."""
@@ -348,8 +355,18 @@ def ros2_shell(cls):
     def _run_shell(cls, command, cwd, env=None, timeout=180, check=True):
         """Run a command in the preferred shell and return the completed process."""
         shell_path, _ = cls.ros2_shell()
+        shell_command = command
+        if env is not None:
+            exported_vars = []
+            for var_name in ("PATH", "CONDA_PREFIX", "RMW_IMPLEMENTATION", "ROS_LOG_DIR",
+                             "DYLD_LIBRARY_PATH", "DYLD_FALLBACK_LIBRARY_PATH", "LD_LIBRARY_PATH"):
+                if var_name in env:
+                    exported_vars.append(
+                        f"export {var_name}={shlex.quote(env[var_name])};"
+                    )
+            shell_command = f"{' '.join(exported_vars)} {command}"
         result = subprocess.run(
-            [shell_path, "-lc", command],
+            [shell_path, "-c", shell_command],
             cwd=cwd,
             env=env,
             text=True,
@@ -389,6 +406,11 @@ def _terminate_process(process, timeout=10):
 
     def _build_generated_package(self, ros2_dir, env):
         """Build the generated ROS2 package with the active Python executable."""
+        for stale_dir in ("build", "install", "log"):
+            stale_path = os.path.join(ros2_dir, stale_dir)
+            if os.path.exists(stale_path):
+                shutil.rmtree(stale_path)
+
         python_executable = shlex.quote(sys.executable)
         self._run_shell(
             f"source {self.ros2_shell()[1]} >/dev/null 2>&1 || true; "
@@ -401,11 +423,18 @@ def _build_generated_package(self, ros2_dir, env):
     def _spawn_ros_process(self, command, ros2_dir, env):
         """Start a long-running ROS2 command in a fresh process group."""
         shell_path, setup_script = self.ros2_shell()
+        exported_vars = []
+        for var_name in ("PATH", "CONDA_PREFIX", "RMW_IMPLEMENTATION", "ROS_LOG_DIR",
+                         "DYLD_LIBRARY_PATH", "DYLD_FALLBACK_LIBRARY_PATH", "LD_LIBRARY_PATH"):
+            if var_name in env:
+                exported_vars.append(
+                    f"export {var_name}={shlex.quote(env[var_name])};"
+                )
         return subprocess.Popen(
             [
                 shell_path,
-                "-lc",
-                f"source {setup_script} && {command}"
+                "-c",
+                f"{' '.join(exported_vars)} source {setup_script} && {command}"
             ],
             cwd=ros2_dir,
             env=env,

Original file line number	Diff line number	Diff line change
`@@ -326,7 +326,7 @@ private:`
`326`	`326`	`*/`
`327`	`327`	`void updateInputData()`
`328`	`328`	`{`
`329`		`- init_penalty = (params.initial_penalty > 1.0)`
	`329`	`+ init_penalty = (params.initial_penalty > std::numeric_limits<double>::epsilon())`
`330`	`330`	`? params.initial_penalty`
`331`	`331`	`: ROS_NODE_MPC_CONTROLLER_DEFAULT_INITIAL_PENALTY;`
`332`	`332`