Back port .auto control packages (#571)

* Implement Lateral and Longitudinal Control Muxer * [#570] Porting wf_simulator * [#1189] Deactivate flaky test in 'trajectory_follower_nodes' * [#1189] Fix flacky test in 'trajectory_follower_nodes/latlon_muxer' * [#1057] Add osqp_interface package * [#1057] Add library code for MPC-based lateral control * [#1271] Use std::abs instead of abs * [#1057] Implement Lateral Controller for Cargo ODD * [#1246] Resolve "Test case names currently use snake_case but should be CamelCase" * [#1325] Deactivate flaky smoke test in 'trajectory_follower_nodes' * [#1058] Add library code of longitudinal controller * Fix build error for trajectory follower Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix build error for trajectory follower nodes Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [#1272] Add AckermannControlCommand support to simple_planning_simulator * [#1058] Add Longitudinal Controller node * [#1058] Rename velocity_controller -> longitudinal_controller * [#1058] Update CMakeLists.txt for the longitudinal_controller_node * [#1058] Add smoke test python launch file * [#1058] Use LowPassFilter1d from trajectory_follower * [#1058] Use autoware_auto_msgs * [#1058] Changes for .auto (debug msg tmp fix, common func, tf listener) * [#1058] Remove unused parameters * [#1058] Fix ros test * [#1058] Rm default params from declare_parameters + use autoware types * [#1058] Use default param file to setup NodeOptions in the ros test * [#1058] Fix docstring * [#1058] Replace receiving a Twist with a VehicleKinematicState * [#1058] Change class variables format to m_ prefix * [#1058] Fix plugin name of LongitudinalController in CMakeLists.txt * [#1058] Fix copyright dates * [#1058] Reorder includes * [#1058] Add some tests (~89% coverage without disabling flaky tests) * [#1058] Add more tests (90+% coverage without disabling flaky tests) * [#1058] Use Float32MultiArrayDiagnostic message for debug and slope * [#1058] Calculate wheel_base value from vehicle parameters * [#1058] Cleanup redundant logger setting in tests * [#1058] Set ROS_DOMAIN_ID when running tests to prevent CI failures * [#1058] Remove TF listener and use published vehicle state instead * [#1058] Change smoke tests to use autoware_testing * [#1058] Add plotjuggler cfg for both lateral and longitudinal control * [#1058] Improve design documents * [#1058] Disable flaky test * [#1058] Properly transform vehicle state in longitudinal node * [#1058] Fix TF buffer of lateral controller * [#1058] Tuning of lateral controller for LGSVL * [#1058] Fix formating * [#1058] Fix /tf_static sub to be transient_local * [#1058] Fix yaw recalculation of reverse trajs in the lateral controller * modify trajectory_follower for galactic build Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * [#1379] Update trajectory_follower * [#1379] Update simple_planning_simulator * [#1379] Update trajectory_follower_nodes * apply trajectory msg modification in control Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * move directory Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * remote control/trajectory_follower level dorectpry Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * remove .iv trajectory follower Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * use .auto trajectory_follower Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * remove .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * use .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * add tmp_autoware_auto_dependencies Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * tmporally add autoware_auto_msgs Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * apply .auto message split Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix build depend Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix packages using osqp * fix autoware_auto_geometry * ignore lint of some packages * ignore ament_lint of some packages * ignore lint/pre-commit of trajectory_follower_nodes * disable unit tests of some packages Co-authored-by: Maxime CLEMENT <maxime.clement@tier4.jp> Co-authored-by: Joshua Whitley <josh.whitley@autoware.org> Co-authored-by: Igor Bogoslavskyi <igor.bogoslavskyi@gmail.com> Co-authored-by: MIURA Yasuyuki <kokosabu@gmail.com> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: tomoya.kimura <tomoya.kimura@tier4.jp>
autowarefoundation · taikitanaka3 · Dec 3, 2021 · Nov 30, 2021 · Nov 30, 2021 · Nov 30, 2021
commit d987f212c67a8f939840a747d60334416aa9a3e3
diff --git a/common/osqp_interface/CMakeLists.txt b/common/osqp_interface/CMakeLists.txt
@@ -1,51 +1,78 @@
-cmake_minimum_required(VERSION 3.5)
-project(osqp_interface)
+# Copyright 2021 The Autoware Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
 
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-  set(CMAKE_CXX_STANDARD_REQUIRED ON)
-  set(CMAKE_CXX_EXTENSIONS OFF)
-endif()
+cmake_minimum_required(VERSION 3.5)
 
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Ofast)
-endif()
+project(osqp_interface)
 
+# require that dependencies from package.xml be available
 find_package(ament_cmake_auto REQUIRED)
-ament_auto_find_build_dependencies()
-
-find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
-
-find_package(osqp_vendor REQUIRED)
-
 find_package(osqp REQUIRED)
-get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES)
-get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH)
+get_target_property(OSQP_INCLUDE_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES)
 
