diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 982b2960..f91661b0 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -43,7 +43,8 @@ jobs: python_tests: name: Python tests (${{ matrix.name }}) - needs: rust_tests + needs: + - rust_tests runs-on: ${{ matrix.os }} timeout-minutes: 45 strategy: @@ -102,6 +103,55 @@ jobs: if: runner.os == 'macOS' run: bash ./ci/script.sh python-tests + ros2_tests: + name: ROS2 tests + needs: python_tests + runs-on: ubuntu-latest + timeout-minutes: 45 + container: + image: ubuntu:noble + options: --user 0 + env: + DO_DOCKER: 0 + steps: + - uses: actions/checkout@v5 + + - name: Install container bootstrap dependencies + run: | + apt-get update + DEBIAN_FRONTEND=noninteractive apt-get install -y \ + build-essential \ + cmake \ + curl \ + ca-certificates \ + git \ + gnupg2 \ + locales \ + lsb-release + + - uses: actions-rust-lang/setup-rust-toolchain@v1 + with: + toolchain: stable + rustflags: "" + + - uses: actions/setup-python@v6 + with: + python-version: "3.12" + + - name: Setup ROS 2 + # `ros-tooling/setup-ros@v0.7` still runs as a Node.js 20 action. + # Force it onto Node 24 now so CI keeps working as GitHub deprecates + # Node 20, and upgrade `setup-ros` to a Node 24-compatible release + # when one becomes available. + env: + FORCE_JAVASCRIPT_ACTIONS_TO_NODE24: true + uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: jazzy + + - name: Run ROS2 Python tests + run: bash ./ci/script.sh ros2-tests + ocp_tests: name: OCP tests (${{ matrix.name }}) needs: python_tests diff --git a/ci/script.sh b/ci/script.sh index 18f1f2b7..b8d988b7 100755 --- a/ci/script.sh +++ b/ci/script.sh @@ -60,6 +60,35 @@ run_python_core_tests() { generated_clippy_tests } +run_python_ros2_tests() { + export PYTHONPATH=. + set +u + if [ -n "${ROS_DISTRO:-}" ] && [ -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]; then + # setup-ros installs the ROS underlay but does not source it for our shell + source "/opt/ros/${ROS_DISTRO}/setup.bash" + elif [ -f "/opt/ros/jazzy/setup.bash" ]; then + source "/opt/ros/jazzy/setup.bash" + else + set -u + echo "ROS2 environment setup script not found" + exit 1 + fi + set -u + + if ! python -c "import em, lark, catkin_pkg" >/dev/null 2>&1; then + # ROS2 build helpers run under the active Python interpreter. The test venv + # already has NumPy from `pip install .`, but we also need the ROS-side + # Python packages used during interface and package metadata generation. + # Empy 4 has broken older ROS message generators in the past, so keep it + # on the 3.x API here. + python -m pip install "empy<4" lark catkin_pkg + fi + + command -v ros2 >/dev/null + command -v colcon >/dev/null + python -W ignore test/test_ros2.py -v +} + run_python_ocp_tests() { export PYTHONPATH=. python -W ignore test/test_ocp.py -v @@ -84,6 +113,11 @@ main() { setup_python_test_env run_python_core_tests ;; + ros2-tests) + echo "Running ROS2 Python tests" + setup_python_test_env + run_python_ros2_tests + ;; ocp-tests) echo "Running OCP Python tests" setup_python_test_env diff --git a/docs/contributing.md b/docs/contributing.mdx similarity index 70% rename from docs/contributing.md rename to docs/contributing.mdx index dcae4bae..25cf7cee 100644 --- a/docs/contributing.md +++ b/docs/contributing.mdx @@ -5,6 +5,9 @@ title: Contributing to OpEn description: How do I contribute to OpEn --- +import Tabs from '@theme/Tabs'; +import TabItem from '@theme/TabItem'; + ## How can I contribute to OpEn? Thank you for considering contributing to Optimization Engine (OpEn)! @@ -134,23 +137,111 @@ Each time the major or minor number of the Rust library is updated, a new crate In order to release a new version make sure that you have done the following: -- Updated [CHANGELOG] -- Updated the version in (SemVer): - - [CHANGELOG] - - [Cargo.toml] - - [setup.py] -- Resolved all associated issues on github (and you have created tests for these) -- Updated the documentation (Rust/Python API docs + website) -- Merged into master (your pull request has been approved) -- All tests pass on Travis CI and Appveyor -- Set `publish=true` in `Cargo.toml` (set it back to `false` for safety) -- Publish `opengen` on PyPI (if necessary) - - before doing so, make sure that the cargo.toml template - points to the correct version of OpEn -- Changed "Unreleased" into the right version in [CHANGELOG] and created - a release on github (example [release v0.4.0]) +--- + + + + +Checklist: + + + +Then, create a tag and push it... + + ```bash + git tag -a v0.10.0 -m "v0.10.0" + git push --tags + ``` + +Lastly, update the [docker image](https://github.com/alphaville/optimization-engine/tree/master/docker). +This will have to be a new PR. + + + + + + +Checklist: + + + +Then, create a tag and push it... + + ```bash + git tag -a opengen-0.10.0 -m "opengen-0.10.0" + git push --tags + ``` + + + +Lastly, update the [docker image](https://github.com/alphaville/optimization-engine/tree/master/docker). +This will have to be a new PR. + + + + + + + Update the [Dockerfile](https://github.com/alphaville/optimization-engine/blob/master/docker/Dockerfile). + You may need to bump the versions of open and opengen: + + ```Dockerfile + ARG OPENGEN_VERSION=0.10.0 + ARG OPTIMIZATION_ENGINE_CRATE_VERSION=0.11.0 + ``` + + Update the [CHANGELOG](https://github.com/alphaville/optimization-engine/blob/master/docker/CHANGELOG.md). + Update the [README](https://github.com/alphaville/optimization-engine/blob/master/docker/README.md) file. + Build, test, and push with + + ```bash + docker push alphaville/open:0.7.0 + ``` + + Update the [website docs](./docker) and the promo on the [main page](..) + + + + +--- + +To update the website, run +```bash +GIT_USER=alphaville \ + CURRENT_BRANCH=master \ + USE_SSH=true \ + yarn deploy +``` +from within `website/`. Then, update the opengen API docs too; +just push a commit with message starting with `[docit]`. +You can also issue a commit without git-add. Run + +```bash +git commit -m '[docit] update api docs' --allow-empty +``` + + [CHANGELOG]: https://github.com/alphaville/optimization-engine/blob/master/CHANGELOG.md +[VERSION]: https://github.com/alphaville/optimization-engine/blob/master/open-codegen/VERSION [Cargo.toml]: https://github.com/alphaville/optimization-engine/blob/master/Cargo.toml [setup.py]: https://github.com/alphaville/optimization-engine/blob/master/open-codegen/setup.py [release v0.4.0]: https://github.com/alphaville/optimization-engine/releases/tag/v0.4.0 diff --git a/docs/python-c.mdx b/docs/python-c.mdx index bbe387e8..721b5bf9 100644 --- a/docs/python-c.mdx +++ b/docs/python-c.mdx @@ -270,4 +270,4 @@ iterations = 69 outer iterations = 5 solve time = 0.140401 ms ``` -``` + diff --git a/docs/python-ros.md b/docs/python-ros.md index f06fa078..b594e9d2 100644 --- a/docs/python-ros.md +++ b/docs/python-ros.md @@ -2,9 +2,13 @@ id: python-ros title: Generation of ROS packages sidebar_label: ROS packages -description: Code generation for ROS packages using OpEn in Python +description: Code generation for ROS packages using opengen --- +:::note Info +Opengen now supports [ROS2](./python-ros2). +::: + ## What is ROS The [Robot Operating System](https://www.ros.org/) (ROS) is a collection of tools and libraries, as well as a framework that facilitates the data exchange among them. ROS is popular in the robotics community and is used to design and operate modern robotic systems. @@ -21,7 +25,7 @@ OpEn (with opengen version `0.5.0` or newer) can generate ready-to-use ROS packa The input parameters message follows the following specification: -``` +```msg float64[] parameter # parameter p (mandatory) float64[] initial_guess # u0 (optional/recommended) float64[] initial_y # y0 (optional) @@ -40,9 +44,28 @@ initial_y: [] #### Result -A result message (`OptimizationResult`) contains the solution of the parametric optimization problem and details about the solution procedure such as the number of inner/outer iterations and the solution time. The result of an auto-generated OpEn node is a message with the following specification: +A result message (`OptimizationResult`) contains the solution of the parametric optimization problem and details about the solution procedure such as the number of inner/outer iterations and the solution time. +An example of such a message is given below: +```yaml +solution: [0.5317, 0.7975, 0.6761, 0.7760, 0.5214] +inner_iterations: 159 +outer_iterations: 5 +status: 0 +norm_fpr: 2.142283848e-06 +penalty: 111250.0 +lagrange_multipliers: [] +infeasibility_f1: 0.0 +infeasibility_f2: 2.44131958366e-05 +solve_time_ms: 2.665959 ``` + +
+Specification of OptimizationResult + +The message `OptimizationResult` is described by the following message file + +```msg # Constants match the enumeration of status codes uint8 STATUS_CONVERGED=0 uint8 STATUS_NOT_CONVERGED_ITERATIONS=1 @@ -63,20 +86,7 @@ float64 infeasibility_f2 # infeasibility wrt F2 float64 solve_time_ms # solution time in ms ``` -An example of such a message is given below: - -```yaml -solution: [0.5317, 0.7975, 0.6761, 0.7760, 0.5214] -inner_iterations: 159 -outer_iterations: 5 -status: 0 -norm_fpr: 2.142283848e-06 -penalty: 111250.0 -lagrange_multipliers: [] -infeasibility_f1: 0.0 -infeasibility_f2: 2.44131958366e-05 -solve_time_ms: 2.665959 -``` +
### Configuration Parameters diff --git a/docs/python-ros2.mdx b/docs/python-ros2.mdx new file mode 100644 index 00000000..a81bff42 --- /dev/null +++ b/docs/python-ros2.mdx @@ -0,0 +1,235 @@ +--- +id: python-ros2 +title: Generation of ROS2 packages +sidebar_label: ROS2 packages +description: Code generation for ROS2 packages using opengen +--- + +import Tabs from '@theme/Tabs'; +import TabItem from '@theme/TabItem'; + +:::note Info +The functionality presented here was introduced in `opengen` version [`0.11.0a1`](https://pypi.org/project/opengen/#history). +::: + +## What is ROS2 + +[ROS2](https://docs.ros.org/en/jazzy/index.html) is the successor of the Robot Operating System (ROS). It provides tools, libraries, and communication mechanisms that make it easier to build distributed robotic applications. + +In ROS2, functionality is organised in **nodes** which exchange data by publishing and subscribing to **topics** using typed **messages**. This makes ROS2 a natural fit for connecting optimizers, controllers, estimators, and sensors in robotics systems. + +## ROS2 + OpEn + +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). + +## 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. + +In ROS2, these settings are stored using the ROS2 parameter-file format in `config/open_params.yaml`: + +```yaml +/**: + ros__parameters: + result_topic: "result" + params_topic: "parameters" + rate: 10.0 +``` + +## Code generation + +To generate a ROS2 package from Python, create a `RosConfiguration` object and attach it to the build configuration using `.with_ros2(...)`. + +### Example + +```py +import opengen as og +import casadi.casadi as cs + +u = cs.SX.sym("u", 5) +p = cs.SX.sym("p", 2) +phi = og.functions.rosenbrock(u, p) + +problem = og.builder.Problem(u, p, phi) \ + .with_constraints(og.constraints.Ball2(None, 1.5)) + +meta = og.config.OptimizerMeta() \ + .with_optimizer_name("rosenbrock_ros2") + +ros2_config = og.config.RosConfiguration() \ + .with_package_name("parametric_optimizer_ros2") \ + .with_node_name("open_node_ros2") \ + .with_rate(10) + +build_config = og.config.BuildConfiguration() \ + .with_build_directory("my_optimizers") \ + .with_ros2(ros2_config) + +builder = og.builder.OpEnOptimizerBuilder(problem, meta, build_config) +builder.build() +``` + +Note the use of `with_ros2` and note that `RosConfiguration` is the same config +class as in [ROS1](./python-ros). +This generates the optimizer in `my_optimizers/rosenbrock_ros2`, and the ROS2 +package is created inside that directory as `parametric_optimizer_ros2`. +You can inspect the auto-generated ROS2 package [here](https://github.com/alphaville/open_ros/tree/master/ros2). + +## Use the auto-generated ROS2 package + +OpEn generates a `README.md` file inside the generated ROS2 package with detailed instructions. In brief, the workflow is: + +1. Build the package with `colcon build` +2. Source the generated workspace setup script +3. Run the node with `ros2 run` +4. Publish optimization requests on the input topic and read results from the output topic + +For example, from inside the generated package directory: + + + + +```bash +colcon build --packages-select parametric_optimizer_ros2 +source install/setup.bash +ros2 run parametric_optimizer_ros2 open_node_ros2 +``` + + + + +```bash +colcon build --packages-select parametric_optimizer_ros2 +source install/setup.zsh +ros2 run parametric_optimizer_ros2 open_node_ros2 +``` + + + + +In a second terminal: + + + + +```bash +source install/setup.bash +ros2 topic pub --once /parameters parametric_optimizer_ros2/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}" +ros2 topic echo /result --once +``` + + + + +```bash +source install/setup.zsh +ros2 topic pub --once /parameters parametric_optimizer_ros2/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}" +ros2 topic echo /result --once +``` + + + + +If ROS2 cannot write to its default log directory, set an explicit writable log path before running the node: + +```bash +mkdir -p .ros_log +export ROS_LOG_DIR="$PWD/.ros_log" +``` + +:::note Troubleshooting +On some systems, the generated node may start but not appear in the ROS2 graph. If `ros2 topic pub` keeps printing `Waiting for at least 1 matching subscription(s)...`, set +`RMW_IMPLEMENTATION=rmw_fastrtps_cpp` in both terminals before sourcing the generated workspace and running any `ros2` commands: + +```bash +export RMW_IMPLEMENTATION=rmw_fastrtps_cpp +``` + +This should only be needed if ROS2 discovery is not working correctly with your default middleware. +::: + +To verify that the node is visible, you can run: + +```bash +ros2 node list --no-daemon --spin-time 5 +ros2 topic list --no-daemon --spin-time 5 +``` + +The first command should list the running node, for example `/open_node_ros2`. The second should list the available topics, including `/parameters` and `/result`. + +To read a single optimizer response, you can use: + +```bash +ros2 topic echo /result --once +``` + +This subscribes to the result topic, prints one `OptimizationResult` message, and then exits. +The above command will return a message that looks as follows + +```yaml +solution: +- 0.5352476095477849 +- 0.8028586510585609 +- 0.6747818561706652 +- 0.7747513439588263 +- 0.5131839675113338 +inner_iterations: 41 +outer_iterations: 6 +status: 0 +cost: 1.1656771801253916 +norm_fpr: 2.1973496274068953e-05 +penalty: 150000.0 +lagrange_multipliers: [] +infeasibility_f1: 0.0 +infeasibility_f2: 3.3074097972366455e-05 +solve_time_ms: 0.2175 +``` + +
+ See the specification of `OptimizationResult` + ```msg + # Constants match the enumeration of status codes + uint8 STATUS_CONVERGED=0 + 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 + + float64[] solution # solution + uint8 inner_iterations # number of inner iterations + uint16 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 + float64 penalty # penalty value + 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 + ``` +
+ +Instead of starting the node with `ros2 run`, you can also use the generated launch file: + +```bash +ros2 launch parametric_optimizer_ros2 open_optimizer.launch.py +``` + +The launch file starts the auto-generated node and loads its parameters from `config/open_params.yaml`, where you can adjust settings such as the input topic, output topic, and node rate. + + +## Inside the ROS2 package + +The auto-generated ROS2 package contains everything needed to build and run the optimizer as a ROS2 node. + +- `msg/` contains the auto-generated message definitions, including `OptimizationParameters.msg` and `OptimizationResult.msg` +- `src/` contains the C++ node implementation that wraps the optimizer +- `include/` contains the corresponding C++ headers +- `config/open_params.yaml` stores runtime parameters such as the input topic, output topic, and node rate +- `launch/open_optimizer.launch.py` provides a ready-to-use ROS2 launch file +- `CMakeLists.txt` and `package.xml` define the ROS2 package and its build dependencies +- `README.md` contains package-specific build and usage instructions diff --git a/open-codegen/CHANGELOG.md b/open-codegen/CHANGELOG.md index 75b7a86d..e6940a39 100644 --- a/open-codegen/CHANGELOG.md +++ b/open-codegen/CHANGELOG.md @@ -8,6 +8,18 @@ and this project adheres to [Semantic Versioning](http://semver.org/). Note: This is the Changelog file of `opengen` - the Python interface of OpEn +## [0.11.0] - Unreleased + +### Added + +- ROS2 package generation support via `BuildConfiguration.with_ros2(...)`, including auto-generated ROS2 templates, launcher, messages, and package wrapper code +- Dedicated ROS2 tests covering package generation, build configuration behavior, rendered custom package settings, and end-to-end execution of a generated ROS2 node + +### Changed + +- Extended `RosConfiguration` so it can be used for both ROS and ROS2 package generation + + ## [0.10.1] - 2026-03-25 diff --git a/open-codegen/VERSION b/open-codegen/VERSION index 71172b43..05b845f9 100644 --- a/open-codegen/VERSION +++ b/open-codegen/VERSION @@ -1 +1 @@ -0.10.1 \ No newline at end of file +0.11.0a1 \ No newline at end of file diff --git a/open-codegen/opengen/builder/optimizer_builder.py b/open-codegen/opengen/builder/optimizer_builder.py index 316f5422..51500422 100644 --- a/open-codegen/opengen/builder/optimizer_builder.py +++ b/open-codegen/opengen/builder/optimizer_builder.py @@ -12,7 +12,7 @@ import sys from importlib.metadata import version -from .ros_builder import RosBuilder +from .ros_builder import ROS2Builder, RosBuilder _AUTOGEN_COST_FNAME = 'auto_casadi_cost.c' _AUTOGEN_GRAD_FNAME = 'auto_casadi_grad.c' @@ -920,4 +920,11 @@ def build(self): self.__solver_config) ros_builder.build() + if self.__build_config.ros2_config is not None: + ros2_builder = ROS2Builder( + self.__meta, + self.__build_config, + self.__solver_config) + ros2_builder.build() + return self.__info() diff --git a/open-codegen/opengen/builder/ros_builder.py b/open-codegen/opengen/builder/ros_builder.py index 0108a8a5..7db3aeb2 100644 --- a/open-codegen/opengen/builder/ros_builder.py +++ b/open-codegen/opengen/builder/ros_builder.py @@ -1,222 +1,327 @@ +"""Builders for auto-generated ROS1 and ROS2 package wrappers.""" + import opengen.definitions as og_dfn -import os +import datetime import logging -import jinja2 +import os import shutil -import datetime -_ROS_PREFIX = 'ros_node_' +import jinja2 def make_dir_if_not_exists(directory): + """Create ``directory`` if it does not already exist. + + :param directory: Path to the directory to create. + :type directory: str + """ if not os.path.exists(directory): os.makedirs(directory) -def get_template(name): - file_loader = jinja2.FileSystemLoader(og_dfn.templates_dir()) - env = jinja2.Environment(loader=file_loader, autoescape=True) - return env.get_template(name) +def get_ros_template(template_subdir, name): + """Load a Jinja template from a ROS-specific template subdirectory. + :param template_subdir: Template subdirectory name, e.g. ``"ros"`` or + ``"ros2"``. + :type template_subdir: str + :param name: Template file name. + :type name: str -def get_ros_template(name): - file_loader = jinja2.FileSystemLoader(og_dfn.templates_subdir('ros')) + :return: Loaded Jinja template. + :rtype: jinja2.Template + """ + file_loader = jinja2.FileSystemLoader(og_dfn.templates_subdir(template_subdir)) env = jinja2.Environment(loader=file_loader, autoescape=True) return env.get_template(name) -class RosBuilder: +class _BaseRosBuilder: """ - Code generation for ROS-related files + Shared code generation logic for ROS-related packages. - For internal use + This base class contains the common file-generation pipeline used by both + :class:`RosBuilder` and :class:`ROS2Builder`. Subclasses specialize the + process by providing the package configuration object, template + subdirectory, launch file name, and final user-facing instructions. + + :ivar _meta: Optimizer metadata used to render the package templates. + :ivar _build_config: Global build configuration for the generated solver. + :ivar _solver_config: Solver configuration used when rendering node code. + :ivar _logger: Logger dedicated to the concrete builder implementation. """ + #: Template subdirectory under ``opengen/templates`` used by the builder. + _template_subdir = None + #: Fully-qualified logger name for the concrete builder. + _logger_name = None + #: Short logger tag shown in log messages. + _logger_tag = None + #: Launch file generated by the concrete builder. + _launch_file_name = None + def __init__(self, meta, build_config, solver_config): - self.__meta = meta - self.__build_config = build_config - self.__solver_config = solver_config - self.__logger = logging.getLogger('opengen.builder.RosBuilder') + """Initialise a shared ROS package builder. + + :param meta: Optimizer metadata. + :param build_config: Build configuration object. + :param solver_config: Solver configuration object. + """ + self._meta = meta + self._build_config = build_config + self._solver_config = solver_config + self._logger = logging.getLogger(self._logger_name) stream_handler = logging.StreamHandler() stream_handler.setLevel(1) - c_format = logging.Formatter('[%(levelname)s] <> %(message)s') + c_format = logging.Formatter( + f'[%(levelname)s] <<{self._logger_tag}>> %(message)s') stream_handler.setFormatter(c_format) - self.__logger.setLevel(1) - self.__logger.addHandler(stream_handler) + self._logger.setLevel(1) + self._logger.handlers.clear() + self._logger.addHandler(stream_handler) + self._logger.propagate = False + + @property + def _ros_config(self): + """Return the ROS/ROS2 package configuration for the subclass. + + :return: ROS configuration object used by the concrete builder. + :raises NotImplementedError: If a subclass does not provide this hook. + """ + raise NotImplementedError + + def _template(self, name): + """Return a template from the builder's template subdirectory. + + :param name: Template file name. + :type name: str + + :return: Loaded Jinja template. + :rtype: jinja2.Template + """ + return get_ros_template(self._template_subdir, name) + + def _target_dir(self): + """Return the root directory of the generated optimizer project. - def __target_dir(self): + :return: Absolute path to the generated optimizer directory. + :rtype: str + """ return os.path.abspath( os.path.join( - self.__build_config.build_dir, - self.__meta.optimizer_name)) + self._build_config.build_dir, + self._meta.optimizer_name)) + + def _ros_target_dir(self): + """Return the root directory of the generated ROS package. - def __ros_target_dir(self): - ros_config = self.__build_config.ros_config - ros_target_dir_name = ros_config.package_name + :return: Absolute path to the generated ROS/ROS2 package directory. + :rtype: str + """ return os.path.abspath( os.path.join( - self.__build_config.build_dir, - self.__meta.optimizer_name, ros_target_dir_name)) + self._build_config.build_dir, + self._meta.optimizer_name, + self._ros_config.package_name)) - def __generate_ros_dir_structure(self): - self.__logger.info("Generating directory structure") - target_ros_dir = self.__ros_target_dir() + def _generate_ros_dir_structure(self): + """Create the directory structure for the generated ROS package.""" + self._logger.info("Generating directory structure") + target_ros_dir = self._ros_target_dir() make_dir_if_not_exists(target_ros_dir) - make_dir_if_not_exists(os.path.abspath( - os.path.join(target_ros_dir, 'include'))) - make_dir_if_not_exists(os.path.abspath( - os.path.join(target_ros_dir, 'extern_lib'))) - make_dir_if_not_exists(os.path.abspath( - os.path.join(target_ros_dir, 'src'))) - make_dir_if_not_exists(os.path.abspath( - os.path.join(target_ros_dir, 'msg'))) - make_dir_if_not_exists(os.path.abspath( - os.path.join(target_ros_dir, 'config'))) - make_dir_if_not_exists(os.path.abspath( - os.path.join(target_ros_dir, 'launch'))) - - def __generate_ros_package_xml(self): - self.__logger.info("Generating package.xml") - target_ros_dir = self.__ros_target_dir() - template = get_ros_template('package.xml') - output_template = template.render( - meta=self.__meta, ros=self.__build_config.ros_config) + for directory_name in ('include', 'extern_lib', 'src', 'msg', 'config', 'launch'): + make_dir_if_not_exists(os.path.abspath( + os.path.join(target_ros_dir, directory_name))) + + def _generate_ros_package_xml(self): + """Render and write ``package.xml`` for the generated package.""" + self._logger.info("Generating package.xml") + target_ros_dir = self._ros_target_dir() + template = self._template('package.xml') + output_template = template.render(meta=self._meta, ros=self._ros_config) target_rospkg_path = os.path.join(target_ros_dir, "package.xml") with open(target_rospkg_path, "w") as fh: fh.write(output_template) - def __generate_ros_cmakelists(self): - self.__logger.info("Generating CMakeLists") - target_ros_dir = self.__ros_target_dir() - template = get_ros_template('CMakeLists.txt') - output_template = template.render(meta=self.__meta, - ros=self.__build_config.ros_config) + def _generate_ros_cmakelists(self): + """Render and write the package ``CMakeLists.txt`` file.""" + self._logger.info("Generating CMakeLists") + target_ros_dir = self._ros_target_dir() + template = self._template('CMakeLists.txt') + output_template = template.render(meta=self._meta, ros=self._ros_config) target_rospkg_path = os.path.join(target_ros_dir, "CMakeLists.txt") with open(target_rospkg_path, "w") as fh: fh.write(output_template) - def __copy__ros_files(self): - self.__logger.info("Copying external dependencies") - # 1. --- copy header file - target_ros_dir = self.__ros_target_dir() - header_file_name = self.__meta.optimizer_name + '_bindings.hpp' + def _copy_ros_files(self): + """Copy generated bindings, static library, and message files.""" + self._logger.info("Copying external dependencies") + target_ros_dir = self._ros_target_dir() + + header_file_name = self._meta.optimizer_name + '_bindings.hpp' target_include_filename = os.path.abspath( - os.path.join( - target_ros_dir, 'include', header_file_name)) + os.path.join(target_ros_dir, 'include', header_file_name)) original_include_file = os.path.abspath( - os.path.join(self.__target_dir(), header_file_name)) + os.path.join(self._target_dir(), header_file_name)) shutil.copyfile(original_include_file, target_include_filename) - # 2. --- copy library file - lib_file_name = 'lib' + self.__meta.optimizer_name + '.a' - target_lib_file_name = \ - os.path.abspath( - os.path.join( - target_ros_dir, 'extern_lib', lib_file_name)) + lib_file_name = 'lib' + self._meta.optimizer_name + '.a' + target_lib_file_name = os.path.abspath( + os.path.join(target_ros_dir, 'extern_lib', lib_file_name)) original_lib_file = os.path.abspath( os.path.join( - self.__target_dir(), + self._target_dir(), 'target', - self.__build_config.build_mode, + self._build_config.build_mode, lib_file_name)) shutil.copyfile(original_lib_file, target_lib_file_name) - # 3. --- copy msg file OptimizationParameters.msg - original_params_msg = os.path.abspath( - os.path.join( - og_dfn.templates_dir(), 'ros', 'OptimizationParameters.msg')) - target_params_msg = \ - os.path.abspath( + for message_name in ('OptimizationParameters.msg', 'OptimizationResult.msg'): + original_message = os.path.abspath( os.path.join( - target_ros_dir, 'msg', 'OptimizationParameters.msg')) - shutil.copyfile(original_params_msg, target_params_msg) + og_dfn.templates_dir(), + self._template_subdir, + message_name)) + target_message = os.path.abspath( + os.path.join(target_ros_dir, 'msg', message_name)) + shutil.copyfile(original_message, target_message) - # 4. --- copy msg file OptimizationResult.msg - original_result_msg = os.path.abspath( - os.path.join( - og_dfn.templates_dir(), 'ros', 'OptimizationResult.msg')) - target_result_msg = \ - os.path.abspath( - os.path.join( - target_ros_dir, 'msg', 'OptimizationResult.msg')) - shutil.copyfile(original_result_msg, target_result_msg) - - def __generate_ros_params_file(self): - self.__logger.info("Generating open_params.yaml") - target_ros_dir = self.__ros_target_dir() - template = get_ros_template('open_params.yaml') - output_template = template.render(meta=self.__meta, - ros=self.__build_config.ros_config) - target_yaml_fname \ - = os.path.join(target_ros_dir, "config", "open_params.yaml") + def _generate_ros_params_file(self): + """Render and write the runtime parameter YAML file.""" + self._logger.info("Generating open_params.yaml") + target_ros_dir = self._ros_target_dir() + template = self._template('open_params.yaml') + output_template = template.render(meta=self._meta, ros=self._ros_config) + target_yaml_fname = os.path.join(target_ros_dir, "config", "open_params.yaml") with open(target_yaml_fname, "w") as fh: fh.write(output_template) - def __generate_ros_node_header(self): - self.__logger.info("Generating open_optimizer.hpp") - target_ros_dir = self.__ros_target_dir() - template = get_ros_template('open_optimizer.hpp') - output_template = template.render(meta=self.__meta, - ros=self.__build_config.ros_config, - solver_config=self.__solver_config) - target_rosnode_header_path \ - = os.path.join(target_ros_dir, "include", "open_optimizer.hpp") + def _generate_ros_node_header(self): + """Render and write the generated node header file.""" + self._logger.info("Generating open_optimizer.hpp") + target_ros_dir = self._ros_target_dir() + template = self._template('open_optimizer.hpp') + output_template = template.render( + meta=self._meta, + ros=self._ros_config, + solver_config=self._solver_config) + target_rosnode_header_path = os.path.join( + target_ros_dir, "include", "open_optimizer.hpp") with open(target_rosnode_header_path, "w") as fh: fh.write(output_template) - def __generate_ros_node_cpp(self): - self.__logger.info("Generating open_optimizer.cpp") - target_ros_dir = self.__ros_target_dir() - template = get_ros_template('open_optimizer.cpp') - output_template = template.render(meta=self.__meta, - ros=self.__build_config.ros_config, - timestamp_created=datetime.datetime.now()) - target_rosnode_cpp_path \ - = os.path.join(target_ros_dir, "src", "open_optimizer.cpp") + def _generate_ros_node_cpp(self): + """Render and write the generated node implementation file.""" + self._logger.info("Generating open_optimizer.cpp") + target_ros_dir = self._ros_target_dir() + template = self._template('open_optimizer.cpp') + output_template = template.render( + meta=self._meta, + ros=self._ros_config, + timestamp_created=datetime.datetime.now()) + target_rosnode_cpp_path = os.path.join(target_ros_dir, "src", "open_optimizer.cpp") with open(target_rosnode_cpp_path, "w") as fh: fh.write(output_template) - def __generate_ros_launch_file(self): - self.__logger.info("Generating open_optimizer.launch") - target_ros_dir = self.__ros_target_dir() - template = get_ros_template('open_optimizer.launch') - output_template = template.render(meta=self.__meta, - ros=self.__build_config.ros_config) - target_rosnode_launch_path \ - = os.path.join(target_ros_dir, "launch", "open_optimizer.launch") + def _generate_ros_launch_file(self): + """Render and write the package launch file.""" + self._logger.info("Generating %s", self._launch_file_name) + target_ros_dir = self._ros_target_dir() + template = self._template(self._launch_file_name) + output_template = template.render(meta=self._meta, ros=self._ros_config) + target_rosnode_launch_path = os.path.join( + target_ros_dir, "launch", self._launch_file_name) with open(target_rosnode_launch_path, "w") as fh: fh.write(output_template) - def __generate_ros_readme_file(self): - self.__logger.info("Generating README.md") - target_ros_dir = self.__ros_target_dir() - template = get_ros_template('README.md') - output_template = template.render( - ros=self.__build_config.ros_config) - target_readme_path \ - = os.path.join(target_ros_dir, "README.md") + def _generate_ros_readme_file(self): + """Render and write the generated package README.""" + self._logger.info("Generating README.md") + target_ros_dir = self._ros_target_dir() + template = self._template('README.md') + output_template = template.render(ros=self._ros_config) + target_readme_path = os.path.join(target_ros_dir, "README.md") with open(target_readme_path, "w") as fh: fh.write(output_template) - def __symbolic_link_info_message(self): - target_ros_dir = self.__ros_target_dir() - self.__logger.info("ROS package was built successfully. Now run:") - self.__logger.info("ln -s %s ~/catkin_ws/src/", target_ros_dir) - self.__logger.info("cd ~/catkin_ws/; catkin_make") + def _symbolic_link_info_message(self): + """Emit final user-facing setup instructions for the generated package. + + :raises NotImplementedError: If a subclass does not provide this hook. + """ + raise NotImplementedError def build(self): """ - Build ROS-related files + Generate all ROS/ROS2 wrapper files for the current optimizer. + + This method creates the package directory structure, copies the + generated solver artefacts, renders all templates, and logs final setup + instructions for the user. """ - self.__generate_ros_dir_structure() # generate necessary folders - self.__generate_ros_package_xml() # generate package.xml - self.__generate_ros_cmakelists() # generate CMakeLists.txt - self.__copy__ros_files() # Copy certain files - # # - C++ bindings, library, msg - self.__generate_ros_params_file() # generate params file - self.__generate_ros_node_header() # generate node .hpp file - self.__generate_ros_node_cpp() # generate main node .cpp file - self.__generate_ros_launch_file() # generate launch file - self.__generate_ros_readme_file() # final touch: create README.md - self.__symbolic_link_info_message() # Info: create symbolic link + self._generate_ros_dir_structure() + self._generate_ros_package_xml() + self._generate_ros_cmakelists() + self._copy_ros_files() + self._generate_ros_params_file() + self._generate_ros_node_header() + self._generate_ros_node_cpp() + self._generate_ros_launch_file() + self._generate_ros_readme_file() + self._symbolic_link_info_message() + + +class RosBuilder(_BaseRosBuilder): + """ + Builder for ROS1 package generation. + + This specialization uses the ``templates/ros`` template set and the + ROS1-specific configuration stored in + :attr:`opengen.config.build_config.BuildConfiguration.ros_config`. + """ + + _template_subdir = 'ros' + _logger_name = 'opengen.builder.RosBuilder' + _logger_tag = 'ROS' + _launch_file_name = 'open_optimizer.launch' + + @property + def _ros_config(self): + """Return the ROS1 package configuration.""" + return self._build_config.ros_config + + def _symbolic_link_info_message(self): + """Log the final ROS1 workspace integration instructions.""" + target_ros_dir = self._ros_target_dir() + self._logger.info("ROS package was built successfully. Now run:") + self._logger.info("ln -s %s ~/catkin_ws/src/", target_ros_dir) + self._logger.info("cd ~/catkin_ws/; catkin_make") + + +class ROS2Builder(_BaseRosBuilder): + """ + Builder for ROS2 package generation. + + This specialization uses the ``templates/ros2`` template set and the + ROS2-specific configuration stored in + :attr:`opengen.config.build_config.BuildConfiguration.ros2_config`. + """ + + _template_subdir = 'ros2' + _logger_name = 'opengen.builder.ROS2Builder' + _logger_tag = 'ROS2' + _launch_file_name = 'open_optimizer.launch.py' + + @property + def _ros_config(self): + """Return the ROS2 package configuration.""" + return self._build_config.ros2_config + + def _symbolic_link_info_message(self): + """Log the final ROS2 workspace integration instructions.""" + target_ros_dir = self._ros_target_dir() + self._logger.info("ROS2 package was built successfully. Now run:") + self._logger.info("ln -s %s ~/ros2_ws/src/", target_ros_dir) + self._logger.info("cd ~/ros2_ws/; colcon build --packages-select %s", + self._ros_config.package_name) diff --git a/open-codegen/opengen/config/build_config.py b/open-codegen/opengen/config/build_config.py index 939150d1..41d6f64f 100644 --- a/open-codegen/opengen/config/build_config.py +++ b/open-codegen/opengen/config/build_config.py @@ -57,6 +57,7 @@ def __init__(self, build_dir="."): self.__build_c_bindings = False self.__build_python_bindings = False self.__ros_config = None + self.__ros2_config = None self.__tcp_interface_config = None self.__local_path = None self.__allocator = RustAllocator.DefaultAllocator @@ -135,6 +136,14 @@ def ros_config(self) -> RosConfiguration: """ return self.__ros_config + @property + def ros2_config(self) -> RosConfiguration: + """ROS2 package configuration + + :return: instance of RosConfiguration + """ + return self.__ros2_config + @property def allocator(self) -> RustAllocator: """ @@ -257,6 +266,21 @@ def with_ros(self, ros_config: RosConfiguration): """ self.__build_c_bindings = True # no C++ bindings, no ROS package mate self.__ros_config = ros_config + self.__ros2_config = None + return self + + def with_ros2(self, ros_config: RosConfiguration): + """ + Activates the generation of a ROS2 package. The caller must provide an + instance of RosConfiguration + + :param ros_config: Configuration of ROS2 package + + :return: current instance of BuildConfiguration + """ + self.__build_c_bindings = True # no C++ bindings, no ROS package + self.__ros2_config = ros_config + self.__ros_config = None return self def with_tcp_interface_config(self, tcp_interface_config=TcpServerConfiguration()): @@ -300,4 +324,6 @@ def to_dict(self): build_dict["tcp_interface_config"] = self.__tcp_interface_config.to_dict() if self.__ros_config is not None: build_dict["ros_config"] = self.__ros_config.to_dict() + if self.__ros2_config is not None: + build_dict["ros2_config"] = self.__ros2_config.to_dict() return build_dict diff --git a/open-codegen/opengen/config/ros_config.py b/open-codegen/opengen/config/ros_config.py index 206051c1..4e0a6c91 100644 --- a/open-codegen/opengen/config/ros_config.py +++ b/open-codegen/opengen/config/ros_config.py @@ -3,7 +3,7 @@ class RosConfiguration: """ - Configuration of auto-generated ROS package + Configuration of an auto-generated ROS or ROS2 package """ def __init__(self): @@ -61,7 +61,7 @@ def description(self): @property def rate(self): - """ROS node rate in Hz + """ROS/ROS2 node rate in Hz :return: rate, defaults to `10.0` """ @@ -87,7 +87,7 @@ def params_topic_queue_size(self): def with_package_name(self, pkg_name): """ Set the package name, which is the same as the name - of the folder that will store the auto-generated ROS node. + of the folder that will store the auto-generated ROS/ROS2 node. The node name can contain lowercase and uppercase characters and underscores, but not spaces or other symbols @@ -124,6 +124,7 @@ def with_node_name(self, node_name): def with_rate(self, rate): """ Set the rate of the ROS node + or ROS2 node :param rate: rate in Hz :type rate: float @@ -135,7 +136,7 @@ def with_rate(self, rate): def with_description(self, description): """ - Set the description of the ROS package + Set the description of the ROS or ROS2 package :param description: description, defaults to "parametric optimization with OpEn" :type description: string @@ -149,7 +150,7 @@ def with_queue_sizes(self, result_topic_queue_size=100, parameter_topic_queue_size=100): """ - Set queue sizes for ROS node + Set queue sizes for ROS or ROS2 node :param result_topic_queue_size: queue size of results, defaults to 100 :type result_topic_queue_size: int, optional diff --git a/open-codegen/opengen/templates/ros/open_optimizer.cpp b/open-codegen/opengen/templates/ros/open_optimizer.cpp index ad9b4f1b..542fb3c9 100644 --- a/open-codegen/opengen/templates/ros/open_optimizer.cpp +++ b/open-codegen/opengen/templates/ros/open_optimizer.cpp @@ -1,6 +1,7 @@ /** * This is an auto-generated file by Optimization Engine (OpEn) - * OpEn is a free open-source software - see doc.optimization-engine.xyz + * OpEn is a free open-source software - + * see https://alphaville.github.io/optimization-engine * dually licensed under the MIT and Apache v2 licences. * */ diff --git a/open-codegen/opengen/templates/ros2/CMakeLists.txt b/open-codegen/opengen/templates/ros2/CMakeLists.txt new file mode 100644 index 00000000..10d54220 --- /dev/null +++ b/open-codegen/opengen/templates/ros2/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.8) +project({{ros.package_name}}) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) + +# tells CMake's FindPython3 to prefer a venv if one is active +# (instead of the system-wide python) +set(Python3_FIND_VIRTUALENV FIRST) +if(NOT Python3_EXECUTABLE AND DEFINED ENV{VIRTUAL_ENV}) + set(_open_python3_executable "$ENV{VIRTUAL_ENV}/bin/python") + if(EXISTS "${_open_python3_executable}") + set(Python3_EXECUTABLE "${_open_python3_executable}") + endif() +endif() +find_package(Python3 REQUIRED COMPONENTS Interpreter Development NumPy) +set(Python_EXECUTABLE ${Python3_EXECUTABLE}) +set(Python_INCLUDE_DIRS ${Python3_INCLUDE_DIRS}) +set(Python_LIBRARIES ${Python3_LIBRARIES}) +set(Python_NumPy_INCLUDE_DIRS ${Python3_NumPy_INCLUDE_DIRS}) +find_package(rosidl_default_generators REQUIRED) + +set(msg_files + "msg/OptimizationResult.msg" + "msg/OptimizationParameters.msg" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} +) + +ament_export_dependencies(rosidl_default_runtime) + +include_directories( + ${PROJECT_SOURCE_DIR}/include +) + +set(NODE_NAME {{ros.node_name}}) +add_executable(${NODE_NAME} src/open_optimizer.cpp) +ament_target_dependencies(${NODE_NAME} rclcpp) +target_link_libraries( + ${NODE_NAME} + ${PROJECT_SOURCE_DIR}/extern_lib/lib{{meta.optimizer_name}}.a + m + dl +) +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(${NODE_NAME} "${cpp_typesupport_target}") + +install(TARGETS + ${NODE_NAME} + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/open-codegen/opengen/templates/ros2/OptimizationParameters.msg b/open-codegen/opengen/templates/ros2/OptimizationParameters.msg new file mode 100644 index 00000000..870d2981 --- /dev/null +++ b/open-codegen/opengen/templates/ros2/OptimizationParameters.msg @@ -0,0 +1,4 @@ +float64[] parameter # parameter p (mandatory) +float64[] initial_guess # u0 (optional/recommended) +float64[] initial_y # y0 (optional) +float64 initial_penalty # initial penalty (optional) diff --git a/open-codegen/opengen/templates/ros2/OptimizationResult.msg b/open-codegen/opengen/templates/ros2/OptimizationResult.msg new file mode 100644 index 00000000..890e0c23 --- /dev/null +++ b/open-codegen/opengen/templates/ros2/OptimizationResult.msg @@ -0,0 +1,18 @@ +# Constants match the enumeration of status codes +uint8 STATUS_CONVERGED=0 +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 + +float64[] solution # optimizer (solution) +uint8 inner_iterations # number of inner iterations +uint16 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 +float64 penalty # penalty value +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/open-codegen/opengen/templates/ros2/README.md b/open-codegen/opengen/templates/ros2/README.md new file mode 100644 index 00000000..5862a1f0 --- /dev/null +++ b/open-codegen/opengen/templates/ros2/README.md @@ -0,0 +1,142 @@ +# ROS2 Package: {{ros.package_name}} + + +## Installation and Setup + +Move or link the auto-generated ROS2 package (folder `{{ros.package_name}}`) to your workspace source tree (typically `~/ros2_ws/src/`). + +From within the folder `{{ros.package_name}}`, compile with: + +```bash +colcon build --packages-select {{ros.package_name}} +source install/setup.bash +# or source install/setup.zsh if you are using zsh +``` + +If you want to activate logging (recommended), do + +```bash +mkdir -p .ros_log +export ROS_LOG_DIR="$PWD/.ros_log" +``` + + +## Launch and Use + +Start the optimizer in one terminal. The process stays in the foreground while +the node is running. + +```bash +# Terminal 1 +source install/setup.bash +# or: source install/setup.zsh +ros2 run {{ros.package_name}} {{ros.node_name}} +``` + +If ROS2 cannot write to its default log directory, set an explicit writable log +path: + +```bash +mkdir -p .ros_log +export ROS_LOG_DIR="$PWD/.ros_log" +``` + +If the node starts but does not appear in the ROS2 graph, try forcing Fast DDS +in both terminals before sourcing the generated workspace and running any +`ros2` commands: + +```bash +export RMW_IMPLEMENTATION=rmw_fastrtps_cpp +``` + +In a second terminal, source the same environment and verify discovery: + +```bash +# Terminal 2 +source install/setup.bash +# or: source install/setup.zsh +ros2 node list --no-daemon --spin-time 5 +ros2 topic list --no-daemon --spin-time 5 +``` + +You should see the node `/{{ros.node_name}}`, the input topic +`/{{ros.subscriber_subtopic}}`, and the output topic +`/{{ros.publisher_subtopic}}`. + +Then publish a request to the configured parameters topic +(default: `/{{ros.subscriber_subtopic}}`): + +```bash +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}" +``` + +The result will be announced on the configured result topic +(default: `/{{ros.publisher_subtopic}}`): + +```bash +ros2 topic echo /{{ros.publisher_subtopic}} --once +``` + +To get the optimal solution you can do: + +```bash +ros2 topic echo /{{ros.publisher_subtopic}} --field solution +``` + +You can also start the node using the generated launch file: + +```bash +ros2 launch {{ros.package_name}} open_optimizer.launch.py +``` + +The launch file loads its runtime parameters from +[`config/open_params.yaml`](config/open_params.yaml). + + +## Messages + +This package involves two messages: `OptimizationParameters` +and `OptimizationResult`, which are used to define the input +and output values to the node. `OptimizationParameters` specifies +the parameter vector, the initial guess (optional), the initial +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. + +The message structures are defined in the following msg files: + +- [`OptimizationParameters.msg`](msg/OptimizationParameters.msg) +- [`OptimizationResult.msg`](msg/OptimizationResult.msg) + + +## Configure + +You can configure the rate and topic names by editing +[`config/open_params.yaml`](config/open_params.yaml). + + +## Directory structure and contents + +The following auto-generated files are included in your ROS2 package: + +```txt +├── CMakeLists.txt +├── config +│   └── open_params.yaml +├── extern_lib +│   └── librosenbrock.a +├── include +│   ├── open_optimizer.hpp +│   └── rosenbrock_bindings.hpp +├── launch +│   └── open_optimizer.launch.py +├── msg +│   ├── OptimizationParameters.msg +│   └── OptimizationResult.msg +├── package.xml +├── README.md +└── src + └── open_optimizer.cpp +``` diff --git a/open-codegen/opengen/templates/ros2/open_optimizer.cpp b/open-codegen/opengen/templates/ros2/open_optimizer.cpp new file mode 100644 index 00000000..e7c718f5 --- /dev/null +++ b/open-codegen/opengen/templates/ros2/open_optimizer.cpp @@ -0,0 +1,181 @@ +/** + * This is an auto-generated file by Optimization Engine (OpEn) + * OpEn is a free open-source software - see doc.optimization-engine.xyz + * dually licensed under the MIT and Apache v2 licences. + * + */ +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "{{ros.package_name}}/msg/optimization_parameters.hpp" +#include "{{ros.package_name}}/msg/optimization_result.hpp" +#include "{{meta.optimizer_name}}_bindings.hpp" +#include "open_optimizer.hpp" + +namespace {{ros.package_name}} { +class OptimizationEngineNode : public rclcpp::Node { +private: + using OptimizationParametersMsg = {{ros.package_name}}::msg::OptimizationParameters; + using OptimizationResultMsg = {{ros.package_name}}::msg::OptimizationResult; + + OptimizationParametersMsg params_; + OptimizationResultMsg results_; + bool has_received_request_ = false; + double p_[{{meta.optimizer_name|upper}}_NUM_PARAMETERS] = { 0 }; + double u_[{{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES] = { 0 }; + double* y_ = nullptr; + {{meta.optimizer_name}}Cache* cache_ = nullptr; + double init_penalty_ = ROS2_NODE_{{meta.optimizer_name|upper}}_DEFAULT_INITIAL_PENALTY; + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Subscription::SharedPtr subscriber_; + rclcpp::TimerBase::SharedPtr timer_; + + static std::chrono::milliseconds rateToPeriod(double rate) + { + if (rate <= 0.0) { + return std::chrono::milliseconds(100); + } + int period_ms = static_cast(1000.0 / rate); + if (period_ms < 1) { + period_ms = 1; + } + return std::chrono::milliseconds(period_ms); + } + + void updateInputData() + { + 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_.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.size() == {{meta.optimizer_name|upper}}_N1) { + for (size_t i = 0; i < {{meta.optimizer_name|upper}}_N1; ++i) { + y_[i] = params_.initial_y[i]; + } + } + } + + {{meta.optimizer_name}}SolverStatus solve() + { + return {{meta.optimizer_name}}_solve(cache_, u_, p_, y_, &init_penalty_); + } + + void initializeSolverIfNeeded() + { + if (y_ == nullptr) { + y_ = new double[{{meta.optimizer_name|upper}}_N1](); + } + if (cache_ == nullptr) { + cache_ = {{meta.optimizer_name}}_new(); + } + } + + void updateResults({{meta.optimizer_name}}SolverStatus& status) + { + results_.solution.clear(); + for (size_t i = 0; i < {{meta.optimizer_name|upper}}_NUM_DECISION_VARIABLES; ++i) { + results_.solution.push_back(u_[i]); + } + + results_.lagrange_multipliers.clear(); + for (size_t i = 0; i < {{meta.optimizer_name|upper}}_N1; ++i) { + results_.lagrange_multipliers.push_back(status.lagrange[i]); + } + + results_.inner_iterations = status.num_inner_iterations; + results_.outer_iterations = status.num_outer_iterations; + results_.norm_fpr = status.last_problem_norm_fpr; + results_.cost = status.cost; + results_.penalty = status.penalty; + results_.status = static_cast(status.exit_status); + 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; + } + + void receiveRequestCallback(const OptimizationParametersMsg::ConstSharedPtr msg) + { + params_ = *msg; + has_received_request_ = true; + } + + void solveAndPublish() + { + if (!has_received_request_) { + return; + } + initializeSolverIfNeeded(); + updateInputData(); + {{meta.optimizer_name}}SolverStatus status = solve(); + updateResults(status); + publisher_->publish(results_); + } + +public: + OptimizationEngineNode() + : Node(ROS2_NODE_{{meta.optimizer_name|upper}}_NODE_NAME) + { + this->declare_parameter( + "result_topic", + std::string(ROS2_NODE_{{meta.optimizer_name|upper}}_RESULT_TOPIC)); + this->declare_parameter( + "params_topic", + std::string(ROS2_NODE_{{meta.optimizer_name|upper}}_PARAMS_TOPIC)); + this->declare_parameter( + "rate", + double(ROS2_NODE_{{meta.optimizer_name|upper}}_RATE)); + + std::string result_topic = this->get_parameter("result_topic").as_string(); + std::string params_topic = this->get_parameter("params_topic").as_string(); + double rate = this->get_parameter("rate").as_double(); + + publisher_ = this->create_publisher( + result_topic, + ROS2_NODE_{{meta.optimizer_name|upper}}_RESULT_TOPIC_QUEUE_SIZE); + subscriber_ = this->create_subscription( + params_topic, + ROS2_NODE_{{meta.optimizer_name|upper}}_PARAMS_TOPIC_QUEUE_SIZE, + std::bind(&OptimizationEngineNode::receiveRequestCallback, this, std::placeholders::_1)); + timer_ = this->create_wall_timer( + rateToPeriod(rate), + std::bind(&OptimizationEngineNode::solveAndPublish, this)); + } + + ~OptimizationEngineNode() override + { + if (y_ != nullptr) { + delete[] y_; + } + if (cache_ != nullptr) { + {{meta.optimizer_name}}_free(cache_); + } + } +}; +} /* end of namespace {{ros.package_name}} */ + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared<{{ros.package_name}}::OptimizationEngineNode>(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/open-codegen/opengen/templates/ros2/open_optimizer.hpp b/open-codegen/opengen/templates/ros2/open_optimizer.hpp new file mode 100644 index 00000000..a8482fd2 --- /dev/null +++ b/open-codegen/opengen/templates/ros2/open_optimizer.hpp @@ -0,0 +1,40 @@ +#ifndef ROS2_NODE_{{meta.optimizer_name|upper}}_H +#define ROS2_NODE_{{meta.optimizer_name|upper}}_H + +/** + * Default node name + */ +#define ROS2_NODE_{{meta.optimizer_name|upper}}_NODE_NAME "{{ros.node_name}}" + +/** + * Default result (publisher) topic name + */ +#define ROS2_NODE_{{meta.optimizer_name|upper}}_RESULT_TOPIC "{{ros.publisher_subtopic}}" + +/** + * Default parameters (subscriber) topic name + */ +#define ROS2_NODE_{{meta.optimizer_name|upper}}_PARAMS_TOPIC "{{ros.subscriber_subtopic}}" + +/** + * Default execution rate (in Hz) + */ +#define ROS2_NODE_{{meta.optimizer_name|upper}}_RATE {{ros.rate}} + +/** + * Default result topic queue size + */ +#define ROS2_NODE_{{meta.optimizer_name|upper}}_RESULT_TOPIC_QUEUE_SIZE {{ros.result_topic_queue_size}} + +/** + * Default parameters topic queue size + */ +#define ROS2_NODE_{{meta.optimizer_name|upper}}_PARAMS_TOPIC_QUEUE_SIZE {{ros.params_topic_queue_size}} + +/** + * Default initial penalty + */ +#define ROS2_NODE_{{meta.optimizer_name|upper}}_DEFAULT_INITIAL_PENALTY {{solver_config.initial_penalty}} + + +#endif /* Header Sentinel: ROS2_NODE_{{meta.optimizer_name|upper}}_H */ diff --git a/open-codegen/opengen/templates/ros2/open_optimizer.launch.py b/open-codegen/opengen/templates/ros2/open_optimizer.launch.py new file mode 100644 index 00000000..45d7aa60 --- /dev/null +++ b/open-codegen/opengen/templates/ros2/open_optimizer.launch.py @@ -0,0 +1,20 @@ +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package="{{ros.package_name}}", + executable="{{ros.node_name}}", + name="{{ros.node_name}}", + output="screen", + parameters=[PathJoinSubstitution([ + FindPackageShare("{{ros.package_name}}"), + "config", + "open_params.yaml", + ])], + ) + ]) diff --git a/open-codegen/opengen/templates/ros2/open_params.yaml b/open-codegen/opengen/templates/ros2/open_params.yaml new file mode 100644 index 00000000..adde7b45 --- /dev/null +++ b/open-codegen/opengen/templates/ros2/open_params.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + result_topic: "{{ros.publisher_subtopic}}" + params_topic: "{{ros.subscriber_subtopic}}" + rate: {{ "%.1f"|format(ros.rate) if ros.rate == (ros.rate|int) else ros.rate }} diff --git a/open-codegen/opengen/templates/ros2/package.xml b/open-codegen/opengen/templates/ros2/package.xml new file mode 100644 index 00000000..c183538d --- /dev/null +++ b/open-codegen/opengen/templates/ros2/package.xml @@ -0,0 +1,24 @@ + + + {{ros.package_name}} + {{meta.version}} + {{ros.description}} + chung + {{meta.licence}} + + ament_cmake + + rosidl_default_generators + + launch + launch_ros + rclcpp + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/open-codegen/publish-pypi.sh b/open-codegen/publish-pypi.sh old mode 100644 new mode 100755 index 1feca85b..ddaf594d --- a/open-codegen/publish-pypi.sh +++ b/open-codegen/publish-pypi.sh @@ -4,9 +4,30 @@ set -eu # This script facilitates releasing a new version of opengen to PyPI. # It expects a local virtual environment at ./venv with publishing tools. -echo "[OpEnGen] Checking out master" -git checkout master -git pull origin master +version=$(cat VERSION) +current_branch=$(git rev-parse --abbrev-ref HEAD) + +is_alpha_version=false +case "$version" in + *a[0-9]*) + is_alpha_version=true + ;; +esac + +if [ "$current_branch" != "master" ] && [ "$is_alpha_version" = false ]; then + echo "[OpEnGen] Warning: version $version is not an alpha release and the current branch is '$current_branch' (not 'master')." + printf "Proceed anyway? [y/N] " + read -r response + case "$response" in + [yY][eE][sS]|[yY]) + echo "[OpEnGen] Proceeding from branch '$current_branch'" + ;; + *) + echo "[OpEnGen] Publish cancelled" + exit 0 + ;; + esac +fi echo "[OpEnGen] Cleaning previous build artifacts" rm -rf ./build ./dist ./opengen.egg-info @@ -23,7 +44,7 @@ python -m build echo "[OpEnGen] Checking distributions with twine" python -m twine check dist/* -echo "[OpEnGen] Uploading to PyPI..." +echo "[OpEnGen] You are about to publish version $version from branch '$current_branch'." printf "Are you sure? [y/N] " read -r response case "$response" in @@ -37,6 +58,5 @@ case "$response" in esac echo "[OpEnGen] Don't forget to create a tag; run:" -version=$(cat VERSION) echo "\$ git tag -a opengen-$version -m 'opengen-$version'" echo "\$ git push --tags" diff --git a/open-codegen/test/README.md b/open-codegen/test/README.md index 75cd8379..e946a477 100644 --- a/open-codegen/test/README.md +++ b/open-codegen/test/README.md @@ -47,5 +47,6 @@ The generated benchmark looks like this: Run ``` python -W ignore test/test_constraints.py -v +python -W ignore test/test_ros2.py -v python -W ignore test/test.py -v -``` \ No newline at end of file +``` diff --git a/open-codegen/test/test_ros2.py b/open-codegen/test/test_ros2.py new file mode 100644 index 00000000..9390ac8f --- /dev/null +++ b/open-codegen/test/test_ros2.py @@ -0,0 +1,486 @@ +import logging +import os +import re +import shlex +import signal +import shutil +import subprocess +import sys +import time +import unittest + +import casadi.casadi as cs +import opengen as og + + +class BuildConfigurationRos2TestCase(unittest.TestCase): + """Unit tests for ROS2-specific build configuration behavior.""" + + def test_with_ros2_sets_ros2_config_and_enables_c_bindings(self): + """`with_ros2` should store the ROS2 config and enable C bindings.""" + ros2_config = og.config.RosConfiguration().with_package_name("unit_test_ros2_pkg") + build_config = og.config.BuildConfiguration().with_ros2(ros2_config) + + self.assertIs(build_config.ros2_config, ros2_config) + self.assertIsNone(build_config.ros_config) + self.assertTrue(build_config.build_c_bindings) + + build_dict = build_config.to_dict() + self.assertIn("ros2_config", build_dict) + self.assertNotIn("ros_config", build_dict) + self.assertEqual("unit_test_ros2_pkg", build_dict["ros2_config"]["package_name"]) + + def test_ros_and_ros2_configs_clear_each_other(self): + """Selecting ROS1 or ROS2 should clear the other package configuration.""" + ros1_config = og.config.RosConfiguration().with_package_name("unit_test_ros_pkg") + ros2_config = og.config.RosConfiguration().with_package_name("unit_test_ros2_pkg") + build_config = og.config.BuildConfiguration() + + build_config.with_ros2(ros2_config) + self.assertIs(build_config.ros2_config, ros2_config) + self.assertIsNone(build_config.ros_config) + + build_config.with_ros(ros1_config) + self.assertIs(build_config.ros_config, ros1_config) + self.assertIsNone(build_config.ros2_config) + + build_config.with_ros2(ros2_config) + 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.""" + + TEST_DIR = ".python_test_build" + OPTIMIZER_NAME = "rosenbrock_ros2_custom" + PACKAGE_NAME = "custom_parametric_optimizer_ros2" + NODE_NAME = "custom_open_node_ros2" + DESCRIPTION = "custom ROS2 package for generation tests" + RESULT_TOPIC = "custom_result_topic" + PARAMS_TOPIC = "custom_params_topic" + RATE = 17.5 + RESULT_QUEUE_SIZE = 11 + PARAMS_QUEUE_SIZE = 13 + + @staticmethod + def get_open_local_absolute_path(): + """Return the absolute path to the local OpEn repository root.""" + cwd = os.getcwd() + return cwd.split('open-codegen')[0] + + @classmethod + def solverConfig(cls): + """Return a solver configuration shared by the ROS2 generation tests.""" + return Ros2BuildTestCase.solverConfig() + + @classmethod + def setUpCustomRos2PackageGeneration(cls): + """Generate a ROS2 package with non-default configuration values.""" + u = cs.MX.sym("u", 5) + p = cs.MX.sym("p", 2) + phi = og.functions.rosenbrock(u, p) + c = cs.vertcat(1.5 * u[0] - u[1], + cs.fmax(0.0, u[2] - u[3] + 0.1)) + bounds = og.constraints.Ball2(None, 1.5) + meta = og.config.OptimizerMeta() \ + .with_optimizer_name(cls.OPTIMIZER_NAME) + problem = og.builder.Problem(u, p, phi) \ + .with_constraints(bounds) \ + .with_penalty_constraints(c) + ros_config = og.config.RosConfiguration() \ + .with_package_name(cls.PACKAGE_NAME) \ + .with_node_name(cls.NODE_NAME) \ + .with_description(cls.DESCRIPTION) \ + .with_rate(cls.RATE) \ + .with_queue_sizes(cls.RESULT_QUEUE_SIZE, cls.PARAMS_QUEUE_SIZE) \ + .with_publisher_subtopic(cls.RESULT_TOPIC) \ + .with_subscriber_subtopic(cls.PARAMS_TOPIC) + build_config = og.config.BuildConfiguration() \ + .with_open_version(local_path=cls.get_open_local_absolute_path()) \ + .with_build_directory(cls.TEST_DIR) \ + .with_build_mode(og.config.BuildConfiguration.DEBUG_MODE) \ + .with_build_c_bindings() \ + .with_ros2(ros_config) + og.builder.OpEnOptimizerBuilder(problem, + metadata=meta, + build_configuration=build_config, + solver_configuration=cls.solverConfig()) \ + .build() + + @classmethod + def setUpClass(cls): + """Generate the custom ROS2 package once before running tests.""" + cls.setUpCustomRos2PackageGeneration() + + @classmethod + def ros2_package_dir(cls): + """Return the filesystem path to the generated custom ROS2 package.""" + return os.path.join( + cls.TEST_DIR, + cls.OPTIMIZER_NAME, + cls.PACKAGE_NAME) + + def test_custom_ros2_configuration_is_rendered_into_generated_files(self): + """Custom ROS2 config values should appear in the generated package files.""" + ros2_dir = self.ros2_package_dir() + + # The package metadata should reflect the user-provided ROS2 package name + # and description, not the defaults from the templates. + with open(os.path.join(ros2_dir, "package.xml"), encoding="utf-8") as f: + package_xml = f.read() + self.assertIn(f"{self.PACKAGE_NAME}", package_xml) + self.assertIn(f"{self.DESCRIPTION}", package_xml) + + # `open_optimizer.hpp` is where the generated node constants are wired in. + # These assertions make sure the custom topic names, node name, rate, and + # queue sizes are propagated into the generated C++ code. + with open(os.path.join(ros2_dir, "include", "open_optimizer.hpp"), encoding="utf-8") as f: + optimizer_header = f.read() + self.assertIn(f'#define ROS2_NODE_{self.OPTIMIZER_NAME.upper()}_NODE_NAME "{self.NODE_NAME}"', + optimizer_header) + self.assertIn(f'#define ROS2_NODE_{self.OPTIMIZER_NAME.upper()}_RESULT_TOPIC "{self.RESULT_TOPIC}"', + optimizer_header) + self.assertIn(f'#define ROS2_NODE_{self.OPTIMIZER_NAME.upper()}_PARAMS_TOPIC "{self.PARAMS_TOPIC}"', + optimizer_header) + self.assertIn(f"#define ROS2_NODE_{self.OPTIMIZER_NAME.upper()}_RATE {self.RATE}", + optimizer_header) + self.assertIn( + f"#define ROS2_NODE_{self.OPTIMIZER_NAME.upper()}_RESULT_TOPIC_QUEUE_SIZE {self.RESULT_QUEUE_SIZE}", + optimizer_header) + self.assertIn( + f"#define ROS2_NODE_{self.OPTIMIZER_NAME.upper()}_PARAMS_TOPIC_QUEUE_SIZE {self.PARAMS_QUEUE_SIZE}", + optimizer_header) + + # The runtime YAML configuration should carry the custom topic names and + # timer rate so the launched node uses the intended ROS2 parameters. + with open(os.path.join(ros2_dir, "config", "open_params.yaml"), encoding="utf-8") as f: + params_yaml = f.read() + self.assertIn(f'result_topic: "{self.RESULT_TOPIC}"', params_yaml) + self.assertIn(f'params_topic: "{self.PARAMS_TOPIC}"', params_yaml) + self.assertIn(f"rate: {self.RATE}", params_yaml) + + # The generated launch file should point to the correct package and + # executable so `ros2 launch` can start the generated node. + with open(os.path.join(ros2_dir, "launch", "open_optimizer.launch.py"), encoding="utf-8") as f: + launch_file = f.read() + self.assertIn(f'package="{self.PACKAGE_NAME}"', launch_file) + self.assertIn(f'executable="{self.NODE_NAME}"', launch_file) + self.assertIn(f'name="{self.NODE_NAME}"', launch_file) + self.assertIn(f'FindPackageShare("{self.PACKAGE_NAME}")', launch_file) + + +class Ros2BuildTestCase(unittest.TestCase): + """Integration tests for auto-generated ROS2 packages.""" + + TEST_DIR = ".python_test_build" + OPTIMIZER_NAME = "rosenbrock_ros2" + PACKAGE_NAME = "parametric_optimizer_ros2" + NODE_NAME = "open_node_ros2" + + @staticmethod + def get_open_local_absolute_path(): + """Return the absolute path to the local OpEn repository root.""" + cwd = os.getcwd() + return cwd.split('open-codegen')[0] + + @classmethod + def solverConfig(cls): + """Return a solver configuration shared by the ROS2 tests.""" + return og.config.SolverConfiguration() \ + .with_lbfgs_memory(15) \ + .with_tolerance(1e-4) \ + .with_initial_tolerance(1e-4) \ + .with_delta_tolerance(1e-4) \ + .with_initial_penalty(15.0) \ + .with_penalty_weight_update_factor(10.0) \ + .with_max_inner_iterations(155) \ + .with_max_duration_micros(1e8) \ + .with_max_outer_iterations(50) \ + .with_sufficient_decrease_coefficient(0.05) \ + .with_cbfgs_parameters(1.5, 1e-10, 1e-12) \ + .with_preconditioning(False) + + @classmethod + def setUpRos2PackageGeneration(cls): + """Generate the ROS2 package used by the ROS2 integration tests.""" + u = cs.MX.sym("u", 5) + p = cs.MX.sym("p", 2) + phi = og.functions.rosenbrock(u, p) + c = cs.vertcat(1.5 * u[0] - u[1], + cs.fmax(0.0, u[2] - u[3] + 0.1)) + bounds = og.constraints.Ball2(None, 1.5) + meta = og.config.OptimizerMeta() \ + .with_optimizer_name(cls.OPTIMIZER_NAME) + problem = og.builder.Problem(u, p, phi) \ + .with_constraints(bounds) \ + .with_penalty_constraints(c) + ros_config = og.config.RosConfiguration() \ + .with_package_name(cls.PACKAGE_NAME) \ + .with_node_name(cls.NODE_NAME) \ + .with_rate(35) \ + .with_description("really cool ROS2 node") + build_config = og.config.BuildConfiguration() \ + .with_open_version(local_path=cls.get_open_local_absolute_path()) \ + .with_build_directory(cls.TEST_DIR) \ + .with_build_mode(og.config.BuildConfiguration.DEBUG_MODE) \ + .with_build_c_bindings() \ + .with_ros2(ros_config) + og.builder.OpEnOptimizerBuilder(problem, + metadata=meta, + build_configuration=build_config, + solver_configuration=cls.solverConfig()) \ + .build() + + @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() + + @classmethod + def ros2_package_dir(cls): + """Return the filesystem path to the generated ROS2 package.""" + return os.path.join( + cls.TEST_DIR, + cls.OPTIMIZER_NAME, + cls.PACKAGE_NAME) + + @classmethod + def ros2_test_env(cls): + """Return the subprocess environment used by ROS2 integration tests.""" + env = os.environ.copy() + ros2_dir = cls.ros2_package_dir() + os.makedirs(os.path.join(ros2_dir, ".ros_log"), exist_ok=True) + # Keep ROS2 logs inside the generated package directory so the tests do + # not depend on a global writable log location. + env["ROS_LOG_DIR"] = os.path.join(ros2_dir, ".ros_log") + # Fast DDS is the most reliable middleware choice in our CI/local test + # setup when checking node discovery from separate processes. + env.setdefault("RMW_IMPLEMENTATION", "rmw_fastrtps_cpp") + env.pop("ROS_LOCALHOST_ONLY", None) + return env + + @classmethod + def ros2_shell(cls): + """Return the preferred shell executable and setup script for ROS2 commands.""" + shell_path = "/bin/bash" + setup_script = "install/setup.bash" + preferred_shell = os.path.basename(os.environ.get("SHELL", "")) + zsh_setup = os.path.join(cls.ros2_package_dir(), "install", "setup.zsh") + if preferred_shell == "zsh" and os.path.isfile(zsh_setup): + shell_path = "/bin/zsh" + setup_script = "install/setup.zsh" + return shell_path, setup_script + + @classmethod + 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() + result = subprocess.run( + [shell_path, "-lc", command], + cwd=cwd, + env=env, + text=True, + capture_output=True, + timeout=timeout, + check=False) + if check and result.returncode != 0: + raise AssertionError( + "Command failed with exit code " + f"{result.returncode}: {command}\n" + f"stdout:\n{result.stdout}\n" + f"stderr:\n{result.stderr}" + ) + return result + + @staticmethod + def _terminate_process(process, timeout=10): + """Terminate a spawned shell process and its children, then collect output.""" + if process.poll() is None: + try: + os.killpg(process.pid, signal.SIGTERM) + except ProcessLookupError: + pass + try: + process.wait(timeout=timeout) + except subprocess.TimeoutExpired: + try: + os.killpg(process.pid, signal.SIGKILL) + except ProcessLookupError: + pass + process.wait(timeout=timeout) + try: + stdout, _ = process.communicate(timeout=1) + except subprocess.TimeoutExpired: + stdout = "" + return stdout or "" + + def _build_generated_package(self, ros2_dir, env): + """Build the generated ROS2 package with the active Python executable.""" + python_executable = shlex.quote(sys.executable) + self._run_shell( + f"source {self.ros2_shell()[1]} >/dev/null 2>&1 || true; " + f"colcon build --packages-select {self.PACKAGE_NAME} " + f"--cmake-args -DPython3_EXECUTABLE={python_executable}", + cwd=ros2_dir, + env=env, + timeout=600) + + 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() + return subprocess.Popen( + [ + shell_path, + "-lc", + f"source {setup_script} && {command}" + ], + cwd=ros2_dir, + env=env, + text=True, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + start_new_session=True) + + def _wait_for_node_and_topics(self, ros2_dir, env): + """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): + # `ros2 node list` confirms that the process joined the ROS graph, + # while `ros2 topic list` confirms that the expected interfaces are + # actually being advertised. + node_result = self._run_shell( + f"source {setup_script} && " + "ros2 node list --no-daemon --spin-time 5", + cwd=ros2_dir, + env=env, + timeout=30, + check=False) + topic_result = self._run_shell( + f"source {setup_script} && " + "ros2 topic list --no-daemon --spin-time 5", + cwd=ros2_dir, + env=env, + timeout=30, + check=False) + node_seen = f"/{self.NODE_NAME}" in node_result.stdout + topics_seen = "/parameters" in topic_result.stdout and "/result" in topic_result.stdout + if node_seen and topics_seen: + return + time.sleep(1) + + self.fail( + "Generated ROS2 node did not become discoverable.\n" + f"ros2 node list output:\n{node_result.stdout if node_result else ''}\n" + f"ros2 topic list output:\n{topic_result.stdout if topic_result else ''}") + + def _assert_result_message(self, echo_stdout): + """Assert that the echoed result message indicates a successful solve.""" + # We do not compare the full numeric solution here; instead, we check + # that the generated node returned a structurally valid result and that + # the solver reported convergence. + self.assertIn("solution", echo_stdout) + self.assertRegex( + echo_stdout, + r"solution:\s*\n(?:- .+\n)+", + 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.assertRegex( + echo_stdout, + r"inner_iterations:\s*[1-9]\d*", + msg=f"Expected a positive inner iteration count in result output:\n{echo_stdout}") + self.assertRegex( + echo_stdout, + r"outer_iterations:\s*[1-9]\d*", + msg=f"Expected a positive outer iteration count in result output:\n{echo_stdout}") + self.assertRegex( + echo_stdout, + r"cost:\s*-?\d+(?:\.\d+)?(?:e[+-]?\d+)?", + 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.""" + _, 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}'", + cwd=ros2_dir, + env=env, + timeout=60) + echo_stdout, _ = echo_process.communicate(timeout=60) + finally: + if echo_process.poll() is None: + self._terminate_process(echo_process) + + self._assert_result_message(echo_stdout) + + def test_ros2_package_generation(self): + """Verify the ROS2 package files are generated.""" + ros2_dir = self.ros2_package_dir() + # This is a lightweight smoke test for the generator itself before we + # attempt the slower build/run integration tests below. + self.assertTrue(os.path.isfile(os.path.join(ros2_dir, "package.xml"))) + self.assertTrue(os.path.isfile(os.path.join(ros2_dir, "CMakeLists.txt"))) + self.assertTrue(os.path.isfile( + os.path.join(ros2_dir, "launch", "open_optimizer.launch.py"))) + + def test_generated_ros2_package_works(self): + """Build, run, and call the generated ROS2 package.""" + ros2_dir = self.ros2_package_dir() + env = self.ros2_test_env() + + # First validate the plain `ros2 run` path, which exercises the + # generated executable directly without going through the launch file. + self._build_generated_package(ros2_dir, env) + + node_process = self._spawn_ros_process( + f"ros2 run {self.PACKAGE_NAME} {self.NODE_NAME}", + ros2_dir, + env) + + try: + self._wait_for_node_and_topics(ros2_dir, env) + self._exercise_running_optimizer(ros2_dir, env) + finally: + if node_process.poll() is None: + self._terminate_process(node_process) + + def test_generated_ros2_launch_file_works(self): + """Build the package, launch the node, and verify the launch file works.""" + ros2_dir = self.ros2_package_dir() + env = self.ros2_test_env() + + # Then validate the generated launch description, which should bring up + # the exact same node and parameters via `ros2 launch`. + self._build_generated_package(ros2_dir, env) + + launch_process = self._spawn_ros_process( + f"ros2 launch {self.PACKAGE_NAME} open_optimizer.launch.py", + ros2_dir, + env) + + try: + self._wait_for_node_and_topics(ros2_dir, env) + self._exercise_running_optimizer(ros2_dir, env) + finally: + if launch_process.poll() is None: + self._terminate_process(launch_process) + + +if __name__ == '__main__': + logging.getLogger('retry').setLevel(logging.ERROR) + unittest.main() diff --git a/website/sidebars.js b/website/sidebars.js index 752f7944..a407782f 100644 --- a/website/sidebars.js +++ b/website/sidebars.js @@ -14,6 +14,7 @@ module.exports = { 'python-c', 'python-bindings', 'python-tcp-ip', + 'python-ros2', 'python-ros', 'python-examples', ], diff --git a/website/src/css/custom.css b/website/src/css/custom.css index 09bce1f7..568b4152 100644 --- a/website/src/css/custom.css +++ b/website/src/css/custom.css @@ -32,7 +32,6 @@ body { } .navbar { - backdrop-filter: blur(16px); box-shadow: 0 10px 30px rgba(86, 44, 28, 0.12); } @@ -197,7 +196,7 @@ body { .homeCodeBlock { background: var(--open-page-surface); border: 1px solid var(--open-page-border); - border-radius: 24px; + border-radius: 15px; box-shadow: var(--open-page-shadow); } @@ -368,6 +367,111 @@ body { width: 100%; } +.homeRos2Promo { + display: grid; + grid-template-columns: minmax(0, 1.05fr) minmax(320px, 0.95fr); + gap: 1.5rem; + align-items: center; + width: min(1100px, calc(100% - 2rem)); + margin: 0 auto; + padding: 2rem; + background: + linear-gradient(145deg, rgba(164, 62, 53, 0.88), rgba(141, 33, 183, 0.92)), + #843129; + border: 1px solid rgba(255, 224, 204, 0.2); + border-radius: 28px; + box-shadow: var(--open-page-shadow); +} + +.homeRos2Promo__content, +.homeRos2Promo__code { + min-width: 0; +} + +.homeRos2Promo__content h2 { + margin: 0 0 0.85rem; + color: #fff8f3; + font-size: clamp(2rem, 4vw, 3rem); + line-height: 1.08; +} + +.homeRos2Promo__content p { + color: rgba(255, 248, 243, 0.88); +} + +.homeRos2Promo__robot { + display: block; + width: 200px; + height: 200px; + margin: 0 auto 1rem; +} + +.homeRos2Promo__attribution { + margin: -0.4rem 0 0.9rem; + text-align: center; + font-size: 0.68rem; + line-height: 1.25; +} + +.homeRos2Promo__attribution a { + color: rgba(255, 248, 243, 0.82); + text-decoration: none; +} + +.homeRos2Promo__attribution a:hover { + color: #fff8f3; + text-decoration: underline; +} + +.homeRos2Promo__codeBlock { + margin-top: 0; + background: rgba(255, 248, 243, 0.96); + border-color: rgba(255, 224, 204, 0.3); +} + +.homeRos2Promo__codeBlock .theme-code-block { + margin-bottom: 0; +} + +.homeDockerPromo { + display: grid; + grid-template-columns: minmax(0, 1.02fr) minmax(320px, 0.98fr); + gap: 1.5rem; + align-items: center; + width: min(1100px, calc(100% - 2rem)); + margin: 0 auto; +} + +.homeDockerPromo__content, +.homeDockerPromo__visual { + min-width: 0; +} + +.homeDockerPromo__content h2 { + margin: 0 0 0.85rem; + color: #2f1a14; + font-size: clamp(2rem, 4vw, 3rem); + line-height: 1.08; +} + +.homeDockerPromo__content p { + color: var(--open-page-muted); +} + +.homeDockerPromo__image { + display: block; + width: min(100%, 280px); + margin: 0 auto 1rem; +} + +.homeDockerPromo__codeBlock { + margin-top: 0; +} + +.homeDockerPromo__codeBlock .theme-code-block { + margin-bottom: 0; +} + .homeSplit__copy, .homeSplit__media { min-width: 0; @@ -617,11 +721,47 @@ body { max-width: none; } - .homeOcpPromo { + .homeOcpPromo, + .homeRos2Promo, + .homeDockerPromo { grid-template-columns: 1fr; } } +@media (min-width: 997px) { + .navbar { + backdrop-filter: blur(16px); + } +} + +@media (max-width: 996px) { + .navbar-sidebar, + .navbar-sidebar__items, + .navbar-sidebar__item.menu { + background: #f8eee7; + } + + .navbar-sidebar__brand, + .navbar-sidebar__back, + .navbar-sidebar__close, + .navbar-sidebar .menu__link, + .navbar-sidebar .menu__caret, + .navbar-sidebar .menu__link--sublist::after { + color: #221714; + } + + .navbar-sidebar .menu__link { + font-weight: 500; + } + + .navbar-sidebar .menu__link:hover, + .navbar-sidebar .menu__link--active, + .navbar-sidebar .menu__list-item-collapsible:hover { + background: rgba(122, 31, 31, 0.08); + color: #221714; + } +} + @media (max-width: 640px) { .homeHero { padding-top: 2rem; @@ -640,4 +780,8 @@ body { width: 64px; height: 64px; } + + .homeRos2Promo { + padding: 1.5rem; + } } diff --git a/website/src/pages/index.js b/website/src/pages/index.js index 8a0fa34d..fb458b1e 100644 --- a/website/src/pages/index.js +++ b/website/src/pages/index.js @@ -35,6 +35,17 @@ builder = og.builder.OpEnOptimizerBuilder( ) builder.build()`; +const ros2PromoCode = String.raw`ros2_config = og.config.RosConfiguration() \ + .with_package_name("my_ros_pkg") \ + .with_node_name("open_node_ros2") \ + .with_rate(10.0) + +build_config = og.config.BuildConfiguration() \ + .with_build_directory("my_optimizers") \ + .with_ros2(ros2_config)`; + +const dockerPromoCode = String.raw`docker pull alphaville/open:0.7.0` + const heroStats = [ {label: 'Core language', value: 'Rust'}, {label: 'Primary uses', value: 'MPC, MHE, Robotics'}, @@ -111,7 +122,9 @@ export default function Home() { const assetUrl = (path) => `${baseUrl}${path.replace(/^\//, '')}`; const promoGif = assetUrl('img/open-promo.gif'); const boxLogo = assetUrl('img/box.png'); + const dockerGif = assetUrl('img/docker.gif'); const ocpStatesImage = assetUrl('img/ocp-states.png'); + const ros2RobotImage = assetUrl('img/ros2-robot.png'); const [zoomedImage, setZoomedImage] = useState(null); useEffect(() => { @@ -352,6 +365,94 @@ export default function Home() { + +
+
+
+

