From d137f7c732ddad85b2b002b5f4395188a4686bed Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 15 Oct 2024 03:27:56 +0200 Subject: [PATCH 1/5] refactor(osqp_interface): added autoware prefix to osqp_interface (#8958) Signed-off-by: Esteve Fernandez --- .../CMakeLists.txt | 14 +++++------ .../design/osqp_interface-design.md | 2 +- .../osqp_interface/csc_matrix_conv.hpp | 18 +++++--------- .../osqp_interface/osqp_interface.hpp | 20 ++++++---------- .../osqp_interface/visibility_control.hpp | 6 ++--- .../package.xml | 2 +- .../src/csc_matrix_conv.cpp | 12 +++------- .../src/osqp_interface.cpp | 14 ++++------- .../test/test_csc_matrix_conv.cpp | 18 +++++++------- .../test/test_osqp_interface.cpp | 24 +++++++++---------- .../qp_solver/qp_solver_osqp.hpp | 4 ++-- .../package.xml | 2 +- .../package.xml | 2 +- .../velocity_optimizer.hpp | 4 ++-- .../package.xml | 2 +- .../autoware/path_optimizer/mpt_optimizer.hpp | 4 ++-- planning/autoware_path_optimizer/package.xml | 2 +- .../src/mpt_optimizer.cpp | 14 +++++------ .../autoware/path_smoother/elastic_band.hpp | 4 ++-- planning/autoware_path_smoother/package.xml | 2 +- .../src/elastic_band.cpp | 2 +- .../package.xml | 2 +- .../autoware/velocity_smoother/node.hpp | 2 +- .../smoother/l2_pseudo_jerk_smoother.hpp | 4 ++-- .../smoother/linf_pseudo_jerk_smoother.hpp | 4 ++-- .../autoware_velocity_smoother/package.xml | 2 +- 26 files changed, 81 insertions(+), 105 deletions(-) rename common/{osqp_interface => autoware_osqp_interface}/CMakeLists.txt (78%) rename common/{osqp_interface => autoware_osqp_interface}/design/osqp_interface-design.md (96%) rename common/{osqp_interface/include => autoware_osqp_interface/include/autoware}/osqp_interface/csc_matrix_conv.hpp (83%) rename common/{osqp_interface/include => autoware_osqp_interface/include/autoware}/osqp_interface/osqp_interface.hpp (96%) rename common/{osqp_interface/include => autoware_osqp_interface/include/autoware}/osqp_interface/visibility_control.hpp (89%) rename common/{osqp_interface => autoware_osqp_interface}/package.xml (96%) rename common/{osqp_interface => autoware_osqp_interface}/src/csc_matrix_conv.cpp (94%) rename common/{osqp_interface => autoware_osqp_interface}/src/osqp_interface.cpp (98%) rename common/{osqp_interface => autoware_osqp_interface}/test/test_csc_matrix_conv.cpp (93%) rename common/{osqp_interface => autoware_osqp_interface}/test/test_osqp_interface.cpp (85%) 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 From 0de8669e2d112b1b51edc38b831a3e07963993b7 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 15 Oct 2024 10:32:52 +0900 Subject: [PATCH 2/5] test(no_stopping_area): refactor and add tests (#9009) Signed-off-by: Maxime CLEMENT Co-authored-by: Kosuke Takeuchi --- .../CMakeLists.txt | 13 +- .../package.xml | 8 +- .../src/debug.cpp | 35 +- .../src/manager.cpp | 5 - .../src/manager.hpp | 2 +- .../src/scene_no_stopping_area.cpp | 295 ++---------- .../src/scene_no_stopping_area.hpp | 92 +--- .../src/utils.cpp | 262 +++++++++++ .../src/utils.hpp | 165 +++++++ .../test/test_utils.cpp | 438 ++++++++++++++++++ .../scene_module_interface.hpp | 2 +- 11 files changed, 936 insertions(+), 381 deletions(-) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt index f4646b1b9eb6e..b710924410549 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -6,9 +6,16 @@ autoware_package() pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp - src/manager.cpp - src/scene_no_stopping_area.cpp + DIRECTORY src ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_utils.cpp + ) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +endif() + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index 21664f7596d60..18a35ea12d5f6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -8,6 +8,7 @@ Kosuke Takeuchi Tomoya Kimura Shumpei Wakabayashi + Shumpei Wakabayashi Apache License 2.0 @@ -15,7 +16,6 @@ ament_cmake_auto autoware_cmake - eigen3_cmake_module autoware_behavior_velocity_planner_common autoware_interpolation @@ -23,20 +23,16 @@ autoware_motion_utils autoware_perception_msgs autoware_planning_msgs - autoware_route_handler autoware_universe_utils autoware_vehicle_info_utils - eigen geometry_msgs libboost-dev pluginlib rclcpp - tf2 - tf2_eigen - tf2_geometry_msgs tier4_planning_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp index 09f1908b107d5..b9509e308d1fc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -13,17 +13,13 @@ // limitations under the License. #include "scene_no_stopping_area.hpp" +#include "utils.hpp" #include #include #include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif #include #include @@ -33,13 +29,13 @@ namespace autoware::behavior_velocity_planner namespace { const double marker_lifetime = 0.2; -using DebugData = NoStoppingAreaModule::DebugData; +using DebugData = no_stopping_area::DebugData; using autoware::universe_utils::appendMarkerArray; using autoware::universe_utils::createDefaultMarker; using autoware::universe_utils::createMarkerColor; using autoware::universe_utils::createMarkerScale; -lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly) +lanelet::BasicPoint3d get_centroid_point(const lanelet::BasicPolygon3d & poly) { lanelet::BasicPoint3d p_sum{0.0, 0.0, 0.0}; for (const auto & p : poly) { @@ -48,7 +44,7 @@ lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly) return p_sum / poly.size(); } -geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point) +geometry_msgs::msg::Point to_msg(const lanelet::BasicPoint3d & point) { geometry_msgs::msg::Point msg; msg.x = point.x(); @@ -57,7 +53,7 @@ geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point) return msg; } -visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( +visualization_msgs::msg::MarkerArray create_lanelet_info_marker_array( const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const rclcpp::Time & now) { visualization_msgs::msg::MarkerArray msg; @@ -65,7 +61,7 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( // ID { auto marker = createDefaultMarker( - "map", now, "no_stopping_area_id", no_stopping_area_reg_elem.id(), + "map", now, "no_stopping_area_id", static_cast(no_stopping_area_reg_elem.id()), visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); @@ -73,7 +69,7 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) { const auto poly = detection_area.basicPolygon(); - marker.pose.position = toMsg(poly.front()); + marker.pose.position = to_msg(poly.front()); marker.pose.position.z += 2.0; marker.text = std::to_string(no_stopping_area_reg_elem.id()); @@ -84,7 +80,7 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( // Polygon { auto marker = createDefaultMarker( - "map", now, "no_stopping_area_polygon", no_stopping_area_reg_elem.id(), + "map", now, "no_stopping_area_polygon", static_cast(no_stopping_area_reg_elem.id()), visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(0.1, 0.1, 1.0, 0.500)); marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); @@ -99,8 +95,8 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( const auto & p_front = poly.at(idx_front); const auto & p_back = poly.at(idx_back); - marker.points.push_back(toMsg(p_front)); - marker.points.push_back(toMsg(p_back)); + marker.points.push_back(to_msg(p_front)); + marker.points.push_back(to_msg(p_back)); } } msg.markers.push_back(marker); @@ -112,16 +108,17 @@ visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( const auto stop_line_center_point = (stop_line.value().front().basicPoint() + stop_line.value().back().basicPoint()) / 2; auto marker = createDefaultMarker( - "map", now, "no_stopping_area_correspondence", no_stopping_area_reg_elem.id(), + "map", now, "no_stopping_area_correspondence", + static_cast(no_stopping_area_reg_elem.id()), visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(0.1, 0.1, 1.0, 0.500)); marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) { const auto poly = detection_area.basicPolygon(); - const auto centroid_point = getCentroidPoint(poly); + const auto centroid_point = get_centroid_point(poly); for (size_t i = 0; i < poly.size(); ++i) { - marker.points.push_back(toMsg(centroid_point)); - marker.points.push_back(toMsg(stop_line_center_point)); + marker.points.push_back(to_msg(centroid_point)); + marker.points.push_back(to_msg(stop_line_center_point)); } } msg.markers.push_back(marker); @@ -137,7 +134,7 @@ visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createDebugMarkerArra const rclcpp::Time now = clock_->now(); appendMarkerArray( - createLaneletInfoMarkerArray(no_stopping_area_reg_elem_, now), &debug_marker_array, now); + create_lanelet_info_marker_array(no_stopping_area_reg_elem_, now), &debug_marker_array, now); if (!debug_data_.stuck_points.empty()) { appendMarkerArray( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index 4631195ec47fc..9d66aa6672d36 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -16,17 +16,12 @@ #include #include -#include - -#include #include #include #include #include -#include #include -#include namespace autoware::behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp index 1abd36d585d51..7009e94612580 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -37,7 +37,7 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC const char * getModuleName() override { return "no_stopping_area"; } private: - NoStoppingAreaModule::PlannerParam planner_param_; + NoStoppingAreaModule::PlannerParam planner_param_{}; void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index ac58c62036e26..031f2976da0a7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -14,26 +14,22 @@ #include "scene_no_stopping_area.hpp" -#include -#include +#include "utils.hpp" + +#include #include #include #include #include #include +#include +#include #include +#include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif -#include -#include -#include -#include +#include #include namespace autoware::behavior_velocity_planner @@ -43,7 +39,7 @@ namespace bg = boost::geometry; NoStoppingAreaModule::NoStoppingAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, - const PlannerParam & planner_param, const rclcpp::Logger logger, + const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), @@ -56,55 +52,6 @@ NoStoppingAreaModule::NoStoppingAreaModule( state_machine_.setMarginTime(planner_param_.state_clear_time); } -boost::optional NoStoppingAreaModule::getStopLineGeometry2d( - const tier4_planning_msgs::msg::PathWithLaneId & path, const double stop_line_margin) const -{ - // get stop line from map - { - const auto & stop_line = no_stopping_area_reg_elem_.stopLine(); - if (stop_line) { - return planning_utils::extendLine( - stop_line.value()[0], stop_line.value()[1], planner_data_->stop_line_extend_length); - } - } - // auto gen stop line - { - LineString2d stop_line; - /** - * @brief auto gen no stopping area stop line from area polygon if stop line is not set - * --------------- - * ------col-------------|--> ego path - * | Area | - * --------------- - **/ - - for (const auto & no_stopping_area : no_stopping_area_reg_elem_.noStoppingAreas()) { - const auto & area_poly = lanelet::utils::to2D(no_stopping_area).basicPolygon(); - lanelet::BasicLineString2d path_line; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto p0 = path.points.at(i).point.pose.position; - const auto p1 = path.points.at(i + 1).point.pose.position; - const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; - std::vector collision_points; - bg::intersection(area_poly, line, collision_points); - if (!collision_points.empty()) { - const double yaw = autoware::universe_utils::calcAzimuthAngle(p0, p1); - const double w = planner_data_->vehicle_info_.vehicle_width_m; - const double l = stop_line_margin; - stop_line.emplace_back( - -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw + M_PI_2), - collision_points.front().y() + w * std::sin(yaw + M_PI_2)); - stop_line.emplace_back( - -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw - M_PI_2), - collision_points.front().y() + w * std::sin(yaw - M_PI_2)); - return stop_line; - } - } - } - } - return {}; -} - bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { // Store original path @@ -115,12 +62,16 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason return true; } // Reset data - debug_data_ = DebugData(); + debug_data_ = no_stopping_area::DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; *stop_reason = planning_utils::initializeStopReason(StopReason::NO_STOPPING_AREA); + const no_stopping_area::EgoData ego_data(*planner_data_); + // Get stop line geometry - const auto stop_line = getStopLineGeometry2d(original_path, planner_param_.stop_line_margin); + const auto stop_line = no_stopping_area::get_stop_line_geometry2d( + original_path, no_stopping_area_reg_elem_, planner_param_.stop_line_margin, + planner_data_->stop_line_extend_length, planner_data_->vehicle_info_.vehicle_width_m); if (!stop_line) { setSafe(true); return true; @@ -146,14 +97,16 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason const double margin = planner_param_.stop_line_margin; const double ego_space_in_front_of_stuck_vehicle = margin + vi.vehicle_length_m + planner_param_.stuck_vehicle_front_margin; - const Polygon2d stuck_vehicle_detect_area = generateEgoNoStoppingAreaLanePolygon( - *path, current_pose->pose, ego_space_in_front_of_stuck_vehicle, - planner_param_.detection_area_length); + const Polygon2d stuck_vehicle_detect_area = + no_stopping_area::generate_ego_no_stopping_area_lane_polygon( + *path, current_pose->pose, no_stopping_area_reg_elem_, ego_space_in_front_of_stuck_vehicle, + planner_param_.detection_area_length, planner_param_.path_expand_width, logger_, *clock_); const double ego_space_in_front_of_stop_line = margin + planner_param_.stop_margin + vi.rear_overhang_m; - const Polygon2d stop_line_detect_area = generateEgoNoStoppingAreaLanePolygon( - *path, current_pose->pose, ego_space_in_front_of_stop_line, - planner_param_.detection_area_length); + const Polygon2d stop_line_detect_area = + no_stopping_area::generate_ego_no_stopping_area_lane_polygon( + *path, current_pose->pose, no_stopping_area_reg_elem_, ego_space_in_front_of_stop_line, + planner_param_.detection_area_length, planner_param_.path_expand_width, logger_, *clock_); if (stuck_vehicle_detect_area.outer().empty() && stop_line_detect_area.outer().empty()) { setSafe(true); return true; @@ -162,26 +115,28 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason debug_data_.stop_line_detect_area = toGeomPoly(stop_line_detect_area); // Find stuck vehicle in no stopping area const bool is_entry_prohibited_by_stuck_vehicle = - checkStuckVehiclesInNoStoppingArea(stuck_vehicle_detect_area, predicted_obj_arr_ptr); + check_stuck_vehicles_in_no_stopping_area(stuck_vehicle_detect_area, predicted_obj_arr_ptr); // Find stop line in no stopping area const bool is_entry_prohibited_by_stop_line = - checkStopLinesInNoStoppingArea(*path, stop_line_detect_area); + no_stopping_area::check_stop_lines_in_no_stopping_area( + *path, stop_line_detect_area, debug_data_); const bool is_entry_prohibited = is_entry_prohibited_by_stuck_vehicle || is_entry_prohibited_by_stop_line; - if (!isStoppable(current_pose->pose, stop_point->second)) { + if (!no_stopping_area::is_stoppable( + pass_judge_, current_pose->pose, stop_point->second, ego_data, logger_, *clock_)) { state_machine_.setState(StateMachine::State::GO); setSafe(true); return false; - } else { - state_machine_.setStateWithMarginTime( - is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, - logger_.get_child("state_machine"), *clock_); } + state_machine_.setStateWithMarginTime( + is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, + logger_.get_child("state_machine"), *clock_); + setSafe(state_machine_.getState() != StateMachine::State::STOP); if (!isActivated()) { // ----------------stop reason and stop point-------------------------- - insertStopPoint(*path, *stop_point); + no_stopping_area::insert_stop_point(*path, *stop_point); // For virtual wall debug_data_.stop_poses.push_back(stop_pose); @@ -208,19 +163,19 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason } } else if (state_machine_.getState() == StateMachine::State::GO) { // reset pass judge if current state is go - is_stoppable_ = true; - pass_judged_ = false; + pass_judge_.is_stoppable = true; + pass_judge_.pass_judged = false; } return true; } -bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( +bool NoStoppingAreaModule::check_stuck_vehicles_in_no_stopping_area( const Polygon2d & poly, const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr) { // stuck points by predicted objects for (const auto & object : predicted_obj_arr_ptr->objects) { - if (!isTargetStuckVehicleType(object)) { + if (!no_stopping_area::is_vehicle_type(object)) { continue; // not target vehicle type } const auto obj_v = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x); @@ -248,182 +203,4 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( } return false; } -bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea( - const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly) -{ - const double stop_vel = std::numeric_limits::min(); - - // if the detected stop point is near goal, it's ignored. - static constexpr double close_to_goal_distance = 1.0; - - // stuck points by stop line - for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto p0 = path.points.at(i).point.pose.position; - const auto p1 = path.points.at(i + 1).point.pose.position; - const auto v0 = path.points.at(i).point.longitudinal_velocity_mps; - const auto v1 = path.points.at(i + 1).point.longitudinal_velocity_mps; - if (v0 > stop_vel && v1 > stop_vel) { - continue; - } - // judge if stop point p0 is near goal, by its distance to the path end. - const double dist_to_path_end = - autoware::motion_utils::calcSignedArcLength(path.points, i, path.points.size() - 1); - if (dist_to_path_end < close_to_goal_distance) { - // exit with false, cause position is near goal. - return false; - } - - const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; - std::vector collision_points; - bg::intersection(poly, line, collision_points); - if (!collision_points.empty()) { - geometry_msgs::msg::Point point; - point.x = collision_points.front().x(); - point.y = collision_points.front().y(); - point.z = 0.0; - debug_data_.stuck_points.emplace_back(point); - return true; - } - } - return false; -} - -Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, - const double margin, const double extra_dist) const -{ - Polygon2d ego_area; // open polygon - double dist_from_start_sum = 0.0; - const double interpolation_interval = 0.5; - bool is_in_area = false; - tier4_planning_msgs::msg::PathWithLaneId interpolated_path; - if (!splineInterpolate(path, interpolation_interval, interpolated_path, logger_)) { - return ego_area; - } - auto & pp = interpolated_path.points; - /* calc closest index */ - const auto closest_idx_opt = - autoware::motion_utils::findNearestIndex(interpolated_path.points, ego_pose, 3.0, M_PI_4); - if (!closest_idx_opt) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger_, *clock_, 1000 /* ms */, "autoware::motion_utils::findNearestIndex fail"); - return ego_area; - } - const size_t closest_idx = closest_idx_opt.value(); - - const int num_ignore_nearest = 1; // Do not consider nearest lane polygon - size_t ego_area_start_idx = closest_idx + num_ignore_nearest; - // return if area size is not intentional - if (no_stopping_area_reg_elem_.noStoppingAreas().size() != 1) { - return ego_area; - } - const auto no_stopping_area = no_stopping_area_reg_elem_.noStoppingAreas().front(); - for (size_t i = closest_idx + num_ignore_nearest; i < pp.size() - 1; ++i) { - dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); - const auto & p = pp.at(i).point.pose.position; - if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { - is_in_area = true; - break; - } - if (dist_from_start_sum > extra_dist) { - return ego_area; - } - ++ego_area_start_idx; - } - if (ego_area_start_idx > num_ignore_nearest) { - ego_area_start_idx--; - } - if (!is_in_area) { - return ego_area; - } - double dist_from_area_sum = 0.0; - // decide end idx with extract distance - size_t ego_area_end_idx = ego_area_start_idx; - for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { - dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); - const auto & p = pp.at(i).point.pose.position; - if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { - dist_from_area_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); - } - if (dist_from_start_sum > extra_dist || dist_from_area_sum > margin) { - break; - } - ++ego_area_end_idx; - } - - const auto width = planner_param_.path_expand_width; - ego_area = planning_utils::generatePathPolygon( - interpolated_path, ego_area_start_idx, ego_area_end_idx, width); - return ego_area; -} - -bool NoStoppingAreaModule::isTargetStuckVehicleType( - const autoware_perception_msgs::msg::PredictedObject & object) const -{ - if ( - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::CAR || - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::BUS || - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::TRUCK || - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::TRAILER || - object.classification.front().label == - autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { - return true; - } - return false; -} - -bool NoStoppingAreaModule::isStoppable( - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const -{ - // get vehicle info and compute pass_judge_line_distance - const auto current_velocity = planner_data_->current_velocity->twist.linear.x; - const auto current_acceleration = planner_data_->current_acceleration->accel.accel.linear.x; - const double max_acc = planner_data_->max_stop_acceleration_threshold; - const double max_jerk = planner_data_->max_stop_jerk_threshold; - const double delay_response_time = planner_data_->delay_response_time; - const double stoppable_distance = planning_utils::calcJudgeLineDistWithJerkLimit( - current_velocity, current_acceleration, max_acc, max_jerk, delay_response_time); - const double signed_arc_length = - arc_lane_utils::calcSignedDistance(self_pose, line_pose.position); - const bool distance_stoppable = stoppable_distance < signed_arc_length; - const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 2.0; - // ego vehicle is high speed and can't stop before stop line -> GO - const bool not_stoppable = !distance_stoppable && !slow_velocity; - // stoppable or not is judged only once - RCLCPP_DEBUG( - logger_, "stoppable_dist: %lf signed_arc_length: %lf", stoppable_distance, signed_arc_length); - if (!distance_stoppable && !pass_judged_) { - pass_judged_ = true; - // can't stop using maximum brake consider jerk limit - if (not_stoppable) { - // pass through - is_stoppable_ = false; - RCLCPP_WARN_THROTTLE( - logger_, *clock_, 1000, "[NoStoppingArea] can't stop in front of no stopping area"); - } else { - is_stoppable_ = true; - } - } - return is_stoppable_; -} - -void NoStoppingAreaModule::insertStopPoint( - tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) -{ - size_t insert_idx = static_cast(stop_point.first + 1); - const auto stop_pose = stop_point.second; - - // To PathPointWithLaneId - tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; - stop_point_with_lane_id = path.points.at(insert_idx); - stop_point_with_lane_id.point.pose = stop_pose; - stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; - - // Insert stop point or replace with zero velocity - planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); -} } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 2c553ac457ef0..886c8f04dc702 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -15,48 +15,25 @@ #ifndef SCENE_NO_STOPPING_AREA_HPP_ #define SCENE_NO_STOPPING_AREA_HPP_ -#define EIGEN_MPL2_ONLY +#include "utils.hpp" -#include #include #include #include #include #include -#include #include #include #include -#include - -#include -#include - #include -#include -#include namespace autoware::behavior_velocity_planner { -using PathIndexWithPose = std::pair; // front index, pose -using PathIndexWithPoint2d = std::pair; // front index, point2d -using PathIndexWithOffset = std::pair; // front index, offset - class NoStoppingAreaModule : public SceneModuleInterface { public: - struct DebugData - { - double base_link2front; - std::vector stop_poses; - geometry_msgs::msg::Pose first_stop_pose; - std::vector stuck_points; - geometry_msgs::msg::Polygon stuck_vehicle_detect_area; - geometry_msgs::msg::Polygon stop_line_detect_area; - }; - struct PlannerParam { /** @@ -76,11 +53,10 @@ class NoStoppingAreaModule : public SceneModuleInterface double path_expand_width; //! [m] path width to calculate the edge line for both side }; -public: NoStoppingAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, - const PlannerParam & planner_param, const rclcpp::Logger logger, + const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; @@ -91,77 +67,19 @@ class NoStoppingAreaModule : public SceneModuleInterface private: const int64_t lane_id_; - mutable bool pass_judged_ = false; - mutable bool is_stoppable_ = true; + no_stopping_area::PassJudge pass_judge_; StateMachine state_machine_; //! for state - /** - * @brief check if the object has a target type for stuck check - * @param object target object - * @return true if the object has a target type - */ - bool isTargetStuckVehicleType( - const autoware_perception_msgs::msg::PredictedObject & object) const; - /** * @brief Check if there is a stopped vehicle in stuck vehicle detect area. * @param poly ego focusing area polygon * @param objects_ptr target objects * @return true if exists */ - bool checkStuckVehiclesInNoStoppingArea( + bool check_stuck_vehicles_in_no_stopping_area( const Polygon2d & poly, const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr); - /** - * @brief Check if there is a stop line in "stop line detect area". - * @param path ego-car lane - * @param poly ego focusing area polygon - * @return true if exists - */ - bool checkStopLinesInNoStoppingArea( - const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly); - - /** - * @brief Calculate the polygon of the path from the ego-car position to the end of the - * no stopping lanelet (+ extra distance). - * @param path ego-car lane - * @param ego_pose ego-car pose - * @param margin margin from the end point of the ego-no stopping area lane - * @param extra_dist extra distance from the end point of the no stopping area lanelet - * @return generated polygon - */ - Polygon2d generateEgoNoStoppingAreaLanePolygon( - const tier4_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & ego_pose, const double margin, const double extra_dist) const; - - /** - * @brief Calculate the polygon of the path from the ego-car position to the end of the - * no stopping lanelet (+ extra distance). - * @param path ego-car lane - * @param stop_line_margin stop line margin from the stopping area lane - * @return generated stop line - */ - boost::optional getStopLineGeometry2d( - const tier4_planning_msgs::msg::PathWithLaneId & path, const double stop_line_margin) const; - - /** - * @brief Calculate if it's possible for ego-vehicle to stop before area consider jerk limit - * @param self_pose ego-car pose - * @param line_pose stop line pose on the lane - * @return is stoppable in front of no stopping area - */ - bool isStoppable( - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; - - /** - * @brief insert stop point on ego path - * @param path original path - * @param stop_point stop line point on the lane - */ - void insertStopPoint( - tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); - // Key Feature const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem_; std::shared_ptr last_obstacle_found_time_; @@ -170,7 +88,7 @@ class NoStoppingAreaModule : public SceneModuleInterface PlannerParam planner_param_; // Debug - DebugData debug_data_; + no_stopping_area::DebugData debug_data_; }; } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp new file mode 100644 index 0000000000000..5159233c341ed --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.cpp @@ -0,0 +1,262 @@ +// Copyright 2024 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 "utils.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +namespace autoware::behavior_velocity_planner::no_stopping_area +{ + +bool is_vehicle_type(const autoware_perception_msgs::msg::PredictedObject & object) +{ + // TODO(anyone): should we switch to using functions from the common object_recognition_utils ? + return !object.classification.empty() && + (object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::CAR || + object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::BUS || + object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::TRUCK || + object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::TRAILER || + object.classification.front().label == + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE); +} + +void insert_stop_point( + tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) +{ + auto insert_idx = stop_point.first + 1UL; + const auto & stop_pose = stop_point.second; + + // To PathPointWithLaneId + tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + stop_point_with_lane_id = path.points.at(insert_idx); + stop_point_with_lane_id.point.pose = stop_pose; + stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; + + // Insert stop point or replace with zero velocity + planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); +} + +std::optional generate_stop_line( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const lanelet::ConstPolygons3d & no_stopping_areas, const double ego_width, + const double stop_line_margin) +{ + LineString2d stop_line; + for (const auto & no_stopping_area : no_stopping_areas) { + const auto & area_poly = lanelet::utils::to2D(no_stopping_area).basicPolygon(); + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto p0 = path.points.at(i).point.pose.position; + const auto p1 = path.points.at(i + 1).point.pose.position; + const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; + std::vector collision_points; + boost::geometry::intersection(area_poly, line, collision_points); + if (!collision_points.empty()) { + const double yaw = autoware::universe_utils::calcAzimuthAngle(p0, p1); + const double w = ego_width; + const double l = stop_line_margin; + stop_line.emplace_back( + -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw + M_PI_2), + collision_points.front().y() + w * std::sin(yaw + M_PI_2)); + stop_line.emplace_back( + -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw - M_PI_2), + collision_points.front().y() + w * std::sin(yaw - M_PI_2)); + return stop_line; + } + } + } + return {}; +} + +bool is_stoppable( + PassJudge & pass_judge, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose, const EgoData & ego_data, + const rclcpp::Logger & logger, rclcpp::Clock & clock) +{ + // compute pass_judge_line_distance + const double stoppable_distance = planning_utils::calcJudgeLineDistWithJerkLimit( + ego_data.current_velocity, ego_data.current_acceleration, ego_data.max_stop_acc, + ego_data.max_stop_jerk, ego_data.delay_response_time); + const double signed_arc_length = + arc_lane_utils::calcSignedDistance(self_pose, line_pose.position); + const bool distance_stoppable = stoppable_distance < signed_arc_length; + const bool slow_velocity = ego_data.current_velocity < 2.0; + // ego vehicle is high speed and can't stop before stop line -> GO + const bool not_stoppable = !distance_stoppable && !slow_velocity; + // stoppable or not is judged only once + RCLCPP_DEBUG( + logger, "stoppable_dist: %lf signed_arc_length: %lf", stoppable_distance, signed_arc_length); + if (!distance_stoppable && !pass_judge.pass_judged) { + pass_judge.pass_judged = true; + // can't stop using maximum brake consider jerk limit + if (not_stoppable) { // TODO(someone): this can be replaced by !slow_velocity, is this correct? + // pass through + pass_judge.is_stoppable = false; + RCLCPP_WARN_THROTTLE( + logger, clock, 1000, "[NoStoppingArea] can't stop in front of no stopping area"); + } else { + pass_judge.is_stoppable = true; + } + } + return pass_judge.is_stoppable; +} + +Polygon2d generate_ego_no_stopping_area_lane_polygon( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double margin, + const double max_polygon_length, const double path_expand_width, const rclcpp::Logger & logger, + rclcpp::Clock & clock) +{ + Polygon2d ego_area; // open polygon + double dist_from_start_sum = 0.0; + constexpr double interpolation_interval = 0.5; + bool is_in_area = false; + tier4_planning_msgs::msg::PathWithLaneId interpolated_path; + if (!splineInterpolate(path, interpolation_interval, interpolated_path, logger)) { + return ego_area; + } + const auto & pp = interpolated_path.points; + /* calc closest index */ + const auto closest_idx_opt = autoware::motion_utils::findNearestIndex(pp, ego_pose, 3.0, M_PI_4); + if (!closest_idx_opt) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger, clock, 1000 /* ms */, "autoware::motion_utils::findNearestIndex fail"); + return ego_area; + } + const size_t closest_idx = closest_idx_opt.value(); + + const int num_ignore_nearest = 1; // Do not consider nearest lane polygon + size_t ego_area_start_idx = closest_idx + num_ignore_nearest; + // return if area size is not intentional + if (no_stopping_area_reg_elem.noStoppingAreas().size() != 1) { + return ego_area; + } + const auto no_stopping_area = no_stopping_area_reg_elem.noStoppingAreas().front(); + for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { + dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + const auto & p = pp.at(i).point.pose.position; + // TODO(someone): within will skip points on the edge of polygons so some points can be skipped + // depending on the interpolation + if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { + is_in_area = true; + break; + } + if (dist_from_start_sum > max_polygon_length) { + return ego_area; + } + ++ego_area_start_idx; + } + if (ego_area_start_idx > num_ignore_nearest) { + // TODO(someone): check if this is a bug + // this -1 causes pp[ego_area_start_idx] to be outside the no_stopping_area + // it causes "dist_from_area" to count the first point in the next loop + // moreover it causes the "dist_from_start_sum" to count the same point twice + ego_area_start_idx--; + } + if (!is_in_area) { + return ego_area; + } + double dist_from_area_sum = 0.0; + // decide end idx with extract distance + size_t ego_area_end_idx = ego_area_start_idx; + for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { + dist_from_start_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + const auto & p = pp.at(i).point.pose.position; + // TODO(someone): within will skip points on the edge of polygons so some points can be skipped + // depending on the interpolation + if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { + dist_from_area_sum += autoware::universe_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + } + if (dist_from_start_sum > max_polygon_length || dist_from_area_sum > margin) { + break; + } + ++ego_area_end_idx; + } + + ego_area = planning_utils::generatePathPolygon( + interpolated_path, ego_area_start_idx, ego_area_end_idx, path_expand_width); + return ego_area; +} + +bool check_stop_lines_in_no_stopping_area( + const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, + no_stopping_area::DebugData & debug_data) +{ + const double stop_vel = std::numeric_limits::min(); + + // if the detected stop point is near goal, it's ignored. + static constexpr double close_to_goal_distance = 1.0; + + // stuck points by stop line + for (size_t i = 0; i + 1 < path.points.size(); ++i) { + const auto p0 = path.points.at(i).point.pose.position; + const auto p1 = path.points.at(i + 1).point.pose.position; + const auto v0 = path.points.at(i).point.longitudinal_velocity_mps; + const auto v1 = path.points.at(i + 1).point.longitudinal_velocity_mps; + if (v0 > stop_vel && v1 > stop_vel) { + continue; + } + // judge if stop point p0 is near goal, by its distance to the path end. + const double dist_to_path_end = + autoware::motion_utils::calcSignedArcLength(path.points, i, path.points.size() - 1); + if (dist_to_path_end < close_to_goal_distance) { + // exit with false, cause position is near goal. + return false; + } + + const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; + std::vector collision_points; + bg::intersection(poly, line, collision_points); + if (!collision_points.empty()) { + geometry_msgs::msg::Point point; + point.x = collision_points.front().x(); + point.y = collision_points.front().y(); + point.z = 0.0; + debug_data.stuck_points.emplace_back(point); + return true; + } + } + return false; +} + +std::optional get_stop_line_geometry2d( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, + const double stop_line_margin, const double stop_line_extend_length, const double vehicle_width) +{ + const auto & stop_line = no_stopping_area_reg_elem.stopLine(); + if (stop_line && stop_line->size() >= 2) { + // get stop line from map + return planning_utils::extendLine( + stop_line.value()[0], stop_line.value()[1], stop_line_extend_length); + } + return generate_stop_line( + path, no_stopping_area_reg_elem.noStoppingAreas(), vehicle_width, stop_line_margin); +} + +} // namespace autoware::behavior_velocity_planner::no_stopping_area diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp new file mode 100644 index 0000000000000..73b835b4701dc --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/utils.hpp @@ -0,0 +1,165 @@ +// Copyright 2024 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner::no_stopping_area +{ +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = std::pair; // front index, point2d +using PathIndexWithOffset = std::pair; // front index, offset + +struct PassJudge +{ + bool pass_judged = false; + bool is_stoppable = true; +}; + +struct DebugData +{ + double base_link2front; + std::vector stop_poses; + geometry_msgs::msg::Pose first_stop_pose; + std::vector stuck_points; + geometry_msgs::msg::Polygon stuck_vehicle_detect_area; + geometry_msgs::msg::Polygon stop_line_detect_area; +}; + +// intermediate data about the ego vehicle taken from the PlannerData +struct EgoData +{ + EgoData() = default; + explicit EgoData(const PlannerData & planner_data) + { + current_velocity = planner_data.current_velocity->twist.linear.x; + current_acceleration = planner_data.current_acceleration->accel.accel.linear.x; + max_stop_acc = planner_data.max_stop_acceleration_threshold; + max_stop_jerk = planner_data.max_stop_jerk_threshold; + delay_response_time = planner_data.delay_response_time; + } + + double current_velocity{}; + double current_acceleration{}; + double max_stop_acc{}; + double max_stop_jerk{}; + double delay_response_time{}; +}; + +/** + * @brief check if the object is a vehicle (car, bus, truck, trailer, motorcycle) + * @param object input object + * @return true if the object is a vehicle + */ +bool is_vehicle_type(const autoware_perception_msgs::msg::PredictedObject & object); + +/** + * @brief insert stop point on ego path + * @param path original path + * @param stop_point stop line point on the lane + */ +void insert_stop_point( + tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); + +/** + * @brief generate stop line from no stopping area polygons + * ________________ + * ------|--|--------------|--> ego path + * stop | | Area | + * line | L______________/ + * @param path input path + * @param no_stopping_areas no stopping area polygons + * @param ego_width [m] width of ego + * @param stop_line_margin [m] margin to keep between the stop line and the no stopping areas + **/ +std::optional generate_stop_line( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const lanelet::ConstPolygons3d & no_stopping_areas, const double ego_width, + const double stop_line_margin); + +/** + * @brief Calculate if it's possible for ego-vehicle to stop before area consider jerk limit + * @param [inout] pass_judge the pass judge decision to update + * @param self_pose ego-car pose + * @param line_pose stop line pose on the lane + * @param ego_data planner data with ego pose, velocity, etc + * @param logger ros logger + * @param clock ros clock + * @return is stoppable in front of no stopping area + */ +bool is_stoppable( + PassJudge & pass_judge, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose, const EgoData & ego_data, + const rclcpp::Logger & logger, rclcpp::Clock & clock); + +/** + * @brief Calculate the polygon of the path from the ego-car position to the end of the + * no stopping lanelet (+ extra distance). + * @param path ego-car lane + * @param ego_pose ego-car pose + * @param margin margin from the end point of the ego-no stopping area lane + * @param max_polygon_length maximum length of the polygon + * @return generated polygon + */ +Polygon2d generate_ego_no_stopping_area_lane_polygon( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const double margin, + const double max_polygon_length, const double path_expand_width, const rclcpp::Logger & logger, + rclcpp::Clock & clock); + +/** + * @brief Check if there is a stop line in "stop line detect area". + * @param path ego-car lane + * @param poly ego focusing area polygon + * @param [out] debug_data structure to store the stuck points for debugging + * @return true if exists + */ +bool check_stop_lines_in_no_stopping_area( + const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly, + DebugData & debug_data); + +/** + * @brief Calculate the stop line of a no stopping area + * @details use the stop line of the regulatory element if it exists, otherwise generate it + * @param path ego path + * @param no_stopping_area_reg_elem no_stopping_area regulatory element + * @param stop_line_margin [m] margin between the stop line and the start of the no stopping area + * @param stop_line_extend_length [m] extra length to add on each side of the stop line (only added + * to the stop line of the regulatory element) + * @param vehicle_width [m] width of the ego vehicle + * @return generated stop line + */ +std::optional get_stop_line_geometry2d( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, + const double stop_line_margin, const double stop_line_extend_length, const double vehicle_width); + +} // namespace autoware::behavior_velocity_planner::no_stopping_area + +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp new file mode 100644 index 0000000000000..cc5bbd2903a52 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_utils.cpp @@ -0,0 +1,438 @@ +// Copyright 2024 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 "../src/utils.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include + +template +bool point_in_polygon(const Point & p, const Polygon & poly) +{ + return std::find_if(poly.outer().begin(), poly.outer().end(), [&](const auto & o) { + return p.x() == o.x() && p.y() == o.y(); + }) != poly.outer().end(); +} + +tier4_planning_msgs::msg::PathWithLaneId generate_straight_path( + const size_t nb_points, const float velocity = 0.0, const double resolution = 1.0) +{ + tier4_planning_msgs::msg::PathWithLaneId path; + for (auto x = 0UL; x < nb_points; ++x) { + tier4_planning_msgs::msg::PathPointWithLaneId p; + p.point.pose.position.x = resolution * static_cast(x); + p.point.pose.position.y = 0.0; + p.point.longitudinal_velocity_mps = velocity; + path.points.push_back(p); + } + return path; +} + +TEST(NoStoppingAreaTest, isTargetStuckVehicleType) +{ + using autoware::behavior_velocity_planner::no_stopping_area::is_vehicle_type; + autoware_perception_msgs::msg::PredictedObject object; + EXPECT_NO_FATAL_FAILURE(is_vehicle_type(object)); + autoware_perception_msgs::msg::ObjectClassification classification; + for (const auto label : { + autoware_perception_msgs::msg::ObjectClassification::CAR, + autoware_perception_msgs::msg::ObjectClassification::BUS, + autoware_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_perception_msgs::msg::ObjectClassification::TRAILER, + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + }) { + classification.label = label; + object.classification = {classification}; + EXPECT_TRUE(is_vehicle_type(object)); + } +} + +TEST(NoStoppingAreaTest, insertStopPoint) +{ + using autoware::behavior_velocity_planner::no_stopping_area::insert_stop_point; + constexpr auto nb_points = 10; + constexpr auto default_velocity = 5.0; + const tier4_planning_msgs::msg::PathWithLaneId base_path = + generate_straight_path(nb_points, default_velocity); + autoware::behavior_velocity_planner::no_stopping_area::PathIndexWithPose stop_point; + // stop exactly at a point, expect no new points but a 0 velocity at the stop point and after + auto path = base_path; + stop_point.first = 4UL; + stop_point.second = path.points[stop_point.first].point.pose; + insert_stop_point(path, stop_point); + ASSERT_EQ(path.points.size(), nb_points); + for (auto i = 0UL; i < stop_point.first; ++i) { + EXPECT_EQ(path.points[i].point.longitudinal_velocity_mps, default_velocity); + } + for (auto i = stop_point.first; i < nb_points; ++i) { + EXPECT_EQ(path.points[i].point.longitudinal_velocity_mps, 0.0); + } + + // stop between points + path = base_path; + stop_point.first = 3UL; + stop_point.second.position.x = 3.5; + insert_stop_point(path, stop_point); + ASSERT_EQ(path.points.size(), nb_points + 1); + EXPECT_EQ(path.points[stop_point.first + 1].point.pose.position.x, stop_point.second.position.x); + for (auto i = 0UL; i <= stop_point.first; ++i) { + EXPECT_EQ(path.points[i].point.longitudinal_velocity_mps, default_velocity); + } + for (auto i = stop_point.first + 1; i < nb_points + 1; ++i) { + EXPECT_EQ(path.points[i].point.longitudinal_velocity_mps, 0.0); + } + + // stop at the last point: exception // TODO(anyone): is this okay ? + path = base_path; + stop_point.first = path.points.size() - 1; + stop_point.second = path.points.back().point.pose; + EXPECT_THROW(insert_stop_point(path, stop_point), std::out_of_range); +} + +TEST(NoStoppingAreaTest, generateStopLine) +{ + using autoware::behavior_velocity_planner::no_stopping_area::generate_stop_line; + constexpr auto nb_points = 10; + const tier4_planning_msgs::msg::PathWithLaneId path = generate_straight_path(nb_points); + lanelet::ConstPolygons3d no_stopping_areas; + double ego_width = 0.0; + double stop_line_margin = 0.0; + // no areas, expect no stop line + auto stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_FALSE(stop_line.has_value()); + + // area outside of the path + lanelet::Polygon3d poly; + poly.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -2.0)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -0.5)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -0.5)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -2.0)); + no_stopping_areas.push_back(poly); + stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_FALSE(stop_line.has_value()); + // area on the path, 0 margin and 0 width + poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -1.0)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, 1.0)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 6.0, 1.0)); + poly.push_back(lanelet::Point3d(lanelet::InvalId, 6.0, -1.0)); + no_stopping_areas.push_back(poly); + stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_TRUE(stop_line.has_value()); + ASSERT_EQ(stop_line->size(), 2UL); + EXPECT_EQ(stop_line->front().x(), 5.0); + EXPECT_EQ(stop_line->front().y(), 0.0); + EXPECT_EQ(stop_line->back().x(), 5.0); + EXPECT_EQ(stop_line->back().y(), 0.0); + // set a margin + stop_line_margin = 1.0; + stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_TRUE(stop_line.has_value()); + ASSERT_EQ(stop_line->size(), 2UL); + EXPECT_EQ(stop_line->front().x(), 4.0); + EXPECT_EQ(stop_line->front().y(), 0.0); + EXPECT_EQ(stop_line->back().x(), 4.0); + EXPECT_EQ(stop_line->back().y(), 0.0); + // set a width + ego_width = 2.0; + stop_line = generate_stop_line(path, no_stopping_areas, ego_width, stop_line_margin); + EXPECT_TRUE(stop_line.has_value()); + ASSERT_EQ(stop_line->size(), 2UL); + // TODO(anyone): if we stop at this stop line ego overlaps the 1st area, is this okay ? + EXPECT_EQ(stop_line->front().x(), 4.0); + EXPECT_EQ(stop_line->front().y(), 2.0); + EXPECT_EQ(stop_line->back().x(), 4.0); + EXPECT_EQ(stop_line->back().y(), -2.0); +} + +TEST(NoStoppingAreaTest, isStoppable) +{ + using autoware::behavior_velocity_planner::no_stopping_area::is_stoppable; + autoware::behavior_velocity_planner::no_stopping_area::PassJudge pass_judge; + geometry_msgs::msg::Pose self_pose; + geometry_msgs::msg::Pose line_pose; + rclcpp::Clock clock; + auto logger = rclcpp::get_logger("test_logger"); + logger.set_level(rclcpp::Logger::Level::Debug); + autoware::behavior_velocity_planner::no_stopping_area::EgoData ego_data; + ego_data.current_velocity = 1.0; + ego_data.current_acceleration = 0.0; + ego_data.delay_response_time = 0.0; + ego_data.max_stop_acc = -1.0; + ego_data.max_stop_jerk = -1.0; + // try to stop 1m ahead + self_pose.position.x = 0.0; + self_pose.position.y = 0.0; + line_pose.position.x = 1.0; + line_pose.position.y = 0.0; + + // enough distance to stop and slow velocity + EXPECT_TRUE(is_stoppable(pass_judge, self_pose, line_pose, ego_data, logger, clock)); + EXPECT_TRUE(pass_judge.is_stoppable); + EXPECT_FALSE(pass_judge.pass_judged); + + // unstoppable and fast velocity + ego_data.current_velocity = 10.0; + EXPECT_FALSE(is_stoppable(pass_judge, self_pose, line_pose, ego_data, logger, clock)); + EXPECT_FALSE(pass_judge.is_stoppable); + EXPECT_TRUE(pass_judge.pass_judged); + + // unstoppable and slow velocity, but since we already judged, it is not updated + ego_data.current_velocity = 1.9; + EXPECT_FALSE(is_stoppable(pass_judge, self_pose, line_pose, ego_data, logger, clock)); + EXPECT_FALSE(pass_judge.is_stoppable); + EXPECT_TRUE(pass_judge.pass_judged); + + // reset and result is updated + // TODO(someone): is this the correct behavior ? distance is unstoppable but result is stoppable + pass_judge.pass_judged = false; + EXPECT_TRUE(is_stoppable(pass_judge, self_pose, line_pose, ego_data, logger, clock)); + EXPECT_TRUE(pass_judge.is_stoppable); + EXPECT_TRUE(pass_judge.pass_judged); +} + +TEST(NoStoppingAreaTest, generateEgoNoStoppingAreaLanePolygon) +{ + using autoware::behavior_velocity_planner::no_stopping_area:: + generate_ego_no_stopping_area_lane_polygon; + using autoware::universe_utils::Point2d; + geometry_msgs::msg::Pose ego_pose; // ego at (0,0) + ego_pose.position.x = 0.0; + ego_pose.position.y = 0.0; + const tier4_planning_msgs::msg::PathWithLaneId path = + generate_straight_path(8); // ego path at x = 0, 1,..., 7 and y=0 + double margin = 1.0; // desired margin added to the polygon after the end of the area + double max_polygon_length = 10.0; // maximum length of the generated polygon + double path_expand_width = 2.0; // width of the generated polygon + auto logger = rclcpp::get_logger("test_logger"); + logger.set_level(rclcpp::Logger::Level::Debug); + rclcpp::Clock clock; + + lanelet::Polygon3d no_stopping_area; + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, 1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, 1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -1.0)); + const auto no_stopping_area_reg_elem = + lanelet::autoware::NoStoppingArea::make(lanelet::InvalId, {}, {no_stopping_area}, {}); + // basic case + { + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *no_stopping_area_reg_elem, margin, max_polygon_length, path_expand_width, + logger, clock); + autoware::universe_utils::Polygon2d simplified_poly; + EXPECT_FALSE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + // simplify the polygon to make it easier to check + boost::geometry::simplify(lane_poly, simplified_poly, 1.0); + EXPECT_EQ(simplified_poly.outer().size(), 5UL); // closed polygon so 1st point == last point + // polygon over the path/no_stop_area overlap and with the desired width + // TODO(someone): we expect x=6 (end of area [5] + margin [1]) but current is 5.5 because a + // point is counted twice + // EXPECT_TRUE(point_in_polygon(Point2d(6.0, 2.0), simplified_poly)); + // EXPECT_TRUE(point_in_polygon(Point2d(6.0, -2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, 2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, -2.0), simplified_poly)); + } + + // big margin -> get a polygon until the end of the path + { + const double big_margin = 10.0; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *no_stopping_area_reg_elem, big_margin, max_polygon_length, path_expand_width, + logger, clock); + autoware::universe_utils::Polygon2d simplified_poly; + EXPECT_FALSE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + // simplify the polygon to make it easier to check + boost::geometry::simplify(lane_poly, simplified_poly, 1.0); + EXPECT_EQ(simplified_poly.outer().size(), 5UL); // closed polygon so 1st point == last point + // polygon over the path/no_stop_area overlap and with the desired width + EXPECT_TRUE(point_in_polygon(Point2d(7.0, 2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(7.0, -2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, 2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, -2.0), simplified_poly)); + } + // small max polygon length + { + const double small_polygon_length = 5.0; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *no_stopping_area_reg_elem, margin, small_polygon_length, path_expand_width, + logger, clock); + autoware::universe_utils::Polygon2d simplified_poly; + EXPECT_FALSE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + // simplify the polygon to make it easier to check + boost::geometry::simplify(lane_poly, simplified_poly, 1.0); + EXPECT_EQ(simplified_poly.outer().size(), 5UL); // closed polygon so 1st point == last point + // polygon over the path/no_stop_area overlap and with the desired width + // TODO(someone): the length calculation is currently buggy + // ego at x=0, no_stopping_area starts at x=3 so we expect a lane polygon of length = 2.0, but + // some point is counted twice so the length is only 1.5 (interpolation interval is 0.5) + // EXPECT_TRUE(point_in_polygon(Point2d(4.0, 2.0), simplified_poly)); + // EXPECT_TRUE(point_in_polygon(Point2d(4.0, -2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, 2.0), simplified_poly)); + EXPECT_TRUE(point_in_polygon(Point2d(3.0, -2.0), simplified_poly)); + } + + // cases where the polygon returned is empty + // path is empty + { + tier4_planning_msgs::msg::PathWithLaneId empty_path; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + empty_path, ego_pose, *no_stopping_area_reg_elem, margin, max_polygon_length, + path_expand_width, logger, clock); + EXPECT_TRUE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + } + // path points do not enter the no stopping area + // ego is far from the path + { + auto far_ego_pose = ego_pose; + far_ego_pose.position.y = 10.0; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, far_ego_pose, *no_stopping_area_reg_elem, margin, max_polygon_length, path_expand_width, + logger, clock); + EXPECT_TRUE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + } + // no stopping area starts after the minimum length + { + const double short_max_polygon_length = 2.0; + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *no_stopping_area_reg_elem, margin, short_max_polygon_length, + path_expand_width, logger, clock); + EXPECT_TRUE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + } + // path crosses the no stopping area but the current interpolation (0.5) is not enough to detect + // it + // TODO(someone): should this be fixed ? + { + lanelet::Polygon3d small_no_stopping_area; + small_no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.1, -1.0)); + small_no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.1, 1.0)); + small_no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.4, 1.0)); + small_no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.4, -1.0)); + const auto small_no_stopping_area_reg_elem = + lanelet::autoware::NoStoppingArea::make(lanelet::InvalId, {}, {small_no_stopping_area}, {}); + const auto lane_poly = generate_ego_no_stopping_area_lane_polygon( + path, ego_pose, *small_no_stopping_area_reg_elem, margin, max_polygon_length, + path_expand_width, logger, clock); + EXPECT_TRUE(lane_poly.outer().empty()); + EXPECT_TRUE(lane_poly.inners().empty()); + } +} + +TEST(NoStoppingAreaTest, checkStopLinesInNoStoppingArea) +{ + using autoware::behavior_velocity_planner::no_stopping_area::check_stop_lines_in_no_stopping_area; + tier4_planning_msgs::msg::PathWithLaneId path; + autoware::universe_utils::Polygon2d poly; + autoware::behavior_velocity_planner::no_stopping_area::DebugData debug_data; + + // empty inputs + EXPECT_FALSE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); + + constexpr auto nb_points = 10; + constexpr auto non_stopped_velocity = 5.0; + const tier4_planning_msgs::msg::PathWithLaneId non_stopping_path = + generate_straight_path(nb_points, non_stopped_velocity); + path = non_stopping_path; + // set x=4 and x=5 to be stopping points + path.points[4].point.longitudinal_velocity_mps = 0.0; + path.points[5].point.longitudinal_velocity_mps = 0.0; + // empty polygon + EXPECT_FALSE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); + // non stopping path + poly.outer().emplace_back(3.0, -1.0); + poly.outer().emplace_back(3.0, 1.0); + poly.outer().emplace_back(5.0, 1.0); + poly.outer().emplace_back(5.0, -1.0); + poly.outer().push_back(poly.outer().front()); // close the polygon + EXPECT_FALSE(check_stop_lines_in_no_stopping_area(non_stopping_path, poly, debug_data)); + // stop in the area + EXPECT_TRUE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); + // if stop in the area is within 1m of the end of the path we ignore it: only for 1st stop + path.points[8].point.longitudinal_velocity_mps = 0.0; + path.points[9].point.longitudinal_velocity_mps = 0.0; + EXPECT_TRUE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); + // if stop in the area is within 1m of the end of the path we ignore it + path.points[4].point.longitudinal_velocity_mps = non_stopped_velocity; + path.points[5].point.longitudinal_velocity_mps = non_stopped_velocity; + EXPECT_FALSE(check_stop_lines_in_no_stopping_area(path, poly, debug_data)); +} + +TEST(NoStoppingAreaTest, getStopLineGeometry2d) +{ + using autoware::behavior_velocity_planner::no_stopping_area::generate_stop_line; + using autoware::behavior_velocity_planner::no_stopping_area::get_stop_line_geometry2d; + const tier4_planning_msgs::msg::PathWithLaneId path = generate_straight_path(10); + lanelet::Polygon3d no_stopping_area; + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, -1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 3.0, 1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, 1.0)); + no_stopping_area.push_back(lanelet::Point3d(lanelet::InvalId, 5.0, -1.0)); + double stop_line_margin = 1.0; + double stop_line_extend_length = 1.0; + double vehicle_width = 1.0; + { // get stop line of the regulatory element extended by the extend length + lanelet::LineString3d reg_elem_stop_line; + reg_elem_stop_line.push_back(lanelet::Point3d(lanelet::InvalId, 0.0, 0.0)); + reg_elem_stop_line.push_back(lanelet::Point3d(lanelet::InvalId, 1.0, 0.0)); + const auto no_stopping_area_reg_elem = lanelet::autoware::NoStoppingArea::make( + lanelet::InvalId, {}, {no_stopping_area}, reg_elem_stop_line); + const auto stop_line = get_stop_line_geometry2d( + path, *no_stopping_area_reg_elem, stop_line_margin, stop_line_extend_length, vehicle_width); + ASSERT_TRUE(stop_line.has_value()); + ASSERT_EQ(stop_line->size(), 2UL); + EXPECT_EQ(stop_line->front().x(), -1.0); + EXPECT_EQ(stop_line->front().y(), 0.0); + EXPECT_EQ(stop_line->back().x(), 2.0); + EXPECT_EQ(stop_line->back().y(), 0.0); + } + { // regulatory element has no stop line -> get the same stop line as generate_stop_line + const auto no_stopping_area_reg_elem = + lanelet::autoware::NoStoppingArea::make(lanelet::InvalId, {}, {no_stopping_area}, {}); + const auto stop_line = get_stop_line_geometry2d( + path, *no_stopping_area_reg_elem, stop_line_margin, stop_line_extend_length, vehicle_width); + const auto generated_stop_line = + generate_stop_line(path, {no_stopping_area}, vehicle_width, stop_line_margin); + ASSERT_TRUE(stop_line.has_value()); + ASSERT_TRUE(generated_stop_line.has_value()); + ASSERT_EQ(stop_line->size(), generated_stop_line->size()); + for (auto i = 0UL; i < stop_line->size(); ++i) { + EXPECT_EQ(stop_line->at(i).x(), generated_stop_line->at(i).x()); + EXPECT_EQ(stop_line->at(i).y(), generated_stop_line->at(i).y()); + } + } +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index 29f3794516ef5..9dd645f08fdad 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -274,7 +274,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) + static bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) { bool enable_rtc = true; From 8c63c61381b88527ffb5fb2ce8120db5e53f1179 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Tue, 15 Oct 2024 12:31:21 +0900 Subject: [PATCH 3/5] refactor(bpp_common, motion_utils): move path shifter util functions to autoware::motion_utils (#9081) * remove unused function Signed-off-by: Go Sakayori * mover path shifter utils function to autoware motion utils Signed-off-by: Go Sakayori * minor change in license header Signed-off-by: Go Sakayori * fix warning message Signed-off-by: Go Sakayori * remove header file Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- .../motion_utils/trajectory/path_shift.hpp | 71 ++++++++++++ .../src/trajectory/path_shift.cpp | 105 ++++++++++++++++++ .../src/pull_over_planner/shift_pull_over.cpp | 3 +- .../src/scene.cpp | 9 +- .../src/utils/calculation.cpp | 10 +- .../utils/path_shifter/path_shifter.hpp | 31 ------ .../src/utils/path_shifter/path_shifter.cpp | 101 ----------------- .../src/scene.cpp | 3 +- .../src/shift_pull_out.cpp | 3 +- .../helper.hpp | 17 +-- .../src/scene.cpp | 10 +- .../src/shift_line_generator.cpp | 4 +- 12 files changed, 210 insertions(+), 157 deletions(-) create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp create mode 100644 common/autoware_motion_utils/src/trajectory/path_shift.cpp diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp new file mode 100644 index 0000000000000..f4602ffff83e4 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp @@ -0,0 +1,71 @@ +// Copyright 2024 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 AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ + +namespace autoware::motion_utils +{ +/** + * @brief Calculates the velocity required for shifting + * @param lateral lateral distance + * @param jerk lateral jerk + * @param longitudinal_distance longitudinal distance + * @return velocity + */ +double calc_feasible_velocity_from_jerk( + const double lateral, const double jerk, const double longitudinal_distance); + +/** + * @brief Calculates the lateral distance required for shifting + * @param longitudinal longitudinal distance + * @param jerk lateral jerk + * @param velocity velocity + * @return lateral distance + */ +double calc_lateral_dist_from_jerk( + const double longitudinal, const double jerk, const double velocity); + +/** + * @brief Calculates the lateral distance required for shifting + * @param lateral lateral distance + * @param jerk lateral jerk + * @param velocity velocity + * @return longitudinal distance + */ +double calc_longitudinal_dist_from_jerk( + const double lateral, const double jerk, const double velocity); + +/** + * @brief Calculates the total time required for shifting + * @param lateral lateral distance + * @param jerk lateral jerk + * @param acc lateral acceleration + * @return time + */ +double calc_shift_time_from_jerk(const double lateral, const double jerk, const double acc); + +/** + * @brief Calculates the required jerk from lateral/longitudinal distance + * @param lateral lateral distance + * @param longitudinal longitudinal distance + * @param velocity velocity + * @return jerk + */ +double calc_jerk_from_lat_lon_distance( + const double lateral, const double longitudinal, const double velocity); + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ diff --git a/common/autoware_motion_utils/src/trajectory/path_shift.cpp b/common/autoware_motion_utils/src/trajectory/path_shift.cpp new file mode 100644 index 0000000000000..6f57f34a56190 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory/path_shift.cpp @@ -0,0 +1,105 @@ +// Copyright 2024 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 "autoware/motion_utils/trajectory/path_shift.hpp" + +#include "autoware/motion_utils/trajectory/trajectory.hpp" + +namespace autoware::motion_utils +{ +double calc_feasible_velocity_from_jerk( + const double lateral, const double jerk, const double longitudinal_distance) +{ + const double j = std::abs(jerk); + const double l = std::abs(lateral); + const double d = std::abs(longitudinal_distance); + if (j < 1.0e-8 || l < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate velocity due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; + } + return d / (4.0 * std::pow(0.5 * l / j, 1.0 / 3.0)); +} + +double calc_lateral_dist_from_jerk( + const double longitudinal, const double jerk, const double velocity) +{ + const double j = std::abs(jerk); + const double d = std::abs(longitudinal); + const double v = std::abs(velocity); + + if (j < 1.0e-8 || v < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate lateral distance due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; + } + return 2.0 * std::pow(d / (4.0 * v), 3.0) * j; +} + +double calc_longitudinal_dist_from_jerk( + const double lateral, const double jerk, const double velocity) +{ + const double j = std::abs(jerk); + const double l = std::abs(lateral); + const double v = std::abs(velocity); + if (j < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate longitudinal distance due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; + } + return 4.0 * std::pow(0.5 * l / j, 1.0 / 3.0) * v; +} + +double calc_shift_time_from_jerk(const double lateral, const double jerk, const double acc) +{ + const double j = std::abs(jerk); + const double a = std::abs(acc); + const double l = std::abs(lateral); + if (j < 1.0e-8 || a < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate shift time due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; // TODO(Horibe) maybe invalid arg? + } + + // time with constant jerk + double tj = a / j; + + // time with constant acceleration (zero jerk) + double ta = (std::sqrt(a * a + 4.0 * j * j * l / a) - 3.0 * a) / (2.0 * j); + + if (ta < 0.0) { + // it will not hit the acceleration limit this time + tj = std::pow(l / (2.0 * j), 1.0 / 3.0); + ta = 0.0; + } + + const double t_total = 4.0 * tj + 2.0 * ta; + return t_total; +} + +double calc_jerk_from_lat_lon_distance( + const double lateral, const double longitudinal, const double velocity) +{ + constexpr double ep = 1.0e-3; + const double lat = std::abs(lateral); + const double lon = std::max(std::abs(longitudinal), ep); + const double v = std::abs(velocity); + return 0.5 * lat * std::pow(4.0 * v / lon, 3); +} + +} // namespace autoware::motion_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index 30f250634c028..f70dbbd9c1e50 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -18,6 +18,7 @@ #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include #include #include @@ -181,7 +182,7 @@ std::optional ShiftPullOver::generatePullOverPath( .y; // calculate shift start pose on road lane - const double pull_over_distance = PathShifter::calcLongitudinalDistFromJerk( + const double pull_over_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 532a99a67195b..5961435085346 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -23,6 +23,8 @@ #include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include +#include #include #include #include @@ -1612,7 +1614,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto [min_lateral_acc, max_lateral_acc] = lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity); - const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + const auto lane_changing_time = autoware::motion_utils::calc_shift_time_from_jerk( shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; @@ -1876,9 +1878,8 @@ bool NormalLaneChange::calcAbortPath() PathShifter path_shifter; path_shifter.setPath(lane_changing_path); path_shifter.addShiftLine(shift_line); - const auto lateral_jerk = - autoware::behavior_path_planner::PathShifter::calcJerkFromLatLonDistance( - shift_line.end_shift_length, abort_start_dist, current_velocity); + const auto lateral_jerk = autoware::motion_utils::calc_jerk_from_lat_lon_distance( + shift_line.end_shift_length, abort_start_dist, current_velocity); path_shifter.setVelocity(current_velocity); const auto lateral_acc_range = lane_change_parameters_->lane_change_lat_acc_map.find(current_velocity); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 1c4aede51074a..762cada4e59d2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -15,6 +15,7 @@ #include #include +#include #include @@ -163,7 +164,7 @@ double calc_maximum_lane_change_length( const auto [min_lat_acc, max_lat_acc] = lane_change_parameters.lane_change_lat_acc_map.find(vel); const auto t_lane_changing = - PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + autoware::motion_utils::calc_shift_time_from_jerk(shift_interval, lat_jerk, max_lat_acc); const auto lane_changing_length = vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; @@ -208,7 +209,8 @@ std::vector calc_all_min_lc_lengths( min_lc_lengths.reserve(shift_intervals.size()); const auto min_lc_length = [&](const auto shift_interval) { - const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + const auto t = + autoware::motion_utils::calc_shift_time_from_jerk(shift_interval, lat_jerk, max_lat_acc); return min_vel * t; }; @@ -248,7 +250,7 @@ std::vector calc_all_max_lc_lengths( // lane changing section const auto [min_lat_acc, max_lat_acc] = lc_param_ptr->lane_change_lat_acc_map.find(vel); const auto t_lane_changing = - PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + autoware::motion_utils::calc_shift_time_from_jerk(shift_interval, lat_jerk, max_lat_acc); const auto lane_changing_length = vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; @@ -410,7 +412,7 @@ std::vector calc_shift_phase_metrics( for (double lat_acc = min_lateral_acc; lat_acc < max_lateral_acc + eps; lat_acc += lateral_acc_resolution) { - const auto lane_changing_duration = PathShifter::calcShiftTimeFromJerk( + const auto lane_changing_duration = autoware::motion_utils::calc_shift_time_from_jerk( shift_length, common_data_ptr->lc_param_ptr->lane_changing_lateral_jerk, lat_acc); const double lane_changing_accel = calc_lane_changing_acceleration( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 8164ef659b7b1..4364bb1744a7e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -108,11 +108,6 @@ class PathShifter */ std::vector getShiftLines() const { return shift_lines_; } - /** - * @brief Get shift points size. - */ - size_t getShiftLinesSize() const { return shift_lines_.size(); } - /** * @brief Get base offset. */ @@ -138,36 +133,10 @@ class PathShifter */ void removeBehindShiftLineAndSetBaseOffset(const size_t nearest_idx); - //////////////////////////////////////// - // Utility Functions - //////////////////////////////////////// - - static double calcFeasibleVelocityFromJerk( - const double lateral, const double jerk, const double longitudinal_distance); - - static double calcLateralDistFromJerk( - const double longitudinal, const double jerk, const double velocity); - - static double calcLongitudinalDistFromJerk( - const double lateral, const double jerk, const double velocity); - - static double calcShiftTimeFromJerk(const double lateral, const double jerk, const double acc); - - static double calcJerkFromLatLonDistance( - const double lateral, const double longitudinal, const double velocity); - - double getTotalShiftLength() const; - double getLastShiftLength() const; std::optional getLastShiftLine() const; - /** - * @brief Calculate the theoretical lateral jerk by spline shifting for current shift_lines_. - * @return Jerk array. The size is same as the shift points. - */ - std::vector calcLateralJerk() const; - private: // The reference path along which the shift will be performed. PathWithLaneId reference_path_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index 69c538fa6cb54..22baffe80fdf6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -395,27 +395,6 @@ std::pair, std::vector> PathShifter::calcBaseLengths return {base_lon, base_lat}; } -std::vector PathShifter::calcLateralJerk() const -{ - const auto arclength_arr = utils::calcPathArcLengthArray(reference_path_); - - constexpr double epsilon = 1.0e-8; // to avoid 0 division - - std::vector lateral_jerk{}; - - // TODO(Watanabe) write docs. - double current_shift = base_offset_; - for (const auto & shift_line : shift_lines_) { - const double delta_shift = shift_line.end_shift_length - current_shift; - const auto shifting_arclength = std::max( - arclength_arr.at(shift_line.end_idx) - arclength_arr.at(shift_line.start_idx), epsilon); - lateral_jerk.push_back((delta_shift / 2.0) / std::pow(shifting_arclength / 4.0, 3.0)); - current_shift = shift_line.end_shift_length; - } - - return lateral_jerk; -} - void PathShifter::updateShiftLinesIndices(ShiftLineArray & shift_lines) const { if (reference_path_.points.empty()) { @@ -542,86 +521,6 @@ void PathShifter::shiftBaseLength(ShiftedPath * path, double offset) } } -double PathShifter::calcShiftTimeFromJerk(const double lateral, const double jerk, const double acc) -{ - const double j = std::abs(jerk); - const double a = std::abs(acc); - const double l = std::abs(lateral); - if (j < 1.0e-8 || a < 1.0e-8) { - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - - // time with constant jerk - double tj = a / j; - - // time with constant acceleration (zero jerk) - double ta = (std::sqrt(a * a + 4.0 * j * j * l / a) - 3.0 * a) / (2.0 * j); - - if (ta < 0.0) { - // it will not hit the acceleration limit this time - tj = std::pow(l / (2.0 * j), 1.0 / 3.0); - ta = 0.0; - } - - const double t_total = 4.0 * tj + 2.0 * ta; - return t_total; -} - -double PathShifter::calcFeasibleVelocityFromJerk( - const double lateral, const double jerk, const double longitudinal_distance) -{ - const double j = std::abs(jerk); - const double l = std::abs(lateral); - const double d = std::abs(longitudinal_distance); - if (j < 1.0e-8) { - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - return d / (4.0 * std::pow(0.5 * l / j, 1.0 / 3.0)); -} - -double PathShifter::calcLateralDistFromJerk( - const double longitudinal, const double jerk, const double velocity) -{ - const double j = std::abs(jerk); - const double d = std::abs(longitudinal); - const double v = std::abs(velocity); - if (j < 1.0e-8) { - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - return 2.0 * std::pow(d / (4.0 * v), 3.0) * j; -} - -double PathShifter::calcLongitudinalDistFromJerk( - const double lateral, const double jerk, const double velocity) -{ - const double j = std::abs(jerk); - const double l = std::abs(lateral); - const double v = std::abs(velocity); - if (j < 1.0e-8) { - return 1.0e10; // TODO(Horibe) maybe invalid arg? - } - return 4.0 * std::pow(0.5 * l / j, 1.0 / 3.0) * v; -} - -double PathShifter::calcJerkFromLatLonDistance( - const double lateral, const double longitudinal, const double velocity) -{ - constexpr double ep = 1.0e-3; - const double lat = std::abs(lateral); - const double lon = std::max(std::abs(longitudinal), ep); - const double v = std::abs(velocity); - return 0.5 * lat * std::pow(4.0 * v / lon, 3); -} - -double PathShifter::getTotalShiftLength() const -{ - double sum = base_offset_; - for (const auto & l : shift_lines_) { - sum += l.end_shift_length; - } - return sum; -} - double PathShifter::getLastShiftLength() const { if (shift_lines_.empty()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp index d831f65b23117..e1f6d0bb6af9c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -20,6 +20,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/behavior_path_side_shift_module/utils.hpp" +#include #include #include @@ -346,7 +347,7 @@ ShiftLine SideShiftModule::calcShiftLine() const const double dist_to_end = [&]() { const double shift_length = requested_lateral_offset_ - getClosestShiftLength(); - const double jerk_shifting_distance = path_shifter_.calcLongitudinalDistFromJerk( + const double jerk_shifting_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_length, p->shifting_lateral_jerk, std::max(ego_speed, p->min_shifting_speed)); const double shifting_distance = std::max(jerk_shifting_distance, p->min_shifting_distance); const double dist_to_end = dist_to_start + shifting_distance; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 3f750a7f1a87b..2d94ceb85de28 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -22,6 +22,7 @@ #include "autoware/motion_utils/trajectory/path_with_lane_id.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include #include #include @@ -306,7 +307,7 @@ std::vector ShiftPullOut::calcPullOutPaths( path_shifter.setPath(road_lane_reference_path); const double shift_time = - PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc); + autoware::motion_utils::calc_shift_time_from_jerk(shift_length, lateral_jerk, lateral_acc); const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, /* max acc */ 1.0); const auto pull_out_distance = calcPullOutLongitudinalDistance( longitudinal_acc, shift_time, shift_length, maximum_curvature, diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp index 0f3536eeb4e7c..b59dc9e91e23c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -20,6 +20,8 @@ #include "autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include + #include #include #include @@ -118,26 +120,27 @@ class AvoidanceHelper const auto nominal_speed = std::max(getEgoSpeed(), p->nominal_avoidance_speed); const auto nominal_jerk = p->lateral_min_jerk_map.at(getConstraintsMapIndex(nominal_speed, p->velocity_map)); - return PathShifter::calcLongitudinalDistFromJerk(shift_length, nominal_jerk, nominal_speed); + return autoware::motion_utils::calc_longitudinal_dist_from_jerk( + shift_length, nominal_jerk, nominal_speed); } double getMinAvoidanceDistance(const double shift_length) const { const auto & p = parameters_; - return PathShifter::calcLongitudinalDistFromJerk( + return autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_length, p->lateral_max_jerk_map.front(), p->velocity_map.front()); } double getMaxAvoidanceDistance(const double shift_length) const { - const auto distance_from_jerk = PathShifter::calcLongitudinalDistFromJerk( + const auto distance_from_jerk = autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_length, getLateralMinJerkLimit(), getAvoidanceEgoSpeed()); return std::max(getNominalAvoidanceDistance(shift_length), distance_from_jerk); } double getSharpAvoidanceDistance(const double shift_length) const { - return PathShifter::calcLongitudinalDistFromJerk( + return autoware::motion_utils::calc_longitudinal_dist_from_jerk( shift_length, getLateralMaxJerkLimit(), getAvoidanceEgoSpeed()); } @@ -227,8 +230,8 @@ class AvoidanceHelper const auto max_shift_length = std::max( std::abs(parameters_->max_right_shift_length), std::abs(parameters_->max_left_shift_length)); - const auto dynamic_distance = - PathShifter::calcLongitudinalDistFromJerk(max_shift_length, getLateralMinJerkLimit(), speed); + const auto dynamic_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk( + max_shift_length, getLateralMinJerkLimit(), speed); return std::clamp( 1.5 * dynamic_distance + getNominalPrepareDistance(), @@ -312,7 +315,7 @@ class AvoidanceHelper { const auto JERK_BUFFER = 0.1; // [m/sss] return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { - return PathShifter::calcJerkFromLatLonDistance( + return autoware::motion_utils::calc_jerk_from_lat_lon_distance( line.getRelativeLength(), line.getRelativeLongitudinal(), getAvoidanceEgoSpeed()) < getLateralMaxJerkLimit() + JERK_BUFFER; }); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 4d13d4164d263..2c5950bfeae0c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -1316,7 +1316,7 @@ void StaticObstacleAvoidanceModule::addNewShiftLines( const double road_velocity = avoid_data_.reference_path.points.at(front_new_shift_line.start_idx) .point.longitudinal_velocity_mps; - const double shift_time = PathShifter::calcShiftTimeFromJerk( + const double shift_time = autoware::motion_utils::calc_shift_time_from_jerk( front_new_shift_line.getRelativeLength(), helper_->getLateralMaxJerkLimit(), helper_->getLateralMaxAccelLimit()); const double longitudinal_acc = @@ -1673,7 +1673,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_); // insert slow down speed. - const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( + const double current_target_velocity = autoware::motion_utils::calc_feasible_velocity_from_jerk( shift_length, helper_->getLateralMinJerkLimit(), to_stop_line); if (current_target_velocity < getEgoSpeed()) { RCLCPP_DEBUG(getLogger(), "current velocity exceeds target slow down speed."); @@ -1692,7 +1692,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( } // target speed with nominal jerk limits. - const double v_target = PathShifter::calcFeasibleVelocityFromJerk( + const double v_target = autoware::motion_utils::calc_feasible_velocity_from_jerk( shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance); const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; const double v_insert = @@ -1880,7 +1880,7 @@ void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_ const auto lower_speed = object.value().avoid_required ? 0.0 : parameters_->min_slow_down_speed; // insert slow down speed. - const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( + const double current_target_velocity = autoware::motion_utils::calc_feasible_velocity_from_jerk( shift_length, helper_->getLateralMinJerkLimit(), distance_to_object); if (current_target_velocity < getEgoSpeed() + parameters_->buf_slow_down_speed) { utils::static_obstacle_avoidance::insertDecelPoint( @@ -1901,7 +1901,7 @@ void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_ } // target speed with nominal jerk limits. - const double v_target = PathShifter::calcFeasibleVelocityFromJerk( + const double v_target = autoware::motion_utils::calc_feasible_velocity_from_jerk( shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance); const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; const double v_insert = std::max(v_target - parameters_->buf_slow_down_speed, lower_speed); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index 10d003fcad90f..917736b1680db 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -179,7 +179,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } // calculate lateral jerk. - const auto required_jerk = PathShifter::calcJerkFromLatLonDistance( + const auto required_jerk = autoware::motion_utils::calc_jerk_from_lat_lon_distance( avoiding_shift, avoidance_distance, helper_->getAvoidanceEgoSpeed()); // relax lateral jerk limit. avoidable. @@ -201,7 +201,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } // output avoidance path under lateral jerk constraints. - const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk( + const auto feasible_relative_shift_length = autoware::motion_utils::calc_lateral_dist_from_jerk( avoidance_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed()); if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { From bb825f0021e5aa0e1962ca982bb537b1562df2da Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Tue, 15 Oct 2024 14:12:45 +0900 Subject: [PATCH 4/5] test(motion_utils): add test for path shift (#9083) * remove unused function Signed-off-by: Go Sakayori * mover path shifter utils function to autoware motion utils Signed-off-by: Go Sakayori * minor change in license header Signed-off-by: Go Sakayori * fix warning message Signed-off-by: Go Sakayori * remove header file Signed-off-by: Go Sakayori * add test file Signed-off-by: Go Sakayori * add unit test to all function Signed-off-by: Go Sakayori * fix spelling Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- .../test/src/trajectory/test_path_shift.cpp | 163 ++++++++++++++++++ 1 file changed, 163 insertions(+) create mode 100644 common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp diff --git a/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp b/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp new file mode 100644 index 0000000000000..fbdcc7a8f6f81 --- /dev/null +++ b/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp @@ -0,0 +1,163 @@ +// Copyright 2024 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 "autoware/motion_utils/trajectory/path_shift.hpp" + +#include + +TEST(path_shift_test, calc_feasible_velocity_from_jerk) +{ + using autoware::motion_utils::calc_feasible_velocity_from_jerk; + + double longitudinal_distance = 0.0; + double lateral_distance = 0.0; + double lateral_jerk = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), + 1.0e10); + + // Condition: zero lateral distance + lateral_jerk = 1.0; + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), + 1.0e10); + + // Condition: zero longitudinal distance + lateral_distance = 2.0; + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 0.0); + + // Condition: random condition + longitudinal_distance = 50.0; + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 12.5); +} + +TEST(path_shift_test, calc_lateral_dist_from_jerk) +{ + using autoware::motion_utils::calc_lateral_dist_from_jerk; + + double longitudinal_distance = 0.0; + double lateral_jerk = 0.0; + double velocity = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10); + + // Condition: zero velocity + lateral_jerk = 2.0; + EXPECT_DOUBLE_EQ( + calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10); + + // Condition: zero longitudinal distance + velocity = 10.0; + EXPECT_DOUBLE_EQ(calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 0.0); + + // Condition: random condition + longitudinal_distance = 100.0; + EXPECT_DOUBLE_EQ( + calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 62.5); +} + +TEST(path_shift_test, calc_longitudinal_dist_from_jerk) +{ + using autoware::motion_utils::calc_longitudinal_dist_from_jerk; + + double lateral_distance = 0.0; + double lateral_jerk = 0.0; + double velocity = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 1.0e10); + + // Condition: zero lateral distance + lateral_jerk = -1.0; + velocity = 10.0; + EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0); + + // Condition: zero velocity + velocity = 0.0; + lateral_distance = 54.0; + EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0); + + // Condition: random + velocity = 8.0; + EXPECT_DOUBLE_EQ( + calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 96.0); +} + +TEST(path_shift_test, calc_shift_time_from_jerk) +{ + constexpr double epsilon = 1e-6; + + using autoware::motion_utils::calc_shift_time_from_jerk; + + double lateral_distance = 0.0; + double lateral_jerk = 0.0; + double lateral_acceleration = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10); + + // Condition: zero lateral acceleration + lateral_jerk = -2.0; + EXPECT_DOUBLE_EQ( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10); + + // Condition: zero lateral distance + lateral_acceleration = -4.0; + EXPECT_DOUBLE_EQ( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 0.0); + + // Condition: random (TODO: use DOUBLE_EQ in future. currently not precise enough) + lateral_distance = 80.0; + EXPECT_NEAR( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 11.16515139, + epsilon); +} + +TEST(path_shift_test, calc_jerk_from_lat_lon_distance) +{ + using autoware::motion_utils::calc_jerk_from_lat_lon_distance; + + double lateral_distance = 0.0; + double longitudinal_distance = 100.0; + double velocity = 10.0; + + // Condition: zero lateral distance + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0); + + // Condition: zero velocity + lateral_distance = 5.0; + velocity = 0.0; + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0); + + // Condition: zero longitudinal distance + longitudinal_distance = 0.0; + velocity = 10.0; + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 1.6 * 1e14); + + // Condition: random + longitudinal_distance = 100.0; + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.16); +} From 328ffa9fb4c6ab0084ffcc65f8a428ed32252431 Mon Sep 17 00:00:00 2001 From: Kazunori-Nakajima <169115284+Kazunori-Nakajima@users.noreply.github.com> Date: Tue, 15 Oct 2024 17:54:28 +0900 Subject: [PATCH 5/5] feat(costmap_generator, control_validator, scenario_selector, surround_obstacle_checker, vehicle_cmd_gate): add processing time pub. (#9065) * feat(costmap_generator, control_validator, scenario_selector, surround_obstacle_checker, vehicle_cmd_gate): Add: processing_time_pub Signed-off-by: Kazunori-Nakajima * fix: pre-commit Signed-off-by: Kasunori-Nakajima * feat(costmap_generator): fix: No output when not Active. Signed-off-by: Kasunori-Nakajima * fix: clang-format Signed-off-by: Kasunori-Nakajima * Re: fix: clang-format Signed-off-by: Kasunori-Nakajima --------- Signed-off-by: Kazunori-Nakajima Signed-off-by: Kasunori-Nakajima --- .../control_validator/control_validator.hpp | 5 +++++ .../src/control_validator.cpp | 11 ++++++++++ .../src/vehicle_cmd_gate.cpp | 22 +++++++++++++++++++ .../src/vehicle_cmd_gate.hpp | 6 +++++ .../costmap_generator.hpp | 7 ++++++ .../costmap_generator_node.cpp | 14 ++++++++++++ .../autoware/scenario_selector/node.hpp | 6 +++++ .../autoware_scenario_selector/src/node.cpp | 11 ++++++++++ .../src/node.cpp | 11 ++++++++++ .../src/node.hpp | 2 ++ .../config/processing_time_checker.param.yaml | 5 +++++ 11 files changed, 100 insertions(+) diff --git a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp index 56a1e39391b10..551ee08aa3327 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -20,12 +20,14 @@ #include "autoware_vehicle_info_utils/vehicle_info.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" +#include #include #include #include #include #include +#include #include #include @@ -143,6 +145,7 @@ class ControlValidator : public rclcpp::Node universe_utils::InterProcessPollingSubscriber::SharedPtr sub_reference_traj_; rclcpp::Publisher::SharedPtr pub_status_; rclcpp::Publisher::SharedPtr pub_markers_; + rclcpp::Publisher::SharedPtr pub_processing_time_; // system parameters int64_t diag_error_count_threshold_ = 0; @@ -167,6 +170,8 @@ class ControlValidator : public rclcpp::Node Odometry::ConstSharedPtr current_kinematics_; + autoware::universe_utils::StopWatch stop_watch; + std::shared_ptr debug_pose_publisher_; }; } // namespace autoware::control_validator diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 2370d274fecc8..d601319d47bc5 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -47,6 +47,9 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) pub_markers_ = create_publisher("~/output/markers", 1); + pub_processing_time_ = + this->create_publisher("~/debug/processing_time_ms", 1); + debug_pose_publisher_ = std::make_shared(this); setup_parameters(); @@ -134,6 +137,8 @@ bool ControlValidator::is_data_ready() void ControlValidator::on_predicted_trajectory(const Trajectory::ConstSharedPtr msg) { + stop_watch.tic(); + current_predicted_trajectory_ = msg; current_reference_trajectory_ = sub_reference_traj_->takeData(); current_kinematics_ = sub_kinematics_->takeData(); @@ -162,6 +167,12 @@ void ControlValidator::publish_debug_info() debug_pose_publisher_->push_warning_msg(front_pose, "INVALID CONTROL"); } debug_pose_publisher_->publish(); + + // Publish ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + pub_processing_time_->publish(processing_time_msg); } void ControlValidator::validate( diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index bf61b69857849..b028455ab3354 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -77,6 +77,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) engage_pub_ = create_publisher("output/engage", durable_qos); pub_external_emergency_ = create_publisher("output/external_emergency", durable_qos); operation_mode_pub_ = create_publisher("output/operation_mode", durable_qos); + processing_time_pub_ = + this->create_publisher("~/debug/processing_time_ms", 1); is_filter_activated_pub_ = create_publisher("~/is_filter_activated", durable_qos); @@ -375,6 +377,8 @@ T VehicleCmdGate::getContinuousTopic( void VehicleCmdGate::onTimer() { + stop_watch.tic(); + // Subscriber for auto const auto msg_auto_command_turn_indicator = auto_turn_indicator_cmd_sub_.takeData(); if (msg_auto_command_turn_indicator) @@ -573,12 +577,18 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) vehicle_cmd_emergency.emergency = (use_emergency_handling_ && is_system_emergency_); vehicle_cmd_emergency.stamp = filtered_control.stamp; + // ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + // Publish commands vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency); control_cmd_pub_->publish(filtered_control); published_time_publisher_->publish_if_subscribed(control_cmd_pub_, filtered_control.stamp); adapi_pause_->publish(); moderate_stop_interface_->publish(); + processing_time_pub_->publish(processing_time_msg); // Save ControlCmd to steering angle when disengaged prev_control_cmd_ = filtered_control; @@ -616,12 +626,18 @@ void VehicleCmdGate::publishEmergencyStopControlCommands() vehicle_cmd_emergency.stamp = stamp; vehicle_cmd_emergency.emergency = true; + // ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + // Publish topics vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency); control_cmd_pub_->publish(control_cmd); turn_indicator_cmd_pub_->publish(turn_indicator); hazard_light_cmd_pub_->publish(hazard_light); gear_cmd_pub_->publish(gear); + processing_time_pub_->publish(processing_time_msg); } void VehicleCmdGate::publishStatus() @@ -638,12 +654,18 @@ void VehicleCmdGate::publishStatus() external_emergency.stamp = stamp; external_emergency.emergency = is_external_emergency_stop_; + // ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + gate_mode_pub_->publish(current_gate_mode_); engage_pub_->publish(autoware_engage); pub_external_emergency_->publish(external_emergency); operation_mode_pub_->publish(current_operation_mode_); adapi_pause_->publish(); moderate_stop_interface_->publish(); + processing_time_pub_->publish(processing_time_msg); } Control VehicleCmdGate::filterControlCommand(const Control & in) diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 6863015443363..c2716ea3baa6a 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -114,6 +116,7 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Publisher::SharedPtr filter_activated_marker_pub_; rclcpp::Publisher::SharedPtr filter_activated_marker_raw_pub_; rclcpp::Publisher::SharedPtr filter_activated_flag_pub_; + rclcpp::Publisher::SharedPtr processing_time_pub_; // Parameter callback OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onParameter( @@ -268,6 +271,9 @@ class VehicleCmdGate : public rclcpp::Node std::unique_ptr vehicle_stop_checker_; double stop_check_duration_; + // processing time + autoware::universe_utils::StopWatch stop_watch; + // debug MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); void publishMarkers(const IsFilterActivated & filter_activated); diff --git a/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp index b08d16c68fa6f..434440359f282 100644 --- a/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp @@ -49,6 +49,8 @@ #include "autoware_costmap_generator/points_to_costmap.hpp" #include "costmap_generator_node_parameters.hpp" +#include +#include #include #include #include @@ -58,6 +60,7 @@ #include #include #include +#include #include #include @@ -91,6 +94,7 @@ class CostmapGenerator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_costmap_; rclcpp::Publisher::SharedPtr pub_occupancy_grid_; rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr pub_processing_time_ms_; rclcpp::Subscription::SharedPtr sub_points_; rclcpp::Subscription::SharedPtr sub_objects_; @@ -175,6 +179,9 @@ class CostmapGenerator : public rclcpp::Node /// \brief calculate cost for final output grid_map::Matrix generateCombinedCostmap(); + + /// \brief measure processing time + autoware::universe_utils::StopWatch stop_watch; }; } // namespace autoware::costmap_generator diff --git a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp index 9012b7f3250b4..07bf9eb33ffbe 100644 --- a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp @@ -192,6 +192,8 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) pub_processing_time_ = create_publisher("processing_time", 1); time_keeper_ = std::make_shared(pub_processing_time_); + pub_processing_time_ms_ = + this->create_publisher("~/debug/processing_time_ms", 1); // Timer const auto period_ns = rclcpp::Rate(param_->update_rate).period(); @@ -277,7 +279,13 @@ void CostmapGenerator::onScenario(const tier4_planning_msgs::msg::Scenario::Cons void CostmapGenerator::onTimer() { autoware::universe_utils::ScopedTimeTrack scoped_time_track(__func__, *time_keeper_); + stop_watch.tic(); if (!isActive()) { + // Publish ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + pub_processing_time_ms_->publish(processing_time_msg); return; } @@ -467,6 +475,12 @@ void CostmapGenerator::publishCostmap(const grid_map::GridMap & costmap) auto out_gridmap_msg = grid_map::GridMapRosConverter::toMessage(costmap); out_gridmap_msg->header = header; pub_costmap_->publish(*out_gridmap_msg); + + // Publish ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + pub_processing_time_ms_->publish(processing_time_msg); } } // namespace autoware::costmap_generator diff --git a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp index 487c643fe5673..244876ee2cb22 100644 --- a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -37,6 +38,7 @@ #endif #include #include +#include #include #include @@ -95,6 +97,7 @@ class ScenarioSelectorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_parking_trajectory_; rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_scenario_; + rclcpp::Publisher::SharedPtr pub_processing_time_; // polling subscribers universe_utils::InterProcessPollingSubscriber< @@ -130,6 +133,9 @@ class ScenarioSelectorNode : public rclcpp::Node static constexpr double lane_stopping_timeout_s = 5.0; static constexpr double empty_parking_trajectory_timeout_s = 3.0; + + // processing time + autoware::universe_utils::StopWatch stop_watch; }; } // namespace autoware::scenario_selector #endif // AUTOWARE__SCENARIO_SELECTOR__NODE_HPP_ diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index 1f4547d04c8fc..eb3303766205f 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -330,6 +330,9 @@ bool ScenarioSelectorNode::isDataReady() void ScenarioSelectorNode::updateData() { + { + stop_watch.tic(); + } { auto msg = sub_parking_state_->takeData(); is_parking_completed_ = msg ? msg->data : is_parking_completed_; @@ -370,6 +373,12 @@ void ScenarioSelectorNode::onTimer() } pub_scenario_->publish(scenario); + + // Publish ProcessingTime + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + pub_processing_time_->publish(processing_time_msg); } void ScenarioSelectorNode::onLaneDrivingTrajectory( @@ -466,6 +475,8 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); published_time_publisher_ = std::make_unique(this); + pub_processing_time_ = + this->create_publisher("~/debug/processing_time_ms", 1); } } // namespace autoware::scenario_selector diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index 719a507ab569b..da0dd0ec09a26 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -109,6 +110,8 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio "~/output/velocity_limit_clear_command", rclcpp::QoS{1}.transient_local()); pub_velocity_limit_ = this->create_publisher( "~/output/max_velocity", rclcpp::QoS{1}.transient_local()); + pub_processing_time_ = + this->create_publisher("~/debug/processing_time_ms", 1); using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( @@ -151,6 +154,9 @@ bool SurroundObstacleCheckerNode::getUseDynamicObject() const void SurroundObstacleCheckerNode::onTimer() { + autoware::universe_utils::StopWatch stop_watch; + stop_watch.tic(); + odometry_ptr_ = sub_odometry_.takeData(); pointcloud_ptr_ = sub_pointcloud_.takeData(); object_ptr_ = sub_dynamic_objects_.takeData(); @@ -256,6 +262,11 @@ void SurroundObstacleCheckerNode::onTimer() no_start_reason_diag = makeStopReasonDiag("obstacle", odometry_ptr_->pose.pose); } + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = stop_watch.toc(); + pub_processing_time_->publish(processing_time_msg); + pub_stop_reason_->publish(no_start_reason_diag); debug_ptr_->publish(); } diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index f04a3aba07baf..480a937a4a909 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -106,6 +107,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_processing_time_; // parameter callback result OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml b/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml index 84e7d79079dae..033b20d234fd9 100644 --- a/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml +++ b/system/autoware_processing_time_checker/config/processing_time_checker.param.yaml @@ -2,9 +2,11 @@ ros__parameters: update_rate: 10.0 processing_time_topic_name_list: + - /control/control_validator/debug/processing_time_ms - /control/trajectory_follower/controller_node_exe/lateral/debug/processing_time_ms - /control/trajectory_follower/controller_node_exe/longitudinal/debug/processing_time_ms - /control/trajectory_follower/lane_departure_checker_node/debug/processing_time_ms + - /control/vehicle_cmd_gate/debug/processing_time_ms - /perception/object_recognition/prediction/map_based_prediction/debug/processing_time_ms - /perception/object_recognition/tracking/multi_object_tracker/debug/processing_time_ms - /planning/planning_validator/debug/processing_time_ms @@ -33,5 +35,8 @@ - /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/debug/processing_time_ms - /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/processing_time_ms - /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/processing_time_ms + - /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/processing_time_ms + - /planning/scenario_planning/parking/costmap_generator/debug/processing_time_ms + - /planning/scenario_planning/scenario_selector/debug/processing_time_ms - /planning/scenario_planning/velocity_smoother/debug/processing_time_ms - /simulation/shape_estimation/debug/processing_time_ms