-ament_auto_add_library(osqp_interface SHARED
-  src/osqp_interface.cpp
+ament_auto_find_build_dependencies(REQUIRED
+  ${${PROJECT_NAME}_BUILD_DEPENDS}
+  ${${PROJECT_NAME}_BUILDTOOL_DEPENDS}
+)
+
+set(OSQP_INTERFACE_LIB_SRC
   src/csc_matrix_conv.cpp
-  include/osqp_interface/osqp_interface.hpp
+  src/osqp_interface.cpp
+)
+
+set(OSQP_INTERFACE_LIB_HEADERS
   include/osqp_interface/csc_matrix_conv.hpp
+  include/osqp_interface/osqp_interface.hpp
+  include/osqp_interface/visibility_control.hpp
+)
+
+# generate library
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  ${OSQP_INTERFACE_LIB_SRC}
+  ${OSQP_INTERFACE_LIB_HEADERS}
 )
+autoware_set_compile_options(${PROJECT_NAME})
+target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast -Wno-error=useless-cast)
 
 target_include_directories(osqp_interface PUBLIC "${OSQP_INCLUDE_DIR}")
-ament_target_dependencies(osqp_interface Eigen3)
-ament_target_dependencies(osqp_interface osqp_vendor)
+ament_target_dependencies(osqp_interface
+  Eigen3
+  osqp_vendor
+)
 
 # crucial so downstream package builds because osqp_interface exposes osqp.hpp
 ament_export_include_directories("${OSQP_INCLUDE_DIR}")
 # crucial so the linking order is correct in a downstream package: libosqp_interface.a should come before libosqp.a
 ament_export_libraries(osqp::osqp)
 
+# Testing
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
-  find_package(ament_cmake_gtest REQUIRED)
-  ament_add_gtest(osqp_interface_test test/osqp_interface_test.cpp)
-  target_link_libraries(osqp_interface_test osqp_interface ${EIGEN3_LIBRARIES})
+  # Unit tests
+  set(TEST_SOURCES
+    test/test_osqp_interface.cpp
+    test/test_csc_matrix_conv.cpp
+  )
+  set(TEST_OSQP_INTERFACE_EXE test_osqp_interface)
+  ament_add_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES})
+  autoware_set_compile_options(${TEST_OSQP_INTERFACE_EXE})
+  target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME})
 endif()
 
