diff --git a/common/osqp_interface/CMakeLists.txt b/common/autoware_osqp_interface/CMakeLists.txt similarity index 78% rename from common/osqp_interface/CMakeLists.txt rename to common/autoware_osqp_interface/CMakeLists.txt index 5a4231c15e8fd..e9ed0ce25f2f8 100644 --- a/common/osqp_interface/CMakeLists.txt +++ b/common/autoware_osqp_interface/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(osqp_interface) +project(autoware_osqp_interface) find_package(autoware_cmake REQUIRED) autoware_package() @@ -17,9 +17,9 @@ set(OSQP_INTERFACE_LIB_SRC ) set(OSQP_INTERFACE_LIB_HEADERS - include/osqp_interface/csc_matrix_conv.hpp - include/osqp_interface/osqp_interface.hpp - include/osqp_interface/visibility_control.hpp + include/autoware/osqp_interface/csc_matrix_conv.hpp + include/autoware/osqp_interface/osqp_interface.hpp + include/autoware/osqp_interface/visibility_control.hpp ) ament_auto_add_library(${PROJECT_NAME} SHARED @@ -28,18 +28,18 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast -Wno-error=useless-cast) -target_include_directories(osqp_interface +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC "${OSQP_INCLUDE_DIR}" "${EIGEN3_INCLUDE_DIR}" ) -ament_target_dependencies(osqp_interface +ament_target_dependencies(${PROJECT_NAME} Eigen3 osqp_vendor ) -# crucial so downstream package builds because osqp_interface exposes osqp.hpp +# crucial so downstream package builds because autoware_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) diff --git a/common/osqp_interface/design/osqp_interface-design.md b/common/autoware_osqp_interface/design/osqp_interface-design.md similarity index 96% rename from common/osqp_interface/design/osqp_interface-design.md rename to common/autoware_osqp_interface/design/osqp_interface-design.md index e9ff4a2a3bc3a..887a4b4d9af3f 100644 --- a/common/osqp_interface/design/osqp_interface-design.md +++ b/common/autoware_osqp_interface/design/osqp_interface-design.md @@ -1,6 +1,6 @@ # Interface for the OSQP library -This is the design document for the `osqp_interface` package. +This is the design document for the `autoware_osqp_interface` package. ## Purpose / Use cases diff --git a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp similarity index 83% rename from common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp rename to common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp index a919bc1f1c038..8c21553152d70 100644 --- a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp +++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp @@ -12,21 +12,17 @@ // 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_ +#ifndef AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ +#define AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ +#include "autoware/osqp_interface/visibility_control.hpp" #include "osqp/glob_opts.h" // for 'c_int' type ('long' or 'long long') -#include "osqp_interface/visibility_control.hpp" #include #include -namespace autoware -{ -namespace common -{ -namespace osqp +namespace autoware::osqp_interface { /// \brief Compressed-Column-Sparse Matrix struct OSQP_INTERFACE_PUBLIC CSC_Matrix @@ -46,8 +42,6 @@ OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & /// \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 +} // namespace autoware::osqp_interface -#endif // OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ +#endif // AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp similarity index 96% rename from common/osqp_interface/include/osqp_interface/osqp_interface.hpp rename to common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp index f126ba9329d3e..ff3013bd61514 100644 --- a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp +++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OSQP_INTERFACE__OSQP_INTERFACE_HPP_ -#define OSQP_INTERFACE__OSQP_INTERFACE_HPP_ +#ifndef AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ +#define AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ +#include "autoware/osqp_interface/csc_matrix_conv.hpp" +#include "autoware/osqp_interface/visibility_control.hpp" #include "osqp/osqp.h" -#include "osqp_interface/csc_matrix_conv.hpp" -#include "osqp_interface/visibility_control.hpp" #include #include @@ -28,11 +28,7 @@ #include #include -namespace autoware -{ -namespace common -{ -namespace osqp +namespace autoware::osqp_interface { constexpr c_float INF = 1e30; @@ -193,8 +189,6 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface void logUnsolvedStatus(const std::string & prefix_message = "") const; }; -} // namespace osqp -} // namespace common -} // namespace autoware +} // namespace autoware::osqp_interface -#endif // OSQP_INTERFACE__OSQP_INTERFACE_HPP_ +#endif // AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/osqp_interface/include/osqp_interface/visibility_control.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp similarity index 89% rename from common/osqp_interface/include/osqp_interface/visibility_control.hpp rename to common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp index 070a5c5f1542c..a6cdaa8a0a74e 100644 --- a/common/osqp_interface/include/osqp_interface/visibility_control.hpp +++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ -#define OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ //////////////////////////////////////////////////////////////////////////////// #if defined(__WIN32) @@ -34,4 +34,4 @@ #error "Unsupported Build Configuration" #endif -#endif // OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ diff --git a/common/osqp_interface/package.xml b/common/autoware_osqp_interface/package.xml similarity index 96% rename from common/osqp_interface/package.xml rename to common/autoware_osqp_interface/package.xml index 41ee5bb9055a6..d49ce63bd8c93 100644 --- a/common/osqp_interface/package.xml +++ b/common/autoware_osqp_interface/package.xml @@ -1,7 +1,7 @@ - osqp_interface + autoware_osqp_interface 1.0.0 Interface for the OSQP solver Maxime CLEMENT diff --git a/common/osqp_interface/src/csc_matrix_conv.cpp b/common/autoware_osqp_interface/src/csc_matrix_conv.cpp similarity index 94% rename from common/osqp_interface/src/csc_matrix_conv.cpp rename to common/autoware_osqp_interface/src/csc_matrix_conv.cpp index 1944face4516b..c6e1a0a42d938 100644 --- a/common/osqp_interface/src/csc_matrix_conv.cpp +++ b/common/autoware_osqp_interface/src/csc_matrix_conv.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "osqp_interface/csc_matrix_conv.hpp" +#include "autoware/osqp_interface/csc_matrix_conv.hpp" #include #include @@ -21,11 +21,7 @@ #include #include -namespace autoware -{ -namespace common -{ -namespace osqp +namespace autoware::osqp_interface { CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) { @@ -136,6 +132,4 @@ void printCSCMatrix(const CSC_Matrix & csc_mat) std::cout << "]\n"; } -} // namespace osqp -} // namespace common -} // namespace autoware +} // namespace autoware::osqp_interface diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/autoware_osqp_interface/src/osqp_interface.cpp similarity index 98% rename from common/osqp_interface/src/osqp_interface.cpp rename to common/autoware_osqp_interface/src/osqp_interface.cpp index f0dbc3ab22e4a..9ebf132f65028 100644 --- a/common/osqp_interface/src/osqp_interface.cpp +++ b/common/autoware_osqp_interface/src/osqp_interface.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "osqp_interface/osqp_interface.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" +#include "autoware/osqp_interface/csc_matrix_conv.hpp" #include "osqp/osqp.h" -#include "osqp_interface/csc_matrix_conv.hpp" #include #include @@ -25,11 +25,7 @@ #include #include -namespace autoware -{ -namespace common -{ -namespace osqp +namespace autoware::osqp_interface { OSQPInterface::OSQPInterface(const c_float eps_abs, const bool polish) : m_work{nullptr, OSQPWorkspaceDeleter} @@ -436,6 +432,4 @@ void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const // log with warning RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), output_message.c_str()); } -} // namespace osqp -} // namespace common -} // namespace autoware +} // namespace autoware::osqp_interface diff --git a/common/osqp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp similarity index 93% rename from common/osqp_interface/test/test_csc_matrix_conv.cpp rename to common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp index 765f1a1afed3b..a63f979a966bf 100644 --- a/common/osqp_interface/test/test_csc_matrix_conv.cpp +++ b/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/osqp_interface/csc_matrix_conv.hpp" #include "gtest/gtest.h" -#include "osqp_interface/csc_matrix_conv.hpp" #include @@ -23,8 +23,8 @@ TEST(TestCscMatrixConv, Nominal) { - using autoware::common::osqp::calCSCMatrix; - using autoware::common::osqp::CSC_Matrix; + using autoware::osqp_interface::calCSCMatrix; + using autoware::osqp_interface::CSC_Matrix; Eigen::MatrixXd rect1(1, 2); rect1 << 0.0, 1.0; @@ -117,8 +117,8 @@ TEST(TestCscMatrixConv, Nominal) } TEST(TestCscMatrixConv, Trapezoidal) { - using autoware::common::osqp::calCSCMatrixTrapezoidal; - using autoware::common::osqp::CSC_Matrix; + using autoware::osqp_interface::calCSCMatrixTrapezoidal; + using autoware::osqp_interface::CSC_Matrix; Eigen::MatrixXd square1(2, 2); Eigen::MatrixXd square2(3, 3); @@ -166,10 +166,10 @@ TEST(TestCscMatrixConv, Trapezoidal) } TEST(TestCscMatrixConv, Print) { - using autoware::common::osqp::calCSCMatrix; - using autoware::common::osqp::calCSCMatrixTrapezoidal; - using autoware::common::osqp::CSC_Matrix; - using autoware::common::osqp::printCSCMatrix; + using autoware::osqp_interface::calCSCMatrix; + using autoware::osqp_interface::calCSCMatrixTrapezoidal; + using autoware::osqp_interface::CSC_Matrix; + using autoware::osqp_interface::printCSCMatrix; Eigen::MatrixXd square1(2, 2); Eigen::MatrixXd rect1(1, 2); square1 << 1.0, 2.0, 2.0, 4.0; diff --git a/common/osqp_interface/test/test_osqp_interface.cpp b/common/autoware_osqp_interface/test/test_osqp_interface.cpp similarity index 85% rename from common/osqp_interface/test/test_osqp_interface.cpp rename to common/autoware_osqp_interface/test/test_osqp_interface.cpp index 4644ec6c3e019..d03713f82566f 100644 --- a/common/osqp_interface/test/test_osqp_interface.cpp +++ b/common/autoware_osqp_interface/test/test_osqp_interface.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/osqp_interface/osqp_interface.hpp" #include "gtest/gtest.h" -#include "osqp_interface/osqp_interface.hpp" #include @@ -39,9 +39,9 @@ namespace TEST(TestOsqpInterface, BasicQp) { - using autoware::common::osqp::calCSCMatrix; - using autoware::common::osqp::calCSCMatrixTrapezoidal; - using autoware::common::osqp::CSC_Matrix; + using autoware::osqp_interface::calCSCMatrix; + using autoware::osqp_interface::calCSCMatrixTrapezoidal; + using autoware::osqp_interface::CSC_Matrix; auto check_result = [](const std::tuple, std::vector, int, int, int> & result) { @@ -66,12 +66,12 @@ TEST(TestOsqpInterface, BasicQp) 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, -autoware::common::osqp::INF}; - const std::vector u = {1.0, 0.7, 0.7, autoware::common::osqp::INF}; + const std::vector l = {1.0, 0.0, 0.0, -autoware::osqp_interface::INF}; + const std::vector u = {1.0, 0.7, 0.7, autoware::osqp_interface::INF}; { // Define problem during optimization - autoware::common::osqp::OSQPInterface osqp; + autoware::osqp_interface::OSQPInterface osqp; std::tuple, std::vector, int, int, int> result = osqp.optimize(P, A, q, l, u); check_result(result); @@ -79,7 +79,7 @@ TEST(TestOsqpInterface, BasicQp) { // Define problem during initialization - autoware::common::osqp::OSQPInterface osqp(P, A, q, l, u, 1e-6); + autoware::osqp_interface::OSQPInterface osqp(P, A, q, l, u, 1e-6); std::tuple, std::vector, int, int, int> result = osqp.optimize(); check_result(result); } @@ -92,7 +92,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - autoware::common::osqp::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6); + autoware::osqp_interface::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6); osqp.optimize(); // Redefine problem before optimization @@ -105,7 +105,7 @@ TEST(TestOsqpInterface, BasicQp) // Define problem during initialization with csc matrix CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); CSC_Matrix A_csc = calCSCMatrix(A); - autoware::common::osqp::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6); + autoware::osqp_interface::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6); std::tuple, std::vector, int, int, int> result = osqp.optimize(); check_result(result); } @@ -118,7 +118,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - autoware::common::osqp::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); + autoware::osqp_interface::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); osqp.optimize(); // Redefine problem before optimization @@ -138,7 +138,7 @@ TEST(TestOsqpInterface, BasicQp) std::vector q_ini(2, 0.0); std::vector l_ini(4, 0.0); std::vector u_ini(4, 0.0); - autoware::common::osqp::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); + autoware::osqp_interface::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); osqp.optimize(); // Redefine problem before optimization diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index 1e1295cb06b1e..ab9b3392449ee 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ #include "autoware/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" -#include "osqp_interface/osqp_interface.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" namespace autoware::motion::control::mpc_lateral_controller @@ -58,7 +58,7 @@ class QPSolverOSQP : public QPSolverInterface double getObjVal() const override { return osqpsolver_.getObjVal(); } private: - autoware::common::osqp::OSQPInterface osqpsolver_; + autoware::osqp_interface::OSQPInterface osqpsolver_; rclcpp::Logger logger_; }; } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index 5c5f8886d6ed3..ee9ede3598256 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -20,6 +20,7 @@ autoware_control_msgs autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_trajectory_follower_base autoware_universe_utils @@ -29,7 +30,6 @@ diagnostic_updater eigen geometry_msgs - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml index 548129eb06f8c..c4b6fbfeb4ca6 100644 --- a/control/autoware_trajectory_follower_base/package.xml +++ b/control/autoware_trajectory_follower_base/package.xml @@ -23,6 +23,7 @@ autoware_control_msgs autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_universe_utils autoware_vehicle_info_utils @@ -31,7 +32,6 @@ diagnostic_updater eigen geometry_msgs - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp index a2ac86ee6c86b..ca17b428e369a 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp @@ -15,7 +15,7 @@ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ #include "autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" -#include "osqp_interface/osqp_interface.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include @@ -69,7 +69,7 @@ class VelocityOptimizer double over_j_weight_; // QPSolver - autoware::common::osqp::OSQPInterface qp_solver_; + autoware::osqp_interface::OSQPInterface qp_solver_; }; #endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 90a5e666f5f44..ed3cd2169e343 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -22,6 +22,7 @@ autoware_interpolation autoware_lanelet2_extension autoware_motion_utils + autoware_osqp_interface autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager @@ -31,7 +32,6 @@ geometry_msgs nav_msgs object_recognition_utils - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 3d9192c93d5b1..9a397e255da13 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -17,6 +17,7 @@ #include "autoware/interpolation/linear_interpolation.hpp" #include "autoware/interpolation/spline_interpolation_points_2d.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/state_equation_generator.hpp" #include "autoware/path_optimizer/type_alias.hpp" @@ -24,7 +25,6 @@ #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" -#include "osqp_interface/osqp_interface.hpp" #include #include @@ -228,7 +228,7 @@ class MPTOptimizer MPTParam mpt_param_; StateEquationGenerator state_equation_generator_; - std::unique_ptr osqp_solver_ptr_; + std::unique_ptr osqp_solver_ptr_; const double osqp_epsilon_ = 1.0e-3; diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index 1a7869b6a87fd..cdd7123d21ab8 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -16,13 +16,13 @@ autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs nav_msgs - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 8b48f78d117bb..df07f3f98957a 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -414,7 +414,7 @@ MPTOptimizer::MPTOptimizer( StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_); // osqp solver - osqp_solver_ptr_ = std::make_unique(osqp_epsilon_); + osqp_solver_ptr_ = std::make_unique(osqp_epsilon_); // publisher debug_fixed_traj_pub_ = node->create_publisher("~/debug/mpt_fixed_traj", 1); @@ -1330,8 +1330,8 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( // NOTE: The following takes 1 [ms] Eigen::MatrixXd A = Eigen::MatrixXd::Zero(A_rows, N_v); - Eigen::VectorXd lb = Eigen::VectorXd::Constant(A_rows, -autoware::common::osqp::INF); - Eigen::VectorXd ub = Eigen::VectorXd::Constant(A_rows, autoware::common::osqp::INF); + Eigen::VectorXd lb = Eigen::VectorXd::Constant(A_rows, -autoware::osqp_interface::INF); + Eigen::VectorXd ub = Eigen::VectorXd::Constant(A_rows, autoware::osqp_interface::INF); size_t A_rows_end = 0; // 1. State equation @@ -1502,9 +1502,9 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( // initialize or update solver according to warm start time_keeper_->start_track("initOsqp"); - const autoware::common::osqp::CSC_Matrix P_csc = - autoware::common::osqp::calCSCMatrixTrapezoidal(H); - const autoware::common::osqp::CSC_Matrix A_csc = autoware::common::osqp::calCSCMatrix(A); + const autoware::osqp_interface::CSC_Matrix P_csc = + autoware::osqp_interface::calCSCMatrixTrapezoidal(H); + const autoware::osqp_interface::CSC_Matrix A_csc = autoware::osqp_interface::calCSCMatrix(A); if ( prev_solution_status_ == 1 && mpt_param_.enable_warm_start && prev_mat_n_ == H.rows() && prev_mat_m_ == A.rows()) { @@ -1515,7 +1515,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); } else { RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "no warm start"); - osqp_solver_ptr_ = std::make_unique( + osqp_solver_ptr_ = std::make_unique( P_csc, A_csc, f, lower_bound, upper_bound, osqp_epsilon_); } prev_mat_n_ = H.rows(); diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp index 1d0441c66c481..2d6c1d1b2d002 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/elastic_band.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_HPP_ #define AUTOWARE__PATH_SMOOTHER__ELASTIC_BAND_HPP_ +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/path_smoother/common_structs.hpp" #include "autoware/path_smoother/type_alias.hpp" -#include "osqp_interface/osqp_interface.hpp" #include @@ -109,7 +109,7 @@ class EBPathSmoother rclcpp::Publisher::SharedPtr debug_eb_traj_pub_; rclcpp::Publisher::SharedPtr debug_eb_fixed_traj_pub_; - std::unique_ptr osqp_solver_ptr_; + std::unique_ptr osqp_solver_ptr_; std::shared_ptr> prev_eb_traj_points_ptr_{nullptr}; std::vector insertFixedPoint( diff --git a/planning/autoware_path_smoother/package.xml b/planning/autoware_path_smoother/package.xml index b9b79bb6ceaf6..582553c2a94b2 100644 --- a/planning/autoware_path_smoother/package.xml +++ b/planning/autoware_path_smoother/package.xml @@ -16,12 +16,12 @@ autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils geometry_msgs nav_msgs - osqp_interface rclcpp rclcpp_components std_msgs diff --git a/planning/autoware_path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp index 4222e0fe98438..0bec4e46c8b42 100644 --- a/planning/autoware_path_smoother/src/elastic_band.cpp +++ b/planning/autoware_path_smoother/src/elastic_band.cpp @@ -381,7 +381,7 @@ void EBPathSmoother::updateConstraint( osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); osqp_solver_ptr_->updateEpsRel(p.qp_param.eps_rel); } else { - osqp_solver_ptr_ = std::make_unique( + osqp_solver_ptr_ = std::make_unique( P, A, q, lower_bound, upper_bound, p.qp_param.eps_abs); osqp_solver_ptr_->updateEpsRel(p.qp_param.eps_rel); osqp_solver_ptr_->updateEpsAbs(p.qp_param.eps_abs); diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 2abae4af2ca67..4f1399533ed46 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -23,6 +23,7 @@ autoware_map_projection_loader autoware_mission_planner autoware_motion_utils + autoware_osqp_interface autoware_path_optimizer autoware_path_smoother autoware_perception_msgs @@ -33,7 +34,6 @@ geometry_msgs global_parameter_loader map_loader - osqp_interface rclcpp rclcpp_components tier4_map_msgs diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index d6033b66d5d6f..3f1c313e052da 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -17,6 +17,7 @@ #include "autoware/motion_utils/trajectory/conversion.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" @@ -31,7 +32,6 @@ #include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" #include "autoware/velocity_smoother/trajectory_utils.hpp" -#include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/transform_listener.h" diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 8f6bbc2236eb6..fda8e8f31df95 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -16,10 +16,10 @@ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" -#include "osqp_interface/osqp_interface.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -57,7 +57,7 @@ class L2PseudoJerkSmoother : public SmootherBase private: Param smoother_param_; - autoware::common::osqp::OSQPInterface qp_solver_; + autoware::osqp_interface::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("l2_pseudo_jerk_smoother")}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index e27a4db10e748..119cb4d358de2 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -16,10 +16,10 @@ #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware/velocity_smoother/smoother/smoother_base.hpp" -#include "osqp_interface/osqp_interface.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -57,7 +57,7 @@ class LinfPseudoJerkSmoother : public SmootherBase private: Param smoother_param_; - autoware::common::osqp::OSQPInterface qp_solver_; + autoware::osqp_interface::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("linf_pseudo_jerk_smoother")}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 65684e414e90d..222b189057820 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -23,13 +23,13 @@ autoware_interpolation autoware_motion_utils + autoware_osqp_interface autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils autoware_vehicle_info_utils geometry_msgs nav_msgs - osqp_interface qp_interface rclcpp tf2