Migrate to ROS2

.clang-format-common.sh

@@ -1,12 +1,12 @@
 # This script is meant to be sourced from other scripts
 
-# Check for clang-format, prefer 10 if available
-if [[ -x "$(command -v clang-format-10)" ]]; then
-  clang_format=clang-format-10
+# Check for clang-format, prefer 14 if available
+if [[ -x "$(command -v clang-format-14)" ]]; then
+  clang_format=clang-format-14
 elif [[ -x "$(command -v clang-format)" ]]; then
   clang_format=clang-format
 else
-  echo "clang-format or clang-format-10 must be installed"
+  echo "clang-format or clang-format-14 must be installed"
   exit 1
 fi
 

.github/workflows/ci.yaml

@@ -13,14 +13,14 @@ on:
 jobs:
 
   clang-format:
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-22.04
     steps:
       - name: Checkout repository code
         uses: actions/checkout@v3
-      - name: Install clang-format-10
+      - name: Install clang-format-14
         run: |
           sudo apt-get -qq update
-          sudo apt-get -qq install clang-format-10
+          sudo apt-get -qq install clang-format-14
       - name: Run clang-format-check
         run: |
           ./.clang-format-check.sh
@@ -29,25 +29,28 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        os: [ubuntu-20.04]
+        os: [ubuntu-22.04, ubuntu-24.04]
         build-type: [Debug, RelWithDebInfo]
-        standalone: [standalone, catkin]
-        solvers-from-source: [SolversFromSource, SolversFromAPT]
+        standalone: [standalone, colcon]
+        solvers-from-source: [SolversFromSource]
     runs-on: ${{ matrix.os }}
     steps:
       - name: Set environment variables
         run: |
-          echo "LD_LIBRARY_PATH=/opt/blasfeo/lib:/opt/hpipm/lib:$LD_LIBRARY_PATH" >> $GITHUB_ENV # for HPIPM
-      - name: Set ROS version
-        if: matrix.standalone == 'catkin'
-        run: |
-          if [ "${{ matrix.os }}" == "ubuntu-20.04" ]
+          set -e
+          set -x
+          echo "LD_LIBRARY_PATH=/opt/blasfeo/lib:/opt/hpipm/lib:/usr/local/lib:$LD_LIBRARY_PATH" >> $GITHUB_ENV # for HPIPM
+          if [ "${{ matrix.os }}" == "ubuntu-22.04" ] && \
+             [ "${{ matrix.build-type }}" == "RelWithDebInfo" ] && \
+             [ "${{ matrix.standalone }}" == "colcon" ] &&  \
+             [ "${{ matrix.solvers-from-source }}" == "SolversFromAPT" ] && \
+             [ "${{ github.repository_owner }}" == "isri-aist" ] && \
+             [ "${{ github.ref }}" == "refs/heads/master" ]
           then
-            echo "ROS_DISTRO=noetic" >> $GITHUB_ENV
-            echo "PYTHON_PACKAGE_PREFIX=python3" >> $GITHUB_ENV
-          else # if [ "${{ matrix.os }}" == "ubuntu-18.04" ]
-            echo "ROS_DISTRO=melodic" >> $GITHUB_ENV
-            echo "PYTHON_PACKAGE_PREFIX=python" >> $GITHUB_ENV
+            echo "UPLOAD_DOCUMENTATION=true" >> $GITHUB_ENV
+            sudo apt-get install -qq doxygen graphviz
+          else
+            echo "UPLOAD_DOCUMENTATION=false" >> $GITHUB_ENV
           fi
       - name: Check secrets
         run: |
@@ -63,50 +66,6 @@ jobs:
           fi
         env:
           GITLAB_TOKEN : ${{ secrets.GITLAB_TOKEN }}
