Enable qpOASES on Windows and fix Windows build

.github/workflows/windows.yml

@@ -25,6 +25,11 @@ jobs:
       with:
         path: workspace/src/trajopt
 
+    - name: configure-msvc
+      uses: ilammy/msvc-dev-cmd@v1
+      with:
+        arch: x64
+
     - name: vcpkg build
       uses: johnwason/vcpkg-action@v4
       with:
@@ -36,6 +41,7 @@ jobs:
         extra-args: --clean-after-build
         token: ${{ github.token }}
         cache-key: ci-${{ matrix.os }}
+        revision: master
 
     - name: install-depends
       shell: cmd
@@ -44,11 +50,6 @@ jobs:
         python -m pip install colcon-common-extensions -q
         python -m pip install ninja -q
 
-    - name: configure-msvc
-      uses: ilammy/msvc-dev-cmd@v1
-      with:
-        arch: x64
-
     - name: build-workspace
       working-directory: workspace
       shell: cmd

trajopt_ext/qpoases/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.2.1)
+find_package(ros_industrial_cmake_boilerplate REQUIRED)
+
+extract_package_metadata(pkg)
+project(qpoases VERSION ${pkg_extracted_version} LANGUAGES CXX)
+
+find_package(qpoases QUIET)
+
+if(NOT qpOASES_FOUND AND WIN32)
+  message(WARNING "No valid qpOAESES version found. Cloning into build directory")
+
+  include(ExternalProject)
+
+  ExternalProject_Add(
+    ${PROJECT_NAME}
+    GIT_REPOSITORY "https://github.com/coin-or/qpOASES.git"
+    GIT_TAG 0b86dbf00c7fce34420bedc5914f71b176fe79d3
+    SOURCE_DIR ${CMAKE_BINARY_DIR}-src
+    BINARY_DIR ${CMAKE_BINARY_DIR}-build
+    PATCH_COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_SOURCE_DIR}/patch_qpoases.cmake
+    CMAKE_CACHE_ARGS
+      -DCMAKE_INSTALL_PREFIX:STRING=${CMAKE_INSTALL_PREFIX}
+      -DCMAKE_BUILD_TYPE:STRING=Release
+      -DCMAKE_TOOLCHAIN_FILE:STRING=${CMAKE_TOOLCHAIN_FILE}
+      -DVCPKG_TARGET_TRIPLET:STRING=${VCPKG_TARGET_TRIPLET}
+      -DBUILD_SHARED_LIBS:BOOL=ON
+      -DQPOASES_BUILD_EXAMPLES:BOOL=OFF)
+
+  # if(TRAJOPT_PACKAGE) find_package(ros_industrial_cmake_boilerplate REQUIRED) set(CPACK_BUILD_SOURCE_DIRS
+  # "${CMAKE_BINARY_DIR}-src;${CMAKE_BINARY_DIR}-build") set(CPACK_INSTALL_CMAKE_PROJECTS
+  # "${CMAKE_BINARY_DIR}-build;${PROJECT_NAME};ALL;/") cpack( VERSION ${pkg_extracted_version} MAINTAINER
+  # <https://github.com/tesseract-robotics/trajopt> VENDOR "ROS-Industrial" DESCRIPTION ${pkg_extracted_description}
+  # PACKAGE_PREFIX ${TRAJOPT_PACKAGE_PREFIX} LINUX_DEPENDS "libeigen3-dev" WINDOWS_DEPENDS "Eigen3") endif()
+endif()
+
+install(FILES package.xml DESTINATION share/${PROJECT_NAME})

trajopt_ext/qpoases/colcon.pkg

@@ -0,0 +1,3 @@
+{
+    "hooks": ["share/osqp_eigen/hook/ament_prefix_path.dsv", "share/osqp_eigen/hook/ros_package_path.dsv"]
+}

trajopt_ext/qpoases/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>qpoases</name>
+  <version>3.2.1</version>
+  <description>Downloads and builds the qpOASES Library</description>
+  <maintainer email="[email protected]">Levi Armstrong</maintainer>
+  <license>LGPL</license>
+
+  <build_depend>ros_industrial_cmake_boilerplate</build_depend>
+
+  <export>
+    <build_type>cmake</build_type>
+  </export>
+
+</package>

trajopt_ext/qpoases/patch_qpoases.cmake

@@ -0,0 +1,12 @@
+file(READ CMakeLists.txt ROOT_FILE)
+string(
+  REPLACE "MESSAGE(FATAL_ERROR \"Compiling qpOASES as a shared library in Windows is not supported.\")"
+          "set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)\ninclude(GNUInstallDirs)"
+          ROOT_FILE2
+          ${ROOT_FILE})
+string(
+  REPLACE "SET(CMAKE_INSTALL_BINDIR \${CMAKE_INSTALL_LIBDIR})"
+          ""
+          ROOT_FILE3
+          ${ROOT_FILE2})
+file(WRITE CMakeLists.txt ${ROOT_FILE3})

trajopt_sco/package.xml

@@ -15,6 +15,7 @@
   <build_export_depend>eigen</build_export_depend>
   <depend>libjsoncpp-dev</depend>
   <depend>osqp</depend>
+  <depend>qpoases</depend>
   <depend>trajopt_utils</depend>
   <depend>boost</depend>
 

trajopt_sco/src/qpoases_interface.cpp

@@ -165,7 +165,7 @@ void qpOASESModel::update()
     int inew = 0;
     for (unsigned iold = 0; iold < cnts_.size(); ++iold)
     {
-      const Cnt& cnt = cnts_[iold];
+      Cnt& cnt = cnts_[iold];
       if (!cnt.cnt_rep->removed)
       {
         cnts_[inew] = cnt;

trajopt_sco/test/CMakeLists.txt

@@ -30,11 +30,14 @@ endif()
 
 include(GoogleTest)
 
-set(SCO_TEST_SOURCE
-    unit.cpp
+set(SCO_TEST_SOURCE unit.cpp solver-utils-unit.cpp)
+if(HAVE_GUROBI OR HAVE_qpOASES)
+  list(
+    APPEND
+    SCO_TEST_SOURCE
     small-problems-unit.cpp
-    solver-interface-unit.cpp
-    solver-utils-unit.cpp)
+    solver-interface-unit.cpp)
+endif()
 
 add_executable(${PROJECT_NAME}-test ${SCO_TEST_SOURCE})
 target_link_libraries(