New in opegen 0.11

+

ROS2 packages

+

+ OpEn can now generate ROS2 packages directly from a parametric + optimizer. The generated package includes ROS2 messages, + configuration files, a launch file, and a node that exposes the + solver through topics. +

+

+ This makes it easy to connect optimization-based controllers, + estimators, and planning modules into a modern robotics stack + without writing the ROS2 wrapper code by hand. +

+
+ + Learn more + + + Legacy ROS1 + +
+
+
+ Cartoon robot icon +

+ + Bot icons created by pbig - Flaticon + +

+
+ {ros2PromoCode} +
+
+
+
+ +
+
+
+

Docker image

+

Run OpEn in a ready-made container

+

+ OpEn ships with a Docker image that gets you straight into a + working environment with Jupyter, Python, and the tooling needed + to explore examples without local setup friction. +

+

+ It is a convenient way to try the Python interface, browse the + notebooks, and experiment with the OCP workflows in a clean, + reproducible environment. +

+
+ + Learn more + + + Docker Hub + +
+
+
+ OpEn running inside the Docker image with Jupyter +
+ {dockerPromoCode} +
+
+
+
{zoomedImage ? (
{ + if (lang === 'php') { + require('prismjs/components/prism-markup-templating.js'); + } + require(`prismjs/components/prism-${lang}`); + }); + + registerMsgLanguage(PrismObject); + + delete globalThis.Prism; + if (typeof PrismBefore !== 'undefined') { + globalThis.Prism = PrismObject; + } +} diff --git a/website/static/img/ros2-robot.png b/website/static/img/ros2-robot.png new file mode 100644 index 00000000..f5ec4f94 Binary files /dev/null and b/website/static/img/ros2-robot.png differ