-      - name: Install ROS
-        if: matrix.standalone == 'catkin'
-        run: |
-          set -e
-          set -x
-          sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
-          wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-          sudo apt-get update -qq
-          sudo apt-get install -qq ros-${ROS_DISTRO}-ros-base ${PYTHON_PACKAGE_PREFIX}-catkin-tools ${PYTHON_PACKAGE_PREFIX}-rosdep doxygen graphviz
-      - name: Setup catkin workspace
-        if: matrix.standalone == 'catkin'
-        run: |
-          set -e
-          set -x
-          mkdir -p ${GITHUB_WORKSPACE}/catkin_ws/src/
-          cd ${GITHUB_WORKSPACE}/catkin_ws
-          set +x
-          . /opt/ros/${ROS_DISTRO}/setup.bash
-          set -x
-          catkin init
-          catkin build --limit-status-rate 0.1
-      - name: Checkout repository code
-        if: matrix.standalone == 'catkin'
-        uses: actions/checkout@v3
-        with:
-          submodules: recursive
-          path: catkin_ws/src/QpSolverCollection
-      - name: Checkout repository code
-        if: matrix.standalone == 'standalone'
-        uses: actions/checkout@v3
-        with:
-          submodules: recursive
-      - name: Rosdep install
-        if: matrix.standalone == 'catkin'
-        run: |
-          set -e
-          set -x
-          cd ${GITHUB_WORKSPACE}/catkin_ws
-          set +x
-          . devel/setup.bash
-          set -x
-          sudo rosdep init
-          rosdep update
-          rosdep install -y -r --from-paths src --ignore-src
       - name: Install solvers via apt
         if: matrix.solvers-from-source == 'SolversFromAPT'
         uses: jrl-umi3218/github-actions/install-dependencies@master
@@ -117,6 +76,8 @@ jobs:
               mc-rtc:
                 cloudsmith: mc-rtc/head
             apt: libmetis-dev libeigen3-dev libgtest-dev graphviz doxygen libeigen-qld-dev libeigen-quadprog-dev libjrl-qp-dev libqpoases-dev libosqp-eigen-dev libnasoq-dev libhpipm-dev libproxsuite-dev libqpmad-dev
+          ros: |
+            apt: ros-base
       - name: Install solvers from source
         if: matrix.solvers-from-source == 'SolversFromSource'
         uses: jrl-umi3218/github-actions/install-dependencies@master
@@ -134,7 +95,7 @@ jobs:
             - path: coin-or/qpOASES
               options: -DBUILD_SHARED_LIBS=ON -DQPOASES_BUILD_EXAMPLES=OFF
             - path: osqp/osqp
-            - path: robotology/osqp-eigen
+            - path: chuoru/osqp-eigen
             - path: mmurooka/nasoq
               ref: cmake-install
               options: -DCMAKE_INSTALL_PREFIX=/usr/local -DNASOQ_BLAS_BACKEND=OpenBLAS -DNASOQ_USE_CLAPACK=ON -DNASOQ_BUILD_CLI=OFF -DNASOQ_BUILD_EXAMPLES=OFF -DNASOQ_BUILD_TESTS=OFF -DNASOQ_BUILD_DOCS=OFF
@@ -146,6 +107,8 @@ jobs:
               ref: main
               options: -DBUILD_TESTING=OFF -DBUILD_WITH_VECTORIZATION_SUPPORT=OFF
             - path: asherikov/qpmad
+          ros: |
+            apt: ros-base
       - name: Install private solvers
         if: env.ENABLE_PRIVATE_SOLVERS == 'ON'
         uses: jrl-umi3218/github-actions/install-dependencies@master
@@ -154,38 +117,28 @@ jobs:
           github: |
             - path: git@gite.lirmm.fr:multi-contact/eigen-lssol
               options: -DBUILD_TESTING=OFF -DPYTHON_BINDING=OFF -DINSTALL_DOCUMENTATION=OFF