-ament_auto_package()
+# ament package generation and installing
+ament_auto_package(INSTALL_TO_SHARE
+)
diff --git a/common/osqp_interface/design/osqp_interface-design.md b/common/osqp_interface/design/osqp_interface-design.md
@@ -0,0 +1,61 @@
+Interface for the OSQP library {#osqp_interface-package-design}
+===========
+
+This is the design document for the `osqp_interface` package.
+
+
+# Purpose / Use cases
+<!-- Required -->
+<!-- Things to consider:
+    - Why did we implement this feature? -->
+This packages provides a C++ interface for the [OSQP library](https://osqp.org/docs/solver/index.html).
+
+
+# Design
+<!-- Required -->
+<!-- Things to consider:
+    - How does it work? -->
+The class `OSQPInterface` takes a problem formulation as Eigen matrices and vectors, converts these objects into
+C-style Compressed-Column-Sparse matrices and dynamic arrays, loads the data into the OSQP workspace dataholder, and runs the optimizer.
+
+## Inputs / Outputs / API
+<!-- Required -->
+<!-- Things to consider:
+    - How do you use the package / API? -->
+ The interface can be used in several ways:
+
+   1. Initialize the interface WITHOUT data. Load the problem formulation at the optimization call.
+   ```
+        osqp_interface = OSQPInterface();
+        osqp_interface.optimize(P, A, q, l, u);
+   ```
+
+   2. Initialize the interface WITH data.
+   ```
+        osqp_interface = OSQPInterface(P, A, q, l, u);
+        osqp_interface.optimize();
+   ```
+
+   3. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs.
+   ```
+        osqp_interface = OSQPInterface(P, A, q, l, u);
+        osqp_interface.optimize();
+        osqp.initializeProblem(P_new, A_new, q_new, l_new, u_new);
+        osqp_interface.optimize();
+   ```
+
+The optimization results are returned as a vector by the optimization function.
+```
+ std::tuple<std::vector<double>, std::vector<double>> result = osqp_interface.optimize();
+ std::vector<double> param = std::get<0>(result);
+ double x_0 = param[0];
+ double x_1 = param[1];
+```
+
+# References / External links
+<!-- Optional -->
+- OSQP library: https://osqp.org/
+
+# Related issues
+<!-- Required -->
+- This package was introduced as a dependency of the MPC-based lateral controller: https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057
diff --git a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp b/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp
@@ -1,45 +1,52 @@
-// Copyright 2020 Tier IV, Inc.
+// Copyright 2021 The Autoware Foundation
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
 // You may obtain a copy of the License at
 //
-//     http://www.apache.org/licenses/LICENSE-2.0
+//     http://www.apache.org/licenses/LICENSE-2.0
 //
 // Unless required by applicable law or agreed to in writing, software
 // distributed under the License is distributed on an "AS IS" BASIS,
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
+
 #ifndef OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_
 #define OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_
 
-#include <eigen3/Eigen/Core>
-
-#include <osqp/types.h>  // for 'c_int' type ('long' or 'long long')
-
 #include <vector>
 
+#include "eigen3/Eigen/Core"
+#include "osqp/glob_opts.h"  // for 'c_int' type ('long' or 'long long')
+#include "osqp_interface/visibility_control.hpp"
+
+namespace autoware
+{
+namespace common
+{
 namespace osqp
 {
-// Struct for containing a 'Compressed-Column-Sparse Matrix' object.
-//
-// Elements:
-//   vals: Vector of non-zero values. Ex: [4,1,1,2]
-//   row_idxs:  Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner')
-//   col_idxs:  Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer')
-struct CSC_Matrix
+/// \brief Compressed-Column-Sparse Matrix
+struct OSQP_INTERFACE_PUBLIC CSC_Matrix
 {
-  std::vector<c_float> vals;
-  std::vector<c_int> row_idxs;
-  std::vector<c_int> col_idxs;
+  /// Vector of non-zero values. Ex: [4,1,1,2]
+  std::vector<c_float> m_vals;
+  /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner')
+  std::vector<c_int> m_row_idxs;
+  /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer')
+  std::vector<c_int> m_col_idxs;
 };
 
-CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat);
-CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat);
-
-void printCSCMatrix(CSC_Matrix & csc_mat);
+/// \brief Calculate CSC matrix from Eigen matrix
+OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat);
+/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix
+OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat);
+/// \brief Print the given CSC matrix to the standard output
+OSQP_INTERFACE_PUBLIC void printCSCMatrix(const CSC_Matrix & csc_mat);
 
 }  // namespace osqp
+}  // namespace common
+}  // namespace autoware
 
 #endif  // OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_