From 206bc1e0ebb5c79ee1b523cf5394ebef5e7dd167 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 18 May 2023 15:10:47 +0900 Subject: [PATCH 01/11] refactor(avoidance): use magic enum (#3745) Signed-off-by: satoshi-ota --- .../src/marker_util/avoidance/debug.cpp | 26 +++---------------- 1 file changed, 3 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp index e991b24111e9a..3d04dcd2f56b3 100644 --- a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp @@ -17,6 +17,8 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include + #include #include @@ -208,29 +210,7 @@ MarkerArray createEgoStatusMarkerArray( { std::ostringstream string_stream; string_stream << "ego_state:"; - switch (data.state) { - case AvoidanceState::NOT_AVOID: - string_stream << "NOT_AVOID"; - break; - case AvoidanceState::AVOID_PATH_NOT_READY: - string_stream << "AVOID_PATH_NOT_READY"; - marker.color = createMarkerColor(1.0, 0.0, 0.0, 0.999); - break; - case AvoidanceState::YIELD: - string_stream << "YIELD"; - marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999); - break; - case AvoidanceState::AVOID_PATH_READY: - string_stream << "AVOID_PATH_READY"; - marker.color = createMarkerColor(0.0, 1.0, 0.0, 0.999); - break; - case AvoidanceState::AVOID_EXECUTE: - string_stream << "AVOID_EXECUTE"; - marker.color = createMarkerColor(0.0, 1.0, 0.0, 0.999); - break; - default: - throw std::domain_error("invalid behavior"); - } + string_stream << magic_enum::enum_name(data.state); marker.text = string_stream.str(); marker.pose.position.z += 2.0; marker.id++; From 9bb9aefdf1911f630c19d928668fd69ea4379df2 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 18 May 2023 17:05:31 +0900 Subject: [PATCH 02/11] feat(qp_interface): add new package which will contain various qp solvers (#3697) Signed-off-by: Takayuki Murooka --- common/qp_interface/CMakeLists.txt | 59 +++ .../design/qp_interface-design.md | 48 +++ .../qp_interface/osqp_csc_matrix_conv.hpp | 46 ++ .../include/qp_interface/osqp_interface.hpp | 148 +++++++ .../include/qp_interface/qp_interface.hpp | 59 +++ common/qp_interface/package.xml | 29 ++ .../qp_interface/src/osqp_csc_matrix_conv.cpp | 134 ++++++ common/qp_interface/src/osqp_interface.cpp | 408 ++++++++++++++++++ common/qp_interface/src/qp_interface.cpp | 71 +++ .../test/test_csc_matrix_conv.cpp | 181 ++++++++ .../qp_interface/test/test_osqp_interface.cpp | 168 ++++++++ 11 files changed, 1351 insertions(+) create mode 100644 common/qp_interface/CMakeLists.txt create mode 100644 common/qp_interface/design/qp_interface-design.md create mode 100644 common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp create mode 100644 common/qp_interface/include/qp_interface/osqp_interface.hpp create mode 100644 common/qp_interface/include/qp_interface/qp_interface.hpp create mode 100644 common/qp_interface/package.xml create mode 100644 common/qp_interface/src/osqp_csc_matrix_conv.cpp create mode 100644 common/qp_interface/src/osqp_interface.cpp create mode 100644 common/qp_interface/src/qp_interface.cpp create mode 100644 common/qp_interface/test/test_csc_matrix_conv.cpp create mode 100644 common/qp_interface/test/test_osqp_interface.cpp diff --git a/common/qp_interface/CMakeLists.txt b/common/qp_interface/CMakeLists.txt new file mode 100644 index 0000000000000..92876a09f66e9 --- /dev/null +++ b/common/qp_interface/CMakeLists.txt @@ -0,0 +1,59 @@ +cmake_minimum_required(VERSION 3.14) +project(qp_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) + +# after find_package(osqp_vendor) in ament_auto_find_build_dependencies +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) + +set(QP_INTERFACE_LIB_SRC + src/qp_interface.cpp + src/osqp_interface.cpp + src/osqp_csc_matrix_conv.cpp +) + +set(QP_INTERFACE_LIB_HEADERS + include/qp_interface/qp_interface.hpp + include/qp_interface/osqp_interface.hpp + include/qp_interface/osqp_csc_matrix_conv.hpp +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + ${QP_INTERFACE_LIB_SRC} + ${QP_INTERFACE_LIB_HEADERS} +) +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast -Wno-error=useless-cast) + +target_include_directories(qp_interface + SYSTEM PUBLIC + "${OSQP_INCLUDE_DIR}" + "${EIGEN3_INCLUDE_DIR}" +) + +ament_target_dependencies(qp_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) + +if(BUILD_TESTING) + set(TEST_SOURCES + test/test_osqp_interface.cpp + test/test_csc_matrix_conv.cpp + ) + set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) + ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) + target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) +endif() + +ament_auto_package(INSTALL_TO_SHARE +) diff --git a/common/qp_interface/design/qp_interface-design.md b/common/qp_interface/design/qp_interface-design.md new file mode 100644 index 0000000000000..3843e5a742d1e --- /dev/null +++ b/common/qp_interface/design/qp_interface-design.md @@ -0,0 +1,48 @@ +# Interface for QP solvers + +This is the design document for the `qp_interface` package. + +## Purpose / Use cases + +This packages provides a C++ interface for QP solvers. +Currently, supported QP solvers are + +- [OSQP library](https://osqp.org/docs/solver/index.html) + +## Design + +The class `QPInterface` 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 QP workspace dataholder, and runs the optimizer. + +## Inputs / Outputs / API + +The interface can be used in several ways: + +1. Initialize the interface, and load the problem formulation at the optimization call. + + ```cpp + QPInterface qp_interface; + qp_interface.optimize(P, A, q, l, u); + ``` + +2. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. + + ```cpp + QPInterface qp_interface(true); + qp_interface.optimize(P, A, q, l, u); + qp_interface.optimize(P_new, A_new, q_new, l_new, u_new); + ``` + + The optimization results are returned as a vector by the optimization function. + + ```cpp + const auto solution = qp_interface.optimize(); + double x_0 = solution[0]; + double x_1 = solution[1]; + ``` + +## References / External links + +- OSQP library: + +## Related issues diff --git a/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp b/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp new file mode 100644 index 0000000000000..74ec4c1282f46 --- /dev/null +++ b/common/qp_interface/include/qp_interface/osqp_csc_matrix_conv.hpp @@ -0,0 +1,46 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. + +#ifndef QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ +#define QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ + +#include "osqp/glob_opts.h" + +#include + +#include + +namespace qp +{ +/// \brief Compressed-Column-Sparse Matrix +struct CSC_Matrix +{ + /// Vector of non-zero values. Ex: [4,1,1,2] + std::vector m_vals; + /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') + std::vector m_row_idxs; + /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') + std::vector m_col_idxs; +}; + +/// \brief Calculate CSC matrix from Eigen matrix +CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); +/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix +CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); +/// \brief Print the given CSC matrix to the standard output +void printCSCMatrix(const CSC_Matrix & csc_mat); + +} // namespace qp + +#endif // QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ diff --git a/common/qp_interface/include/qp_interface/osqp_interface.hpp b/common/qp_interface/include/qp_interface/osqp_interface.hpp new file mode 100644 index 0000000000000..e8bb160094f35 --- /dev/null +++ b/common/qp_interface/include/qp_interface/osqp_interface.hpp @@ -0,0 +1,148 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. + +#ifndef QP_INTERFACE__OSQP_INTERFACE_HPP_ +#define QP_INTERFACE__OSQP_INTERFACE_HPP_ + +#include "osqp/osqp.h" +#include "qp_interface/osqp_csc_matrix_conv.hpp" +#include "qp_interface/qp_interface.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace qp +{ +constexpr c_float INF = 1e30; + +class OSQPInterface : public QPInterface +{ +public: + /// \brief Constructor without problem formulation + OSQPInterface( + const bool enable_warm_start = false, + const c_float eps_abs = std::numeric_limits::epsilon(), const bool polish = true); + /// \brief Constructor with problem setup + /// \param P: (n,n) matrix defining relations between parameters. + /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. + /// \param q: (n) vector defining the linear cost of the problem. + /// \param l: (m) vector defining the lower bound problem constraint. + /// \param u: (m) vector defining the upper bound problem constraint. + /// \param eps_abs: Absolute convergence tolerance. + OSQPInterface( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u, + const bool enable_warm_start = false, + const c_float eps_abs = std::numeric_limits::epsilon()); + OSQPInterface( + const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, + const std::vector & l, const std::vector & u, + const bool enable_warm_start = false, + const c_float eps_abs = std::numeric_limits::epsilon()); + ~OSQPInterface(); + + static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; + + std::vector optimize( + CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, + const std::vector & u); + + int getIteration() const override; + int getStatus() const override; + int getPolishStatus() const; + std::vector getDualSolution() const; + + void updateEpsAbs(const double eps_abs) override; + void updateEpsRel(const double eps_rel) override; + void updateVerbose(const bool verbose) override; + + // Updates problem parameters while keeping solution in memory. + // + // Args: + // P_new: (n,n) matrix defining relations between parameters. + // A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound. + // q_new: (n) vector defining the linear cost of the problem. + // l_new: (m) vector defining the lower bound problem constraint. + // u_new: (m) vector defining the upper bound problem constraint. + void updateP(const Eigen::MatrixXd & P_new); + void updateCscP(const CSC_Matrix & P_csc); + void updateA(const Eigen::MatrixXd & A_new); + void updateCscA(const CSC_Matrix & A_csc); + void updateQ(const std::vector & q_new); + void updateL(const std::vector & l_new); + void updateU(const std::vector & u_new); + void updateBounds(const std::vector & l_new, const std::vector & u_new); + + void updateMaxIter(const int iter); + void updateRhoInterval(const int rho_interval); + void updateRho(const double rho); + void updateAlpha(const double alpha); + void updateScaling(const int scaling); + void updatePolish(const bool polish); + void updatePolishRefinementIteration(const int polish_refine_iter); + void updateCheckTermination(const int check_termination); + + /// \brief Get the number of iteration taken to solve the problem + inline int64_t getTakenIter() const { return static_cast(m_latest_work_info.iter); } + /// \brief Get the status message for the latest problem solved + inline std::string getStatusMessage() const + { + return static_cast(m_latest_work_info.status); + } + /// \brief Get the runtime of the latest problem solved + inline double getRunTime() const { return m_latest_work_info.run_time; } + /// \brief Get the objective value the latest problem solved + inline double getObjVal() const { return m_latest_work_info.obj_val; } + /// \brief Returns flag asserting interface condition (Healthy condition: 0). + inline int64_t getExitFlag() const { return m_exitflag; } + + void logUnsolvedStatus(const std::string & prefix_message = "") const; + + // Setter functions for warm start + bool setWarmStart( + const std::vector & primal_variables, const std::vector & dual_variables); + bool setPrimalVariables(const std::vector & primal_variables); + bool setDualVariables(const std::vector & dual_variables); + +private: + std::unique_ptr> m_work; + std::unique_ptr m_settings; + std::unique_ptr m_data; + // store last work info since work is cleaned up at every execution to prevent memory leak. + OSQPInfo m_latest_work_info; + // Number of parameters to optimize + int64_t m_param_n; + // Flag to check if the current work exists + bool m_work_initialized = false; + // Exitflag + int64_t m_exitflag; + + void initializeProblemImpl( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) override; + + void initializeCSCProblemImpl( + CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, + const std::vector & u); + + std::vector optimizeImpl() override; +}; +} // namespace qp + +#endif // QP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/qp_interface/include/qp_interface/qp_interface.hpp b/common/qp_interface/include/qp_interface/qp_interface.hpp new file mode 100644 index 0000000000000..1ddd606166026 --- /dev/null +++ b/common/qp_interface/include/qp_interface/qp_interface.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. + +#ifndef QP_INTERFACE__QP_INTERFACE_HPP_ +#define QP_INTERFACE__QP_INTERFACE_HPP_ + +#include + +#include +#include +#include +#include +#include + +namespace qp +{ +class QPInterface +{ +public: + explicit QPInterface(const bool enable_warm_start) : m_enable_warm_start(enable_warm_start) {} + + std::vector optimize( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u); + + virtual int getIteration() const = 0; + virtual int getStatus() const = 0; + + virtual void updateEpsAbs([[maybe_unused]] const double eps_abs) = 0; + virtual void updateEpsRel([[maybe_unused]] const double eps_rel) = 0; + virtual void updateVerbose([[maybe_unused]] const bool verbose) {} + +protected: + bool m_enable_warm_start; + + void initializeProblem( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u); + + virtual void initializeProblemImpl( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) = 0; + + virtual std::vector optimizeImpl() = 0; +}; +} // namespace qp + +#endif // QP_INTERFACE__QP_INTERFACE_HPP_ diff --git a/common/qp_interface/package.xml b/common/qp_interface/package.xml new file mode 100644 index 0000000000000..e41d591337aa6 --- /dev/null +++ b/common/qp_interface/package.xml @@ -0,0 +1,29 @@ + + + + qp_interface + 1.0.0 + Interface for the QP solvers + Takayuki Murooka + Fumiya Watanabe + Maxime CLEMENT + Satoshi Ota + Apache 2.0 + + ament_cmake_auto + autoware_cmake + + eigen + osqp_vendor + rclcpp + rclcpp_components + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/common/qp_interface/src/osqp_csc_matrix_conv.cpp b/common/qp_interface/src/osqp_csc_matrix_conv.cpp new file mode 100644 index 0000000000000..0faf7586463fd --- /dev/null +++ b/common/qp_interface/src/osqp_csc_matrix_conv.cpp @@ -0,0 +1,134 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. + +#include "qp_interface/osqp_csc_matrix_conv.hpp" + +#include +#include + +#include +#include +#include + +namespace qp +{ +CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) +{ + const size_t elem = static_cast(mat.nonZeros()); + const Eigen::Index rows = mat.rows(); + const Eigen::Index cols = mat.cols(); + + std::vector vals; + vals.reserve(elem); + std::vector row_idxs; + row_idxs.reserve(elem); + std::vector col_idxs; + col_idxs.reserve(elem); + + // Construct CSC matrix arrays + c_float val; + c_int elem_count = 0; + + col_idxs.push_back(0); + + for (Eigen::Index j = 0; j < cols; j++) { // col iteration + for (Eigen::Index i = 0; i < rows; i++) { // row iteration + // Get values of nonzero elements + val = mat(i, j); + if (std::fabs(val) < 1e-9) { + continue; + } + elem_count += 1; + + // Store values + vals.push_back(val); + row_idxs.push_back(i); + } + + col_idxs.push_back(elem_count); + } + + CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; + + return csc_matrix; +} + +CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) +{ + const size_t elem = static_cast(mat.nonZeros()); + const Eigen::Index rows = mat.rows(); + const Eigen::Index cols = mat.cols(); + + if (rows != cols) { + throw std::invalid_argument("Matrix must be square (n, n)"); + } + + std::vector vals; + vals.reserve(elem); + std::vector row_idxs; + row_idxs.reserve(elem); + std::vector col_idxs; + col_idxs.reserve(elem); + + // Construct CSC matrix arrays + c_float val; + Eigen::Index trap_last_idx = 0; + c_int elem_count = 0; + + col_idxs.push_back(0); + + for (Eigen::Index j = 0; j < cols; j++) { // col iteration + for (Eigen::Index i = 0; i <= trap_last_idx; i++) { // row iteration + // Get values of nonzero elements + val = mat(i, j); + if (std::fabs(val) < 1e-9) { + continue; + } + elem_count += 1; + + // Store values + vals.push_back(val); + row_idxs.push_back(i); + } + + col_idxs.push_back(elem_count); + trap_last_idx += 1; + } + + CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; + + return csc_matrix; +} + +void printCSCMatrix(const CSC_Matrix & csc_mat) +{ + std::cout << "["; + for (const c_float & val : csc_mat.m_vals) { + std::cout << val << ", "; + } + std::cout << "]\n"; + + std::cout << "["; + for (const c_int & row : csc_mat.m_row_idxs) { + std::cout << row << ", "; + } + std::cout << "]\n"; + + std::cout << "["; + for (const c_int & col : csc_mat.m_col_idxs) { + std::cout << col << ", "; + } + std::cout << "]\n"; +} +} // namespace qp diff --git a/common/qp_interface/src/osqp_interface.cpp b/common/qp_interface/src/osqp_interface.cpp new file mode 100644 index 0000000000000..8a4414b287568 --- /dev/null +++ b/common/qp_interface/src/osqp_interface.cpp @@ -0,0 +1,408 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. + +#include "qp_interface/osqp_interface.hpp" + +#include "osqp/osqp.h" +#include "qp_interface/osqp_csc_matrix_conv.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace qp +{ +OSQPInterface::OSQPInterface(const bool enable_warm_start, const c_float eps_abs, const bool polish) +: QPInterface(enable_warm_start), m_work{nullptr, OSQPWorkspaceDeleter} +{ + m_settings = std::make_unique(); + m_data = std::make_unique(); + + if (m_settings) { + osqp_set_default_settings(m_settings.get()); + m_settings->alpha = 1.6; // Change alpha parameter + m_settings->eps_rel = 1.0E-4; + m_settings->eps_abs = eps_abs; + m_settings->eps_prim_inf = 1.0E-4; + m_settings->eps_dual_inf = 1.0E-4; + m_settings->warm_start = true; + m_settings->max_iter = 4000; + m_settings->verbose = false; + m_settings->polish = polish; + } + m_exitflag = 0; +} + +OSQPInterface::OSQPInterface( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u, const bool enable_warm_start, + const c_float eps_abs) +: OSQPInterface(enable_warm_start, eps_abs) +{ + initializeProblem(P, A, q, l, u); +} + +OSQPInterface::OSQPInterface( + const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, + const std::vector & l, const std::vector & u, const bool enable_warm_start, + const c_float eps_abs) +: OSQPInterface(enable_warm_start, eps_abs) +{ + initializeCSCProblemImpl(P, A, q, l, u); +} + +OSQPInterface::~OSQPInterface() +{ + if (m_data->P) free(m_data->P); + if (m_data->A) free(m_data->A); +} + +void OSQPInterface::initializeProblemImpl( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + initializeCSCProblemImpl(P_csc, A_csc, q, l, u); +} + +void OSQPInterface::initializeCSCProblemImpl( + CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, + const std::vector & u) +{ + // Dynamic float arrays + std::vector q_tmp(q.begin(), q.end()); + std::vector l_tmp(l.begin(), l.end()); + std::vector u_tmp(u.begin(), u.end()); + double * q_dyn = q_tmp.data(); + double * l_dyn = l_tmp.data(); + double * u_dyn = u_tmp.data(); + + /********************** + * OBJECTIVE FUNCTION + **********************/ + m_param_n = static_cast(q.size()); + m_data->m = static_cast(l.size()); + + /***************** + * POPULATE DATA + *****************/ + m_data->n = m_param_n; + if (m_data->P) free(m_data->P); + m_data->P = csc_matrix( + m_data->n, m_data->n, static_cast(P_csc.m_vals.size()), P_csc.m_vals.data(), + P_csc.m_row_idxs.data(), P_csc.m_col_idxs.data()); + m_data->q = q_dyn; + if (m_data->A) free(m_data->A); + m_data->A = csc_matrix( + m_data->m, m_data->n, static_cast(A_csc.m_vals.size()), A_csc.m_vals.data(), + A_csc.m_row_idxs.data(), A_csc.m_col_idxs.data()); + m_data->l = l_dyn; + m_data->u = u_dyn; + + // Setup workspace + OSQPWorkspace * workspace; + m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get()); + m_work.reset(workspace); + m_work_initialized = true; +} + +void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept +{ + if (ptr != nullptr) { + osqp_cleanup(ptr); + } +} + +void OSQPInterface::updateEpsAbs(const double eps_abs) +{ + m_settings->eps_abs = eps_abs; // for default setting + if (m_work_initialized) { + osqp_update_eps_abs(m_work.get(), eps_abs); // for current work + } +} + +void OSQPInterface::updateEpsRel(const double eps_rel) +{ + m_settings->eps_rel = eps_rel; // for default setting + if (m_work_initialized) { + osqp_update_eps_rel(m_work.get(), eps_rel); // for current work + } +} + +void OSQPInterface::updateMaxIter(const int max_iter) +{ + m_settings->max_iter = max_iter; // for default setting + if (m_work_initialized) { + osqp_update_max_iter(m_work.get(), max_iter); // for current work + } +} + +void OSQPInterface::updateVerbose(const bool is_verbose) +{ + m_settings->verbose = is_verbose; // for default setting + if (m_work_initialized) { + osqp_update_verbose(m_work.get(), is_verbose); // for current work + } +} + +void OSQPInterface::updateRhoInterval(const int rho_interval) +{ + m_settings->adaptive_rho_interval = rho_interval; // for default setting +} + +void OSQPInterface::updateRho(const double rho) +{ + m_settings->rho = rho; + if (m_work_initialized) { + osqp_update_rho(m_work.get(), rho); + } +} + +void OSQPInterface::updateAlpha(const double alpha) +{ + m_settings->alpha = alpha; + if (m_work_initialized) { + osqp_update_alpha(m_work.get(), alpha); + } +} + +void OSQPInterface::updateScaling(const int scaling) +{ + m_settings->scaling = scaling; +} + +void OSQPInterface::updatePolish(const bool polish) +{ + m_settings->polish = polish; + if (m_work_initialized) { + osqp_update_polish(m_work.get(), polish); + } +} + +void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) +{ + if (polish_refine_iter < 0) { + std::cerr << "Polish refinement iterations must be positive" << std::endl; + return; + } + + m_settings->polish_refine_iter = polish_refine_iter; + if (m_work_initialized) { + osqp_update_polish_refine_iter(m_work.get(), polish_refine_iter); + } +} + +void OSQPInterface::updateCheckTermination(const int check_termination) +{ + if (check_termination < 0) { + std::cerr << "Check termination must be positive" << std::endl; + return; + } + + m_settings->check_termination = check_termination; + if (m_work_initialized) { + osqp_update_check_termination(m_work.get(), check_termination); + } +} + +bool OSQPInterface::setWarmStart( + const std::vector & primal_variables, const std::vector & dual_variables) +{ + return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); +} + +bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) +{ + if (!m_work_initialized) { + return false; + } + + const auto result = osqp_warm_start_x(m_work.get(), primal_variables.data()); + if (result != 0) { + std::cerr << "Failed to set primal variables for warm start" << std::endl; + return false; + } + + return true; +} + +bool OSQPInterface::setDualVariables(const std::vector & dual_variables) +{ + if (!m_work_initialized) { + return false; + } + + const auto result = osqp_warm_start_y(m_work.get(), dual_variables.data()); + if (result != 0) { + std::cerr << "Failed to set dual variables for warm start" << std::endl; + return false; + } + + return true; +} + +void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const +{ + const int status = getStatus(); + if (status == 1) { + // No need to log since optimization was solved. + return; + } + + // create message + std::string output_message = ""; + if (prefix_message != "") { + output_message = prefix_message + " "; + } + + const auto status_message = getStatusMessage(); + output_message += "Optimization failed due to " + status_message; + + // log with warning + RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), output_message.c_str()); +} + +void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) +{ + /* + // Transform 'P' into an 'upper trapezoidal matrix' + Eigen::MatrixXd P_trap = P_new.triangularView(); + // Transform 'P' into a sparse matrix and extract data as dynamic arrays + Eigen::SparseMatrix P_sparse = P_trap.sparseView(); + double *P_val_ptr = P_sparse.valuePtr(); + // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) + c_int P_elem_N = P_sparse.nonZeros(); + */ + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); + osqp_update_P( + m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); +} + +void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) +{ + osqp_update_P( + m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); +} + +void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) +{ + /* + // Transform 'A' into a sparse matrix and extract data as dynamic arrays + Eigen::SparseMatrix A_sparse = A_new.sparseView(); + double *A_val_ptr = A_sparse.valuePtr(); + // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) + c_int A_elem_N = A_sparse.nonZeros(); + */ + CSC_Matrix A_csc = calCSCMatrix(A_new); + osqp_update_A( + m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); + return; +} + +void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) +{ + osqp_update_A( + m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); +} + +void OSQPInterface::updateQ(const std::vector & q_new) +{ + std::vector q_tmp(q_new.begin(), q_new.end()); + double * q_dyn = q_tmp.data(); + osqp_update_lin_cost(m_work.get(), q_dyn); +} + +void OSQPInterface::updateL(const std::vector & l_new) +{ + std::vector l_tmp(l_new.begin(), l_new.end()); + double * l_dyn = l_tmp.data(); + osqp_update_lower_bound(m_work.get(), l_dyn); +} + +void OSQPInterface::updateU(const std::vector & u_new) +{ + std::vector u_tmp(u_new.begin(), u_new.end()); + double * u_dyn = u_tmp.data(); + osqp_update_upper_bound(m_work.get(), u_dyn); +} + +void OSQPInterface::updateBounds( + const std::vector & l_new, const std::vector & u_new) +{ + std::vector l_tmp(l_new.begin(), l_new.end()); + std::vector u_tmp(u_new.begin(), u_new.end()); + double * l_dyn = l_tmp.data(); + double * u_dyn = u_tmp.data(); + osqp_update_bounds(m_work.get(), l_dyn, u_dyn); +} + +int OSQPInterface::getIteration() const +{ + return m_work->info->iter; +} + +int OSQPInterface::getStatus() const +{ + return static_cast(m_latest_work_info.status_val); +} + +int OSQPInterface::getPolishStatus() const +{ + return static_cast(m_latest_work_info.status_polish); +} + +std::vector OSQPInterface::getDualSolution() const +{ + double * sol_y = m_work->solution->y; + std::vector dual_solution(sol_y, sol_y + m_data->m); + return dual_solution; +} + +std::vector OSQPInterface::optimizeImpl() +{ + osqp_solve(m_work.get()); + + double * sol_x = m_work->solution->x; + double * sol_y = m_work->solution->y; + std::vector sol_primal(sol_x, sol_x + m_param_n); + std::vector sol_lagrange_multiplier(sol_y, sol_y + m_data->m); + + m_latest_work_info = *(m_work->info); + + if (!m_enable_warm_start) { + m_work.reset(); + m_work_initialized = false; + } + + return sol_primal; +} + +std::vector OSQPInterface::optimize( + CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, + const std::vector & u) +{ + initializeCSCProblemImpl(P, A, q, l, u); + const auto result = optimizeImpl(); + + return result; +} + +} // namespace qp diff --git a/common/qp_interface/src/qp_interface.cpp b/common/qp_interface/src/qp_interface.cpp new file mode 100644 index 0000000000000..20859301156f9 --- /dev/null +++ b/common/qp_interface/src/qp_interface.cpp @@ -0,0 +1,71 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. + +#include "qp_interface/qp_interface.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace qp +{ +void QPInterface::initializeProblem( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + // check if arguments are valid + std::stringstream ss; + if (P.rows() != P.cols()) { + ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows() + << ", P.cols() = " << P.cols(); + throw std::invalid_argument(ss.str()); + } + if (P.rows() != static_cast(q.size())) { + ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows() + << ", q.size() = " << q.size(); + throw std::invalid_argument(ss.str()); + } + if (P.rows() != A.cols()) { + ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows() + << ", A.cols() = " << A.cols(); + throw std::invalid_argument(ss.str()); + } + if (A.rows() != static_cast(l.size())) { + ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows() + << ", l.size() = " << l.size(); + throw std::invalid_argument(ss.str()); + } + if (A.rows() != static_cast(u.size())) { + ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows() + << ", u.size() = " << u.size(); + throw std::invalid_argument(ss.str()); + } + + initializeProblemImpl(P, A, q, l, u); +} + +std::vector QPInterface::optimize( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + initializeProblem(P, A, q, l, u); + const auto result = optimizeImpl(); + + return result; +} +} // namespace qp diff --git a/common/qp_interface/test/test_csc_matrix_conv.cpp b/common/qp_interface/test/test_csc_matrix_conv.cpp new file mode 100644 index 0000000000000..fc604d762d2c4 --- /dev/null +++ b/common/qp_interface/test/test_csc_matrix_conv.cpp @@ -0,0 +1,181 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. + +#include "gtest/gtest.h" +#include "qp_interface/osqp_csc_matrix_conv.hpp" + +#include + +#include +#include +#include + +TEST(TestCscMatrixConv, Nominal) +{ + using qp::calCSCMatrix; + using qp::CSC_Matrix; + + Eigen::MatrixXd rect1(1, 2); + rect1 << 0.0, 1.0; + + const CSC_Matrix rect_m1 = calCSCMatrix(rect1); + ASSERT_EQ(rect_m1.m_vals.size(), size_t(1)); + EXPECT_EQ(rect_m1.m_vals[0], 1.0); + ASSERT_EQ(rect_m1.m_row_idxs.size(), size_t(1)); + EXPECT_EQ(rect_m1.m_row_idxs[0], c_int(0)); + ASSERT_EQ(rect_m1.m_col_idxs.size(), size_t(3)); // nb of columns + 1 + EXPECT_EQ(rect_m1.m_col_idxs[0], c_int(0)); + EXPECT_EQ(rect_m1.m_col_idxs[1], c_int(0)); + EXPECT_EQ(rect_m1.m_col_idxs[2], c_int(1)); + + Eigen::MatrixXd rect2(2, 4); + rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; + + const CSC_Matrix rect_m2 = calCSCMatrix(rect2); + ASSERT_EQ(rect_m2.m_vals.size(), size_t(4)); + EXPECT_EQ(rect_m2.m_vals[0], 1.0); + EXPECT_EQ(rect_m2.m_vals[1], 6.0); + EXPECT_EQ(rect_m2.m_vals[2], 3.0); + EXPECT_EQ(rect_m2.m_vals[3], 7.0); + ASSERT_EQ(rect_m2.m_row_idxs.size(), size_t(4)); + EXPECT_EQ(rect_m2.m_row_idxs[0], c_int(0)); + EXPECT_EQ(rect_m2.m_row_idxs[1], c_int(1)); + EXPECT_EQ(rect_m2.m_row_idxs[2], c_int(0)); + EXPECT_EQ(rect_m2.m_row_idxs[3], c_int(1)); + ASSERT_EQ(rect_m2.m_col_idxs.size(), size_t(5)); // nb of columns + 1 + EXPECT_EQ(rect_m2.m_col_idxs[0], c_int(0)); + EXPECT_EQ(rect_m2.m_col_idxs[1], c_int(1)); + EXPECT_EQ(rect_m2.m_col_idxs[2], c_int(2)); + EXPECT_EQ(rect_m2.m_col_idxs[3], c_int(4)); + EXPECT_EQ(rect_m2.m_col_idxs[4], c_int(4)); + + // Example from http://netlib.org/linalg/html_templates/node92.html + Eigen::MatrixXd square2(6, 6); + square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0, 3.0, 9.0, 0.0, 0.0, 0.0, 3.0, 0.0, 7.0, 8.0, 7.0, 0.0, + 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; + + const CSC_Matrix square_m2 = calCSCMatrix(square2); + ASSERT_EQ(square_m2.m_vals.size(), size_t(19)); + EXPECT_EQ(square_m2.m_vals[0], 10.0); + EXPECT_EQ(square_m2.m_vals[1], 3.0); + EXPECT_EQ(square_m2.m_vals[2], 3.0); + EXPECT_EQ(square_m2.m_vals[3], 9.0); + EXPECT_EQ(square_m2.m_vals[4], 7.0); + EXPECT_EQ(square_m2.m_vals[5], 8.0); + EXPECT_EQ(square_m2.m_vals[6], 4.0); + EXPECT_EQ(square_m2.m_vals[7], 8.0); + EXPECT_EQ(square_m2.m_vals[8], 8.0); + EXPECT_EQ(square_m2.m_vals[9], 7.0); + EXPECT_EQ(square_m2.m_vals[10], 7.0); + EXPECT_EQ(square_m2.m_vals[11], 9.0); + EXPECT_EQ(square_m2.m_vals[12], -2.0); + EXPECT_EQ(square_m2.m_vals[13], 5.0); + EXPECT_EQ(square_m2.m_vals[14], 9.0); + EXPECT_EQ(square_m2.m_vals[15], 2.0); + EXPECT_EQ(square_m2.m_vals[16], 3.0); + EXPECT_EQ(square_m2.m_vals[17], 13.0); + EXPECT_EQ(square_m2.m_vals[18], -1.0); + ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(19)); + EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); + EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); + EXPECT_EQ(square_m2.m_row_idxs[2], c_int(3)); + EXPECT_EQ(square_m2.m_row_idxs[3], c_int(1)); + EXPECT_EQ(square_m2.m_row_idxs[4], c_int(2)); + EXPECT_EQ(square_m2.m_row_idxs[5], c_int(4)); + EXPECT_EQ(square_m2.m_row_idxs[6], c_int(5)); + EXPECT_EQ(square_m2.m_row_idxs[7], c_int(2)); + EXPECT_EQ(square_m2.m_row_idxs[8], c_int(3)); + EXPECT_EQ(square_m2.m_row_idxs[9], c_int(2)); + EXPECT_EQ(square_m2.m_row_idxs[10], c_int(3)); + EXPECT_EQ(square_m2.m_row_idxs[11], c_int(4)); + EXPECT_EQ(square_m2.m_row_idxs[12], c_int(0)); + EXPECT_EQ(square_m2.m_row_idxs[13], c_int(3)); + EXPECT_EQ(square_m2.m_row_idxs[14], c_int(4)); + EXPECT_EQ(square_m2.m_row_idxs[15], c_int(5)); + EXPECT_EQ(square_m2.m_row_idxs[16], c_int(1)); + EXPECT_EQ(square_m2.m_row_idxs[17], c_int(4)); + EXPECT_EQ(square_m2.m_row_idxs[18], c_int(5)); + ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(7)); // nb of columns + 1 + EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); + EXPECT_EQ(square_m2.m_col_idxs[1], c_int(3)); + EXPECT_EQ(square_m2.m_col_idxs[2], c_int(7)); + EXPECT_EQ(square_m2.m_col_idxs[3], c_int(9)); + EXPECT_EQ(square_m2.m_col_idxs[4], c_int(12)); + EXPECT_EQ(square_m2.m_col_idxs[5], c_int(16)); + EXPECT_EQ(square_m2.m_col_idxs[6], c_int(19)); +} +TEST(TestCscMatrixConv, Trapezoidal) +{ + using qp::calCSCMatrixTrapezoidal; + using qp::CSC_Matrix; + + Eigen::MatrixXd square1(2, 2); + Eigen::MatrixXd square2(3, 3); + Eigen::MatrixXd rect1(1, 2); + square1 << 1.0, 2.0, 2.0, 4.0; + square2 << 0.0, 2.0, 0.0, 4.0, 5.0, 6.0, 0.0, 0.0, 0.0; + rect1 << 0.0, 1.0; + + const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); + // Trapezoidal: skip the lower left triangle (2.0 in this example) + ASSERT_EQ(square_m1.m_vals.size(), size_t(3)); + EXPECT_EQ(square_m1.m_vals[0], 1.0); + EXPECT_EQ(square_m1.m_vals[1], 2.0); + EXPECT_EQ(square_m1.m_vals[2], 4.0); + ASSERT_EQ(square_m1.m_row_idxs.size(), size_t(3)); + EXPECT_EQ(square_m1.m_row_idxs[0], c_int(0)); + EXPECT_EQ(square_m1.m_row_idxs[1], c_int(0)); + EXPECT_EQ(square_m1.m_row_idxs[2], c_int(1)); + ASSERT_EQ(square_m1.m_col_idxs.size(), size_t(3)); + EXPECT_EQ(square_m1.m_col_idxs[0], c_int(0)); + EXPECT_EQ(square_m1.m_col_idxs[1], c_int(1)); + EXPECT_EQ(square_m1.m_col_idxs[2], c_int(3)); + + const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); + ASSERT_EQ(square_m2.m_vals.size(), size_t(3)); + EXPECT_EQ(square_m2.m_vals[0], 2.0); + EXPECT_EQ(square_m2.m_vals[1], 5.0); + EXPECT_EQ(square_m2.m_vals[2], 6.0); + ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(3)); + EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); + EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); + EXPECT_EQ(square_m2.m_row_idxs[2], c_int(1)); + ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(4)); + EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); + EXPECT_EQ(square_m2.m_col_idxs[1], c_int(0)); + EXPECT_EQ(square_m2.m_col_idxs[2], c_int(2)); + EXPECT_EQ(square_m2.m_col_idxs[3], c_int(3)); + + try { + const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); + FAIL() << "calCSCMatrixTrapezoidal should fail with non-square inputs"; + } catch (const std::invalid_argument & e) { + EXPECT_EQ(e.what(), std::string("Matrix must be square (n, n)")); + } +} +TEST(TestCscMatrixConv, Print) +{ + using qp::calCSCMatrix; + using qp::calCSCMatrixTrapezoidal; + using qp::CSC_Matrix; + using qp::printCSCMatrix; + Eigen::MatrixXd square1(2, 2); + Eigen::MatrixXd rect1(1, 2); + square1 << 1.0, 2.0, 2.0, 4.0; + rect1 << 0.0, 1.0; + const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); + const CSC_Matrix rect_m1 = calCSCMatrix(rect1); + printCSCMatrix(square_m1); + printCSCMatrix(rect_m1); +} diff --git a/common/qp_interface/test/test_osqp_interface.cpp b/common/qp_interface/test/test_osqp_interface.cpp new file mode 100644 index 0000000000000..06e8f53d6c740 --- /dev/null +++ b/common/qp_interface/test/test_osqp_interface.cpp @@ -0,0 +1,168 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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. + +#include "gtest/gtest.h" +#include "qp_interface/osqp_interface.hpp" + +#include + +#include +#include + +namespace +{ +// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py +// +// min 1/2 * x' * P * x + q' * x +// s.t. lb <= A * x <= ub +// +// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] +// [1, 2] [1] [1, 0] [ 0] [0.7] +// [0, 1] [ 0] [0.7] +// [0, 1] [-inf] [inf] +// +// The optimal solution is +// x = [0.3, 0.7]' +// y = [-2.9, 0.0, 0.2, 0.0]` +// obj = 1.88 + +// cppcheck-suppress syntaxError +TEST(TestOsqpInterface, BasicQp) +{ + using qp::calCSCMatrix; + using qp::calCSCMatrixTrapezoidal; + using qp::CSC_Matrix; + + auto check_result = []( + const auto & solution, const int solution_status, const int polish_status) { + EXPECT_EQ(solution_status, 1); + EXPECT_EQ(polish_status, 1); + + static const auto ep = 1.0e-8; + + ASSERT_EQ(solution.size(), size_t(2)); + EXPECT_NEAR(solution[0], 0.3, ep); + EXPECT_NEAR(solution[1], 0.7, ep); + }; + + const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); + const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); + const std::vector q = {1.0, 1.0}; + const std::vector l = {1.0, 0.0, 0.0, -qp::INF}; + const std::vector u = {1.0, 0.7, 0.7, qp::INF}; + + { + // Define problem during optimization + qp::OSQPInterface osqp(false, 1e-6); + const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); + const auto solution_status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, solution_status, polish_status); + } + + { + // Define problem during initialization + qp::OSQPInterface osqp(false, 1e-6); + const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); + const auto solution_status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, solution_status, polish_status); + } + + { + std::tuple, std::vector, int, int, int> result; + // Dummy initial problem + Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); + qp::OSQPInterface osqp(false, 1e-6); + osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); + } + + { + // Define problem during initialization with csc matrix + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + qp::OSQPInterface osqp(false, 1e-6); + + const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); + const auto solution_status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, solution_status, polish_status); + } + + { + std::tuple, std::vector, int, int, int> result; + // Dummy initial problem with csc matrix + CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); + CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); + qp::OSQPInterface osqp(false, 1e-6); + osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); + + // Redefine problem before optimization + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + + const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); + const auto solution_status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, solution_status, polish_status); + } + + // add warm startup + { + // Dummy initial problem with csc matrix + CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); + CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); + qp::OSQPInterface osqp(true, 1e-6); // enable warm start + osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); + + // Redefine problem before optimization + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + { + const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); + const auto solution_status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, solution_status, polish_status); + + osqp.updateCheckTermination(1); + const auto primal_val = solution; + const auto dual_val = osqp.getDualSolution(); + for (size_t i = 0; i < primal_val.size(); ++i) { + std::cerr << primal_val.at(i) << std::endl; + } + osqp.setWarmStart(primal_val, dual_val); + } + + { + const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); + const auto solution_status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, solution_status, polish_status); + } + + // NOTE: This should be true, but currently optimize function reset the workspace, which + // disables warm start. EXPECT_EQ(osqp.getTakenIter(), 1); + } +} +} // namespace From e179ded36f896eb268c5043f71de0b5de1716107 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 18 May 2023 17:07:03 +0900 Subject: [PATCH 03/11] fix(behavior_path_planner): extract obstacles from drivable area without intersection (#3741) * fix(behavior_path_planner): extract obstacles from drivable area without intersection Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../behavior_path_planner/src/utils/utils.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 2f5548b16c8db..36bdf3b8bb98d 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -224,6 +224,7 @@ std::vector generatePolygonInsideBounds( full_polygon.push_back(polygon_point); } + // 1. check the case where the polygon intersects the bound std::vector inside_poly; bool has_intersection = false; // NOTE: between obstacle polygon and bound for (int i = 0; i < static_cast(full_polygon.size()); ++i) { @@ -262,10 +263,24 @@ std::vector generatePolygonInsideBounds( inside_poly.push_back(intersect_point); continue; } - if (has_intersection) { return inside_poly; } + + // 2. check the case where the polygon does not intersect the bound + const bool is_polygon_fully_inside_bounds = [&]() { + for (const auto & curr_poly : full_polygon) { + const bool is_curr_outside = curr_poly.is_outside_bounds(is_object_right); + if (is_curr_outside) { + return false; + } + } + return true; + }(); + if (is_polygon_fully_inside_bounds) { + return full_polygon; + } + return std::vector{}; } From add39cb5596fcaac0effb1dc266df6add203c335 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 18 May 2023 17:30:58 +0900 Subject: [PATCH 04/11] docs(dynamic_avoidance): add logic to extract objects from drivable area (#3728) * docs(dynamic_avoidance): add logic to extract objects from drivable area Signed-off-by: Takayuki Murooka * update docs Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- ...r_path_planner_dynamic_avoidance_design.md | 36 ++- .../drivable_area_extraction_width.drawio.svg | 145 +++++++++++ .../opposite_directional_object.svg | 235 ++++++++++++++++++ .../same_directional_object.svg | 190 ++++++++++++++ 4 files changed, 605 insertions(+), 1 deletion(-) create mode 100644 planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg create mode 100644 planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg create mode 100644 planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md index b428eadf3e7fd..5ac4e47a724d7 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md @@ -9,7 +9,41 @@ This module is under development. In the current implementation, the dynamic obstacles to avoid is extracted from the drivable area. Then the motion planner, in detail obstacle_avoidance_planner, will generate an avoiding trajectory. -### Parameters +## Overview of drivable area modification + +### Filtering obstacles to avoid + +The dynamics obstacles meeting the following condition will be avoided. + +- The type is designated as `target_object.*`. +- The norm of the obstacle's velocity projected to the ego's path is smaller than `target_object.min_obstacle_vel`. +- The obstacle is in the next lane to the ego's lane, which will not cut-into the ego's lane according to the highest-prioritized predicted path. + +### Drivable area modification + +To realize dynamic obstacles for avoidance, the time dimension should be take into an account considering the dynamics. +However, it will make the planning problem much harder to solve. +Therefore, we project the time dimension to the 2D pose dimension. + +Currently, the predicted paths of predicted objects are not so stable. +Therefore, instead of using the predicted paths, we assume that the obstacle will run parallel to the ego's path. + +First, a maximum lateral offset to avoid is calculated as follows. +The polygon's width to extract from the drivable area is the obstacle width and double `drivable_area_generation.lat_offset_from_obstacle`. +We can limit the lateral shift offset by `drivable_area_generation.max_lat_offset_to_avoid`. + +![drivable_area_extraction_width](../image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg) + +Then, extracting the same directional and opposite directional obstacles from the drivable area will work as follows considering TTC (time to collision). +Regarding the same directional obstacles, obstacles whose TTC is negative will be ignored (e.g. The obstacle is in front of the ego, and the obstacle's velocity is larger than the ego's velocity.). + +Same directional obstacles +![same_directional_object](../image/dynamic_avoidance/same_directional_object.svg) + +Opposite directional obstacles +![opposite_directional_object](../image/dynamic_avoidance/opposite_directional_object.svg) + +## Parameters | Name | Unit | Type | Description | Default value | | :----------------------------------------------------------------- | :---- | :----- | :---------------------------------------------------------- | :------------ | diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg b/planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg new file mode 100644 index 0000000000000..e0741781998bf --- /dev/null +++ b/planning/behavior_path_planner/image/dynamic_avoidance/drivable_area_extraction_width.drawio.svg @@ -0,0 +1,145 @@ + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ max lateral offset to avoid +
+
+
+ + max lateral o... + + + + + + +
+
+
+ lateral offset from obstacle +
+
+
+ + lateral offse... + + + + + + +
+
+
+ reference path +
+
+
+ + reference path + + + + + + +
+
+
+ obstacle polygon to extract from the drivable area +
+
+
+ + obstacle polygon to extrac... + + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg b/planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg new file mode 100644 index 0000000000000..d9b4f6dc35bf1 --- /dev/null +++ b/planning/behavior_path_planner/image/dynamic_avoidance/opposite_directional_object.svg @@ -0,0 +1,235 @@ + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + + + + + +
+
+
+ reference path +
+
+
+ + reference path + + + + + + + + + + + +
+
+
+ time to collision (cropped) +
+ * +
+ obstacle velocity +
+
+
+ + time to collision (cropped)... + + + + + + + + + + + + + +
+
+
+ margin time to avoid +
+ * +
+ obstacle velocity +
+
+
+ + margin time to avoid... + + + + + + + + + + + + + +
+
+
+ time to collision (not cropped) +
+ * +
+ obstacle velocity +
+
+
+ + time to collision (not cropped... + + + + + + + + + + + + + +
+
+
+ margin time to avoid +
+ * +
+ obstacle velocity +
+
+
+ + margin time to avoid... + + + + + + + + + +
+
+
+ collision point +
+
+
+ + collision point + + + + + + +
+
+
+ collision point +
+
+
+ + collision point + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg b/planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg new file mode 100644 index 0000000000000..41d0a4b5b8983 --- /dev/null +++ b/planning/behavior_path_planner/image/dynamic_avoidance/same_directional_object.svg @@ -0,0 +1,190 @@ + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + + + + + + + + +
+
+
+ reference path +
+
+
+ + reference path + + + + + + + + + + + + + + +
+
+
+ vehicle length +
+
+
+ + vehicle length + + + + + + + + + + + + +
+
+
+ margin time to avoid +
+ * +
+ obstacle velocity +
+
+
+ + margin time to avoid... + + + + + + +
+
+
+ Ignore this obstacle +
+ since TTC is negative +
+
+
+ + Ignore this obstacle... + + + + + + +
+
+
+ Consider this obstacle +
+ since TTC is positive +
+
+
+ + Consider this obstacle... + + + + + + +
+
+
+ collision point +
+
+
+ + collision point + + + + + + + + Text is not SVG - cannot display + + + From 07f6d9a4265e60fac2130b8ee08b5b5dd32f54ac Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 18 May 2023 19:11:38 +0900 Subject: [PATCH 05/11] chore(autoware_auto_geometry): add maintainer (#3748) * chore(autoware_auto_geometry): add maintainer Signed-off-by: satoshi-ota * Update common/autoware_auto_geometry/package.xml Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * Update common/autoware_auto_geometry/package.xml Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * style(pre-commit): autofix --------- Signed-off-by: satoshi-ota Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/autoware_auto_geometry/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/common/autoware_auto_geometry/package.xml b/common/autoware_auto_geometry/package.xml index 73943452718f4..f53412298a485 100644 --- a/common/autoware_auto_geometry/package.xml +++ b/common/autoware_auto_geometry/package.xml @@ -4,9 +4,11 @@ autoware_auto_geometry 1.0.0 Geometry related algorithms - Apex.AI, Inc. + Satoshi Ota Apache License 2.0 + Apex.AI, Inc. + ament_cmake_auto autoware_cmake From a8fecbd9b784d9468f2224af17ab6d7a540dd7bf Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 18 May 2023 19:18:03 +0900 Subject: [PATCH 06/11] fix(obstacle_velicity_limiter): fix invalid access to odometry pointer (#3752) Signed-off-by: tomoya.kimura --- .../obstacle_velocity_limiter_node.hpp | 3 +-- .../src/obstacle_velocity_limiter_node.cpp | 15 +++++++++------ 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index a74128f57d6fd..13fac28291c3d 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -100,9 +100,8 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node void onTrajectory(const Trajectory::ConstSharedPtr msg); /// @brief validate the inputs of the node - /// @param[in] ego_idx trajectory index closest to the current ego pose /// @return true if the inputs are valid - bool validInputs(const boost::optional & ego_idx); + bool validInputs(); }; } // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index 6144f338159da..a738fff7b0e5f 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -170,10 +170,16 @@ rcl_interfaces::msg::SetParametersResult ObstacleVelocityLimiterNode::onParamete void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { + if (!validInputs()) return; const auto t_start = std::chrono::system_clock::now(); const auto ego_idx = motion_utils::findNearestIndex(msg->points, current_odometry_ptr_->pose.pose); - if (!validInputs(ego_idx)) return; + if (!ego_idx) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), rcutils_duration_value_t(1000), + "Cannot calculate ego index on the trajectory"); + return; + } auto original_traj = *msg; if (preprocessing_params_.calculate_steering_angles) calculateSteeringAngles(original_traj, projection_params_.wheel_base); @@ -229,21 +235,18 @@ void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr } } -bool ObstacleVelocityLimiterNode::validInputs(const boost::optional & ego_idx) +bool ObstacleVelocityLimiterNode::validInputs() { constexpr auto one_sec = rcutils_duration_value_t(1000); if (!occupancy_grid_ptr_) RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), one_sec, "Occupancy grid not yet received"); if (!dynamic_obstacles_ptr_) RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), one_sec, "Dynamic obstacles not yet received"); - if (!ego_idx) - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), one_sec, "Cannot calculate ego index on the trajectory"); if (!current_odometry_ptr_) RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), one_sec, "Current ego velocity not yet received"); - return occupancy_grid_ptr_ && dynamic_obstacles_ptr_ && ego_idx && current_odometry_ptr_; + return occupancy_grid_ptr_ && dynamic_obstacles_ptr_ && current_odometry_ptr_; } } // namespace obstacle_velocity_limiter From 632a2ff449a9e14b6bea46ea235d3f5f38d23bc2 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 18 May 2023 20:43:29 +0900 Subject: [PATCH 07/11] fix(behavior_path_planner): fix invalid access for null drivable area bound (#3758) * fix(behavior_path_planner): fix invalid access for null drivable area bound Signed-off-by: tomoya.kimura * fix message Signed-off-by: tomoya.kimura --------- Signed-off-by: tomoya.kimura --- planning/behavior_path_planner/src/utils/utils.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 36bdf3b8bb98d..2de726ac82315 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1376,6 +1376,14 @@ void generateDrivableArea( auto left_bound = calcBound(route_handler, lanes, enable_expanding_polygon, true); auto right_bound = calcBound(route_handler, lanes, enable_expanding_polygon, false); + if (left_bound.empty() || right_bound.empty()) { + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), clock, 1000, + "The right or left bound of drivable area is empty"); + return; + } + // Insert points after goal lanelet::ConstLanelet goal_lanelet; if ( From 3f63c15a28920273f641c174231e87855bcb4f29 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 18 May 2023 21:05:36 +0900 Subject: [PATCH 08/11] fix(behavior_path_planner): remove setting maximum velocity after lane change (#3756) Signed-off-by: Fumiya Watanabe --- planning/behavior_path_planner/src/utils/lane_change/utils.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 57c9cfbb83edc..3518d31f60814 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -173,8 +173,6 @@ std::optional constructCandidatePath( lane_changing_start_point.point.longitudinal_velocity_mps); continue; } - point.point.longitudinal_velocity_mps = - std::min(point.point.longitudinal_velocity_mps, static_cast(lane_changing_velocity)); const auto nearest_idx = motion_utils::findNearestIndex(target_segment.points, point.point.pose); point.lane_ids = target_segment.points.at(*nearest_idx).lane_ids; From 39be26e6855bb93d227f750675c0b1d8d6f679f7 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 19 May 2023 08:42:54 +0900 Subject: [PATCH 09/11] docs(obstacle_stop_planner): fix dead link error in pre-commit optional (#3746) fix(obstacle_stop_planner): fix dead link Signed-off-by: satoshi-ota --- planning/obstacle_stop_planner/README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index aaaff2055361c..ef5b0facd236e 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -37,10 +37,10 @@ | `max_velocity` | double | max velocity [m/s] | | `chattering_threshold` | double | even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] | | `enable_z_axis_obstacle_filtering` | bool | filter obstacles in z axis (height) [-] | -| `z_axis_filtering_buffer` | double | additional buffer for z axis filtering [m]] | -| `use_predicted_objects` | bool | whether to use predicted objects for collision and slowdown detection [-]] | -| `predicted_object_filtering_threshold` | double | threshold for filtering predicted objects [valid only publish_obstacle_polygon true](m)] | -| `publish_obstacle_polygon` | bool | if use_predicted_objects is true, node publishes collision polygon [-]] | +| `z_axis_filtering_buffer` | double | additional buffer for z axis filtering [m] | +| `use_predicted_objects` | bool | whether to use predicted objects for collision and slowdown detection [-] | +| `predicted_object_filtering_threshold` | double | threshold for filtering predicted objects [valid only publish_obstacle_polygon true] [m] | +| `publish_obstacle_polygon` | bool | if use_predicted_objects is true, node publishes collision polygon [-] | ## Obstacle Stop Planner From 22a37bc26e60ce7d984d9d1ad84cdacfdb3888eb Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Fri, 19 May 2023 09:01:14 +0900 Subject: [PATCH 10/11] chore: update CODEOWNERS (#3665) Signed-off-by: GitHub Co-authored-by: satoshi-ota --- .github/CODEOWNERS | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index c95ff3dd3a8e3..a2daafa395cd5 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -3,7 +3,7 @@ ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp @autowarefoundation/autoware-global-codeowners common/autoware_auto_common/** opensource@apex.ai @autowarefoundation/autoware-global-codeowners -common/autoware_auto_geometry/** opensource@apex.ai @autowarefoundation/autoware-global-codeowners +common/autoware_auto_geometry/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp @autowarefoundation/autoware-global-codeowners common/autoware_auto_tf2/** jit.ray.c@gmail.com @autowarefoundation/autoware-global-codeowners common/autoware_point_types/** taichi.higashide@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -24,9 +24,10 @@ common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satosh common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners common/polar_grid/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners +common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners common/signal_processing/** ali.boyali@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners -common/tensorrt_common/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners +common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai shumpei.wakabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -116,7 +117,7 @@ perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.m perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp @autowarefoundation/autoware-global-codeowners -perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners +perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners perception/traffic_light_classifier/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners perception/traffic_light_selector/** isamu.takagi@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -141,6 +142,10 @@ planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp yuta planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/sampling_based_planner/bezier_sampler/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/sampling_based_planner/path_sampler/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners planning/scenario_selector/** fumiya.watanabe@tier4.jp kenji.miyake@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/surround_obstacle_checker/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners From 4b6994311bda6fd96ba00ba9cfc9a9e996216b18 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 19 May 2023 00:35:29 +0000 Subject: [PATCH 11/11] docs(autoware_auto_geometry): fix link to paper (#3077) * fix: fix link to paper Signed-off-by: Esteve Fernandez * try a different link Signed-off-by: Esteve Fernandez * chore(markdownlint): add ignore link Signed-off-by: satoshi-ota --------- Signed-off-by: Esteve Fernandez Signed-off-by: satoshi-ota Co-authored-by: satoshi-ota --- .markdown-link-check.json | 3 +++ .../design/polygon_intersection_2d-design.md | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/.markdown-link-check.json b/.markdown-link-check.json index c71a3e4253687..2279120c7083b 100644 --- a/.markdown-link-check.json +++ b/.markdown-link-check.json @@ -9,6 +9,9 @@ }, { "pattern": "^https://github.com/.*/discussions/new" + }, + { + "pattern": "^https://doi.org/10.2172/7309916" } ], "retryOn429": true, diff --git a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md b/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md index dc4b33b56b4bc..f651c218bc80d 100644 --- a/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md +++ b/common/autoware_auto_geometry/design/polygon_intersection_2d-design.md @@ -12,7 +12,7 @@ association and in any application that deals with the objects around the percei ## Design -[\(Livermore, Calif, 1977\)](https://www.osti.gov/biblio/7309916/) mention the following +[\(Livermore, Calif, 1977\)](https://doi.org/10.2172/7309916) mention the following observations about convex polygon intersection: - Intersection of two convex polygons is a convex polygon