-      - name: Catkin build
-        if: matrix.standalone == 'catkin'
-        run: |
-          set -e
-          set -x
-          cd ${GITHUB_WORKSPACE}/catkin_ws
-          set +x
-          . devel/setup.bash
-          set -x
-          catkin build --limit-status-rate 0.1 -DCMAKE_BUILD_TYPE=${{ matrix.build-type }} \
-          -DDEFAULT_ENABLE_ALL=ON -DENABLE_LSSOL=${{ env.ENABLE_PRIVATE_SOLVERS }} \
-          -DSKIP_PRIVATE_SOLVER_TEST=${{ env.SKIP_PRIVATE_SOLVER_TEST }} -DFORCE_ALL_SOLVER_TEST=ON -DINSTALL_DOCUMENTATION=ON
-      - name: Run tests
-        if: matrix.standalone == 'catkin'
-        run: |
-          set -e
-          set -x
-          cd ${GITHUB_WORKSPACE}/catkin_ws
-          set +x
-          . devel/setup.bash
-          set -x
-          catkin build --limit-status-rate 0.1 --catkin-make-args run_tests -- qp_solver_collection --no-deps
-          catkin_test_results --verbose --all build
-      - name: Build and test
+      - name: Checkout repository code
+        if: matrix.standalone == 'standalone'
+        uses: actions/checkout@v3
+        with:
+          submodules: recursive
+      - name: Standalone build
         if: matrix.standalone == 'standalone'
         uses: jrl-umi3218/github-actions/build-cmake-project@master
         with:
           build-type: ${{ matrix.build-type }}
-          options: -DDEFAULT_ENABLE_ALL=ON -DENABLE_LSSOL=${{ env.ENABLE_PRIVATE_SOLVERS }} -DSKIP_PRIVATE_SOLVER_TEST=${{ env.SKIP_PRIVATE_SOLVER_TEST }} -DFORCE_ALL_SOLVER_TEST=ON -DINSTALL_DOCUMENTATION=ON
+          options: -DINSTALL_DOCUMENTATION=${{ env.UPLOAD_DOCUMENTATION }} -DDEFAULT_ENABLE_ALL=ON -DENABLE_LSSOL=${{ env.ENABLE_PRIVATE_SOLVERS }} -DSKIP_PRIVATE_SOLVER_TEST=${{ env.SKIP_PRIVATE_SOLVER_TEST }} -DFORCE_ALL_SOLVER_TEST=ON
+      - name: Colcon build
+        if: matrix.standalone == 'colcon'
+        uses: jrl-umi3218/github-actions/build-colcon-project@master
+        with:
+          build-type: ${{ matrix.build-type }}
+          cmake-args: -DUSE_ROS2=ON -DINSTALL_DOCUMENTATION=${{ env.UPLOAD_DOCUMENTATION }} -DDEFAULT_ENABLE_ALL=ON -DENABLE_LSSOL=${{ env.ENABLE_PRIVATE_SOLVERS }} -DSKIP_PRIVATE_SOLVER_TEST=${{ env.SKIP_PRIVATE_SOLVER_TEST }} -DFORCE_ALL_SOLVER_TEST=ON 
+          build-packages: qp_solver_collection
+          test-packages: qp_solver_collection
       - name: Upload documentation
         # Only run for one configuration and on origin master branch
-        if: matrix.os == 'ubuntu-20.04' && matrix.build-type == 'RelWithDebInfo' && matrix.standalone == 'catkin' && matrix.solvers-from-source == 'SolversFromAPT' && github.repository_owner == 'isri-aist' && github.ref == 'refs/heads/master'
+        if: env.UPLOAD_DOCUMENTATION == 'true'
         run: |
           set -e
           set -x

.github/workflows/package.yaml

@@ -20,8 +20,14 @@ jobs:
   package:
     uses: jrl-umi3218/github-actions/.github/workflows/package-project.yml@master
     with:
+      latest-cmake: true
       deps: '["isri-aist/ForceControlCollection"]'
       main-repo: isri-aist/QpSolverCollection
+      matrix: |
+        {
+          "dist": ["focal"],
+          "arch": ["amd64"]
+        }
     secrets:
       CLOUDSMITH_API_KEY: ${{ secrets.CLOUDSMITH_API_KEY }}
       GH_TOKEN: ${{ secrets.GH_PAGES_TOKEN }}

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.14)
 project(qp_solver_collection)
 
 # Options for solvers
