ROS2 support

- first implementation of ROS2 pkg generation
- basic unit tests (it works)

Author: alphaville

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()

open-codegen/opengen/builder/ros_builder.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

open-codegen/opengen/config/build_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

open-codegen/opengen/config/ros_config.py

@@ -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.
  *
  */

open-codegen/opengen/templates/ros/open_optimizer.cpp

@@ -0,0 +1,55 @@
+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)
+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()

open-codegen/opengen/templates/ros2/CMakeLists.txt

@@ -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)

open-codegen/opengen/templates/ros2/OptimizationParameters.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

open-codegen/opengen/templates/ros2/OptimizationResult.msg

@@ -0,0 +1,126 @@
+# 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/`).
+
+Compile with:
+
+```console
+cd ~/ros2_ws/
+colcon build --packages-select {{ros.package_name}}
+source install/setup.bash
+```
+
+If you build the package in-place from its own directory instead of a larger
+workspace, source the generated setup script from `install/`:
+
+```console
+# bash
+source install/setup.bash
+
+# zsh
+source install/setup.zsh
+```
+
+On macOS, ROS2 logging may need an explicit writable directory:
+
+```console
+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.
+
+```console
+# terminal 1
+source install/setup.bash   # or: source install/setup.zsh
+ros2 run {{ros.package_name}} {{ros.node_name}}
+```
+
+In a second terminal, source the same environment and verify discovery:
+
+```console
+# 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}}`):
+
+```console
+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}}`):
+
+```console
+ros2 topic echo /{{ros.publisher_subtopic}}
+```
+
+To get the optimal solution you can do:
+
+```console
+ros2 topic echo /{{ros.publisher_subtopic}} --field solution
+```
+
+
+## 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
+```