@@ -15,19 +15,17 @@ option(ENABLE_NASOQ "Enable NASOQ" ${DEFAULT_ENABLE_VALUE})
 option(ENABLE_HPIPM "Enable HPIPM" ${DEFAULT_ENABLE_VALUE})
 option(ENABLE_PROXQP "Enable PROXQP" ${DEFAULT_ENABLE_VALUE})
 option(ENABLE_QPMAD "Enable QPMAD" ${DEFAULT_ENABLE_VALUE})
+option(USE_ROS2 "Use ROS2" OFF)
 
 option(FORCE_ALL_SOLVER_TEST "Test for all solvers regardless of options" OFF)
 mark_as_advanced(FORCE FORCE_ALL_SOLVER_TEST)
 
 option(SKIP_PRIVATE_SOLVER_TEST "Skip private solver test" OFF)
 mark_as_advanced(FORCE SKIP_PRIVATE_SOLVER_TEST)
 
-if(DEFINED CATKIN_DEVEL_PREFIX)
-  find_package(catkin REQUIRED COMPONENTS
-    roscpp
-    )
-else()
-  option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON)
+option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON)
+
+if(NOT USE_ROS2)
   option(BUILD_TESTING "Build test" ON)
   if(BUILD_TESTING)
     enable_testing()
@@ -93,15 +91,9 @@ if(NOT ${EXIST_ENABLED_SOLVER})
   message(FATAL_ERROR "None of the QP solvers are enabled. Enable at least one QP solver.")
 endif()
 
-if(DEFINED CATKIN_DEVEL_PREFIX)
-  catkin_package(
-    CATKIN_DEPENDS
-    roscpp
-    DEPENDS EIGEN3
-    INCLUDE_DIRS include
-    LIBRARIES QpSolverCollection
-    CFG_EXTRAS qp_solver_options.cmake
-    )
+if(USE_ROS2)
+  find_package(ament_cmake REQUIRED)
+  find_package(rclcpp REQUIRED)
 endif()
 
 set(QP_SOLVER_OPTIONS_HEADER_FILE "${PROJECT_SOURCE_DIR}/include/qp_solver_collection/QpSolverOptions.h")
@@ -112,7 +104,7 @@ configure_file("${QP_SOLVER_OPTIONS_CMAKE_FILE}.in" "${QP_SOLVER_OPTIONS_CMAKE_F
 
 add_subdirectory(src)
 
-if(NOT DEFINED CATKIN_DEVEL_PREFIX)
+if(NOT USE_ROS2)
   install(EXPORT ${PROJECT_NAME}
     FILE ${PROJECT_NAME}Targets.cmake
     DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}
@@ -161,11 +153,22 @@ set(\@PROJECT_NAME\@_LIBRARIES \@PROJECT_NAME\@::QpSolverCollection)
     )
 endif()
 
-if(CATKIN_ENABLE_TESTING OR BUILD_TESTING)
+if(BUILD_TESTING)
   add_subdirectory(tests)
 endif()
 
 OPTION(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF)
 if(INSTALL_DOCUMENTATION)
   add_subdirectory(doc)
 endif()
+
+if(USE_ROS2)
+  ament_export_libraries(QpSolverCollection)
+  ament_export_include_directories(include)
+  ament_export_dependencies(
+    rclcpp
+    Eigen3
+  )
+  ament_export_targets(${PROJECT_NAME})
+  ament_package()
+endif()

README.md

@@ -1,3 +1,5 @@
+This is the branch for ROS2; use the ros1 branch for ROS1.
+
 # [QpSolverCollection](https://github.com/isri-aist/QpSolverCollection)
 Unified C++ interface for quadratic programming solvers
 
@@ -28,24 +30,24 @@ It is assumed that ROS is installed.
 
 1. Install the QP solver you wish to use according to [this section](https://github.com/isri-aist/QpSolverCollection#qp-solver-installation). You can skip installing QP solvers that you do not use.
 
-2. Setup catkin workspace.
+2. Setup colcon workspace.
 ```bash
 $ mkdir -p ~/ros/ws_qp_solver_collection/src
-$ cd ~/ros/ws_qp_solver_collection
-$ wstool init src
-$ wstool set -t src isri-aist/QpSolverCollection git@github.com:isri-aist/QpSolverCollection.git --git -y
-$ wstool update -t src
+$ git clone https://github.com/isri-aist/QpSolverCollection
 ```
 
 3. Install dependent packages.
 ```bash
+$ cd ~/ros
 $ source /opt/ros/${ROS_DISTRO}/setup.bash
 $ rosdep install -y -r --from-paths src --ignore-src
 ```
 
 4. Build a package.
 ```bash
-$ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo <qp-solver-flags> --catkin-make-args all tests
+$ cd ~/ros
+$ colcon build --packages-select qp_solver_collection --merge-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo <qp-solver-flags>
+$ colcon test --merge-install --packages-select qp_solver_collection # [optional] to compile and run tests 
 ```
 See [this section](https://github.com/isri-aist/QpSolverCollection#qp-solver-installation) for `<qp-solver-flags>`.
 

doc/Doxyfile.in

@@ -58,7 +58,7 @@ PROJECT_LOGO           =
 # entered, it will be relative to the location where doxygen was started. If
 # left blank the current directory will be used.
 
-OUTPUT_DIRECTORY       = @CMAKE_CURRENT_BINARY_DIR@
+OUTPUT_DIRECTORY       = @PROJECT_BINARY_DIR@/doc
 
 # If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
 # directories (in 2 levels) under the output directory of each output format and

include/qp_solver_collection/QpSolverCollection.h

@@ -16,10 +16,10 @@
 #  define QSC_WARN_STREAM(x) std::cerr << x << "\n"
 #  define QSC_INFO_STREAM(x) std::cout << x << "\n"
 #else
-#  include <ros/console.h>
-#  define QSC_ERROR_STREAM ROS_ERROR_STREAM
-#  define QSC_WARN_STREAM ROS_WARN_STREAM
-#  define QSC_INFO_STREAM ROS_INFO_STREAM
+#  include <rclcpp/rclcpp.hpp>
+#  define QSC_ERROR_STREAM(msg) RCLCPP_ERROR_STREAM(rclcpp::get_logger("QpSolverCollection"), msg)
+#  define QSC_WARN_STREAM(msg) RCLCPP_WARN_STREAM(rclcpp::get_logger("QpSolverCollection"), msg)
+#  define QSC_INFO_STREAM(msg) RCLCPP_INFO_STREAM(rclcpp::get_logger("QpSolverCollection"), msg)
 #endif
 
 namespace Eigen
@@ -490,9 +490,6 @@ class QpSolverHpipm : public QpSolver
   /** \brief Constructor. */
   QpSolverHpipm();
 
-  /** \brief Destructor. */
-  ~QpSolverHpipm();
-
   /** \brief Solve QP. */
   virtual Eigen::VectorXd solve(int dim_var,
                                 int dim_eq,
@@ -521,13 +518,13 @@ class QpSolverHpipm : public QpSolver
   std::unique_ptr<struct d_dense_qp_ipm_arg> ipm_arg_;
   std::unique_ptr<struct d_dense_qp_ipm_ws> ipm_ws_;
 
-  void * qp_dim_mem_ = nullptr;
-  void * qp_mem_ = nullptr;
-  void * qp_sol_mem_ = nullptr;
-  void * ipm_arg_mem_ = nullptr;
-  void * ipm_ws_mem_ = nullptr;
+  std::unique_ptr<uint8_t[]> qp_dim_mem_ = nullptr;
+  std::unique_ptr<uint8_t[]> qp_mem_ = nullptr;
+  std::unique_ptr<uint8_t[]> qp_sol_mem_ = nullptr;
+  std::unique_ptr<uint8_t[]> ipm_arg_mem_ = nullptr;
+  std::unique_ptr<uint8_t[]> ipm_ws_mem_ = nullptr;
 
-  double * opt_x_mem_;
+  std::unique_ptr<double[]> opt_x_mem_ = nullptr;
 };
 #endif