diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index ab267ec3ac007..4119e69ed72e1 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -1,5 +1,6 @@ *:*/test/* *:*/examples/* +*:*/benchmarks/* checkersReport missingInclude diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 5bbc4d1c6177b..4fea7c85e1926 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -204,7 +204,7 @@ simulator/autoware_carla_interface/** maxime.clement@tier4.jp mradityagio@gmail. simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp simulator/fault_injection/** keisuke.shima@tier4.jp simulator/learning_based_vehicle_model/** maxime.clement@tier4.jp nagy.tomas@tier4.jp -simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp kotaro.yoshimoto@tier4.jp simulator/tier4_dummy_object_rviz_plugin/** yukihiro.saito@tier4.jp simulator/vehicle_door_simulator/** isamu.takagi@tier4.jp system/autoware_component_monitor/** baris@leodrive.ai memin@leodrive.ai yavuz@leodrive.ai diff --git a/common/autoware_kalman_filter/CHANGELOG.rst b/common/autoware_kalman_filter/CHANGELOG.rst deleted file mode 100644 index a7e68f8843e74..0000000000000 --- a/common/autoware_kalman_filter/CHANGELOG.rst +++ /dev/null @@ -1,55 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_kalman_filter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(kalman_filter): prefix package and namespace with autoware (`#7787 `_) - * refactor(kalman_filter): prefix package and namespace with autoware - * move headers to include/autoware/ - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_kalman_filter/CMakeLists.txt b/common/autoware_kalman_filter/CMakeLists.txt deleted file mode 100644 index 076d2d3cad4e8..0000000000000 --- a/common/autoware_kalman_filter/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_kalman_filter) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) - -include_directories( - SYSTEM - ${EIGEN3_INCLUDE_DIR} -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/kalman_filter.cpp - src/time_delay_kalman_filter.cpp - include/autoware/kalman_filter/kalman_filter.hpp - include/autoware/kalman_filter/time_delay_kalman_filter.hpp -) - -if(BUILD_TESTING) - file(GLOB_RECURSE test_files test/*.cpp) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) - - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) -endif() - -ament_auto_package() diff --git a/common/autoware_kalman_filter/README.md b/common/autoware_kalman_filter/README.md deleted file mode 100644 index 7c0feb9c2a61a..0000000000000 --- a/common/autoware_kalman_filter/README.md +++ /dev/null @@ -1,9 +0,0 @@ -# kalman_filter - -## Purpose - -This common package contains the kalman filter with time delay and the calculation of the kalman filter. - -## Assumptions / Known limits - -TBD. diff --git a/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp deleted file mode 100644 index 74db04f6e838b..0000000000000 --- a/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp +++ /dev/null @@ -1,214 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ -#define AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ - -#include -#include - -namespace autoware::kalman_filter -{ - -/** - * @file kalman_filter.h - * @brief kalman filter class - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -class KalmanFilter -{ -public: - /** - * @brief No initialization constructor. - */ - KalmanFilter(); - - /** - * @brief constructor with initialization - * @param x initial state - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param C coefficient matrix of x for measurement model - * @param Q covariance matrix for process model - * @param R covariance matrix for measurement model - * @param P initial covariance of estimated state - */ - KalmanFilter( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P); - - /** - * @brief destructor - */ - ~KalmanFilter(); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param C coefficient matrix of x for measurement model - * @param Q covariance matrix for process model - * @param R covariance matrix for measurement model - * @param P initial covariance of estimated state - */ - bool init( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param P initial covariance of estimated state - */ - bool init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0); - - /** - * @brief set A of process model - * @param A coefficient matrix of x for process model - */ - void setA(const Eigen::MatrixXd & A); - - /** - * @brief set B of process model - * @param B coefficient matrix of u for process model - */ - void setB(const Eigen::MatrixXd & B); - - /** - * @brief set C of measurement model - * @param C coefficient matrix of x for measurement model - */ - void setC(const Eigen::MatrixXd & C); - - /** - * @brief set covariance matrix Q for process model - * @param Q covariance matrix for process model - */ - void setQ(const Eigen::MatrixXd & Q); - - /** - * @brief set covariance matrix R for measurement model - * @param R covariance matrix for measurement model - */ - void setR(const Eigen::MatrixXd & R); - - /** - * @brief get current kalman filter state - * @param x kalman filter state - */ - void getX(Eigen::MatrixXd & x) const; - - /** - * @brief get current kalman filter covariance - * @param P kalman filter covariance - */ - void getP(Eigen::MatrixXd & P) const; - - /** - * @brief get component of current kalman filter state - * @param i index of kalman filter state - * @return value of i's component of the kalman filter state x[i] - */ - double getXelement(unsigned int i) const; - - /** - * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix. - * This is mainly for EKF with variable matrix. - * @param u input for model - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param Q covariance matrix for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict( - const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is - * mainly for EKF with variable matrix. - * @param x_next predicted state - * @param A coefficient matrix of x for process model - * @param Q covariance matrix for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is - * mainly for EKF with variable matrix. - * @param x_next predicted state - * @param A coefficient matrix of x for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A); - - /** - * @brief calculate kalman filter state by prediction model with A, B and Q being class member - * variables. - * @param u input for the model - * @return bool to check matrix operations are being performed properly - */ - bool predict(const Eigen::MatrixXd & u); - - /** - * @brief calculate kalman filter state by measurement model with y_pred, C and R matrix. This is - * mainly for EKF with variable matrix. - * @param y measured values - * @param y output values expected from measurement model - * @param C coefficient matrix of x for measurement model - * @param R covariance matrix for measurement model - * @return bool to check matrix operations are being performed properly - */ - bool update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C, - const Eigen::MatrixXd & R); - - /** - * @brief calculate kalman filter state by measurement model with C and R matrix. This is mainly - * for EKF with variable matrix. - * @param y measured values - * @param C coefficient matrix of x for measurement model - * @param R covariance matrix for measurement model - * @return bool to check matrix operations are being performed properly - */ - bool update(const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R); - - /** - * @brief calculate kalman filter state by measurement model with C and R being class member - * variables. - * @param y measured values - * @return bool to check matrix operations are being performed properly - */ - bool update(const Eigen::MatrixXd & y); - -protected: - Eigen::MatrixXd x_; //!< @brief current estimated state - Eigen::MatrixXd - A_; //!< @brief coefficient matrix of x for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd - B_; //!< @brief coefficient matrix of u for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd C_; //!< @brief coefficient matrix of x for measurement model y[k] = C * x[k] - Eigen::MatrixXd Q_; //!< @brief covariance matrix for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd R_; //!< @brief covariance matrix for measurement model y[k] = C * x[k] - Eigen::MatrixXd P_; //!< @brief covariance of estimated state -}; -} // namespace autoware::kalman_filter -#endif // AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ diff --git a/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp deleted file mode 100644 index 80375b7579e62..0000000000000 --- a/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ -#define AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ - -#include "autoware/kalman_filter/kalman_filter.hpp" - -#include -#include - -#include - -namespace autoware::kalman_filter -{ -/** - * @file time_delay_kalman_filter.h - * @brief kalman filter with delayed measurement class - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -class TimeDelayKalmanFilter : public KalmanFilter -{ -public: - /** - * @brief No initialization constructor. - */ - TimeDelayKalmanFilter(); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param P0 initial covariance of estimated state - * @param max_delay_step Maximum number of delay steps, which determines the dimension of the - * extended kalman filter - */ - void init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P, const int max_delay_step); - - /** - * @brief get latest time estimated state - */ - Eigen::MatrixXd getLatestX() const; - - /** - * @brief get latest time estimation covariance - */ - Eigen::MatrixXd getLatestP() const; - - /** - * @brief calculate kalman filter covariance by precision model with time delay. This is mainly - * for EKF of nonlinear process model. - * @param x_next predicted state by prediction model - * @param A coefficient matrix of x for process model - * @param Q covariance matrix for process model - */ - bool predictWithDelay( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance by measurement model with time delay. This is mainly - * for EKF of nonlinear process model. - * @param y measured values - * @param C coefficient matrix of x for measurement model - * @param R covariance matrix for measurement model - * @param delay_step measurement delay - */ - bool updateWithDelay( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R, - const int delay_step); - -private: - int max_delay_step_; //!< @brief maximum number of delay steps - int dim_x_; //!< @brief dimension of latest state - int dim_x_ex_; //!< @brief dimension of extended state with dime delay -}; -} // namespace autoware::kalman_filter -#endif // AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ diff --git a/common/autoware_kalman_filter/package.xml b/common/autoware_kalman_filter/package.xml deleted file mode 100644 index e51bb06e9de43..0000000000000 --- a/common/autoware_kalman_filter/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - autoware_kalman_filter - 0.40.0 - The kalman filter package - Yukihiro Saito - Takeshi Ishita - Koji Minoda - Apache License 2.0 - - Takamasa Horibe - - ament_cmake_auto - autoware_cmake - - eigen - eigen3_cmake_module - - ament_cmake_cppcheck - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_kalman_filter/src/kalman_filter.cpp b/common/autoware_kalman_filter/src/kalman_filter.cpp deleted file mode 100644 index bbd963675f9e2..0000000000000 --- a/common/autoware_kalman_filter/src/kalman_filter.cpp +++ /dev/null @@ -1,161 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/kalman_filter.hpp" - -namespace autoware::kalman_filter -{ -KalmanFilter::KalmanFilter() -{ -} -KalmanFilter::KalmanFilter( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P) -{ - init(x, A, B, C, Q, R, P); -} -KalmanFilter::~KalmanFilter() -{ -} -bool KalmanFilter::init( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P) -{ - if ( - x.cols() == 0 || x.rows() == 0 || A.cols() == 0 || A.rows() == 0 || B.cols() == 0 || - B.rows() == 0 || C.cols() == 0 || C.rows() == 0 || Q.cols() == 0 || Q.rows() == 0 || - R.cols() == 0 || R.rows() == 0 || P.cols() == 0 || P.rows() == 0) { - return false; - } - x_ = x; - A_ = A; - B_ = B; - C_ = C; - Q_ = Q; - R_ = R; - P_ = P; - return true; -} -bool KalmanFilter::init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0) -{ - if (x.cols() == 0 || x.rows() == 0 || P0.cols() == 0 || P0.rows() == 0) { - return false; - } - x_ = x; - P_ = P0; - return true; -} - -void KalmanFilter::setA(const Eigen::MatrixXd & A) -{ - A_ = A; -} -void KalmanFilter::setB(const Eigen::MatrixXd & B) -{ - B_ = B; -} -void KalmanFilter::setC(const Eigen::MatrixXd & C) -{ - C_ = C; -} -void KalmanFilter::setQ(const Eigen::MatrixXd & Q) -{ - Q_ = Q; -} -void KalmanFilter::setR(const Eigen::MatrixXd & R) -{ - R_ = R; -} -void KalmanFilter::getX(Eigen::MatrixXd & x) const -{ - x = x_; -} -void KalmanFilter::getP(Eigen::MatrixXd & P) const -{ - P = P_; -} -double KalmanFilter::getXelement(unsigned int i) const -{ - return x_(i); -} - -bool KalmanFilter::predict( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) -{ - if ( - x_.rows() != x_next.rows() || A.cols() != P_.rows() || Q.cols() != Q.rows() || - A.rows() != Q.cols()) { - return false; - } - x_ = x_next; - P_ = A * P_ * A.transpose() + Q; - return true; -} -bool KalmanFilter::predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A) -{ - return predict(x_next, A, Q_); -} - -bool KalmanFilter::predict( - const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & Q) -{ - if (A.cols() != x_.rows() || B.cols() != u.rows()) { - return false; - } - const Eigen::MatrixXd x_next = A * x_ + B * u; - return predict(x_next, A, Q); -} -bool KalmanFilter::predict(const Eigen::MatrixXd & u) -{ - return predict(u, A_, B_, Q_); -} - -bool KalmanFilter::update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C, - const Eigen::MatrixXd & R) -{ - if ( - P_.cols() != C.cols() || R.rows() != R.cols() || R.rows() != C.rows() || - y.rows() != y_pred.rows() || y.rows() != C.rows()) { - return false; - } - const Eigen::MatrixXd PCT = P_ * C.transpose(); - const Eigen::MatrixXd K = PCT * ((R + C * PCT).inverse()); - - if (isnan(K.array()).any() || isinf(K.array()).any()) { - return false; - } - - x_ = x_ + K * (y - y_pred); - P_ = P_ - K * (C * P_); - return true; -} - -bool KalmanFilter::update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R) -{ - if (C.cols() != x_.rows()) { - return false; - } - const Eigen::MatrixXd y_pred = C * x_; - return update(y, y_pred, C, R); -} -bool KalmanFilter::update(const Eigen::MatrixXd & y) -{ - return update(y, C_, R_); -} -} // namespace autoware::kalman_filter diff --git a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp deleted file mode 100644 index 4d1dd8f33b7a6..0000000000000 --- a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/time_delay_kalman_filter.hpp" - -#include - -namespace autoware::kalman_filter -{ -TimeDelayKalmanFilter::TimeDelayKalmanFilter() -{ -} - -void TimeDelayKalmanFilter::init( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0, const int max_delay_step) -{ - max_delay_step_ = max_delay_step; - dim_x_ = x.rows(); - dim_x_ex_ = dim_x_ * max_delay_step; - - x_ = Eigen::MatrixXd::Zero(dim_x_ex_, 1); - P_ = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_); - - for (int i = 0; i < max_delay_step_; ++i) { - x_.block(i * dim_x_, 0, dim_x_, 1) = x; - P_.block(i * dim_x_, i * dim_x_, dim_x_, dim_x_) = P0; - } -} - -Eigen::MatrixXd TimeDelayKalmanFilter::getLatestX() const -{ - return x_.block(0, 0, dim_x_, 1); -} - -Eigen::MatrixXd TimeDelayKalmanFilter::getLatestP() const -{ - return P_.block(0, 0, dim_x_, dim_x_); -} - -bool TimeDelayKalmanFilter::predictWithDelay( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) -{ - /* - * time delay model: - * - * [A 0 0] [P11 P12 P13] [Q 0 0] - * A = [I 0 0], P = [P21 P22 P23], Q = [0 0 0] - * [0 I 0] [P31 P32 P33] [0 0 0] - * - * covariance calculation in prediction : P = A * P * A' + Q - * - * [A*P11*A'*+Q A*P11 A*P12] - * P = [ P11*A' P11 P12] - * [ P21*A' P21 P22] - */ - - const int d_dim_x = dim_x_ex_ - dim_x_; - - /* slide states in the time direction */ - Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, 1); - x_tmp.block(0, 0, dim_x_, 1) = x_next; - x_tmp.block(dim_x_, 0, d_dim_x, 1) = x_.block(0, 0, d_dim_x, 1); - x_ = x_tmp; - - /* update P with delayed measurement A matrix structure */ - Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_); - P_tmp.block(0, 0, dim_x_, dim_x_) = A * P_.block(0, 0, dim_x_, dim_x_) * A.transpose() + Q; - P_tmp.block(0, dim_x_, dim_x_, d_dim_x) = A * P_.block(0, 0, dim_x_, d_dim_x); - P_tmp.block(dim_x_, 0, d_dim_x, dim_x_) = P_.block(0, 0, d_dim_x, dim_x_) * A.transpose(); - P_tmp.block(dim_x_, dim_x_, d_dim_x, d_dim_x) = P_.block(0, 0, d_dim_x, d_dim_x); - P_ = P_tmp; - - return true; -} - -bool TimeDelayKalmanFilter::updateWithDelay( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R, - const int delay_step) -{ - if (delay_step >= max_delay_step_) { - std::cerr << "delay step is larger than max_delay_step. ignore update." << std::endl; - return false; - } - - const int dim_y = y.rows(); - - /* set measurement matrix */ - Eigen::MatrixXd C_ex = Eigen::MatrixXd::Zero(dim_y, dim_x_ex_); - C_ex.block(0, dim_x_ * delay_step, dim_y, dim_x_) = C; - - /* update */ - if (!update(y, C_ex, R)) { - return false; - } - - return true; -} -} // namespace autoware::kalman_filter diff --git a/common/autoware_kalman_filter/test/test_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_kalman_filter.cpp deleted file mode 100644 index 34e23ef9d06e2..0000000000000 --- a/common/autoware_kalman_filter/test/test_kalman_filter.cpp +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/kalman_filter.hpp" - -#include - -using autoware::kalman_filter::KalmanFilter; - -TEST(kalman_filter, kf) -{ - KalmanFilter kf_; - - Eigen::MatrixXd x_t(2, 1); - x_t << 1, 2; - - Eigen::MatrixXd P_t(2, 2); - P_t << 1, 0, 0, 1; - - Eigen::MatrixXd Q_t(2, 2); - Q_t << 0.01, 0, 0, 0.01; - - Eigen::MatrixXd R_t(2, 2); - R_t << 0.09, 0, 0, 0.09; - - Eigen::MatrixXd C_t(2, 2); - C_t << 1, 0, 0, 1; - - Eigen::MatrixXd A_t(2, 2); - A_t << 1, 0, 0, 1; - - Eigen::MatrixXd B_t(2, 2); - B_t << 1, 0, 0, 1; - - // Initialize the filter and check if initialization was successful - EXPECT_TRUE(kf_.init(x_t, A_t, B_t, C_t, Q_t, R_t, P_t)); - - // Perform prediction - Eigen::MatrixXd u_t(2, 1); - u_t << 0.1, 0.1; - EXPECT_TRUE(kf_.predict(u_t)); - - // Check the updated state and covariance matrix - Eigen::MatrixXd x_predict_expected = A_t * x_t + B_t * u_t; - Eigen::MatrixXd P_predict_expected = A_t * P_t * A_t.transpose() + Q_t; - - Eigen::MatrixXd x_predict; - kf_.getX(x_predict); - Eigen::MatrixXd P_predict; - kf_.getP(P_predict); - - EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); - EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); - - // Perform update - Eigen::MatrixXd y_t(2, 1); - y_t << 1.05, 2.05; - EXPECT_TRUE(kf_.update(y_t)); - - // Check the updated state and covariance matrix - const Eigen::MatrixXd PCT_t = P_predict_expected * C_t.transpose(); - const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_t * PCT_t).inverse()); - const Eigen::MatrixXd y_pred = C_t * x_predict_expected; - Eigen::MatrixXd x_update_expected = x_predict_expected + K_t * (y_t - y_pred); - Eigen::MatrixXd P_update_expected = P_predict_expected - K_t * (C_t * P_predict_expected); - - Eigen::MatrixXd x_update; - kf_.getX(x_update); - Eigen::MatrixXd P_update; - kf_.getP(P_update); - - EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); - EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); - EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); - EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); -} - -int main(int argc, char * argv[]) -{ - testing::InitGoogleTest(&argc, argv); - bool result = RUN_ALL_TESTS(); - return result; -} diff --git a/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp deleted file mode 100644 index 50c22fae123bc..0000000000000 --- a/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/time_delay_kalman_filter.hpp" - -#include - -using autoware::kalman_filter::TimeDelayKalmanFilter; - -TEST(time_delay_kalman_filter, td_kf) -{ - TimeDelayKalmanFilter td_kf_; - - Eigen::MatrixXd x_t(3, 1); - x_t << 1.0, 2.0, 3.0; - Eigen::MatrixXd P_t(3, 3); - P_t << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.3; - const int max_delay_step = 5; - const int dim_x = x_t.rows(); - const int dim_x_ex = dim_x * max_delay_step; - // Initialize the filter - td_kf_.init(x_t, P_t, max_delay_step); - - // Check if initialization was successful - Eigen::MatrixXd x_init = td_kf_.getLatestX(); - Eigen::MatrixXd P_init = td_kf_.getLatestP(); - Eigen::MatrixXd x_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, 1); - Eigen::MatrixXd P_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); - for (int i = 0; i < max_delay_step; ++i) { - x_ex_t.block(i * dim_x, 0, dim_x, 1) = x_t; - P_ex_t.block(i * dim_x, i * dim_x, dim_x, dim_x) = P_t; - } - - EXPECT_EQ(x_init.rows(), 3); - EXPECT_EQ(x_init.cols(), 1); - EXPECT_EQ(P_init.rows(), 3); - EXPECT_EQ(P_init.cols(), 3); - EXPECT_NEAR(x_init(0, 0), x_t(0, 0), 1e-5); - EXPECT_NEAR(x_init(1, 0), x_t(1, 0), 1e-5); - EXPECT_NEAR(x_init(2, 0), x_t(2, 0), 1e-5); - EXPECT_NEAR(P_init(0, 0), P_t(0, 0), 1e-5); - EXPECT_NEAR(P_init(1, 1), P_t(1, 1), 1e-5); - EXPECT_NEAR(P_init(2, 2), P_t(2, 2), 1e-5); - - // Define prediction parameters - Eigen::MatrixXd A_t(3, 3); - A_t << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0; - Eigen::MatrixXd Q_t(3, 3); - Q_t << 0.01, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.03; - Eigen::MatrixXd x_next(3, 1); - x_next << 2.0, 4.0, 6.0; - - // Perform prediction - EXPECT_TRUE(td_kf_.predictWithDelay(x_next, A_t, Q_t)); - - // Check the prediction state and covariance matrix - Eigen::MatrixXd x_predict = td_kf_.getLatestX(); - Eigen::MatrixXd P_predict = td_kf_.getLatestP(); - Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex, 1); - x_tmp.block(0, 0, dim_x, 1) = A_t * x_t; - x_tmp.block(dim_x, 0, dim_x_ex - dim_x, 1) = x_ex_t.block(0, 0, dim_x_ex - dim_x, 1); - x_ex_t = x_tmp; - Eigen::MatrixXd x_predict_expected = x_ex_t.block(0, 0, dim_x, 1); - Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); - P_tmp.block(0, 0, dim_x, dim_x) = A_t * P_ex_t.block(0, 0, dim_x, dim_x) * A_t.transpose() + Q_t; - P_tmp.block(0, dim_x, dim_x, dim_x_ex - dim_x) = - A_t * P_ex_t.block(0, 0, dim_x, dim_x_ex - dim_x); - P_tmp.block(dim_x, 0, dim_x_ex - dim_x, dim_x) = - P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x) * A_t.transpose(); - P_tmp.block(dim_x, dim_x, dim_x_ex - dim_x, dim_x_ex - dim_x) = - P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x_ex - dim_x); - P_ex_t = P_tmp; - Eigen::MatrixXd P_predict_expected = P_ex_t.block(0, 0, dim_x, dim_x); - EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); - EXPECT_NEAR(x_predict(2, 0), x_predict_expected(2, 0), 1e-5); - EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); - EXPECT_NEAR(P_predict(2, 2), P_predict_expected(2, 2), 1e-5); - - // Define update parameters - Eigen::MatrixXd C_t(3, 3); - C_t << 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5; - Eigen::MatrixXd R_t(3, 3); - R_t << 0.001, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.003; - Eigen::MatrixXd y_t(3, 1); - y_t << 1.05, 2.05, 3.05; - const int delay_step = 2; // Choose an appropriate delay step - const int dim_y = y_t.rows(); - - // Perform update - EXPECT_TRUE(td_kf_.updateWithDelay(y_t, C_t, R_t, delay_step)); - - // Check the updated state and covariance matrix - Eigen::MatrixXd x_update = td_kf_.getLatestX(); - Eigen::MatrixXd P_update = td_kf_.getLatestP(); - - Eigen::MatrixXd C_ex_t = Eigen::MatrixXd::Zero(dim_y, dim_x_ex); - const Eigen::MatrixXd PCT_t = P_ex_t * C_ex_t.transpose(); - const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_ex_t * PCT_t).inverse()); - const Eigen::MatrixXd y_pred = C_ex_t * x_ex_t; - x_ex_t = x_ex_t + K_t * (y_t - y_pred); - P_ex_t = P_ex_t - K_t * (C_ex_t * P_ex_t); - Eigen::MatrixXd x_update_expected = x_ex_t.block(0, 0, dim_x, 1); - Eigen::MatrixXd P_update_expected = P_ex_t.block(0, 0, dim_x, dim_x); - EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); - EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); - EXPECT_NEAR(x_update(2, 0), x_update_expected(2, 0), 1e-5); - EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); - EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); - EXPECT_NEAR(P_update(2, 2), P_update_expected(2, 2), 1e-5); -} diff --git a/common/autoware_osqp_interface/CHANGELOG.rst b/common/autoware_osqp_interface/CHANGELOG.rst deleted file mode 100644 index d400b77f4bf61..0000000000000 --- a/common/autoware_osqp_interface/CHANGELOG.rst +++ /dev/null @@ -1,51 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_osqp_interface -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* fix(autoware_osqp_interface): fix clang-tidy errors (`#9440 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Ryuta Kambe, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(osqp_interface): added autoware prefix to osqp_interface (`#8958 `_) -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_osqp_interface/CMakeLists.txt b/common/autoware_osqp_interface/CMakeLists.txt deleted file mode 100644 index b770af659e52a..0000000000000 --- a/common/autoware_osqp_interface/CMakeLists.txt +++ /dev/null @@ -1,58 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_osqp_interface) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) - -# after find_package(osqp_vendor) in ament_auto_find_build_dependencies -find_package(osqp REQUIRED) -get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) -get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH) - -set(OSQP_INTERFACE_LIB_SRC - src/csc_matrix_conv.cpp - src/osqp_interface.cpp -) - -set(OSQP_INTERFACE_LIB_HEADERS - 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 - ${OSQP_INTERFACE_LIB_SRC} - ${OSQP_INTERFACE_LIB_HEADERS} -) -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast) - -target_include_directories(${PROJECT_NAME} - SYSTEM PUBLIC - "${OSQP_INCLUDE_DIR}" - "${EIGEN3_INCLUDE_DIR}" -) - -ament_target_dependencies(${PROJECT_NAME} - Eigen3 - osqp_vendor -) - -# 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) - -if(BUILD_TESTING) - set(TEST_SOURCES - test/test_osqp_interface.cpp - test/test_csc_matrix_conv.cpp - ) - set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) - ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) - target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) -endif() - -ament_auto_package(INSTALL_TO_SHARE -) diff --git a/common/autoware_osqp_interface/design/osqp_interface-design.md b/common/autoware_osqp_interface/design/osqp_interface-design.md deleted file mode 100644 index 887a4b4d9af3f..0000000000000 --- a/common/autoware_osqp_interface/design/osqp_interface-design.md +++ /dev/null @@ -1,70 +0,0 @@ -# Interface for the OSQP library - -This is the design document for the `autoware_osqp_interface` package. - -## Purpose / Use cases - - - - -This packages provides a C++ interface for the [OSQP library](https://osqp.org/docs/solver/index.html). - -## Design - - - - -The class `OSQPInterface` takes a problem formulation as Eigen matrices and vectors, converts these objects into -C-style Compressed-Column-Sparse matrices and dynamic arrays, loads the data into the OSQP workspace dataholder, and runs the optimizer. - -## Inputs / Outputs / API - - - - -The interface can be used in several ways: - -1. Initialize the interface WITHOUT data. Load the problem formulation at the optimization call. - - ```cpp - osqp_interface = OSQPInterface(); - osqp_interface.optimize(P, A, q, l, u); - ``` - -2. Initialize the interface WITH data. - - ```cpp - osqp_interface = OSQPInterface(P, A, q, l, u); - osqp_interface.optimize(); - ``` - -3. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. - - ```cpp - osqp_interface = OSQPInterface(P, A, q, l, u); - osqp_interface.optimize(); - osqp.initializeProblem(P_new, A_new, q_new, l_new, u_new); - osqp_interface.optimize(); - ``` - - The optimization results are returned as a vector by the optimization function. - - ```cpp - std::tuple, std::vector> result = osqp_interface.optimize(); - std::vector param = std::get<0>(result); - double x_0 = param[0]; - double x_1 = param[1]; - ``` - -## References / External links - - - -- OSQP library: - -## Related issues - - diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp deleted file mode 100644 index 8c21553152d70..0000000000000 --- a/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#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 - -#include - -namespace autoware::osqp_interface -{ -/// \brief Compressed-Column-Sparse Matrix -struct OSQP_INTERFACE_PUBLIC CSC_Matrix -{ - /// Vector of non-zero values. Ex: [4,1,1,2] - std::vector m_vals; - /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') - std::vector m_row_idxs; - /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') - std::vector m_col_idxs; -}; - -/// \brief Calculate CSC matrix from Eigen matrix -OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); -/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix -OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); -/// \brief Print the given CSC matrix to the standard output -OSQP_INTERFACE_PUBLIC void printCSCMatrix(const CSC_Matrix & csc_mat); - -} // namespace autoware::osqp_interface - -#endif // AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp deleted file mode 100644 index ff3013bd61514..0000000000000 --- a/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp +++ /dev/null @@ -1,194 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#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 -#include - -#include -#include -#include -#include -#include - -namespace autoware::osqp_interface -{ -constexpr c_float INF = 1e30; - -/** - * Implementation of a native C++ interface for the OSQP solver. - * - */ -class OSQP_INTERFACE_PUBLIC OSQPInterface -{ -private: - std::unique_ptr> m_work; - std::unique_ptr m_settings; - std::unique_ptr m_data; - // store last work info since work is cleaned up at every execution to prevent memory leak. - OSQPInfo m_latest_work_info; - // Number of parameters to optimize - int64_t m_param_n; - // Flag to check if the current work exists - bool m_work_initialized = false; - // Exitflag - int64_t m_exitflag; - - // Runs the solver on the stored problem. - std::tuple, std::vector, int64_t, int64_t, int64_t> solve(); - - static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; - -public: - /// \brief Constructor without problem formulation - explicit OSQPInterface( - const c_float eps_abs = std::numeric_limits::epsilon(), const bool polish = true); - /// \brief Constructor with problem setup - /// \param P: (n,n) matrix defining relations between parameters. - /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - /// \param q: (n) vector defining the linear cost of the problem. - /// \param l: (m) vector defining the lower bound problem constraint. - /// \param u: (m) vector defining the upper bound problem constraint. - /// \param eps_abs: Absolute convergence tolerance. - OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs); - OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs); - ~OSQPInterface(); - - /**************** - * OPTIMIZATION - ****************/ - /// \brief Solves the stored convex quadratic program (QP) problem using the OSQP solver. - // - /// \return The function returns a tuple containing the solution as two float vectors. - /// \return The first element of the tuple contains the 'primal' solution. - /// \return The second element contains the 'lagrange multiplier' solution. - /// \return The third element contains an integer with solver polish status information. - - /// \details How to use: - /// \details 1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem. - /// \details 2. Initialize the interface and set up the problem. - /// \details osqp_interface = OSQPInterface(P, A, q, l, u, 1e-6); - /// \details 3. Call the optimization function. - /// \details std::tuple, std::vector> result; - /// \details result = osqp_interface.optimize(); - /// \details 4. Access the optimized parameters. - /// \details std::vector param = std::get<0>(result); - /// \details double x_0 = param[0]; - /// \details double x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t, int64_t> optimize(); - - /// \brief Solves convex quadratic programs (QPs) using the OSQP solver. - /// \return The function returns a tuple containing the solution as two float vectors. - /// \return The first element of the tuple contains the 'primal' solution. - /// \return The second element contains the 'lagrange multiplier' solution. - /// \return The third element contains an integer with solver polish status information. - /// \details How to use: - /// \details 1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem. - /// \details 2. Initialize the interface. - /// \details osqp_interface = OSQPInterface(1e-6); - /// \details 3. Call the optimization function with the problem formulation. - /// \details std::tuple, std::vector> result; - /// \details result = osqp_interface.optimize(P, A, q, l, u, 1e-6); - /// \details 4. Access the optimized parameters. - /// \details std::vector param = std::get<0>(result); - /// \details double x_0 = param[0]; - /// \details double x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t, int64_t> optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - /// \brief Converts the input data and sets up the workspace object. - /// \param P (n,n) matrix defining relations between parameters. - /// \param A (m,n) matrix defining parameter constraints relative to the lower and upper bound. - /// \param q (n) vector defining the linear cost of the problem. - /// \param l (m) vector defining the lower bound problem constraint. - /// \param u (m) vector defining the upper bound problem constraint. - int64_t initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - int64_t initializeProblem( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - // Setter functions for warm start - bool setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables); - bool setPrimalVariables(const std::vector & primal_variables); - bool setDualVariables(const std::vector & dual_variables); - - // Updates problem parameters while keeping solution in memory. - // - // Args: - // P_new: (n,n) matrix defining relations between parameters. - // A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - // q_new: (n) vector defining the linear cost of the problem. - // l_new: (m) vector defining the lower bound problem constraint. - // u_new: (m) vector defining the upper bound problem constraint. - void updateP(const Eigen::MatrixXd & P_new); - void updateCscP(const CSC_Matrix & P_csc); - void updateA(const Eigen::MatrixXd & A_new); - void updateCscA(const CSC_Matrix & A_csc); - void updateQ(const std::vector & q_new); - void updateL(const std::vector & l_new); - void updateU(const std::vector & u_new); - void updateBounds(const std::vector & l_new, const std::vector & u_new); - void updateEpsAbs(const double eps_abs); - void updateEpsRel(const double eps_rel); - void updateMaxIter(const int iter); - void updateVerbose(const bool verbose); - void updateRhoInterval(const int rho_interval); - void updateRho(const double rho); - void updateAlpha(const double alpha); - void updateScaling(const int scaling); - void updatePolish(const bool polish); - void updatePolishRefinementIteration(const int polish_refine_iter); - void updateCheckTermination(const int check_termination); - - /// \brief Get the number of iteration taken to solve the problem - inline int64_t getTakenIter() const { return static_cast(m_latest_work_info.iter); } - /// \brief Get the status message for the latest problem solved - inline std::string getStatusMessage() const - { - return static_cast(m_latest_work_info.status); - } - /// \brief Get the status value for the latest problem solved - inline int64_t getStatus() const { return static_cast(m_latest_work_info.status_val); } - /// \brief Get the status polish for the latest problem solved - inline int64_t getStatusPolish() const - { - return static_cast(m_latest_work_info.status_polish); - } - /// \brief Get the runtime of the latest problem solved - inline double getRunTime() const { return m_latest_work_info.run_time; } - /// \brief Get the objective value the latest problem solved - inline double getObjVal() const { return m_latest_work_info.obj_val; } - /// \brief Returns flag asserting interface condition (Healthy condition: 0). - inline int64_t getExitFlag() const { return m_exitflag; } - - void logUnsolvedStatus(const std::string & prefix_message = "") const; -}; - -} // namespace autoware::osqp_interface - -#endif // AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp deleted file mode 100644 index a6cdaa8a0a74e..0000000000000 --- a/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) -#define OSQP_INTERFACE_PUBLIC __declspec(dllexport) -#define OSQP_INTERFACE_LOCAL -#else // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) -#define OSQP_INTERFACE_PUBLIC __declspec(dllimport) -#define OSQP_INTERFACE_LOCAL -#endif // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) -#elif defined(__linux__) -#define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default"))) -#define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default"))) -#define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden"))) -#else -#error "Unsupported Build Configuration" -#endif - -#endif // AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_osqp_interface/package.xml b/common/autoware_osqp_interface/package.xml deleted file mode 100644 index 92a2afeccc4e0..0000000000000 --- a/common/autoware_osqp_interface/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - autoware_osqp_interface - 0.40.0 - Interface for the OSQP solver - Maxime CLEMENT - Takayuki Murooka - Fumiya Watanabe - Satoshi Ota - Apache 2.0 - - ament_cmake_auto - autoware_cmake - - eigen - osqp_vendor - rclcpp - rclcpp_components - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - eigen - - - ament_cmake - - diff --git a/common/autoware_osqp_interface/src/csc_matrix_conv.cpp b/common/autoware_osqp_interface/src/csc_matrix_conv.cpp deleted file mode 100644 index c6e1a0a42d938..0000000000000 --- a/common/autoware_osqp_interface/src/csc_matrix_conv.cpp +++ /dev/null @@ -1,135 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/osqp_interface/csc_matrix_conv.hpp" - -#include -#include - -#include -#include -#include - -namespace autoware::osqp_interface -{ -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i < rows; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - if (rows != cols) { - throw std::invalid_argument("Matrix must be square (n, n)"); - } - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - Eigen::Index trap_last_idx = 0; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i <= trap_last_idx; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - trap_last_idx += 1; - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -void printCSCMatrix(const CSC_Matrix & csc_mat) -{ - std::cout << "["; - for (const c_float & val : csc_mat.m_vals) { - std::cout << val << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & row : csc_mat.m_row_idxs) { - std::cout << row << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & col : csc_mat.m_col_idxs) { - std::cout << col << ", "; - } - std::cout << "]\n"; -} - -} // namespace autoware::osqp_interface diff --git a/common/autoware_osqp_interface/src/osqp_interface.cpp b/common/autoware_osqp_interface/src/osqp_interface.cpp deleted file mode 100644 index ceeae626222ca..0000000000000 --- a/common/autoware_osqp_interface/src/osqp_interface.cpp +++ /dev/null @@ -1,435 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/osqp_interface/osqp_interface.hpp" - -#include "autoware/osqp_interface/csc_matrix_conv.hpp" -#include "osqp/osqp.h" - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::osqp_interface -{ -OSQPInterface::OSQPInterface(const c_float eps_abs, const bool polish) -: m_work{nullptr, OSQPWorkspaceDeleter} -{ - m_settings = std::make_unique(); - m_data = std::make_unique(); - - if (m_settings) { - osqp_set_default_settings(m_settings.get()); - m_settings->alpha = 1.6; // Change alpha parameter - m_settings->eps_rel = 1.0E-4; - m_settings->eps_abs = eps_abs; - m_settings->eps_prim_inf = 1.0E-4; - m_settings->eps_dual_inf = 1.0E-4; - m_settings->warm_start = true; - m_settings->max_iter = 4000; - m_settings->verbose = false; - m_settings->polish = polish; - } - m_exitflag = 0; -} - -OSQPInterface::OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs) -: OSQPInterface(eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -OSQPInterface::OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs) -: OSQPInterface(eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -OSQPInterface::~OSQPInterface() -{ - if (m_data->P) free(m_data->P); - if (m_data->A) free(m_data->A); -} - -void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept -{ - if (ptr != nullptr) { - osqp_cleanup(ptr); - } -} - -void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) -{ - /* - // Transform 'P' into an 'upper trapezoidal matrix' - Eigen::MatrixXd P_trap = P_new.triangularView(); - // Transform 'P' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix P_sparse = P_trap.sparseView(); - double *P_val_ptr = P_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int P_elem_N = P_sparse.nonZeros(); - */ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); -} - -void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) -{ - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); -} - -void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) -{ - /* - // Transform 'A' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix A_sparse = A_new.sparseView(); - double *A_val_ptr = A_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int A_elem_N = A_sparse.nonZeros(); - */ - CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); - return; -} - -void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) -{ - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); -} - -void OSQPInterface::updateQ(const std::vector & q_new) -{ - std::vector q_tmp(q_new.begin(), q_new.end()); - double * q_dyn = q_tmp.data(); - osqp_update_lin_cost(m_work.get(), q_dyn); -} - -void OSQPInterface::updateL(const std::vector & l_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - double * l_dyn = l_tmp.data(); - osqp_update_lower_bound(m_work.get(), l_dyn); -} - -void OSQPInterface::updateU(const std::vector & u_new) -{ - std::vector u_tmp(u_new.begin(), u_new.end()); - double * u_dyn = u_tmp.data(); - osqp_update_upper_bound(m_work.get(), u_dyn); -} - -void OSQPInterface::updateBounds( - const std::vector & l_new, const std::vector & u_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - std::vector u_tmp(u_new.begin(), u_new.end()); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - osqp_update_bounds(m_work.get(), l_dyn, u_dyn); -} - -void OSQPInterface::updateEpsAbs(const double eps_abs) -{ - m_settings->eps_abs = eps_abs; // for default setting - if (m_work_initialized) { - osqp_update_eps_abs(m_work.get(), eps_abs); // for current work - } -} - -void OSQPInterface::updateEpsRel(const double eps_rel) -{ - m_settings->eps_rel = eps_rel; // for default setting - if (m_work_initialized) { - osqp_update_eps_rel(m_work.get(), eps_rel); // for current work - } -} - -void OSQPInterface::updateMaxIter(const int max_iter) -{ - m_settings->max_iter = max_iter; // for default setting - if (m_work_initialized) { - osqp_update_max_iter(m_work.get(), max_iter); // for current work - } -} - -void OSQPInterface::updateVerbose(const bool is_verbose) -{ - m_settings->verbose = is_verbose; // for default setting - if (m_work_initialized) { - osqp_update_verbose(m_work.get(), is_verbose); // for current work - } -} - -void OSQPInterface::updateRhoInterval(const int rho_interval) -{ - m_settings->adaptive_rho_interval = rho_interval; // for default setting -} - -void OSQPInterface::updateRho(const double rho) -{ - m_settings->rho = rho; - if (m_work_initialized) { - osqp_update_rho(m_work.get(), rho); - } -} - -void OSQPInterface::updateAlpha(const double alpha) -{ - m_settings->alpha = alpha; - if (m_work_initialized) { - osqp_update_alpha(m_work.get(), alpha); - } -} - -void OSQPInterface::updateScaling(const int scaling) -{ - m_settings->scaling = scaling; -} - -void OSQPInterface::updatePolish(const bool polish) -{ - m_settings->polish = polish; - if (m_work_initialized) { - osqp_update_polish(m_work.get(), polish); - } -} - -void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) -{ - if (polish_refine_iter < 0) { - std::cerr << "Polish refinement iterations must be positive" << std::endl; - return; - } - - m_settings->polish_refine_iter = polish_refine_iter; - if (m_work_initialized) { - osqp_update_polish_refine_iter(m_work.get(), polish_refine_iter); - } -} - -void OSQPInterface::updateCheckTermination(const int check_termination) -{ - if (check_termination < 0) { - std::cerr << "Check termination must be positive" << std::endl; - return; - } - - m_settings->check_termination = check_termination; - if (m_work_initialized) { - osqp_update_check_termination(m_work.get(), check_termination); - } -} - -bool OSQPInterface::setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables) -{ - return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); -} - -bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) -{ - if (!m_work_initialized) { - return false; - } - - const auto result = osqp_warm_start_x(m_work.get(), primal_variables.data()); - if (result != 0) { - std::cerr << "Failed to set primal variables for warm start" << std::endl; - return false; - } - - return true; -} - -bool OSQPInterface::setDualVariables(const std::vector & dual_variables) -{ - if (!m_work_initialized) { - return false; - } - - const auto result = osqp_warm_start_y(m_work.get(), dual_variables.data()); - if (result != 0) { - std::cerr << "Failed to set dual variables for warm start" << std::endl; - return false; - } - - return true; -} - -int64_t OSQPInterface::initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // check if arguments are valid - std::stringstream ss; - if (P.rows() != P.cols()) { - ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows() - << ", P.cols() = " << P.cols(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != static_cast(q.size())) { - ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows() - << ", q.size() = " << q.size(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != A.cols()) { - ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows() - << ", A.cols() = " << A.cols(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(l.size())) { - ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows() - << ", l.size() = " << l.size(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(u.size())) { - ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows() - << ", u.size() = " << u.size(); - throw std::invalid_argument(ss.str()); - } - - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - return initializeProblem(P_csc, A_csc, q, l, u); -} - -int64_t OSQPInterface::initializeProblem( - CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - // Dynamic float arrays - std::vector q_tmp(q.begin(), q.end()); - std::vector l_tmp(l.begin(), l.end()); - std::vector u_tmp(u.begin(), u.end()); - double * q_dyn = q_tmp.data(); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - - /********************** - * OBJECTIVE FUNCTION - **********************/ - m_param_n = static_cast(q.size()); - m_data->m = static_cast(l.size()); - - /***************** - * POPULATE DATA - *****************/ - m_data->n = m_param_n; - if (m_data->P) free(m_data->P); - m_data->P = csc_matrix( - m_data->n, m_data->n, static_cast(P_csc.m_vals.size()), P_csc.m_vals.data(), - P_csc.m_row_idxs.data(), P_csc.m_col_idxs.data()); - m_data->q = q_dyn; - if (m_data->A) free(m_data->A); - m_data->A = csc_matrix( - m_data->m, m_data->n, static_cast(A_csc.m_vals.size()), A_csc.m_vals.data(), - A_csc.m_row_idxs.data(), A_csc.m_col_idxs.data()); - m_data->l = l_dyn; - m_data->u = u_dyn; - - // Setup workspace - OSQPWorkspace * workspace; - m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get()); - m_work.reset(workspace); - m_work_initialized = true; - - return m_exitflag; -} - -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::solve() -{ - // Solve Problem - osqp_solve(m_work.get()); - - /******************** - * EXTRACT SOLUTION - ********************/ - double * sol_x = m_work->solution->x; - double * sol_y = m_work->solution->y; - std::vector sol_primal(sol_x, sol_x + m_param_n); - std::vector sol_lagrange_multiplier(sol_y, sol_y + m_data->m); - - int64_t status_polish = m_work->info->status_polish; - int64_t status_solution = m_work->info->status_val; - int64_t status_iteration = m_work->info->iter; - - // Result tuple - std::tuple, std::vector, int64_t, int64_t, int64_t> result = - std::make_tuple( - sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration); - - m_latest_work_info = *(m_work->info); - - return result; -} - -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::optimize() -{ - // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); - return result; -} - -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // Allocate memory for problem - initializeProblem(P, A, q, l, u); - - // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); - - m_work.reset(); - m_work_initialized = false; - - return result; -} - -void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const -{ - const int status = getStatus(); - if (status == 1) { - // No need to log since optimization was solved. - return; - } - - // create message - std::string output_message = ""; - if (prefix_message != "") { - output_message = prefix_message + " "; - } - - const auto status_message = getStatusMessage(); - output_message += "Optimization failed due to " + status_message; - - // log with warning - RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), "%s", output_message.c_str()); -} -} // namespace autoware::osqp_interface diff --git a/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp deleted file mode 100644 index a63f979a966bf..0000000000000 --- a/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp +++ /dev/null @@ -1,181 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/osqp_interface/csc_matrix_conv.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include - -TEST(TestCscMatrixConv, Nominal) -{ - using autoware::osqp_interface::calCSCMatrix; - using autoware::osqp_interface::CSC_Matrix; - - Eigen::MatrixXd rect1(1, 2); - rect1 << 0.0, 1.0; - - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - ASSERT_EQ(rect_m1.m_vals.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_vals[0], 1.0); - ASSERT_EQ(rect_m1.m_row_idxs.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_row_idxs[0], c_int(0)); - ASSERT_EQ(rect_m1.m_col_idxs.size(), size_t(3)); // nb of columns + 1 - EXPECT_EQ(rect_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[1], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[2], c_int(1)); - - Eigen::MatrixXd rect2(2, 4); - rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; - - const CSC_Matrix rect_m2 = calCSCMatrix(rect2); - ASSERT_EQ(rect_m2.m_vals.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_vals[0], 1.0); - EXPECT_EQ(rect_m2.m_vals[1], 6.0); - EXPECT_EQ(rect_m2.m_vals[2], 3.0); - EXPECT_EQ(rect_m2.m_vals[3], 7.0); - ASSERT_EQ(rect_m2.m_row_idxs.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_row_idxs[2], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[3], c_int(1)); - ASSERT_EQ(rect_m2.m_col_idxs.size(), size_t(5)); // nb of columns + 1 - EXPECT_EQ(rect_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_col_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(rect_m2.m_col_idxs[3], c_int(4)); - EXPECT_EQ(rect_m2.m_col_idxs[4], c_int(4)); - - // Example from http://netlib.org/linalg/html_templates/node92.html - Eigen::MatrixXd square2(6, 6); - square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0, 3.0, 9.0, 0.0, 0.0, 0.0, 3.0, 0.0, 7.0, 8.0, 7.0, 0.0, - 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; - - const CSC_Matrix square_m2 = calCSCMatrix(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(19)); - EXPECT_EQ(square_m2.m_vals[0], 10.0); - EXPECT_EQ(square_m2.m_vals[1], 3.0); - EXPECT_EQ(square_m2.m_vals[2], 3.0); - EXPECT_EQ(square_m2.m_vals[3], 9.0); - EXPECT_EQ(square_m2.m_vals[4], 7.0); - EXPECT_EQ(square_m2.m_vals[5], 8.0); - EXPECT_EQ(square_m2.m_vals[6], 4.0); - EXPECT_EQ(square_m2.m_vals[7], 8.0); - EXPECT_EQ(square_m2.m_vals[8], 8.0); - EXPECT_EQ(square_m2.m_vals[9], 7.0); - EXPECT_EQ(square_m2.m_vals[10], 7.0); - EXPECT_EQ(square_m2.m_vals[11], 9.0); - EXPECT_EQ(square_m2.m_vals[12], -2.0); - EXPECT_EQ(square_m2.m_vals[13], 5.0); - EXPECT_EQ(square_m2.m_vals[14], 9.0); - EXPECT_EQ(square_m2.m_vals[15], 2.0); - EXPECT_EQ(square_m2.m_vals[16], 3.0); - EXPECT_EQ(square_m2.m_vals[17], 13.0); - EXPECT_EQ(square_m2.m_vals[18], -1.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(19)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[3], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[4], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[5], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[6], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[7], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[8], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[9], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[10], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[11], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[12], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[13], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[14], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[15], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[16], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[17], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[18], c_int(5)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(7)); // nb of columns + 1 - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(3)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(7)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(9)); - EXPECT_EQ(square_m2.m_col_idxs[4], c_int(12)); - EXPECT_EQ(square_m2.m_col_idxs[5], c_int(16)); - EXPECT_EQ(square_m2.m_col_idxs[6], c_int(19)); -} -TEST(TestCscMatrixConv, Trapezoidal) -{ - using autoware::osqp_interface::calCSCMatrixTrapezoidal; - using autoware::osqp_interface::CSC_Matrix; - - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd square2(3, 3); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - square2 << 0.0, 2.0, 0.0, 4.0, 5.0, 6.0, 0.0, 0.0, 0.0; - rect1 << 0.0, 1.0; - - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - // Trapezoidal: skip the lower left triangle (2.0 in this example) - ASSERT_EQ(square_m1.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m1.m_vals[0], 1.0); - EXPECT_EQ(square_m1.m_vals[1], 2.0); - EXPECT_EQ(square_m1.m_vals[2], 4.0); - ASSERT_EQ(square_m1.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[1], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m1.m_col_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_col_idxs[1], c_int(1)); - EXPECT_EQ(square_m1.m_col_idxs[2], c_int(3)); - - const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m2.m_vals[0], 2.0); - EXPECT_EQ(square_m2.m_vals[1], 5.0); - EXPECT_EQ(square_m2.m_vals[2], 6.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(4)); - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(3)); - - try { - const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); - FAIL() << "calCSCMatrixTrapezoidal should fail with non-square inputs"; - } catch (const std::invalid_argument & e) { - EXPECT_EQ(e.what(), std::string("Matrix must be square (n, n)")); - } -} -TEST(TestCscMatrixConv, Print) -{ - using 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; - rect1 << 0.0, 1.0; - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - printCSCMatrix(square_m1); - printCSCMatrix(rect_m1); -} diff --git a/common/autoware_osqp_interface/test/test_osqp_interface.cpp b/common/autoware_osqp_interface/test/test_osqp_interface.cpp deleted file mode 100644 index f65b07e6d792d..0000000000000 --- a/common/autoware_osqp_interface/test/test_osqp_interface.cpp +++ /dev/null @@ -1,164 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/osqp_interface/osqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include - -namespace -{ -// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestOsqpInterface, BasicQp) -{ - 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) { - EXPECT_EQ(std::get<2>(result), 1); // polish succeeded - EXPECT_EQ(std::get<3>(result), 1); // solution succeeded - - static const auto ep = 1.0e-8; - - const auto prime_val = std::get<0>(result); - ASSERT_EQ(prime_val.size(), size_t(2)); - EXPECT_NEAR(prime_val[0], 0.3, ep); - EXPECT_NEAR(prime_val[1], 0.7, ep); - - const auto dual_val = std::get<1>(result); - ASSERT_EQ(dual_val.size(), size_t(4)); - EXPECT_NEAR(dual_val[0], -2.9, ep); - EXPECT_NEAR(dual_val[1], 0.0, ep); - EXPECT_NEAR(dual_val[2], 0.2, ep); - EXPECT_NEAR(dual_val[3], 0.0, ep); - }; - - const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); - const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); - const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -autoware::osqp_interface::INF}; - const std::vector u = {1.0, 0.7, 0.7, autoware::osqp_interface::INF}; - - { - // Define problem during optimization - autoware::osqp_interface::OSQPInterface osqp; - std::tuple, std::vector, int, int, int> result = - osqp.optimize(P, A, q, l, u); - check_result(result); - } - - { - // Define problem during initialization - 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); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem - Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::osqp_interface::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6); - osqp.optimize(); - - // Redefine problem before optimization - osqp.initializeProblem(P, A, q, l, u); - result = osqp.optimize(); - check_result(result); - } - - { - // Define problem during initialization with csc matrix - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - 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); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - 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 - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - osqp.initializeProblem(P_csc, A_csc, q, l, u); - result = osqp.optimize(); - check_result(result); - } - - // add warm startup - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - 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 - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - osqp.initializeProblem(P_csc, A_csc, q, l, u); - result = osqp.optimize(); - check_result(result); - - osqp.updateCheckTermination(1); - const auto primal_val = std::get<0>(result); - const auto dual_val = std::get<1>(result); - for (size_t i = 0; i < primal_val.size(); ++i) { - std::cerr << primal_val.at(i) << std::endl; - } - osqp.setWarmStart(primal_val, dual_val); - result = osqp.optimize(); - check_result(result); - EXPECT_EQ(osqp.getTakenIter(), 1); - } -} -} // namespace diff --git a/common/autoware_qp_interface/CHANGELOG.rst b/common/autoware_qp_interface/CHANGELOG.rst deleted file mode 100644 index f76dac861e1e6..0000000000000 --- a/common/autoware_qp_interface/CHANGELOG.rst +++ /dev/null @@ -1,81 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_qp_interface -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* fix: fix package names in changelog files (`#9500 `_) -* fix(autoware_osqp_interface): fix clang-tidy errors (`#9440 `_) -* 0.39.0 -* update changelog -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(qp_interface): prefix package and namespace with autoware (`#9236 `_) -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Ryuta Kambe, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(qp_interface): prefix package and namespace with autoware (`#9236 `_) -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* fix(qp_interface): fix unreadVariable (`#8349 `_) - * fix:unreadVariable - * fix:unreadVariable - * fix:unreadVariable - --------- -* perf(velocity_smoother): use ProxQP for faster optimization (`#8028 `_) - * perf(velocity_smoother): use ProxQP for faster optimization - * consider max_iteration - * disable warm start - * fix test - --------- -* refactor(qp_interface): clean up the code (`#8029 `_) - * refactor qp_interface - * variable names: m_XXX -> XXX\_ - * update - * update - --------- -* fix(fake_test_node, osqp_interface, qp_interface): remove unnecessary cppcheck inline suppressions (`#7855 `_) - * fix(fake_test_node, osqp_interface, qp_interface): remove unnecessary cppcheck inline suppressions - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Ryuta Kambe, Takayuki Murooka, Yutaka Kondo, kobayu858 - -0.26.0 (2024-04-03) -------------------- -* feat(qp_interface): support proxqp (`#3699 `_) - * feat(qp_interface): add proxqp interface - * update - --------- -* feat(qp_interface): add new package which will contain various qp solvers (`#3697 `_) -* Contributors: Takayuki Murooka diff --git a/common/autoware_qp_interface/CMakeLists.txt b/common/autoware_qp_interface/CMakeLists.txt deleted file mode 100644 index 1df75d2d719a0..0000000000000 --- a/common/autoware_qp_interface/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_qp_interface) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) -find_package(proxsuite REQUIRED) - -# after find_package(osqp_vendor) in ament_auto_find_build_dependencies -find_package(osqp REQUIRED) -get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) -get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH) - -set(QP_INTERFACE_LIB_SRC - src/qp_interface.cpp - src/osqp_interface.cpp - src/osqp_csc_matrix_conv.cpp - src/proxqp_interface.cpp -) - -set(QP_INTERFACE_LIB_HEADERS - include/autoware/qp_interface/qp_interface.hpp - include/autoware/qp_interface/osqp_interface.hpp - include/autoware/qp_interface/osqp_csc_matrix_conv.hpp - include/autoware/qp_interface/proxqp_interface.hpp -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - ${QP_INTERFACE_LIB_SRC} - ${QP_INTERFACE_LIB_HEADERS} -) -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast) - -target_include_directories(${PROJECT_NAME} - SYSTEM PUBLIC - "${OSQP_INCLUDE_DIR}" - "${EIGEN3_INCLUDE_DIR}" -) - -ament_target_dependencies(${PROJECT_NAME} - Eigen3 - osqp_vendor - proxsuite -) - -# crucial so downstream package builds because osqp_interface exposes osqp.hpp -ament_export_include_directories("${OSQP_INCLUDE_DIR}") -# crucial so the linking order is correct in a downstream package: libosqp_interface.a should come before libosqp.a -ament_export_libraries(osqp::osqp) - -if(BUILD_TESTING) - set(TEST_SOURCES - test/test_osqp_interface.cpp - test/test_csc_matrix_conv.cpp - test/test_proxqp_interface.cpp - ) - set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) - ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) - target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) -endif() - -ament_auto_package(INSTALL_TO_SHARE -) diff --git a/common/autoware_qp_interface/design/qp_interface-design.md b/common/autoware_qp_interface/design/qp_interface-design.md deleted file mode 100644 index edc7fa9845206..0000000000000 --- a/common/autoware_qp_interface/design/qp_interface-design.md +++ /dev/null @@ -1,48 +0,0 @@ -# Interface for QP solvers - -This is the design document for the `autoware_qp_interface` package. - -## Purpose / Use cases - -This packages provides a C++ interface for QP solvers. -Currently, supported QP solvers are - -- [OSQP library](https://osqp.org/docs/solver/index.html) - -## Design - -The class `QPInterface` takes a problem formulation as Eigen matrices and vectors, converts these objects into -C-style Compressed-Column-Sparse matrices and dynamic arrays, loads the data into the QP workspace dataholder, and runs the optimizer. - -## Inputs / Outputs / API - -The interface can be used in several ways: - -1. Initialize the interface, and load the problem formulation at the optimization call. - - ```cpp - QPInterface qp_interface; - qp_interface.optimize(P, A, q, l, u); - ``` - -2. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. - - ```cpp - QPInterface qp_interface(true); - qp_interface.optimize(P, A, q, l, u); - qp_interface.optimize(P_new, A_new, q_new, l_new, u_new); - ``` - - The optimization results are returned as a vector by the optimization function. - - ```cpp - const auto solution = qp_interface.optimize(); - double x_0 = solution[0]; - double x_1 = solution[1]; - ``` - -## References / External links - -- OSQP library: - -## Related issues diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp deleted file mode 100644 index 9575d9d18c69f..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ -#define AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ - -#include "osqp/glob_opts.h" - -#include - -#include - -namespace autoware::qp_interface -{ -/// \brief Compressed-Column-Sparse Matrix -struct CSC_Matrix -{ - /// Vector of non-zero values. Ex: [4,1,1,2] - std::vector vals_; - /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') - std::vector row_idxs_; - /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') - std::vector col_idxs_; -}; - -/// \brief Calculate CSC matrix from Eigen matrix -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); -/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix -CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); -/// \brief Print the given CSC matrix to the standard output -void printCSCMatrix(const CSC_Matrix & csc_mat); - -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp deleted file mode 100644 index a5777dd545e9f..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp +++ /dev/null @@ -1,147 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" -#include "autoware/qp_interface/qp_interface.hpp" -#include "osqp/osqp.h" - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -constexpr c_float OSQP_INF = 1e30; - -class OSQPInterface : public QPInterface -{ -public: - /// \brief Constructor without problem formulation - OSQPInterface( - const bool enable_warm_start = false, const int max_iteration = 20000, - const c_float eps_abs = std::numeric_limits::epsilon(), - const c_float eps_rel = std::numeric_limits::epsilon(), const bool polish = true, - const bool verbose = false); - /// \brief Constructor with problem setup - /// \param P: (n,n) matrix defining relations between parameters. - /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - /// \param q: (n) vector defining the linear cost of the problem. - /// \param l: (m) vector defining the lower bound problem constraint. - /// \param u: (m) vector defining the upper bound problem constraint. - /// \param eps_abs: Absolute convergence tolerance. - OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, - const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon()); - OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, - const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon()); - ~OSQPInterface(); - - static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; - - std::vector optimize( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - int getIterationNumber() const override; - bool isSolved() const override; - std::string getStatus() const override; - - int getPolishStatus() const; - std::vector getDualSolution() const; - - void updateEpsAbs(const double eps_abs) override; - void updateEpsRel(const double eps_rel) override; - void updateVerbose(const bool verbose) override; - - // Updates problem parameters while keeping solution in memory. - // - // Args: - // P_new: (n,n) matrix defining relations between parameters. - // A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - // q_new: (n) vector defining the linear cost of the problem. - // l_new: (m) vector defining the lower bound problem constraint. - // u_new: (m) vector defining the upper bound problem constraint. - void updateP(const Eigen::MatrixXd & P_new); - void updateCscP(const CSC_Matrix & P_csc); - void updateA(const Eigen::MatrixXd & A_new); - void updateCscA(const CSC_Matrix & A_csc); - void updateQ(const std::vector & q_new); - void updateL(const std::vector & l_new); - void updateU(const std::vector & u_new); - void updateBounds(const std::vector & l_new, const std::vector & u_new); - - void updateMaxIter(const int iter); - void updateRhoInterval(const int rho_interval); - void updateRho(const double rho); - void updateAlpha(const double alpha); - void updateScaling(const int scaling); - void updatePolish(const bool polish); - void updatePolishRefinementIteration(const int polish_refine_iter); - void updateCheckTermination(const int check_termination); - - /// \brief Get the number of iteration taken to solve the problem - inline int64_t getTakenIter() const { return static_cast(latest_work_info_.iter); } - /// \brief Get the status message for the latest problem solved - inline std::string getStatusMessage() const - { - return static_cast(latest_work_info_.status); - } - /// \brief Get the runtime of the latest problem solved - inline double getRunTime() const { return latest_work_info_.run_time; } - /// \brief Get the objective value the latest problem solved - inline double getObjVal() const { return latest_work_info_.obj_val; } - /// \brief Returns flag asserting interface condition (Healthy condition: 0). - inline int64_t getExitFlag() const { return exitflag_; } - - // Setter functions for warm start - bool setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables); - bool setPrimalVariables(const std::vector & primal_variables); - bool setDualVariables(const std::vector & dual_variables); - -private: - std::unique_ptr> work_; - std::unique_ptr settings_; - std::unique_ptr data_; - // store last work info since work is cleaned up at every execution to prevent memory leak. - OSQPInfo latest_work_info_; - // Number of parameters to optimize - int64_t param_n_; - // Flag to check if the current work exists - bool work__initialized = false; - // Exitflag - int64_t exitflag_; - - void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) override; - - void initializeCSCProblemImpl( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - std::vector optimizeImpl() override; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp deleted file mode 100644 index 324da4b18c6fd..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ - -#include "autoware/qp_interface/qp_interface.hpp" - -#include -#include - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -class ProxQPInterface : public QPInterface -{ -public: - explicit ProxQPInterface( - const bool enable_warm_start, const int max_iteration, const double eps_abs, - const double eps_rel, const bool verbose = false); - - int getIterationNumber() const override; - bool isSolved() const override; - std::string getStatus() const override; - - void updateEpsAbs(const double eps_abs) override; - void updateEpsRel(const double eps_rel) override; - void updateVerbose(const bool verbose) override; - -private: - proxsuite::proxqp::Settings settings_{}; - std::shared_ptr> qp_ptr_{nullptr}; - - void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) override; - - std::vector optimizeImpl() override; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp deleted file mode 100644 index be3c598512bf6..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ - -#include - -#include -#include -#include - -namespace autoware::qp_interface -{ -class QPInterface -{ -public: - explicit QPInterface(const bool enable_warm_start) : enable_warm_start_(enable_warm_start) {} - - std::vector optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - virtual bool isSolved() const = 0; - virtual int getIterationNumber() const = 0; - virtual std::string getStatus() const = 0; - - virtual void updateEpsAbs([[maybe_unused]] const double eps_abs) = 0; - virtual void updateEpsRel([[maybe_unused]] const double eps_rel) = 0; - virtual void updateVerbose([[maybe_unused]] const bool verbose) {} - -protected: - bool enable_warm_start_{false}; - - void initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - virtual void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) = 0; - - virtual std::vector optimizeImpl() = 0; - - std::optional variables_num_{std::nullopt}; - std::optional constraints_num_{std::nullopt}; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/package.xml b/common/autoware_qp_interface/package.xml deleted file mode 100644 index b61d03a36db72..0000000000000 --- a/common/autoware_qp_interface/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - autoware_qp_interface - 0.40.0 - Interface for the QP solvers - Takayuki Murooka - Fumiya Watanabe - Maxime CLEMENT - Satoshi Ota - Apache 2.0 - - ament_cmake_auto - autoware_cmake - - eigen - osqp_vendor - proxsuite - rclcpp - rclcpp_components - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - eigen - - - ament_cmake - - diff --git a/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp b/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp deleted file mode 100644 index 15314a9e4a5fa..0000000000000 --- a/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp +++ /dev/null @@ -1,134 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" - -#include -#include - -#include -#include -#include - -namespace autoware::qp_interface -{ -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i < rows; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - if (rows != cols) { - throw std::invalid_argument("Matrix must be square (n, n)"); - } - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - Eigen::Index trap_last_idx = 0; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i <= trap_last_idx; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - trap_last_idx += 1; - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -void printCSCMatrix(const CSC_Matrix & csc_mat) -{ - std::cout << "["; - for (const c_float & val : csc_mat.vals_) { - std::cout << val << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & row : csc_mat.row_idxs_) { - std::cout << row << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & col : csc_mat.col_idxs_) { - std::cout << col << ", "; - } - std::cout << "]\n"; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/osqp_interface.cpp b/common/autoware_qp_interface/src/osqp_interface.cpp deleted file mode 100644 index fbb8e71f4c251..0000000000000 --- a/common/autoware_qp_interface/src/osqp_interface.cpp +++ /dev/null @@ -1,394 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_interface.hpp" - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" - -#include - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -OSQPInterface::OSQPInterface( - const bool enable_warm_start, const int max_iteration, const c_float eps_abs, - const c_float eps_rel, const bool polish, const bool verbose) -: QPInterface(enable_warm_start), work_{nullptr, OSQPWorkspaceDeleter} -{ - settings_ = std::make_unique(); - data_ = std::make_unique(); - - if (settings_) { - osqp_set_default_settings(settings_.get()); - settings_->alpha = 1.6; // Change alpha parameter - settings_->eps_rel = eps_rel; - settings_->eps_abs = eps_abs; - settings_->eps_prim_inf = 1.0E-4; - settings_->eps_dual_inf = 1.0E-4; - settings_->warm_start = enable_warm_start; - settings_->max_iter = max_iteration; - settings_->verbose = verbose; - settings_->polish = polish; - } - exitflag_ = 0; -} - -OSQPInterface::OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const bool enable_warm_start, - const c_float eps_abs) -: OSQPInterface(enable_warm_start, eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -OSQPInterface::OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const bool enable_warm_start, - const c_float eps_abs) -: OSQPInterface(enable_warm_start, eps_abs) -{ - initializeCSCProblemImpl(P, A, q, l, u); -} - -OSQPInterface::~OSQPInterface() -{ - if (data_->P) free(data_->P); - if (data_->A) free(data_->A); -} - -void OSQPInterface::initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - initializeCSCProblemImpl(P_csc, A_csc, q, l, u); -} - -void OSQPInterface::initializeCSCProblemImpl( - CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - // Dynamic float arrays - std::vector q_tmp(q.begin(), q.end()); - std::vector l_tmp(l.begin(), l.end()); - std::vector u_tmp(u.begin(), u.end()); - double * q_dyn = q_tmp.data(); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - - /********************** - * OBJECTIVE FUNCTION - **********************/ - param_n_ = static_cast(q.size()); - data_->m = static_cast(l.size()); - - /***************** - * POPULATE DATA - *****************/ - data_->n = param_n_; - if (data_->P) free(data_->P); - data_->P = csc_matrix( - data_->n, data_->n, static_cast(P_csc.vals_.size()), P_csc.vals_.data(), - P_csc.row_idxs_.data(), P_csc.col_idxs_.data()); - data_->q = q_dyn; - if (data_->A) free(data_->A); - data_->A = csc_matrix( - data_->m, data_->n, static_cast(A_csc.vals_.size()), A_csc.vals_.data(), - A_csc.row_idxs_.data(), A_csc.col_idxs_.data()); - data_->l = l_dyn; - data_->u = u_dyn; - - // Setup workspace - OSQPWorkspace * workspace; - exitflag_ = osqp_setup(&workspace, data_.get(), settings_.get()); - work_.reset(workspace); - work__initialized = true; -} - -void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept -{ - if (ptr != nullptr) { - osqp_cleanup(ptr); - } -} - -void OSQPInterface::updateEpsAbs(const double eps_abs) -{ - settings_->eps_abs = eps_abs; // for default setting - if (work__initialized) { - osqp_update_eps_abs(work_.get(), eps_abs); // for current work - } -} - -void OSQPInterface::updateEpsRel(const double eps_rel) -{ - settings_->eps_rel = eps_rel; // for default setting - if (work__initialized) { - osqp_update_eps_rel(work_.get(), eps_rel); // for current work - } -} - -void OSQPInterface::updateMaxIter(const int max_iter) -{ - settings_->max_iter = max_iter; // for default setting - if (work__initialized) { - osqp_update_max_iter(work_.get(), max_iter); // for current work - } -} - -void OSQPInterface::updateVerbose(const bool is_verbose) -{ - settings_->verbose = is_verbose; // for default setting - if (work__initialized) { - osqp_update_verbose(work_.get(), is_verbose); // for current work - } -} - -void OSQPInterface::updateRhoInterval(const int rho_interval) -{ - settings_->adaptive_rho_interval = rho_interval; // for default setting -} - -void OSQPInterface::updateRho(const double rho) -{ - settings_->rho = rho; - if (work__initialized) { - osqp_update_rho(work_.get(), rho); - } -} - -void OSQPInterface::updateAlpha(const double alpha) -{ - settings_->alpha = alpha; - if (work__initialized) { - osqp_update_alpha(work_.get(), alpha); - } -} - -void OSQPInterface::updateScaling(const int scaling) -{ - settings_->scaling = scaling; -} - -void OSQPInterface::updatePolish(const bool polish) -{ - settings_->polish = polish; - if (work__initialized) { - osqp_update_polish(work_.get(), polish); - } -} - -void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) -{ - if (polish_refine_iter < 0) { - std::cerr << "Polish refinement iterations must be positive" << std::endl; - return; - } - - settings_->polish_refine_iter = polish_refine_iter; - if (work__initialized) { - osqp_update_polish_refine_iter(work_.get(), polish_refine_iter); - } -} - -void OSQPInterface::updateCheckTermination(const int check_termination) -{ - if (check_termination < 0) { - std::cerr << "Check termination must be positive" << std::endl; - return; - } - - settings_->check_termination = check_termination; - if (work__initialized) { - osqp_update_check_termination(work_.get(), check_termination); - } -} - -bool OSQPInterface::setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables) -{ - return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); -} - -bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) -{ - if (!work__initialized) { - return false; - } - - const auto result = osqp_warm_start_x(work_.get(), primal_variables.data()); - if (result != 0) { - std::cerr << "Failed to set primal variables for warm start" << std::endl; - return false; - } - - return true; -} - -bool OSQPInterface::setDualVariables(const std::vector & dual_variables) -{ - if (!work__initialized) { - return false; - } - - const auto result = osqp_warm_start_y(work_.get(), dual_variables.data()); - if (result != 0) { - std::cerr << "Failed to set dual variables for warm start" << std::endl; - return false; - } - - return true; -} - -void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) -{ - /* - // Transform 'P' into an 'upper trapezoidal matrix' - Eigen::MatrixXd P_trap = P_new.triangularView(); - // Transform 'P' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix P_sparse = P_trap.sparseView(); - double *P_val_ptr = P_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int P_elem_N = P_sparse.nonZeros(); - */ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); -} - -void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) -{ - osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); -} - -void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) -{ - /* - // Transform 'A' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix A_sparse = A_new.sparseView(); - double *A_val_ptr = A_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int A_elem_N = A_sparse.nonZeros(); - */ - CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); - return; -} - -void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) -{ - osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); -} - -void OSQPInterface::updateQ(const std::vector & q_new) -{ - std::vector q_tmp(q_new.begin(), q_new.end()); - double * q_dyn = q_tmp.data(); - osqp_update_lin_cost(work_.get(), q_dyn); -} - -void OSQPInterface::updateL(const std::vector & l_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - double * l_dyn = l_tmp.data(); - osqp_update_lower_bound(work_.get(), l_dyn); -} - -void OSQPInterface::updateU(const std::vector & u_new) -{ - std::vector u_tmp(u_new.begin(), u_new.end()); - double * u_dyn = u_tmp.data(); - osqp_update_upper_bound(work_.get(), u_dyn); -} - -void OSQPInterface::updateBounds( - const std::vector & l_new, const std::vector & u_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - std::vector u_tmp(u_new.begin(), u_new.end()); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - osqp_update_bounds(work_.get(), l_dyn, u_dyn); -} - -int OSQPInterface::getIterationNumber() const -{ - return work_->info->iter; -} - -std::string OSQPInterface::getStatus() const -{ - return "OSQP_SOLVED"; -} - -bool OSQPInterface::isSolved() const -{ - return latest_work_info_.status_val == 1; -} - -int OSQPInterface::getPolishStatus() const -{ - return static_cast(latest_work_info_.status_polish); -} - -std::vector OSQPInterface::getDualSolution() const -{ - double * sol_y = work_->solution->y; - std::vector dual_solution(sol_y, sol_y + data_->m); - return dual_solution; -} - -std::vector OSQPInterface::optimizeImpl() -{ - osqp_solve(work_.get()); - - double * sol_x = work_->solution->x; - std::vector sol_primal(sol_x, sol_x + param_n_); - - latest_work_info_ = *(work_->info); - - if (!enable_warm_start_) { - work_.reset(); - work__initialized = false; - } - - return sol_primal; -} - -std::vector OSQPInterface::optimize( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - initializeCSCProblemImpl(P, A, q, l, u); - const auto result = optimizeImpl(); - - // show polish status if not successful - const int status_polish = static_cast(latest_work_info_.status_polish); - if (status_polish != 1) { - const auto msg = status_polish == 0 ? "unperformed" - : status_polish == -1 ? "unsuccessful" - : "unknown"; - std::cerr << "osqp polish process failed : " << msg << ". The result may be inaccurate" - << std::endl; - } - - return result; -} - -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/proxqp_interface.cpp b/common/autoware_qp_interface/src/proxqp_interface.cpp deleted file mode 100644 index a0ebbf0db0510..0000000000000 --- a/common/autoware_qp_interface/src/proxqp_interface.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/proxqp_interface.hpp" - -#include -#include -#include - -namespace autoware::qp_interface -{ -using proxsuite::proxqp::QPSolverOutput; - -ProxQPInterface::ProxQPInterface( - const bool enable_warm_start, const int max_iteration, const double eps_abs, const double eps_rel, - const bool verbose) -: QPInterface(enable_warm_start) -{ - settings_.max_iter = max_iteration; - settings_.eps_abs = eps_abs; - settings_.eps_rel = eps_rel; - settings_.verbose = verbose; -} - -void ProxQPInterface::initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - const size_t variables_num = q.size(); - const size_t constraints_num = l.size(); - - const bool enable_warm_start = [&]() { - if ( - !enable_warm_start_ // Warm start is designated - || !qp_ptr_ // QP pointer is initialized - // The number of variables is the same as the previous one. - || !variables_num_ || - *variables_num_ != variables_num - // The number of constraints is the same as the previous one - || !constraints_num_ || *constraints_num_ != constraints_num) { - return false; - } - return true; - }(); - - if (!enable_warm_start) { - qp_ptr_ = std::make_shared>( - variables_num, 0, constraints_num); - } - - settings_.initial_guess = - enable_warm_start ? proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT - : proxsuite::proxqp::InitialGuessStatus::NO_INITIAL_GUESS; - - qp_ptr_->settings = settings_; - - Eigen::SparseMatrix P_sparse(variables_num, constraints_num); - P_sparse = P.sparseView(); - - // NOTE: const std vector cannot be converted to eigen vector - std::vector non_const_q = q; - Eigen::VectorXd eigen_q = - Eigen::Map(non_const_q.data(), non_const_q.size()); - std::vector l_std_vec = l; - Eigen::VectorXd eigen_l = - Eigen::Map(l_std_vec.data(), l_std_vec.size()); - std::vector u_std_vec = u; - Eigen::VectorXd eigen_u = - Eigen::Map(u_std_vec.data(), u_std_vec.size()); - - if (enable_warm_start) { - qp_ptr_->update( - P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); - } else { - qp_ptr_->init( - P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); - } -} - -void ProxQPInterface::updateEpsAbs(const double eps_abs) -{ - settings_.eps_abs = eps_abs; -} - -void ProxQPInterface::updateEpsRel(const double eps_rel) -{ - settings_.eps_rel = eps_rel; -} - -void ProxQPInterface::updateVerbose(const bool is_verbose) -{ - settings_.verbose = is_verbose; -} - -bool ProxQPInterface::isSolved() const -{ - if (qp_ptr_) { - return qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED; - } - return false; -} - -int ProxQPInterface::getIterationNumber() const -{ - if (qp_ptr_) { - return qp_ptr_->results.info.iter; - } - return 0; -} - -std::string ProxQPInterface::getStatus() const -{ - if (qp_ptr_) { - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED) { - return "PROXQP_SOLVED"; - } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_MAX_ITER_REACHED) { - return "PROXQP_MAX_ITER_REACHED"; - } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { - return "PROXQP_PRIMAL_INFEASIBLE"; - } - // if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE) { - // return "PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE"; - // } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_DUAL_INFEASIBLE) { - return "PROXQP_DUAL_INFEASIBLE"; - } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_NOT_RUN) { - return "PROXQP_NOT_RUN"; - } - } - return "None"; -} - -std::vector ProxQPInterface::optimizeImpl() -{ - qp_ptr_->solve(); - - std::vector result; - for (Eigen::Index i = 0; i < qp_ptr_->results.x.size(); ++i) { - result.push_back(qp_ptr_->results.x[i]); - } - return result; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/qp_interface.cpp b/common/autoware_qp_interface/src/qp_interface.cpp deleted file mode 100644 index f01e57772dc4f..0000000000000 --- a/common/autoware_qp_interface/src/qp_interface.cpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/qp_interface.hpp" - -#include -#include -#include - -namespace autoware::qp_interface -{ -void QPInterface::initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // check if arguments are valid - std::stringstream ss; - if (P.rows() != P.cols()) { - ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows() - << ", P.cols() = " << P.cols(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != static_cast(q.size())) { - ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows() - << ", q.size() = " << q.size(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != A.cols()) { - ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows() - << ", A.cols() = " << A.cols(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(l.size())) { - ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows() - << ", l.size() = " << l.size(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(u.size())) { - ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows() - << ", u.size() = " << u.size(); - throw std::invalid_argument(ss.str()); - } - - initializeProblemImpl(P, A, q, l, u); - - variables_num_ = q.size(); - constraints_num_ = l.size(); -} - -std::vector QPInterface::optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - initializeProblem(P, A, q, l, u); - const auto result = optimizeImpl(); - - return result; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp deleted file mode 100644 index 1f377a1a24535..0000000000000 --- a/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp +++ /dev/null @@ -1,181 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include - -TEST(TestCscMatrixConv, Nominal) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::CSC_Matrix; - - Eigen::MatrixXd rect1(1, 2); - rect1 << 0.0, 1.0; - - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - ASSERT_EQ(rect_m1.vals_.size(), size_t(1)); - EXPECT_EQ(rect_m1.vals_[0], 1.0); - ASSERT_EQ(rect_m1.row_idxs_.size(), size_t(1)); - EXPECT_EQ(rect_m1.row_idxs_[0], c_int(0)); - ASSERT_EQ(rect_m1.col_idxs_.size(), size_t(3)); // nb of columns + 1 - EXPECT_EQ(rect_m1.col_idxs_[0], c_int(0)); - EXPECT_EQ(rect_m1.col_idxs_[1], c_int(0)); - EXPECT_EQ(rect_m1.col_idxs_[2], c_int(1)); - - Eigen::MatrixXd rect2(2, 4); - rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; - - const CSC_Matrix rect_m2 = calCSCMatrix(rect2); - ASSERT_EQ(rect_m2.vals_.size(), size_t(4)); - EXPECT_EQ(rect_m2.vals_[0], 1.0); - EXPECT_EQ(rect_m2.vals_[1], 6.0); - EXPECT_EQ(rect_m2.vals_[2], 3.0); - EXPECT_EQ(rect_m2.vals_[3], 7.0); - ASSERT_EQ(rect_m2.row_idxs_.size(), size_t(4)); - EXPECT_EQ(rect_m2.row_idxs_[0], c_int(0)); - EXPECT_EQ(rect_m2.row_idxs_[1], c_int(1)); - EXPECT_EQ(rect_m2.row_idxs_[2], c_int(0)); - EXPECT_EQ(rect_m2.row_idxs_[3], c_int(1)); - ASSERT_EQ(rect_m2.col_idxs_.size(), size_t(5)); // nb of columns + 1 - EXPECT_EQ(rect_m2.col_idxs_[0], c_int(0)); - EXPECT_EQ(rect_m2.col_idxs_[1], c_int(1)); - EXPECT_EQ(rect_m2.col_idxs_[2], c_int(2)); - EXPECT_EQ(rect_m2.col_idxs_[3], c_int(4)); - EXPECT_EQ(rect_m2.col_idxs_[4], c_int(4)); - - // Example from http://netlib.org/linalg/html_templates/node92.html - Eigen::MatrixXd square2(6, 6); - square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0, 3.0, 9.0, 0.0, 0.0, 0.0, 3.0, 0.0, 7.0, 8.0, 7.0, 0.0, - 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; - - const CSC_Matrix square_m2 = calCSCMatrix(square2); - ASSERT_EQ(square_m2.vals_.size(), size_t(19)); - EXPECT_EQ(square_m2.vals_[0], 10.0); - EXPECT_EQ(square_m2.vals_[1], 3.0); - EXPECT_EQ(square_m2.vals_[2], 3.0); - EXPECT_EQ(square_m2.vals_[3], 9.0); - EXPECT_EQ(square_m2.vals_[4], 7.0); - EXPECT_EQ(square_m2.vals_[5], 8.0); - EXPECT_EQ(square_m2.vals_[6], 4.0); - EXPECT_EQ(square_m2.vals_[7], 8.0); - EXPECT_EQ(square_m2.vals_[8], 8.0); - EXPECT_EQ(square_m2.vals_[9], 7.0); - EXPECT_EQ(square_m2.vals_[10], 7.0); - EXPECT_EQ(square_m2.vals_[11], 9.0); - EXPECT_EQ(square_m2.vals_[12], -2.0); - EXPECT_EQ(square_m2.vals_[13], 5.0); - EXPECT_EQ(square_m2.vals_[14], 9.0); - EXPECT_EQ(square_m2.vals_[15], 2.0); - EXPECT_EQ(square_m2.vals_[16], 3.0); - EXPECT_EQ(square_m2.vals_[17], 13.0); - EXPECT_EQ(square_m2.vals_[18], -1.0); - ASSERT_EQ(square_m2.row_idxs_.size(), size_t(19)); - EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[2], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[3], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[4], c_int(2)); - EXPECT_EQ(square_m2.row_idxs_[5], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[6], c_int(5)); - EXPECT_EQ(square_m2.row_idxs_[7], c_int(2)); - EXPECT_EQ(square_m2.row_idxs_[8], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[9], c_int(2)); - EXPECT_EQ(square_m2.row_idxs_[10], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[11], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[12], c_int(0)); - EXPECT_EQ(square_m2.row_idxs_[13], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[14], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[15], c_int(5)); - EXPECT_EQ(square_m2.row_idxs_[16], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[17], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[18], c_int(5)); - ASSERT_EQ(square_m2.col_idxs_.size(), size_t(7)); // nb of columns + 1 - EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.col_idxs_[1], c_int(3)); - EXPECT_EQ(square_m2.col_idxs_[2], c_int(7)); - EXPECT_EQ(square_m2.col_idxs_[3], c_int(9)); - EXPECT_EQ(square_m2.col_idxs_[4], c_int(12)); - EXPECT_EQ(square_m2.col_idxs_[5], c_int(16)); - EXPECT_EQ(square_m2.col_idxs_[6], c_int(19)); -} -TEST(TestCscMatrixConv, Trapezoidal) -{ - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd square2(3, 3); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - square2 << 0.0, 2.0, 0.0, 4.0, 5.0, 6.0, 0.0, 0.0, 0.0; - rect1 << 0.0, 1.0; - - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - // Trapezoidal: skip the lower left triangle (2.0 in this example) - ASSERT_EQ(square_m1.vals_.size(), size_t(3)); - EXPECT_EQ(square_m1.vals_[0], 1.0); - EXPECT_EQ(square_m1.vals_[1], 2.0); - EXPECT_EQ(square_m1.vals_[2], 4.0); - ASSERT_EQ(square_m1.row_idxs_.size(), size_t(3)); - EXPECT_EQ(square_m1.row_idxs_[0], c_int(0)); - EXPECT_EQ(square_m1.row_idxs_[1], c_int(0)); - EXPECT_EQ(square_m1.row_idxs_[2], c_int(1)); - ASSERT_EQ(square_m1.col_idxs_.size(), size_t(3)); - EXPECT_EQ(square_m1.col_idxs_[0], c_int(0)); - EXPECT_EQ(square_m1.col_idxs_[1], c_int(1)); - EXPECT_EQ(square_m1.col_idxs_[2], c_int(3)); - - const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); - ASSERT_EQ(square_m2.vals_.size(), size_t(3)); - EXPECT_EQ(square_m2.vals_[0], 2.0); - EXPECT_EQ(square_m2.vals_[1], 5.0); - EXPECT_EQ(square_m2.vals_[2], 6.0); - ASSERT_EQ(square_m2.row_idxs_.size(), size_t(3)); - EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[2], c_int(1)); - ASSERT_EQ(square_m2.col_idxs_.size(), size_t(4)); - EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.col_idxs_[1], c_int(0)); - EXPECT_EQ(square_m2.col_idxs_[2], c_int(2)); - EXPECT_EQ(square_m2.col_idxs_[3], c_int(3)); - - try { - const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); - FAIL() << "calCSCMatrixTrapezoidal should fail with non-square inputs"; - } catch (const std::invalid_argument & e) { - EXPECT_EQ(e.what(), std::string("Matrix must be square (n, n)")); - } -} -TEST(TestCscMatrixConv, Print) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - using autoware::qp_interface::printCSCMatrix; - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - rect1 << 0.0, 1.0; - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - printCSCMatrix(square_m1); - printCSCMatrix(rect_m1); -} diff --git a/common/autoware_qp_interface/test/test_osqp_interface.cpp b/common/autoware_qp_interface/test/test_osqp_interface.cpp deleted file mode 100644 index f97cc2888afa0..0000000000000 --- a/common/autoware_qp_interface/test/test_osqp_interface.cpp +++ /dev/null @@ -1,170 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/osqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include -#include - -namespace -{ -// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestOsqpInterface, BasicQp) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - - auto check_result = - [](const auto & solution, const std::string & status, const int polish_status) { - EXPECT_EQ(status, "OSQP_SOLVED"); - EXPECT_EQ(polish_status, 1); - - static const auto ep = 1.0e-8; - - ASSERT_EQ(solution.size(), size_t(2)); - EXPECT_NEAR(solution[0], 0.3, ep); - EXPECT_NEAR(solution[1], 0.7, ep); - }; - - const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); - const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); - const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -autoware::qp_interface::OSQP_INF}; - const std::vector u = {1.0, 0.7, 0.7, autoware::qp_interface::OSQP_INF}; - - { - // Define problem during optimization - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - // Define problem during initialization - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem - Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); - } - - { - // Define problem during initialization with csc matrix - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - // add warm startup - { - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(true, 4000, 1e-6); // enable warm start - osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - { - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - - osqp.updateCheckTermination(1); - const auto primal_val = solution; - const auto dual_val = osqp.getDualSolution(); - for (size_t i = 0; i < primal_val.size(); ++i) { - std::cerr << primal_val.at(i) << std::endl; - } - osqp.setWarmStart(primal_val, dual_val); - } - - { - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - // NOTE: This should be true, but currently optimize function reset the workspace, which - // disables warm start. - // EXPECT_EQ(osqp.getTakenIter(), 1); - } -} -} // namespace diff --git a/common/autoware_qp_interface/test/test_proxqp_interface.cpp b/common/autoware_qp_interface/test/test_proxqp_interface.cpp deleted file mode 100644 index e47af10c7aabd..0000000000000 --- a/common/autoware_qp_interface/test/test_proxqp_interface.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/proxqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include -#include - -namespace -{ -// Problem taken from -// https://github.com/proxqp/proxqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestProxqpInterface, BasicQp) -{ - auto check_result = [](const auto & solution, const std::string & status) { - EXPECT_EQ(status, "PROXQP_SOLVED"); - - static const auto ep = 1.0e-8; - ASSERT_EQ(solution.size(), size_t(2)); - EXPECT_NEAR(solution[0], 0.3, ep); - EXPECT_NEAR(solution[1], 0.7, ep); - }; - - const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); - const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); - const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -std::numeric_limits::max()}; - const std::vector u = {1.0, 0.7, 0.7, std::numeric_limits::max()}; - - { - // Define problem during optimization - autoware::qp_interface::ProxQPInterface proxqp(false, 4000, 1e-9, 1e-9, false); - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - const auto status = proxqp.getStatus(); - check_result(solution, status); - } - - { - // Define problem during optimization with warm start - autoware::qp_interface::ProxQPInterface proxqp(true, 4000, 1e-9, 1e-9, false); - { - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - const auto status = proxqp.getStatus(); - check_result(solution, status); - EXPECT_NE(proxqp.getIterationNumber(), 1); - } - { - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - const auto status = proxqp.getStatus(); - check_result(solution, status); - EXPECT_EQ(proxqp.getIterationNumber(), 0); - } - } -} -} // namespace diff --git a/common/autoware_test_utils/CMakeLists.txt b/common/autoware_test_utils/CMakeLists.txt index 8bfa44f7bf0ba..20b8f964f879f 100644 --- a/common/autoware_test_utils/CMakeLists.txt +++ b/common/autoware_test_utils/CMakeLists.txt @@ -25,5 +25,4 @@ ament_auto_package(INSTALL_TO_SHARE test_map test_data launch - rviz ) diff --git a/common/autoware_test_utils/launch/psim_intersection.launch.xml b/common/autoware_test_utils/launch/psim_intersection.launch.xml index 723dd7352219d..dc3e75cca1d96 100644 --- a/common/autoware_test_utils/launch/psim_intersection.launch.xml +++ b/common/autoware_test_utils/launch/psim_intersection.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml b/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml index 7dd888a036f9d..58b71524e72b8 100644 --- a/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml +++ b/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/common/autoware_test_utils/rviz/psim_test_map.rviz b/common/autoware_test_utils/rviz/psim_test_map.rviz deleted file mode 100644 index 8475c94b86bb4..0000000000000 --- a/common/autoware_test_utils/rviz/psim_test_map.rviz +++ /dev/null @@ -1,4569 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - Splitter Ratio: 0.557669460773468 - Tree Height: 225 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: AutowareDateTimePanel - Name: AutowareDateTimePanel - - Class: rviz_plugins::AutowareStatePanel - Name: AutowareStatePanel - - Class: rviz_plugins::SimulatedClockPanel - Name: SimulatedClockPanel - - Class: rviz_plugins::TrafficLightPublishPanel - Name: TrafficLightPublishPanel -Visualization Manager: - Class: "" - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - - Class: rviz_common/Group - Displays: - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - ars408_front_center: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera0/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera0/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera1/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera1/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera2/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera2/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera3/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera3/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera4/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera4/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera5/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera5/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera6/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera6/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera7/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera7/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - gnss_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - livox_front_left: - Alpha: 1 - Show Axes: false - Show Trail: false - livox_front_left_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - livox_front_right: - Alpha: 1 - Show Axes: false - Show Trail: false - livox_front_right_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_kit_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - tamagawa/imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_left: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_left_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_rear: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_rear_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_right: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_right_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_top: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_top_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Mass Properties: - Inertia: false - Mass: false - Name: VehicleModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_plugins/PolarGridDisplay - Color: 255; 255; 255 - Delta Range: 10 - Enabled: true - Max Alpha: 0.5 - Max Range: 100 - Max Wave Alpha: 0.5 - Min Alpha: 0.009999999776482582 - Min Wave Alpha: 0.009999999776482582 - Name: PolarGridDisplay - Reference Frame: base_link - Value: true - Wave Color: 255; 255; 255 - Wave Velocity: 40 - - Background Alpha: 0.5 - Background Color: 23; 28; 31 - Class: autoware_overlay_rviz_plugin/SignalDisplay - Dark Traffic Color: 255; 51; 51 - Enabled: true - Gear Topic Test: /vehicle/status/gear_status - Handle Angle Scale: 17 - Hazard Lights Topic: /vehicle/status/hazard_lights_status - Height: 100 - Left: 0 - Light Traffic Color: 255; 153; 153 - Name: SignalDisplay - Primary Color: 174; 174; 174 - Signal Color: 0; 230; 120 - Speed Limit Topic: /planning/scenario_planning/current_max_velocity - Speed Topic: /vehicle/status/velocity_status - Steering Topic: /vehicle/status/steering_status - Top: 10 - Traffic Topic: /planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal - Turn Signals Topic: /vehicle/status/turn_indicators_status - Value: true - Width: 550 - Enabled: true - Name: Vehicle - - Class: rviz_plugins/MrmSummaryOverlayDisplay - Enabled: false - Font Size: 10 - Left: 512 - Max Letter Num: 100 - Name: MRM Summary - Text Color: 25; 255; 240 - Top: 64 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /system/emergency/hazard_status - Value: false - Value height offset: 0 - Enabled: true - Name: System - - Class: rviz_common/Group - Displays: - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 28.71826171875 - Min Value: -7.4224700927734375 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 237 - Min Color: 211; 215; 207 - Min Intensity: 0 - Name: PointCloudMap - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/pointcloud_map - Use Fixed Frame: true - Use rainbow: false - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Lanelet2VectorMap - Namespaces: - bus_stop_area: true - center_lane_line: false - center_line_arrows: false - curbstone: true - lane_start_bound: false - lanelet direction: true - lanelet_id: false - left_lane_bound: true - no_parking_area: true - parking_lots: true - parking_space: true - partitions: true - right_lane_bound: true - road_lanelets: false - shoulder_center_lane_line: false - shoulder_center_line_arrows: true - shoulder_lane_start_bound: true - shoulder_left_lane_bound: true - shoulder_right_lane_bound: true - shoulder_road_lanelets: false - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/vector_map_marker - Value: true - Enabled: true - Name: Map - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - - Alpha: 0.9990000128746033 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: MeasurementRange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/lidar/crop_box_filter/crop_box_polygon - Value: false - Enabled: true - Name: LiDAR - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 233; 185; 110 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: false - Position: - Alpha: 0.20000000298023224 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Head Length: 0.699999988079071 - Head Radius: 1.2000000476837158 - Name: PoseWithCovariance - Shaft Length: 1 - Shaft Radius: 0.5 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/gnss/pose_with_covariance - Value: true - Enabled: false - Name: GNSS - Enabled: true - Name: Sensing - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 170; 255 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovInitial - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/initial_pose_with_covariance - Value: false - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 255; 0 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovAligned - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose_with_covariance - Value: false - - Buffer Size: 200 - Class: rviz_plugins::PoseHistory - Enabled: false - Line: - Alpha: 0.9990000128746033 - Color: 170; 255; 127 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /localization/util/downsample/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 85; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 85; 255; 127 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Aligned - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/points_aligned - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MonteCarloInitialPose - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/monte_carlo_initial_pose_marker - Value: true - Enabled: true - Name: NDT - - Class: rviz_common/Group - Displays: - - Buffer Size: 1000 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 0; 255; 255 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_twist_fusion_filter/pose - Value: true - Enabled: true - Name: EKF - Enabled: true - Name: Localization - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 15 - Min Value: -2 - Value: false - Axis: Z - Channel Name: z - Class: rviz_default_plugins/PointCloud2 - Color: 200; 200; 200 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15 - Min Color: 0; 0; 0 - Min Intensity: -5 - Name: NoGroundPointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/obstacle_segmentation/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Segmentation - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Detection - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_perception_rviz_plugin/TrackedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Dynamic Status: All - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: TrackedObjects - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/tracking/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Tracking - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_perception_rviz_plugin/PredictedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: PredictedObjects - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Maneuver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/prediction/maneuver - Value: false - Enabled: true - Name: Prediction - Enabled: true - Name: ObjectRecognition - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RecognitionResultOnImage - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light/debug/rois - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MapBasedDetectionResult - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers - Value: true - Enabled: true - Name: TrafficLight - - Class: rviz_common/Group - Displays: - - Alpha: 0.5 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map_updates - Use Timestamp: false - Value: true - Enabled: false - Name: OccupancyGrid - Enabled: true - Name: Perception - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: RouteArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/route_marker - Value: true - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.5 - Name: GoalPose - Shaft Length: 3 - Shaft Radius: 0.20000000298023224 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/echo_back_goal_pose - Value: true - - Background Alpha: 0.5 - Background Color: 23; 28; 31 - Class: autoware_mission_details_overlay_rviz_plugin/MissionDetailsDisplay - Enabled: true - Height: 100 - Name: MissionDetailsDisplay - Remaining Distance and Time Topic: /planning/mission_remaining_distance_time - Right: 10 - Text Color: 194; 194; 194 - Top: 10 - Value: true - Width: 170 - Enabled: true - Name: MissionPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: ScenarioTrajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/trajectory - Value: true - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.9990000128746033 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: Path - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/path - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.4000000059604645 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_AvoidanceByLC - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/avoidance_by_lane_change - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_Avoidance - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/static_obstacle_avoidance - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_LaneChangeRight - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/lane_change_right - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_LaneChangeLeft - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/lane_change_left - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_GoalPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/goal_planner - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_StartPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/start_planner - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_AvoidanceByLC - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/avoidance_by_lane_change - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: (old)PathChangeCandidate_LaneChange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/lane_change - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_LaneChangeRight - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/lane_change_right - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_LaneChangeLeft - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/lane_change_left - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_ExternalRequestLaneChangeRight - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/external_request_lane_change_right - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_ExternalRequestLaneChangeLeft - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/external_request_lane_change_left - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_Avoidance - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/static_obstacle_avoidance - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_StartPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/start_planner - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_GoalPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/goal_planner - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Bound - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StaticObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/static_obstacle_avoidance - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (AvoidanceByLC) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/avoidance_by_lane_change - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (LaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/lane_change_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (LaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/lane_change_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ExtLaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/external_request_lane_change_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ExtLaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/external_request_lane_change_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (GoalPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/goal_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StartPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/start_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (BlindSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Crosswalk) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Walkway) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/walkway - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (DetectionArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Intersection) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (MergeFromPrivateArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (NoStoppingArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (OcclusionSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StopLine) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (TrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (VirtualTrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (RunOut) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SpeedBump) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/speed_bump - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (NoDrivableLane) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_drivable_lane - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Arrow - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Crosswalk - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Blind Spot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: TrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: VirtualTrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StopLine - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: DetectionArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: OcclusionSpot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: NoStoppingArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: RunOut - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StaticObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/static_obstacle_avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LeftLaneChange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_left - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: RightLaneChange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_right - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneFollowing - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_following - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: GoalPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/goal_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StartPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SideShift - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/side_shift - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SpeedBump - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/speed_bump - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: DynamicObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/dynamic_obstacle_avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: NoDrivableLane - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_drivable_lane - Value: false - Enabled: false - Name: DebugMarker - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (StaticObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/static_obstacle_avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (AvoidanceByLC) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/avoidance_by_lane_change - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (LaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/lane_change_left - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (LaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/lane_change_right - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (ExtLaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/external_request_lane_change_left - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (ExtLaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/external_request_lane_change_right - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (GoalPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/goal_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (StartPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/start_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (DynamicObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/dynamic_obstacle_avoidance - Value: false - Enabled: false - Name: InfoMarker - Enabled: true - Name: BehaviorPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: false - Name: Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/trajectory - Value: false - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.9990000128746033 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleStop) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SurroundObstacle) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleCruise) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (MotionVelocitySmoother) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/velocity_smoother/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (OutOfLane) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane/virtual_walls - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleVelocityLimiter) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_velocity_limiter/virtual_walls - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (DynamicObstacleStop) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/dynamic_obstacle_stop/virtual_walls - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SurroundObstacleCheck - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Footprint - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 239; 41; 41 - Enabled: false - Name: FootprintOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 10; 21; 255 - Enabled: false - Name: FootprintRecoverOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset - Value: false - Enabled: false - Name: SurroundObstacleChecker - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: ObstacleStop - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: CruiseVirtualWall - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SlowDownVirtualWall - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/slow_down/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DebugMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/marker - Value: true - Enabled: false - Name: ObstacleCruise - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: ObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: OutOfLane - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane/debug_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ObstacleVelocityLimiter - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_velocity_limiter/debug_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DynamicObstacleStop - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/dynamic_obstacle_stop/debug_markers - Value: true - Enabled: false - Name: MotionVelocityPlanner - Enabled: false - Name: DebugMarker - Enabled: true - Name: MotionPlanning - Enabled: true - Name: LaneDriving - - Class: rviz_common/Group - Displays: - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: false - Name: Costmap - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates - Use Timestamp: false - Value: false - - Alpha: 0.9990000128746033 - Arrow Length: 0.30000001192092896 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 255; 25; 0 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PartialPoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (3D) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array - Value: true - - Alpha: 0.9990000128746033 - Arrow Length: 0.5 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 0; 0; 255 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array - Value: true - Enabled: true - Name: Parking - - Class: rviz_plugins/PoseWithUuidStamped - Enabled: true - Length: 1.5 - Name: ModifiedGoal - Radius: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/modified_goal - UUID: - Scale: 0.30000001192092896 - Value: false - Value: true - Enabled: true - Name: ScenarioPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: PlanningErrorMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker - Value: true - Enabled: false - Name: Diagnostic - Enabled: true - Name: Planning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: Predicted Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/controller_node_exe/debug/predicted_trajectory_in_frenet_coordinate - Value: true - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Constant Width: true - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 0.05000000074505806 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: false - Name: Resampled Reference Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/controller_node_exe/debug/resampled_reference_trajectory - Value: false - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Constant Width: true - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: false - Width: 0.20000000298023224 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: true - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (AEB) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/autonomous_emergency_braking/virtual_wall - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Stop Reason - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/longitudinal/stop_reason - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/MPC - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/mpc_follower/debug/markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/PurePursuit - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/controller_node_exe/debug/markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/AEB - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/autonomous_emergency_braking/debug/markers - Value: false - Enabled: true - Name: Control - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: ~ - Enabled: true - Name: Map - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/Camera - Enabled: true - Far Plane Distance: 100 - Image Rendering: background and overlay - Name: PointcloudOnCamera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera6/image_raw - Value: true - Visibility: - Control: - Debug/AEB: true - Debug/MPC: true - Debug/PurePursuit: true - Predicted Trajectory: true - Resampled Reference Trajectory: true - Stop Reason: true - Value: false - VirtualWall: - Value: true - VirtualWall (AEB): true - Debug: - Control: true - Localization: - "": true - Value: true - Map: true - Perception: - "": true - Value: true - Planning: - "": - "": true - Value: true - Value: true - Sensing: - ConcatenatePointCloud: true - RadarRawObjects(white): true - Value: true - Value: true - Localization: - EKF: - PoseHistory: true - Value: true - NDT: - Aligned: true - Initial: true - MonteCarloInitialPose: true - PoseHistory: true - PoseWithCovAligned: true - PoseWithCovInitial: true - Value: true - Value: false - Map: - Lanelet2VectorMap: true - PointCloudMap: true - Value: false - Perception: - ObjectRecognition: - Detection: - DetectedObjects: true - Value: true - Prediction: - Maneuver: true - PredictedObjects: true - Value: true - Tracking: - TrackedObjects: true - Value: true - Value: true - OccupancyGrid: - Map: true - Value: true - Segmentation: - NoGroundPointCloud: true - Value: true - TrafficLight: - MapBasedDetectionResult: true - RecognitionResultOnImage: true - Value: true - Value: false - Planning: - Diagnostic: - PlanningErrorMarker: true - Value: true - MissionPlanning: - GoalPose: true - MissionDetailsDisplay: true - RouteArea: true - Value: true - ScenarioPlanning: - LaneDriving: - BehaviorPlanning: - (old)PathChangeCandidate_LaneChange: true - Bound: true - DebugMarker: - Arrow: true - Blind Spot: true - Crosswalk: true - DetectionArea: true - DynamicObstacleAvoidance: true - GoalPlanner: true - Intersection: true - LaneFollowing: true - LeftLaneChange: true - NoDrivableLane: true - NoStoppingArea: true - OcclusionSpot: true - RightLaneChange: true - RunOut: true - SideShift: true - SpeedBump: true - StartPlanner: true - StaticObstacleAvoidance: true - StopLine: true - TrafficLight: true - Value: true - VirtualTrafficLight: true - InfoMarker: - Info (AvoidanceByLC): true - Info (DynamicObstacleAvoidance): true - Info (ExtLaneChangeLeft): true - Info (ExtLaneChangeRight): true - Info (GoalPlanner): true - Info (LaneChangeLeft): true - Info (LaneChangeRight): true - Info (StartPlanner): true - Info (StaticObstacleAvoidance): true - Value: true - Path: true - PathChangeCandidate_Avoidance: true - PathChangeCandidate_AvoidanceByLC: true - PathChangeCandidate_ExternalRequestLaneChangeLeft: true - PathChangeCandidate_ExternalRequestLaneChangeRight: true - PathChangeCandidate_GoalPlanner: true - PathChangeCandidate_LaneChangeLeft: true - PathChangeCandidate_LaneChangeRight: true - PathChangeCandidate_StartPlanner: true - PathReference_Avoidance: true - PathReference_AvoidanceByLC: true - PathReference_GoalPlanner: true - PathReference_LaneChangeLeft: true - PathReference_LaneChangeRight: true - PathReference_StartPlanner: true - Value: true - VirtualWall: - Value: true - VirtualWall (AvoidanceByLC): true - VirtualWall (BlindSpot): true - VirtualWall (Crosswalk): true - VirtualWall (DetectionArea): true - VirtualWall (ExtLaneChangeLeft): true - VirtualWall (ExtLaneChangeRight): true - VirtualWall (GoalPlanner): true - VirtualWall (Intersection): true - VirtualWall (LaneChangeLeft): true - VirtualWall (LaneChangeRight): true - VirtualWall (MergeFromPrivateArea): true - VirtualWall (NoDrivableLane): true - VirtualWall (NoStoppingArea): true - VirtualWall (OcclusionSpot): true - VirtualWall (RunOut): true - VirtualWall (SpeedBump): true - VirtualWall (StartPlanner): true - VirtualWall (StaticObstacleAvoidance): true - VirtualWall (StopLine): true - VirtualWall (TrafficLight): true - VirtualWall (VirtualTrafficLight): true - VirtualWall (Walkway): true - MotionPlanning: - DebugMarker: - MotionVelocityPlanner: - DynamicObstacleStop: true - ObstacleVelocityLimiter: true - OutOfLane: true - Value: true - ObstacleAvoidance: true - ObstacleCruise: - CruiseVirtualWall: true - DebugMarker: true - SlowDownVirtualWall: true - Value: true - ObstacleStop: true - SurroundObstacleChecker: - Footprint: true - FootprintOffset: true - FootprintRecoverOffset: true - SurroundObstacleCheck: true - Value: true - Value: true - Trajectory: true - Value: true - VirtualWall: - Value: true - VirtualWall (DynamicObstacleStop): true - VirtualWall (MotionVelocitySmoother): true - VirtualWall (ObstacleAvoidance): true - VirtualWall (ObstacleCruise): true - VirtualWall (ObstacleStop): true - VirtualWall (ObstacleVelocityLimiter): true - VirtualWall (OutOfLane): true - VirtualWall (SurroundObstacle): true - Value: true - ModifiedGoal: true - Parking: - Costmap: true - PartialPoseArray: true - PoseArray: true - Value: true - ScenarioTrajectory: true - Value: true - Value: false - Sensing: - GNSS: - PoseWithCovariance: true - Value: true - LiDAR: - ConcatenatePointCloud: true - MeasurementRange: true - Value: true - Value: true - System: - Grid: true - MRM Summary: true - TF: true - Value: false - Vehicle: - PolarGridDisplay: true - SignalDisplay: true - Value: true - VehicleModel: true - Value: true - Zoom Factor: 1 - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - - BUS: - Alpha: 0.5 - Color: 255; 255; 255 - CAR: - Alpha: 0.5 - Color: 255; 255; 255 - CYCLIST: - Alpha: 0.5 - Color: 255; 255; 255 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.5 - Color: 255; 255; 255 - Name: RadarRawObjects(white) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.5 - Color: 255; 255; 255 - Polygon Type: 3d - TRAILER: - Alpha: 0.5 - Color: 255; 255; 255 - TRUCK: - Alpha: 0.5 - Color: 255; 255; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/radar/detected_objects - UNKNOWN: - Alpha: 0.5 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Sensing - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 85; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 85; 255; 127 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: NDT pointclouds - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/points_aligned - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: NDTLoadedPCDMap - Position Transformer: "" - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/debug/loaded_pointcloud_map - Use Fixed Frame: true - Use rainbow: true - Value: true - - Buffer Size: 200 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 170; 255; 127 - Value: true - Width: 0.10000000149011612 - Name: NDTPoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose - Value: true - - Buffer Size: 1000 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 0; 255; 255 - Value: true - Width: 0.10000000149011612 - Name: EKFPoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_twist_fusion_filter/pose - Value: true - Enabled: true - Name: Localization - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Name: Centerpoint(red1) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/centerpoint/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Name: CenterpointROIFusion(red2) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/centerpoint/roi_fusion/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - CAR: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Name: CenterpointValidator(red3) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - TRUCK: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/centerpoint/validation/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - CAR: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Name: PointPainting(light_green1) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - TRUCK: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/pointpainting/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - CAR: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Name: PointPaintingROIFusion(light_green2) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - TRUCK: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/pointpainting/roi_fusion/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - CAR: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Name: PointPaintingValidator(light_green3) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - TRUCK: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/pointpainting/validation/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Name: DetectionByTracker(orange) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/detection_by_tracker/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - CAR: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Name: CameraLidarFusion(purple) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - TRUCK: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/clustering/camera_lidar_fusion/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Name: RadarFarObjects(white) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/radar/far_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Name: Detection(yellow) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - CAR: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Class: autoware_perception_rviz_plugin/TrackedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Dynamic Status: All - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Name: Tracking(green) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - TRUCK: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/tracking/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Class: autoware_perception_rviz_plugin/PredictedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Name: Prediction(light_blue) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Value: true - Visualization Type: Normal - Enabled: true - Name: Perception - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: LaneChangeLeft - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/lane_change_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: LaneChangeRight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/lane_change_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceLeft - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/static_obstacle_avoidance_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceRight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/static_obstacle_avoidance_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceByLCLeft - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/avoidance_by_lc_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceByLCRight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/avoidance_by_lc_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: StartPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/start_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: GoalPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/goal_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Crosswalk - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/crosswalk - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Intersection - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/intersection - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: BlindSpot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/blind_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: TrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DetectionArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/detection_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: NoStoppingArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/no_stopping_area - Value: true - Enabled: true - Name: Objects Of Interest - Enabled: true - Name: Planning - - Class: rviz_common/Group - Displays: ~ - Enabled: true - Name: Control - Enabled: false - Name: Debug - Enabled: true - Global Options: - Background Color: 15; 20; 23 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/goal - - Class: tier4_adapi_rviz_plugins::RouteTool - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rviz/routing/rough_goal - - Acceleration: 0 - Class: rviz_plugins/PedestrianInitialPoseTool - Interactive: false - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 1 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/CarInitialPoseTool - H vehicle height: 2 - Interactive: false - L vehicle length: 4 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 3 - W vehicle width: 1.7999999523162842 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/BusInitialPoseTool - H vehicle height: 3.5 - Interactive: false - L vehicle length: 10.5 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - W vehicle width: 2.5 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: /planning/mission_planning/checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 - Saved: - - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 18 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.009999999776482582 - Pitch: 0.20000000298023224 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 3.141592025756836 - - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: TopDownOrtho - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 -Window Geometry: - AutowareDateTimePanel: - collapsed: false - AutowareStatePanel: - collapsed: false - Displays: - collapsed: false - Height: 939 - Hide Left Dock: false - Hide Right Dock: false - PointcloudOnCamera: - collapsed: false - QMainWindow State: 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 - RecognitionResultOnImage: - collapsed: false - Selection: - collapsed: false - SimulatedClockPanel: - collapsed: false - Tool Properties: - collapsed: false - TrafficLightPublishPanel: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 34 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 080e8f0e6abc3..b828c8d07ac49 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 @@ -25,10 +25,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -139,7 +139,8 @@ 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_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; // system parameters int64_t diag_error_count_threshold_ = 0; diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 0c16fae065939..e0b9c7135e5f0 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -48,8 +48,8 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) pub_markers_ = create_publisher("~/output/markers", 1); - pub_processing_time_ = - this->create_publisher("~/debug/processing_time_ms", 1); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); debug_pose_publisher_ = std::make_shared(this); @@ -181,7 +181,7 @@ void ControlValidator::publish_debug_info() debug_pose_publisher_->publish(); // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_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); diff --git a/control/autoware_lane_departure_checker/CMakeLists.txt b/control/autoware_lane_departure_checker/CMakeLists.txt index 199195fc88b08..19046a09922f0 100644 --- a/control/autoware_lane_departure_checker/CMakeLists.txt +++ b/control/autoware_lane_departure_checker/CMakeLists.txt @@ -14,6 +14,7 @@ ament_auto_add_library(autoware_lane_departure_checker SHARED src/lane_departure_checker_node/lane_departure_checker.cpp src/lane_departure_checker_node/lane_departure_checker_node.cpp src/lane_departure_checker_node/utils.cpp + src/lane_departure_checker_node/parameters.cpp ) rclcpp_components_register_node(${PROJECT_NAME} diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index 1ac984394a25e..101338551cbf3 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -15,15 +15,12 @@ #ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ #define AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ -#include -#include +#include "autoware/lane_departure_checker/parameters.hpp" + #include #include #include -#include -#include -#include #include #include #include @@ -48,81 +45,33 @@ namespace autoware::lane_departure_checker { -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::PoseDeviation; using autoware::universe_utils::Segment2d; -using autoware_planning_msgs::msg::LaneletRoute; -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_planning_msgs::msg::PathWithLaneId; -using TrajectoryPoints = std::vector; typedef boost::geometry::index::rtree> SegmentRtree; -struct Param -{ - double footprint_margin_scale{0.0}; - double footprint_extra_margin{0.0}; - double resample_interval{0.0}; - double max_deceleration{0.0}; - double delay_time{0.0}; - double max_lateral_deviation{0.0}; - double max_longitudinal_deviation{0.0}; - double max_yaw_deviation_deg{0.0}; - double min_braking_distance{0.0}; - // nearest search to ego - double ego_nearest_dist_threshold{0.0}; - double ego_nearest_yaw_threshold{0.0}; -}; - -struct Input -{ - nav_msgs::msg::Odometry::ConstSharedPtr current_odom{}; - lanelet::LaneletMapPtr lanelet_map{}; - LaneletRoute::ConstSharedPtr route{}; - lanelet::ConstLanelets route_lanelets{}; - lanelet::ConstLanelets shoulder_lanelets{}; - Trajectory::ConstSharedPtr reference_trajectory{}; - Trajectory::ConstSharedPtr predicted_trajectory{}; - std::vector boundary_types_to_detect{}; -}; - -struct Output -{ - std::map processing_time_map{}; - bool will_leave_lane{}; - bool is_out_of_lane{}; - bool will_cross_boundary{}; - PoseDeviation trajectory_deviation{}; - lanelet::ConstLanelets candidate_lanelets{}; - TrajectoryPoints resampled_trajectory{}; - std::vector vehicle_footprints{}; - std::vector vehicle_passing_areas{}; -}; - class LaneDepartureChecker { public: - LaneDepartureChecker( + explicit LaneDepartureChecker( std::shared_ptr time_keeper = std::make_shared()) : time_keeper_(time_keeper) { } - Output update(const Input & input); - void setParam(const Param & param, const autoware::vehicle_info_utils::VehicleInfo vehicle_info) + LaneDepartureChecker( + const Param & param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + std::shared_ptr time_keeper = + std::make_shared()) + : param_(param), + vehicle_info_ptr_(std::make_shared(vehicle_info)), + time_keeper_(time_keeper) { - param_ = param; - vehicle_info_ptr_ = std::make_shared(vehicle_info); } + Output update(const Input & input); void setParam(const Param & param) { param_ = param; } - void setVehicleInfo(const autoware::vehicle_info_utils::VehicleInfo vehicle_info) - { - vehicle_info_ptr_ = std::make_shared(vehicle_info); - } - bool checkPathWillLeaveLane( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const; diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp index 7e2229d7b5754..7aaf4816708bd 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ #include "autoware/lane_departure_checker/lane_departure_checker.hpp" +#include "autoware/lane_departure_checker/parameters.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include @@ -24,6 +25,7 @@ #include #include +#include #include #include #include @@ -31,7 +33,6 @@ #include #include #include -#include #include #include @@ -47,21 +48,6 @@ namespace autoware::lane_departure_checker { using autoware_map_msgs::msg::LaneletMapBin; -struct NodeParam -{ - bool will_out_of_lane_checker; - bool out_of_lane_checker; - bool boundary_departure_checker; - - double update_rate; - bool visualize_lanelet; - bool include_right_lanes; - bool include_left_lanes; - bool include_opposite_lanes; - bool include_conflicting_lanes; - std::vector boundary_types_to_detect; -}; - class LaneDepartureCheckerNode : public rclcpp::Node { public: @@ -115,7 +101,8 @@ class LaneDepartureCheckerNode : public rclcpp::Node autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"}; autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ this, "~/debug/processing_time_ms_diag"}; - rclcpp::Publisher::SharedPtr processing_time_publisher_; + rclcpp::Publisher::SharedPtr + processing_time_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp new file mode 100644 index 0000000000000..425b032af425f --- /dev/null +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp @@ -0,0 +1,101 @@ +// 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__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_ +#define AUTOWARE__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_ + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace autoware::lane_departure_checker +{ +using autoware::universe_utils::PoseDeviation; +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware::universe_utils::LinearRing2d; + +struct Param +{ + static Param init(rclcpp::Node & node); + double footprint_margin_scale{}; + double footprint_extra_margin{}; + double resample_interval{}; + double max_deceleration{}; + double delay_time{}; + double max_lateral_deviation{}; + double max_longitudinal_deviation{}; + double max_yaw_deviation_deg{}; + double min_braking_distance{}; + // nearest search to ego + double ego_nearest_dist_threshold{}; + double ego_nearest_yaw_threshold{}; +}; + +struct NodeParam +{ + static NodeParam init(rclcpp::Node & node); + bool will_out_of_lane_checker{}; + bool out_of_lane_checker{}; + bool boundary_departure_checker{}; + + double update_rate{}; + bool visualize_lanelet{}; + bool include_right_lanes{}; + bool include_left_lanes{}; + bool include_opposite_lanes{}; + bool include_conflicting_lanes{}; + std::vector boundary_types_to_detect{}; +}; + +struct Input +{ + nav_msgs::msg::Odometry::ConstSharedPtr current_odom{}; + lanelet::LaneletMapPtr lanelet_map{}; + LaneletRoute::ConstSharedPtr route{}; + lanelet::ConstLanelets route_lanelets{}; + lanelet::ConstLanelets shoulder_lanelets{}; + Trajectory::ConstSharedPtr reference_trajectory{}; + Trajectory::ConstSharedPtr predicted_trajectory{}; + std::vector boundary_types_to_detect{}; +}; + +struct Output +{ + std::map processing_time_map{}; + bool will_leave_lane{}; + bool is_out_of_lane{}; + bool will_cross_boundary{}; + PoseDeviation trajectory_deviation{}; + lanelet::ConstLanelets candidate_lanelets{}; + TrajectoryPoints resampled_trajectory{}; + std::vector vehicle_footprints{}; + std::vector vehicle_passing_areas{}; +}; +} // namespace autoware::lane_departure_checker + +#endif // AUTOWARE__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_ diff --git a/control/autoware_lane_departure_checker/package.xml b/control/autoware_lane_departure_checker/package.xml index 2689b4a4dbcb7..97ad5eaf82e00 100644 --- a/control/autoware_lane_departure_checker/package.xml +++ b/control/autoware_lane_departure_checker/package.xml @@ -14,6 +14,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_internal_debug_msgs autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils @@ -32,7 +33,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 366b84a1f6131..32384b7e6c7dd 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -126,52 +126,24 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o : Node("lane_departure_checker_node", options) { using std::placeholders::_1; - - // Enable feature - node_param_.will_out_of_lane_checker = declare_parameter("will_out_of_lane_checker"); - node_param_.out_of_lane_checker = declare_parameter("out_of_lane_checker"); - node_param_.boundary_departure_checker = declare_parameter("boundary_departure_checker"); - - // Node Parameter - node_param_.update_rate = declare_parameter("update_rate"); - node_param_.visualize_lanelet = declare_parameter("visualize_lanelet"); - node_param_.include_right_lanes = declare_parameter("include_right_lanes"); - node_param_.include_left_lanes = declare_parameter("include_left_lanes"); - node_param_.include_opposite_lanes = declare_parameter("include_opposite_lanes"); - node_param_.include_conflicting_lanes = declare_parameter("include_conflicting_lanes"); - - // Boundary_departure_checker - node_param_.boundary_types_to_detect = - declare_parameter>("boundary_types_to_detect"); + node_param_ = NodeParam::init(*this); + param_ = Param::init(*this); // Vehicle Info const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); vehicle_length_m_ = vehicle_info.vehicle_length_m; - // Core Parameter - param_.footprint_margin_scale = declare_parameter("footprint_margin_scale"); - param_.footprint_extra_margin = declare_parameter("footprint_extra_margin"); - param_.resample_interval = declare_parameter("resample_interval"); - param_.max_deceleration = declare_parameter("max_deceleration"); - param_.delay_time = declare_parameter("delay_time"); - param_.max_lateral_deviation = declare_parameter("max_lateral_deviation"); - param_.max_longitudinal_deviation = declare_parameter("max_longitudinal_deviation"); - param_.max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg"); - param_.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); - param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - param_.min_braking_distance = declare_parameter("min_braking_distance"); - // Parameter Callback set_param_res_ = add_on_set_parameters_callback(std::bind(&LaneDepartureCheckerNode::onParameter, this, _1)); // Core - lane_departure_checker_ = std::make_unique(); - lane_departure_checker_->setParam(param_, vehicle_info); + lane_departure_checker_ = std::make_unique(param_, vehicle_info); // Publisher processing_time_publisher_ = - this->create_publisher("~/debug/processing_time_ms", 1); + this->create_publisher( + "~/debug/processing_time_ms", 1); // Nothing // Diagnostic Updater @@ -342,10 +314,11 @@ void LaneDepartureCheckerNode::onTimer() { const auto & deviation = output_.trajectory_deviation; - debug_publisher_.publish( + debug_publisher_.publish( "deviation/lateral", deviation.lateral); - debug_publisher_.publish("deviation/yaw", deviation.yaw); - debug_publisher_.publish( + debug_publisher_.publish( + "deviation/yaw", deviation.yaw); + debug_publisher_.publish( "deviation/yaw_deg", rad2deg(deviation.yaw)); } processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true); @@ -361,7 +334,7 @@ void LaneDepartureCheckerNode::onTimer() processing_time_map["Total"] = stop_watch.toc("Total"); processing_diag_publisher_.publish(processing_time_map); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = processing_time_map["Total"]; processing_time_publisher_->publish(processing_time_msg); diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp new file mode 100644 index 0000000000000..f3aa23275e35c --- /dev/null +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp @@ -0,0 +1,59 @@ +// 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/lane_departure_checker/parameters.hpp" + +#include +#include + +#include + +namespace autoware::lane_departure_checker +{ +using autoware::universe_utils::getOrDeclareParameter; + +Param Param::init(rclcpp::Node & node) +{ + Param p; + p.footprint_margin_scale = getOrDeclareParameter(node, "footprint_margin_scale"); + p.footprint_extra_margin = getOrDeclareParameter(node, "footprint_extra_margin"); + p.resample_interval = getOrDeclareParameter(node, "resample_interval"); + p.max_deceleration = getOrDeclareParameter(node, "max_deceleration"); + p.delay_time = getOrDeclareParameter(node, "delay_time"); + p.max_lateral_deviation = getOrDeclareParameter(node, "max_lateral_deviation"); + p.max_longitudinal_deviation = getOrDeclareParameter(node, "max_longitudinal_deviation"); + p.max_yaw_deviation_deg = getOrDeclareParameter(node, "max_yaw_deviation_deg"); + p.min_braking_distance = getOrDeclareParameter(node, "min_braking_distance"); + p.ego_nearest_dist_threshold = getOrDeclareParameter(node, "ego_nearest_dist_threshold"); + p.ego_nearest_yaw_threshold = getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); + return p; +} + +NodeParam NodeParam::init(rclcpp::Node & node) +{ + NodeParam p; + p.will_out_of_lane_checker = getOrDeclareParameter(node, "will_out_of_lane_checker"); + p.out_of_lane_checker = getOrDeclareParameter(node, "out_of_lane_checker"); + p.boundary_departure_checker = getOrDeclareParameter(node, "boundary_departure_checker"); + p.update_rate = getOrDeclareParameter(node, "update_rate"); + p.visualize_lanelet = getOrDeclareParameter(node, "visualize_lanelet"); + p.include_right_lanes = getOrDeclareParameter(node, "include_right_lanes"); + p.include_left_lanes = getOrDeclareParameter(node, "include_left_lanes"); + p.include_opposite_lanes = getOrDeclareParameter(node, "include_opposite_lanes"); + p.include_conflicting_lanes = getOrDeclareParameter(node, "include_conflicting_lanes"); + p.boundary_types_to_detect = + getOrDeclareParameter>(node, "boundary_types_to_detect"); + return p; +} +} // namespace autoware::lane_departure_checker diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 4c8d5df2c22a7..166dfa1814562 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -24,11 +24,11 @@ #include "rclcpp/rclcpp.hpp" #include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include #include @@ -41,11 +41,11 @@ namespace autoware::motion::control::mpc_lateral_controller using autoware::motion::control::trajectory_follower::LateralHorizon; using autoware_control_msgs::msg::Lateral; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; using autoware_planning_msgs::msg::Trajectory; using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; using Eigen::MatrixXd; using Eigen::VectorXd; diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp index d7442f64b028a..0f3004775e5bd 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -26,13 +26,13 @@ #include #include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" #include #include @@ -45,11 +45,11 @@ namespace autoware::motion::control::mpc_lateral_controller namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; using autoware_control_msgs::msg::Lateral; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_planning_msgs::msg::Trajectory; using autoware_vehicle_msgs::msg::SteeringReport; using nav_msgs::msg::Odometry; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_debug_msgs::msg::Float32Stamped; using trajectory_follower::LateralHorizon; class MpcLateralController : public trajectory_follower::LateralControllerBase diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index b450af05ea0e1..719d11ef7948b 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -19,6 +19,7 @@ autoware_cmake autoware_control_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_osqp_interface @@ -36,7 +37,6 @@ std_msgs tf2 tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp index 9c30369305f6e..0a34302f8f60c 100644 --- a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp @@ -24,11 +24,11 @@ #include #include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #ifdef ROS_DISTRO_GALACTIC #include "tf2_geometry_msgs/tf2_geometry_msgs.h" @@ -45,12 +45,12 @@ namespace autoware::motion::control::mpc_lateral_controller using autoware::motion::control::trajectory_follower::LateralHorizon; using autoware_control_msgs::msg::Lateral; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; TrajectoryPoint makePoint(const double x, const double y, const float vx) { diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 1d03d1b339af6..db5fa09b5eeee 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -34,11 +34,11 @@ #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_control_msgs/msg/longitudinal.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "visualization_msgs/msg/marker.hpp" #include @@ -98,8 +98,10 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro rclcpp::Clock::SharedPtr clock_; rclcpp::Logger logger_; // ros variables - rclcpp::Publisher::SharedPtr m_pub_slope; - rclcpp::Publisher::SharedPtr m_pub_debug; + rclcpp::Publisher::SharedPtr + m_pub_slope; + rclcpp::Publisher::SharedPtr + m_pub_debug; rclcpp::Publisher::SharedPtr m_pub_virtual_wall_marker; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; diff --git a/control/autoware_pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml index 67e95a9d4c0ac..127ce8e76d69d 100644 --- a/control/autoware_pid_longitudinal_controller/package.xml +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_planning_msgs @@ -37,7 +38,6 @@ std_msgs tf2 tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index ec95ce656fa6f..c87e936de3e40 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -214,9 +214,9 @@ PidLongitudinalController::PidLongitudinalController( : node.declare_parameter("ego_nearest_yaw_threshold"); // [rad] // subscriber, publisher - m_pub_slope = node.create_publisher( + m_pub_slope = node.create_publisher( "~/output/slope_angle", rclcpp::QoS{1}); - m_pub_debug = node.create_publisher( + m_pub_debug = node.create_publisher( "~/output/longitudinal_diagnostic", rclcpp::QoS{1}); m_pub_virtual_wall_marker = node.create_publisher("~/virtual_wall", 1); @@ -931,7 +931,7 @@ void PidLongitudinalController::publishDebugData( m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PUBLISHED, ctrl_cmd.acc); // publish debug values - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; debug_msg.stamp = clock_->now(); for (const auto & v : m_debug_values.getValues()) { debug_msg.data.push_back(static_cast(v)); @@ -939,7 +939,7 @@ void PidLongitudinalController::publishDebugData( m_pub_debug->publish(debug_msg); // slope angle - tier4_debug_msgs::msg::Float32MultiArrayStamped slope_msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped slope_msg{}; slope_msg.stamp = clock_->now(); slope_msg.data.push_back( static_cast(control_data.slope_angle)); diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml index e7b00e4b19cdc..195c077835910 100644 --- a/control/autoware_trajectory_follower_base/package.xml +++ b/control/autoware_trajectory_follower_base/package.xml @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_osqp_interface @@ -37,7 +38,6 @@ std_msgs tf2 tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 9c2ef69e0137b..f2630a7d47f62 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -25,6 +25,7 @@ builtin_interfaces autoware_control_msgs + autoware_internal_debug_msgs autoware_motion_utils autoware_planning_msgs autoware_signal_processing @@ -41,7 +42,6 @@ tf2 tf2_eigen tf2_ros - tier4_debug_msgs autoware_global_parameter_loader builtin_interfaces rosidl_default_runtime diff --git a/control/control_performance_analysis/scripts/control_performance_plot.py b/control/control_performance_analysis/scripts/control_performance_plot.py index c5e5cabd5b059..d4a6038465e12 100644 --- a/control/control_performance_analysis/scripts/control_performance_plot.py +++ b/control/control_performance_analysis/scripts/control_performance_plot.py @@ -17,13 +17,13 @@ import argparse import math +from autoware_internal_debug_msgs.msg import BoolStamped from control_performance_analysis.msg import DrivingMonitorStamped from control_performance_analysis.msg import ErrorStamped import matplotlib.pyplot as plt from nav_msgs.msg import Odometry import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import BoolStamped parser = argparse.ArgumentParser() parser.add_argument("-i", "--interval", help="interval distance to plot") diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index c510b2ea46779..9606dea577bd0 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -25,9 +25,9 @@ #include #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include #include #include -#include #include #include @@ -85,7 +85,8 @@ class ControlEvaluatorNode : public rclcpp::Node LaneletMapBin, autoware::universe_utils::polling_policy::Newest> vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; - rclcpp::Publisher::SharedPtr processing_time_pub_; + rclcpp::Publisher::SharedPtr + processing_time_pub_; rclcpp::Publisher::SharedPtr metrics_pub_; // update Route Handler diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index b0db41b0fdc73..79e89423af6dd 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -34,8 +34,8 @@ ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_opti using std::placeholders::_1; // Publisher - processing_time_pub_ = - this->create_publisher("~/debug/processing_time_ms", 1); + processing_time_pub_ = this->create_publisher( + "~/debug/processing_time_ms", 1); metrics_pub_ = create_publisher("~/metrics", 1); // Parameters @@ -288,7 +288,7 @@ void ControlEvaluatorNode::onTimer() metrics_msg_ = MetricArrayMsg{}; // Publish processing time - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); processing_time_pub_->publish(processing_time_msg); diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 9c268206846d9..8c0b49ce2dd26 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -31,8 +31,8 @@ #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" +#include #include -#include #include #include @@ -145,7 +145,8 @@ class PlanningEvaluatorNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ this, "~/input/acceleration"}; - rclcpp::Publisher::SharedPtr processing_time_pub_; + rclcpp::Publisher::SharedPtr + processing_time_pub_; rclcpp::Publisher::SharedPtr metrics_pub_; std::shared_ptr transform_listener_{nullptr}; std::unique_ptr tf_buffer_; diff --git a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp index 82ba86c65d6af..9b5959948f8cb 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -58,7 +58,8 @@ Accumulator calcLocalLateralTrajectoryDisplacement( autoware::motion_utils::calcLateralOffset(prev.points, ego_pose.position); const auto traj_lateral_deviation = autoware::motion_utils::calcLateralOffset(traj.points, ego_pose.position); - const auto lateral_trajectory_displacement = traj_lateral_deviation - prev_lateral_deviation; + const auto lateral_trajectory_displacement = + std::abs(traj_lateral_deviation - prev_lateral_deviation); stat.add(lateral_trajectory_displacement); return stat; } diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 5889f15e48390..53319ffa4c1fd 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -59,8 +59,8 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op output_metrics_ = declare_parameter("output_metrics"); ego_frame_str_ = declare_parameter("ego_frame"); - processing_time_pub_ = - this->create_publisher("~/debug/processing_time_ms", 1); + processing_time_pub_ = this->create_publisher( + "~/debug/processing_time_ms", 1); // List of metrics to calculate and publish metrics_pub_ = create_publisher("~/metrics", 1); @@ -282,7 +282,7 @@ void PlanningEvaluatorNode::onTimer() metrics_msg_ = MetricArrayMsg{}; // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); processing_time_pub_->publish(processing_time_msg); diff --git a/localization/autoware_ekf_localizer/README.md b/localization/autoware_ekf_localizer/README.md index 802bf7dbb16c3..a46ea7181315f 100644 --- a/localization/autoware_ekf_localizer/README.md +++ b/localization/autoware_ekf_localizer/README.md @@ -44,17 +44,17 @@ This package includes the following features: ### Published Topics -| Name | Type | Description | -| --------------------------------- | ------------------------------------------------ | ----------------------------------------------------- | -| `ekf_odom` | `nav_msgs::msg::Odometry` | Estimated odometry. | -| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose. | -| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance. | -| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose including the yaw bias | -| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance including the yaw bias | -| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. | -| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. | -| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. | -| `debug/processing_time_ms` | `tier4_debug_msgs::msg::Float64Stamped` | The processing time [ms]. | +| Name | Type | Description | +| --------------------------------- | --------------------------------------------------- | ----------------------------------------------------- | +| `ekf_odom` | `nav_msgs::msg::Odometry` | Estimated odometry. | +| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose. | +| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance. | +| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose including the yaw bias | +| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance including the yaw bias | +| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. | +| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. | +| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. | +| `debug/processing_time_ms` | `autoware_internal_debug_msgs::msg::Float64Stamped` | The processing time [ms]. | ### Published TF diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp index 0561e250298ac..84018c043cc14 100644 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp +++ b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp @@ -25,6 +25,8 @@ #include #include +#include +#include #include #include #include @@ -34,8 +36,6 @@ #include #include #include -#include -#include #include #include @@ -78,7 +78,7 @@ class EKFLocalizer : public rclcpp::Node //!< @brief ekf estimated twist with covariance publisher rclcpp::Publisher::SharedPtr pub_twist_cov_; //!< @brief ekf estimated yaw bias publisher - rclcpp::Publisher::SharedPtr pub_yaw_bias_; + rclcpp::Publisher::SharedPtr pub_yaw_bias_; //!< @brief ekf estimated yaw bias publisher rclcpp::Publisher::SharedPtr pub_biased_pose_; //!< @brief ekf estimated yaw bias publisher @@ -86,7 +86,8 @@ class EKFLocalizer : public rclcpp::Node //!< @brief diagnostics publisher rclcpp::Publisher::SharedPtr pub_diag_; //!< @brief processing_time publisher - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; //!< @brief initial pose subscriber rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief measurement pose with covariance subscriber diff --git a/localization/autoware_ekf_localizer/package.xml b/localization/autoware_ekf_localizer/package.xml index 8dc3cc9844c50..e0e37f59d1acb 100644 --- a/localization/autoware_ekf_localizer/package.xml +++ b/localization/autoware_ekf_localizer/package.xml @@ -22,6 +22,7 @@ eigen + autoware_internal_debug_msgs autoware_kalman_filter autoware_localization_util autoware_universe_utils @@ -34,7 +35,6 @@ std_srvs tf2 tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp index 11d7788adfade..5638a5416e6ab 100644 --- a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp +++ b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp @@ -70,13 +70,14 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) pub_twist_ = create_publisher("ekf_twist", 1); pub_twist_cov_ = create_publisher( "ekf_twist_with_covariance", 1); - pub_yaw_bias_ = create_publisher("estimated_yaw_bias", 1); + pub_yaw_bias_ = + create_publisher("estimated_yaw_bias", 1); pub_biased_pose_ = create_publisher("ekf_biased_pose", 1); pub_biased_pose_cov_ = create_publisher( "ekf_biased_pose_with_covariance", 1); pub_diag_ = this->create_publisher("/diagnostics", 10); - pub_processing_time_ = - create_publisher("debug/processing_time_ms", 1); + pub_processing_time_ = create_publisher( + "debug/processing_time_ms", 1); sub_initialpose_ = create_subscription( "initialpose", 1, std::bind(&EKFLocalizer::callback_initial_pose, this, _1)); sub_pose_with_cov_ = create_subscription( @@ -235,9 +236,10 @@ void EKFLocalizer::timer_callback() /* publish processing time */ const double elapsed_time = stop_watch_timer_cb_.toc(); - pub_processing_time_->publish(tier4_debug_msgs::build() - .stamp(current_time) - .data(elapsed_time)); + pub_processing_time_->publish( + autoware_internal_debug_msgs::build() + .stamp(current_time) + .data(elapsed_time)); } /* @@ -343,7 +345,7 @@ void EKFLocalizer::publish_estimate_result( pub_twist_cov_->publish(twist_cov); /* publish yaw bias */ - tier4_debug_msgs::msg::Float64Stamped yawb; + autoware_internal_debug_msgs::msg::Float64Stamped yawb; yawb.stamp = current_ekf_twist.header.stamp; yawb.data = ekf_module_->get_yaw_bias(); pub_yaw_bias_->publish(yawb); diff --git a/localization/autoware_localization_util/CHANGELOG.rst b/localization/autoware_localization_util/CHANGELOG.rst deleted file mode 100644 index be40e3ee8560e..0000000000000 --- a/localization/autoware_localization_util/CHANGELOG.rst +++ /dev/null @@ -1,51 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_localization_util -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - localization (`#9567 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(localization_util)!: prefix package and namespace with autoware (`#8922 `_) - add autoware prefix to localization_util -* Contributors: Masaki Baba, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/localization/autoware_localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt deleted file mode 100644 index de62633124f3d..0000000000000 --- a/localization/autoware_localization_util/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_localization_util) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/util_func.cpp - src/smart_pose_buffer.cpp - src/tree_structured_parzen_estimator.cpp - src/covariance_ellipse.cpp -) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_smart_pose_buffer - test/test_smart_pose_buffer.cpp - src/smart_pose_buffer.cpp - ) - - ament_auto_add_gtest(test_tpe - test/test_tpe.cpp - src/tree_structured_parzen_estimator.cpp - ) -endif() - -ament_auto_package( - INSTALL_TO_SHARE -) diff --git a/localization/autoware_localization_util/README.md b/localization/autoware_localization_util/README.md deleted file mode 100644 index f7fddd9eebf05..0000000000000 --- a/localization/autoware_localization_util/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# autoware_localization_util - -`autoware_localization_util` is a localization utility package. - -This package does not have a node, it is just a library. diff --git a/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp b/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp deleted file mode 100644 index abd0af46856b0..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright 2024 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ - -#include - -#include -#include - -namespace autoware::localization_util -{ - -struct Ellipse -{ - double long_radius; - double short_radius; - double yaw; - Eigen::Matrix2d P; - double size_lateral_direction; -}; - -Ellipse calculate_xy_ellipse( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale); - -visualization_msgs::msg::Marker create_ellipse_marker( - const Ellipse & ellipse, const std_msgs::msg::Header & header, - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); - -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp b/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp deleted file mode 100644 index bda6ff19f2867..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2021 TierIV -// -// 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__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ - -#include - -namespace autoware::localization_util -{ -using Matrix6d = Eigen::Matrix; -using RowMatrixXd = Eigen::Matrix; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp deleted file mode 100644 index 8c10506c36753..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ - -#include "autoware/localization_util/util_func.hpp" - -#include - -#include - -#include - -namespace autoware::localization_util -{ -class SmartPoseBuffer -{ -private: - using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - -public: - struct InterpolateResult - { - PoseWithCovarianceStamped old_pose; - PoseWithCovarianceStamped new_pose; - PoseWithCovarianceStamped interpolated_pose; - }; - - SmartPoseBuffer() = delete; - SmartPoseBuffer( - const rclcpp::Logger & logger, const double & pose_timeout_sec, - const double & pose_distance_tolerance_meters); - - std::optional interpolate(const rclcpp::Time & target_ros_time); - - void push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr); - - void pop_old(const rclcpp::Time & target_ros_time); - - void clear(); - -private: - rclcpp::Logger logger_; - std::deque pose_buffer_; - std::mutex mutex_; // This mutex is for pose_buffer_ - - const double pose_timeout_sec_; - const double pose_distance_tolerance_meters_; - - [[nodiscard]] bool validate_time_stamp_difference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) const; - [[nodiscard]] bool validate_position_difference( - const geometry_msgs::msg::Point & target_point, - const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; -}; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp deleted file mode 100644 index ddf7625c202ec..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ - -/* -A implementation of tree-structured parzen estimator (TPE) -See below pdf for the TPE algorithm detail. -https://papers.nips.cc/paper_files/paper/2011/file/86e8f7ab32cfd12577bc2619bc635690-Paper.pdf - -Optuna is also used as a reference for implementation. -https://github.com/optuna/optuna -*/ - -#include -#include -#include - -namespace autoware::localization_util -{ -class TreeStructuredParzenEstimator -{ -public: - using Input = std::vector; - using Score = double; - struct Trial - { - Input input; - Score score; - }; - - enum Direction { - MINIMIZE = 0, - MAXIMIZE = 1, - }; - - enum Index { - TRANS_X = 0, - TRANS_Y = 1, - TRANS_Z = 2, - ANGLE_X = 3, - ANGLE_Y = 4, - ANGLE_Z = 5, - INDEX_NUM = 6, - }; - - TreeStructuredParzenEstimator() = delete; - TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, - std::vector sample_stddev); - void add_trial(const Trial & trial); - [[nodiscard]] Input get_next_input() const; - -private: - static constexpr double max_good_rate = 0.10; - static constexpr int64_t n_ei_candidates = 100; - - static std::mt19937_64 engine; - - [[nodiscard]] double compute_log_likelihood_ratio(const Input & input) const; - [[nodiscard]] static double log_gaussian_pdf( - const Input & input, const Input & mu, const Input & sigma); - - std::vector trials_; - int64_t above_num_; - const Direction direction_; - const int64_t n_startup_trials_; - const int64_t input_dimension_; - const std::vector sample_mean_; - const std::vector sample_stddev_; - Input base_stddev_; -}; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp deleted file mode 100644 index bef9968f34b6f..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ - -#include -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include -#endif - -#include -#include -#include -#include -#include -#include - -namespace autoware::localization_util -{ -// ref by http://takacity.blog.fc2.com/blog-entry-69.html -std_msgs::msg::ColorRGBA exchange_color_crc(double x); - -double calc_diff_for_radian(const double lhs_rad, const double rhs_rad); - -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose); -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose); -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); - -geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( - const double r_rad, const double p_rad, const double y_rad); -geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( - const double r_deg, const double p_deg, const double y_deg); - -geometry_msgs::msg::Twist calc_twist( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b); - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, - const rclcpp::Time & time_stamp); - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp); - -Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose); -Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose); -geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix); -Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos); - -template -T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform) -{ - T output; - tf2::doTransform(input, output, transform); - return output; -} - -double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); - -void output_pose_with_cov_to_log( - const rclcpp::Logger & logger, const std::string & prefix, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); - -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/autoware_localization_util/package.xml b/localization/autoware_localization_util/package.xml deleted file mode 100644 index 89ad7c24840dd..0000000000000 --- a/localization/autoware_localization_util/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - autoware_localization_util - 0.40.0 - The autoware_localization_util package - Yamato Ando - Masahiro Sakamoto - Shintaro Sakoda - Kento Yabuuchi - NGUYEN Viet Anh - Taiki Yamada - Ryu Yamamoto - Apache License 2.0 - Yamato Ando - - ament_cmake_auto - autoware_cmake - - diagnostic_msgs - geometry_msgs - rclcpp - std_msgs - tf2 - tf2_eigen - tf2_geometry_msgs - visualization_msgs - - ament_cmake_cppcheck - ament_lint_auto - - - ament_cmake - - diff --git a/localization/autoware_localization_util/src/covariance_ellipse.cpp b/localization/autoware_localization_util/src/covariance_ellipse.cpp deleted file mode 100644 index 847f89e0da9b3..0000000000000 --- a/localization/autoware_localization_util/src/covariance_ellipse.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// Copyright 2024 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/localization_util/covariance_ellipse.hpp" - -#include - -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -namespace autoware::localization_util -{ - -Ellipse calculate_xy_ellipse( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale) -{ - // input geometry_msgs::PoseWithCovariance contain 6x6 matrix - Eigen::Matrix2d xy_covariance; - const auto cov = pose_with_covariance.covariance; - xy_covariance(0, 0) = cov[0 * 6 + 0]; - xy_covariance(0, 1) = cov[0 * 6 + 1]; - xy_covariance(1, 0) = cov[1 * 6 + 0]; - xy_covariance(1, 1) = cov[1 * 6 + 1]; - - Eigen::SelfAdjointEigenSolver eigensolver(xy_covariance); - - Ellipse ellipse; - - // eigen values and vectors are sorted in ascending order - ellipse.long_radius = scale * std::sqrt(eigensolver.eigenvalues()(1)); - ellipse.short_radius = scale * std::sqrt(eigensolver.eigenvalues()(0)); - - // principal component vector - const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1); - ellipse.yaw = std::atan2(pc_vector.y(), pc_vector.x()); - - // ellipse size along lateral direction (body-frame) - ellipse.P = xy_covariance; - const double yaw_vehicle = tf2::getYaw(pose_with_covariance.pose.orientation); - const Eigen::Matrix2d & p_inv = ellipse.P.inverse(); - Eigen::MatrixXd e(2, 1); - e(0, 0) = std::cos(yaw_vehicle); - e(1, 0) = std::sin(yaw_vehicle); - const double d = std::sqrt((e.transpose() * p_inv * e)(0, 0) / p_inv.determinant()); - ellipse.size_lateral_direction = scale * d; - - return ellipse; -} - -visualization_msgs::msg::Marker create_ellipse_marker( - const Ellipse & ellipse, const std_msgs::msg::Header & header, - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) -{ - tf2::Quaternion quat; - quat.setEuler(0, 0, ellipse.yaw); - - const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0); - const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0); - visualization_msgs::msg::Marker marker; - marker.header = header; - marker.ns = "error_ellipse"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::SPHERE; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = pose_with_covariance.pose; - marker.pose.orientation = tf2::toMsg(quat); - marker.scale.x = ellipse_long_radius * 2; - marker.scale.y = ellipse_short_radius * 2; - marker.scale.z = 0.01; - marker.color.a = 0.1; - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - return marker; -} - -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/smart_pose_buffer.cpp b/localization/autoware_localization_util/src/smart_pose_buffer.cpp deleted file mode 100644 index 3b529d68cf6c5..0000000000000 --- a/localization/autoware_localization_util/src/smart_pose_buffer.cpp +++ /dev/null @@ -1,158 +0,0 @@ -// Copyright 2023- Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/localization_util/smart_pose_buffer.hpp" - -namespace autoware::localization_util -{ -SmartPoseBuffer::SmartPoseBuffer( - const rclcpp::Logger & logger, const double & pose_timeout_sec, - const double & pose_distance_tolerance_meters) -: logger_(logger), - pose_timeout_sec_(pose_timeout_sec), - pose_distance_tolerance_meters_(pose_distance_tolerance_meters) -{ -} - -std::optional SmartPoseBuffer::interpolate( - const rclcpp::Time & target_ros_time) -{ - InterpolateResult result; - - { - std::lock_guard lock(mutex_); - - if (pose_buffer_.size() < 2) { - RCLCPP_INFO(logger_, "pose_buffer_.size() < 2"); - return std::nullopt; - } - - const rclcpp::Time time_first = pose_buffer_.front()->header.stamp; - const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; - - if (target_ros_time < time_first) { - RCLCPP_INFO(logger_, "Mismatch between pose timestamp and current timestamp"); - return std::nullopt; - } - - // [time_last < target_ros_time] is acceptable here. - // It is possible that the target_ros_time (often sensor timestamp) is newer than the latest - // timestamp of buffered pose (often EKF). - // However, if the timestamp difference is too large, - // it will later be rejected by validate_time_stamp_difference. - - // get the nearest poses - result.old_pose = *pose_buffer_.front(); - for (const PoseWithCovarianceStamped::ConstSharedPtr & pose_cov_msg_ptr : pose_buffer_) { - result.new_pose = *pose_cov_msg_ptr; - const rclcpp::Time pose_time_stamp = result.new_pose.header.stamp; - if (pose_time_stamp > target_ros_time) { - break; - } - result.old_pose = *pose_cov_msg_ptr; - } - } - - // check the time stamp - const bool is_old_pose_valid = validate_time_stamp_difference( - result.old_pose.header.stamp, target_ros_time, pose_timeout_sec_); - const bool is_new_pose_valid = validate_time_stamp_difference( - result.new_pose.header.stamp, target_ros_time, pose_timeout_sec_); - - // check the position jumping (ex. immediately after the initial pose estimation) - const bool is_pose_diff_valid = validate_position_difference( - result.old_pose.pose.pose.position, result.new_pose.pose.pose.position, - pose_distance_tolerance_meters_); - - // all validations must be true - if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { - return std::nullopt; - } - - // interpolate the pose - const geometry_msgs::msg::PoseStamped interpolated_pose_msg = - interpolate_pose(result.old_pose, result.new_pose, target_ros_time); - result.interpolated_pose.header = interpolated_pose_msg.header; - result.interpolated_pose.pose.pose = interpolated_pose_msg.pose; - // This does not interpolate the covariance. - // interpolated_pose.pose.covariance is always set to old_pose.covariance - result.interpolated_pose.pose.covariance = result.old_pose.pose.covariance; - return result; -} - -void SmartPoseBuffer::push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr) -{ - std::lock_guard lock(mutex_); - if (!pose_buffer_.empty()) { - // Check for non-chronological timestamp order - // This situation can arise when replaying a rosbag multiple times - const rclcpp::Time end_time = pose_buffer_.back()->header.stamp; - const rclcpp::Time msg_time = pose_msg_ptr->header.stamp; - if (msg_time < end_time) { - // Clear the buffer if timestamps are reversed to maintain chronological order - pose_buffer_.clear(); - } - } - pose_buffer_.push_back(pose_msg_ptr); -} - -void SmartPoseBuffer::pop_old(const rclcpp::Time & target_ros_time) -{ - std::lock_guard lock(mutex_); - while (!pose_buffer_.empty()) { - if (rclcpp::Time(pose_buffer_.front()->header.stamp) >= target_ros_time) { - break; - } - pose_buffer_.pop_front(); - } -} - -void SmartPoseBuffer::clear() -{ - std::lock_guard lock(mutex_); - pose_buffer_.clear(); -} - -bool SmartPoseBuffer::validate_time_stamp_difference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) const -{ - const double dt = std::abs((target_time - reference_time).seconds()); - const bool success = dt < time_tolerance_sec; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " - "difference is %lf[sec] (the tolerance is %lf[sec]).", - reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); - } - return success; -} - -bool SmartPoseBuffer::validate_position_difference( - const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, - const double distance_tolerance_m_) const -{ - const double distance = norm(target_point, reference_point); - const bool success = distance < distance_tolerance_m_; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The distance from reference position to target position is %lf[m] (the " - "tolerance is %lf[m]).", - distance, distance_tolerance_m_); - } - return success; -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp deleted file mode 100644 index e9f0318d1e06d..0000000000000 --- a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp +++ /dev/null @@ -1,185 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/localization_util/tree_structured_parzen_estimator.hpp" - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::localization_util -{ -// random number generator -std::mt19937_64 TreeStructuredParzenEstimator::engine(0); - -TreeStructuredParzenEstimator::TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, - std::vector sample_stddev) -: above_num_(0), - direction_(direction), - n_startup_trials_(n_startup_trials), - input_dimension_(INDEX_NUM), - sample_mean_(std::move(sample_mean)), - sample_stddev_(std::move(sample_stddev)) -{ - if (sample_mean_.size() != ANGLE_Z) { - std::cerr << "sample_mean size is invalid" << std::endl; - throw std::runtime_error("sample_mean size is invalid"); - } - if (sample_stddev_.size() != ANGLE_Z) { - std::cerr << "sample_stddev size is invalid" << std::endl; - throw std::runtime_error("sample_stddev size is invalid"); - } - // base_stddev_ is defined as the stable convergence range of ndt_scan_matcher. - base_stddev_.resize(input_dimension_); - base_stddev_[TRANS_X] = 0.25; // [m] - base_stddev_[TRANS_Y] = 0.25; // [m] - base_stddev_[TRANS_Z] = 0.25; // [m] - base_stddev_[ANGLE_X] = 1.0 / 180.0 * M_PI; // [rad] - base_stddev_[ANGLE_Y] = 1.0 / 180.0 * M_PI; // [rad] - base_stddev_[ANGLE_Z] = 2.5 / 180.0 * M_PI; // [rad] -} - -void TreeStructuredParzenEstimator::add_trial(const Trial & trial) -{ - trials_.push_back(trial); - std::sort(trials_.begin(), trials_.end(), [this](const Trial & lhs, const Trial & rhs) { - return (direction_ == Direction::MAXIMIZE ? lhs.score > rhs.score : lhs.score < rhs.score); - }); - above_num_ = std::min( - {static_cast(10), - static_cast(static_cast(trials_.size()) * max_good_rate)}); -} - -TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const -{ - std::normal_distribution dist_normal_trans_x( - sample_mean_[TRANS_X], sample_stddev_[TRANS_X]); - std::normal_distribution dist_normal_trans_y( - sample_mean_[TRANS_Y], sample_stddev_[TRANS_Y]); - std::normal_distribution dist_normal_trans_z( - sample_mean_[TRANS_Z], sample_stddev_[TRANS_Z]); - std::normal_distribution dist_normal_angle_x( - sample_mean_[ANGLE_X], sample_stddev_[ANGLE_X]); - std::normal_distribution dist_normal_angle_y( - sample_mean_[ANGLE_Y], sample_stddev_[ANGLE_Y]); - std::uniform_real_distribution dist_uniform_angle_z(-M_PI, M_PI); - - if (static_cast(trials_.size()) < n_startup_trials_ || above_num_ == 0) { - // Random sampling based on prior until the number of trials reaches `n_startup_trials_`. - Input input(input_dimension_); - input[TRANS_X] = dist_normal_trans_x(engine); - input[TRANS_Y] = dist_normal_trans_y(engine); - input[TRANS_Z] = dist_normal_trans_z(engine); - input[ANGLE_X] = dist_normal_angle_x(engine); - input[ANGLE_Y] = dist_normal_angle_y(engine); - input[ANGLE_Z] = dist_uniform_angle_z(engine); - return input; - } - - Input best_input; - double best_log_likelihood_ratio = std::numeric_limits::lowest(); - for (int64_t i = 0; i < n_ei_candidates; i++) { - Input input(input_dimension_); - input[TRANS_X] = dist_normal_trans_x(engine); - input[TRANS_Y] = dist_normal_trans_y(engine); - input[TRANS_Z] = dist_normal_trans_z(engine); - input[ANGLE_X] = dist_normal_angle_x(engine); - input[ANGLE_Y] = dist_normal_angle_y(engine); - input[ANGLE_Z] = dist_uniform_angle_z(engine); - const double log_likelihood_ratio = compute_log_likelihood_ratio(input); - if (log_likelihood_ratio > best_log_likelihood_ratio) { - best_log_likelihood_ratio = log_likelihood_ratio; - best_input = input; - } - } - return best_input; -} - -double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & input) const -{ - const auto n = static_cast(trials_.size()); - - // The above KDE and the below KDE are calculated respectively, and the ratio is the criteria to - // select best sample. - std::vector above_logs; - std::vector below_logs; - - for (int64_t i = 0; i < n; i++) { - const double log_p = log_gaussian_pdf(input, trials_[i].input, base_stddev_); - if (i < above_num_) { - const double w = 1.0 / static_cast(above_num_); - const double log_w = std::log(w); - above_logs.push_back(log_p + log_w); - } else { - const double w = 1.0 / static_cast(n - above_num_); - const double log_w = std::log(w); - below_logs.push_back(log_p + log_w); - } - } - - auto log_sum_exp = [](const std::vector & log_vec) { - const double max = *std::max_element(log_vec.begin(), log_vec.end()); - double sum = std::accumulate( - log_vec.begin(), log_vec.end(), 0.0, - [max](double total, double log_v) { return total + std::exp(log_v - max); }); - return max + std::log(sum); - }; - - const double above = log_sum_exp(above_logs); - const double below = log_sum_exp(below_logs); - - // Multiply by a constant so that the score near the "below sample" becomes lower. - // cspell:disable-line TODO(Shintaro Sakoda): It's theoretically incorrect, consider it again - // later. - const double r = above - below * 5.0; - return r; -} - -double TreeStructuredParzenEstimator::log_gaussian_pdf( - const Input & input, const Input & mu, const Input & sigma) -{ - const double log_2pi = std::log(2.0 * M_PI); - auto log_gaussian_pdf_1d = [&](const double diff, const double sigma) { - return -0.5 * log_2pi - std::log(sigma) - (diff * diff) / (2.0 * sigma * sigma); - }; - - const auto n = static_cast(input.size()); - - double result = 0.0; - for (int64_t i = 0; i < n; i++) { - double diff = input[i] - mu[i]; - if (i == ANGLE_Z) { - // Normalize the loop variable to [-pi, pi) - while (diff >= M_PI) { - diff -= 2 * M_PI; - } - while (diff < -M_PI) { - diff += 2 * M_PI; - } - } - // Experimentally, it is better to consider only trans_xy and yaw, so ignore trans_z, angle_x, - // angle_y. - if (i == TRANS_Z || i == ANGLE_X || i == ANGLE_Y) { - continue; - } - result += log_gaussian_pdf_1d(diff, sigma[i]); - } - return result; -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/util_func.cpp b/localization/autoware_localization_util/src/util_func.cpp deleted file mode 100644 index 17187a8d732f0..0000000000000 --- a/localization/autoware_localization_util/src/util_func.cpp +++ /dev/null @@ -1,257 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/localization_util/util_func.hpp" - -#include "autoware/localization_util/matrix_type.hpp" - -#include -#include - -namespace autoware::localization_util -{ -// ref by http://takacity.blog.fc2.com/blog-entry-69.html -std_msgs::msg::ColorRGBA exchange_color_crc(double x) -{ - std_msgs::msg::ColorRGBA color; - - x = std::max(x, 0.0); - x = std::min(x, 0.9999); - - if (x <= 0.25) { - color.b = 1.0; - color.g = static_cast(std::sin(x * 2.0 * M_PI)); - color.r = 0; - } else if (x <= 0.5) { - color.b = static_cast(std::sin(x * 2 * M_PI)); - color.g = 1.0; - color.r = 0; - } else if (x <= 0.75) { - color.b = 0; - color.g = 1.0; - color.r = static_cast(-std::sin(x * 2.0 * M_PI)); - } else { - color.b = 0; - color.g = static_cast(-std::sin(x * 2.0 * M_PI)); - color.r = 1.0; - } - color.a = 0.999; - return color; -} - -double calc_diff_for_radian(const double lhs_rad, const double rhs_rad) -{ - double diff_rad = lhs_rad - rhs_rad; - if (diff_rad > M_PI) { - diff_rad = diff_rad - 2 * M_PI; - } else if (diff_rad < -M_PI) { - diff_rad = diff_rad + 2 * M_PI; - } - return diff_rad; -} - -Eigen::Map make_eigen_covariance(const std::array & covariance) -{ - return {covariance.data(), 6, 6}; -} - -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose) -{ - geometry_msgs::msg::Vector3 rpy; - tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); - tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); - return rpy; -} - -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose) -{ - return get_rpy(pose.pose); -} - -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) -{ - return get_rpy(pose.pose.pose); -} - -geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( - const double r_rad, const double p_rad, const double y_rad) -{ - tf2::Quaternion q; - q.setRPY(r_rad, p_rad, y_rad); - geometry_msgs::msg::Quaternion quaternion_msg; - quaternion_msg.x = q.x(); - quaternion_msg.y = q.y(); - quaternion_msg.z = q.z(); - quaternion_msg.w = q.w(); - return quaternion_msg; -} - -geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( - const double r_deg, const double p_deg, const double y_deg) -{ - const double r_rad = r_deg * M_PI / 180.0; - const double p_rad = p_deg * M_PI / 180.0; - const double y_rad = y_deg * M_PI / 180.0; - return rpy_rad_to_quaternion(r_rad, p_rad, y_rad); -} - -geometry_msgs::msg::Twist calc_twist( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b) -{ - const rclcpp::Duration dt = rclcpp::Time(pose_b.header.stamp) - rclcpp::Time(pose_a.header.stamp); - const double dt_s = dt.seconds(); - - if (dt_s == 0) { - return geometry_msgs::msg::Twist(); - } - - const auto pose_a_rpy = get_rpy(pose_a); - const auto pose_b_rpy = get_rpy(pose_b); - - geometry_msgs::msg::Vector3 diff_xyz; - geometry_msgs::msg::Vector3 diff_rpy; - - diff_xyz.x = pose_b.pose.position.x - pose_a.pose.position.x; - diff_xyz.y = pose_b.pose.position.y - pose_a.pose.position.y; - diff_xyz.z = pose_b.pose.position.z - pose_a.pose.position.z; - diff_rpy.x = calc_diff_for_radian(pose_b_rpy.x, pose_a_rpy.x); - diff_rpy.y = calc_diff_for_radian(pose_b_rpy.y, pose_a_rpy.y); - diff_rpy.z = calc_diff_for_radian(pose_b_rpy.z, pose_a_rpy.z); - - geometry_msgs::msg::Twist twist; - twist.linear.x = diff_xyz.x / dt_s; - twist.linear.y = diff_xyz.y / dt_s; - twist.linear.z = diff_xyz.z / dt_s; - twist.angular.x = diff_rpy.x / dt_s; - twist.angular.y = diff_rpy.y / dt_s; - twist.angular.z = diff_rpy.z / dt_s; - - return twist; -} - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, - const rclcpp::Time & time_stamp) -{ - const rclcpp::Time pose_a_time_stamp = pose_a.header.stamp; - const rclcpp::Time pose_b_time_stamp = pose_b.header.stamp; - if ( - (pose_a_time_stamp.seconds() == 0.0) || (pose_b_time_stamp.seconds() == 0.0) || - (time_stamp.seconds() == 0.0)) { - return geometry_msgs::msg::PoseStamped(); - } - - const auto twist = calc_twist(pose_a, pose_b); - const double dt = (time_stamp - pose_a_time_stamp).seconds(); - - const auto pose_a_rpy = get_rpy(pose_a); - - geometry_msgs::msg::Vector3 xyz; - geometry_msgs::msg::Vector3 rpy; - xyz.x = pose_a.pose.position.x + twist.linear.x * dt; - xyz.y = pose_a.pose.position.y + twist.linear.y * dt; - xyz.z = pose_a.pose.position.z + twist.linear.z * dt; - rpy.x = pose_a_rpy.x + twist.angular.x * dt; - rpy.y = pose_a_rpy.y + twist.angular.y * dt; - rpy.z = pose_a_rpy.z + twist.angular.z * dt; - - tf2::Quaternion tf_quaternion; - tf_quaternion.setRPY(rpy.x, rpy.y, rpy.z); - - geometry_msgs::msg::PoseStamped pose; - pose.header = pose_a.header; - pose.header.stamp = time_stamp; - pose.pose.position.x = xyz.x; - pose.pose.position.y = xyz.y; - pose.pose.position.z = xyz.z; - pose.pose.orientation = tf2::toMsg(tf_quaternion); - return pose; -} - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp) -{ - geometry_msgs::msg::PoseStamped tmp_pose_a; - tmp_pose_a.header = pose_a.header; - tmp_pose_a.pose = pose_a.pose.pose; - - geometry_msgs::msg::PoseStamped tmp_pose_b; - tmp_pose_b.header = pose_b.header; - tmp_pose_b.pose = pose_b.pose.pose; - - return interpolate_pose(tmp_pose_a, tmp_pose_b, time_stamp); -} - -Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose) -{ - Eigen::Affine3d eigen_pose; - tf2::fromMsg(ros_pose, eigen_pose); - return eigen_pose; -} - -Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose) -{ - Eigen::Affine3d eigen_pose_affine = pose_to_affine3d(ros_pose); - Eigen::Matrix4f eigen_pose_matrix = eigen_pose_affine.matrix().cast(); - return eigen_pose_matrix; -} - -Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos) -{ - Eigen::Vector3d eigen_pos; - eigen_pos.x() = ros_pos.x; - eigen_pos.y() = ros_pos.y; - eigen_pos.z() = ros_pos.z; - return eigen_pos; -} - -geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix) -{ - Eigen::Affine3d eigen_pose_affine; - eigen_pose_affine.matrix() = eigen_pose_matrix.cast(); - geometry_msgs::msg::Pose ros_pose = tf2::toMsg(eigen_pose_affine); - return ros_pose; -} - -double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) -{ - const double dx = p1.x - p2.x; - const double dy = p1.y - p2.y; - const double dz = p1.z - p2.z; - return std::sqrt(dx * dx + dy * dy + dz * dz); -} - -void output_pose_with_cov_to_log( - const rclcpp::Logger & logger, const std::string & prefix, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov) -{ - const Eigen::Map covariance = - make_eigen_covariance(pose_with_cov.pose.covariance); - const geometry_msgs::msg::Pose pose = pose_with_cov.pose.pose; - geometry_msgs::msg::Vector3 rpy = get_rpy(pose); - rpy.x = rpy.x * 180.0 / M_PI; - rpy.y = rpy.y * 180.0 / M_PI; - rpy.z = rpy.z * 180.0 / M_PI; - - RCLCPP_DEBUG_STREAM( - logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << "," - << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y - << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x - << "," << rpy.y << "," << rpy.z << "," << covariance(0, 0) << "," - << covariance(1, 1) << "," << covariance(2, 2) << "," << covariance(3, 3) - << "," << covariance(4, 4) << "," << covariance(5, 5)); -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp deleted file mode 100644 index d55555682be84..0000000000000 --- a/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2023- Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/localization_util/smart_pose_buffer.hpp" -#include "autoware/localization_util/util_func.hpp" - -#include -#include - -#include -#include - -#include -#include -#include - -using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; -using SmartPoseBuffer = autoware::localization_util::SmartPoseBuffer; - -bool compare_pose( - const PoseWithCovarianceStamped & pose_a, const PoseWithCovarianceStamped & pose_b) -{ - return pose_a.header.stamp == pose_b.header.stamp && - pose_a.header.frame_id == pose_b.header.frame_id && - pose_a.pose.covariance == pose_b.pose.covariance && - pose_a.pose.pose.position.x == pose_b.pose.pose.position.x && - pose_a.pose.pose.position.y == pose_b.pose.pose.position.y && - pose_a.pose.pose.position.z == pose_b.pose.pose.position.z && - pose_a.pose.pose.orientation.x == pose_b.pose.pose.orientation.x && - pose_a.pose.pose.orientation.y == pose_b.pose.pose.orientation.y && - pose_a.pose.pose.orientation.z == pose_b.pose.pose.orientation.z && - pose_a.pose.pose.orientation.w == pose_b.pose.pose.orientation.w; -} - -TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT -{ - rclcpp::Logger logger = rclcpp::get_logger("test_logger"); - const double pose_timeout_sec = 10.0; - const double pose_distance_tolerance_meters = 100.0; - SmartPoseBuffer smart_pose_buffer(logger, pose_timeout_sec, pose_distance_tolerance_meters); - - // first data - PoseWithCovarianceStamped::SharedPtr old_pose_ptr = std::make_shared(); - old_pose_ptr->header.stamp.sec = 10; - old_pose_ptr->header.stamp.nanosec = 0; - old_pose_ptr->pose.pose.position.x = 10.0; - old_pose_ptr->pose.pose.position.y = 20.0; - old_pose_ptr->pose.pose.position.z = 0.0; - old_pose_ptr->pose.pose.orientation = - autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 0.0); - smart_pose_buffer.push_back(old_pose_ptr); - - // second data - PoseWithCovarianceStamped::SharedPtr new_pose_ptr = std::make_shared(); - new_pose_ptr->header.stamp.sec = 20; - new_pose_ptr->header.stamp.nanosec = 0; - new_pose_ptr->pose.pose.position.x = 20.0; - new_pose_ptr->pose.pose.position.y = 40.0; - new_pose_ptr->pose.pose.position.z = 0.0; - new_pose_ptr->pose.pose.orientation = - autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 90.0); - smart_pose_buffer.push_back(new_pose_ptr); - - // interpolate - builtin_interfaces::msg::Time target_ros_time_msg; - target_ros_time_msg.sec = 15; - target_ros_time_msg.nanosec = 0; - const std::optional & interpolate_result = - smart_pose_buffer.interpolate(target_ros_time_msg); - ASSERT_TRUE(interpolate_result.has_value()); - const SmartPoseBuffer::InterpolateResult result = interpolate_result.value(); - - // check old - EXPECT_TRUE(compare_pose(result.old_pose, *old_pose_ptr)); - - // check new - EXPECT_TRUE(compare_pose(result.new_pose, *new_pose_ptr)); - - // check interpolated - EXPECT_EQ(result.interpolated_pose.header.stamp.sec, 15); - EXPECT_EQ(result.interpolated_pose.header.stamp.nanosec, static_cast(0)); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.x, 15.0); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.y, 30.0); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.z, 0.0); - const auto rpy = autoware::localization_util::get_rpy(result.interpolated_pose.pose.pose); - EXPECT_NEAR(rpy.x * 180 / M_PI, 0.0, 1e-6); - EXPECT_NEAR(rpy.y * 180 / M_PI, 0.0, 1e-6); - EXPECT_NEAR(rpy.z * 180 / M_PI, 45.0, 1e-6); -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/localization/autoware_localization_util/test/test_tpe.cpp b/localization/autoware_localization_util/test/test_tpe.cpp deleted file mode 100644 index 2d71a385246b7..0000000000000 --- a/localization/autoware_localization_util/test/test_tpe.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// 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/localization_util/tree_structured_parzen_estimator.hpp" - -#include - -#include -#include -#include -#include -#include - -using TreeStructuredParzenEstimator = autoware::localization_util::TreeStructuredParzenEstimator; - -TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) -{ - auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { - double value = 0.0; - const auto n = static_cast(input.size()); - for (int64_t i = 0; i < n; i++) { - const double v = input[i] * 10; - value += v * v; - } - return value; - }; - - constexpr int64_t k_outer_trials_num = 20; - constexpr int64_t k_inner_trials_num = 200; - std::cout << std::fixed; - std::vector mean_scores; - std::vector sample_mean(5, 0.0); - std::vector sample_stddev{1.0, 1.0, 0.1, 0.1, 0.1}; - - for (const int64_t n_startup_trials : {k_inner_trials_num, k_inner_trials_num / 2}) { - const std::string method = ((n_startup_trials == k_inner_trials_num) ? "Random" : "TPE"); - - std::vector scores; - for (int64_t i = 0; i < k_outer_trials_num; i++) { - double best_score = std::numeric_limits::lowest(); - TreeStructuredParzenEstimator estimator( - TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, sample_mean, - sample_stddev); - for (int64_t trial = 0; trial < k_inner_trials_num; trial++) { - const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); - const double score = -sphere_function(input); - estimator.add_trial({input, score}); - best_score = std::max(best_score, score); - } - scores.push_back(best_score); - } - - const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); - const double mean = sum / static_cast(scores.size()); - mean_scores.push_back(mean); - double sq_sum = std::accumulate( - scores.begin(), scores.end(), 0.0, - [mean](double total, double score) { return total + (score - mean) * (score - mean); }); - const double stddev = std::sqrt(sq_sum / static_cast(scores.size())); - - std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; - } - ASSERT_LT(mean_scores[0], mean_scores[1]); -} diff --git a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp index ae07d54ad53d6..ac759504de557 100644 --- a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp @@ -173,9 +173,9 @@ void DistanceBasedCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp index 9f325b36676be..5794cfe37a45b 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp @@ -141,9 +141,9 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp index a31d5ec5dd6d6..db5e46a0eff5a 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp @@ -100,9 +100,9 @@ void VoxelBasedCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp index d0bcf381cd62f..775a931ac4c4c 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp @@ -173,9 +173,9 @@ void VoxelDistanceBasedCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_crosswalk_traffic_light_estimator/README.md b/perception/autoware_crosswalk_traffic_light_estimator/README.md index b14fefbd43beb..1b24b115f5812 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/README.md +++ b/perception/autoware_crosswalk_traffic_light_estimator/README.md @@ -1,40 +1,85 @@ -# crosswalk_traffic_light_estimator +# autoware_crosswalk_traffic_light_estimator ## Purpose -`crosswalk_traffic_light_estimator` is a module that estimates pedestrian traffic signals from HDMap and detected vehicle traffic signals. +`autoware_crosswalk_traffic_light_estimator` estimates pedestrian traffic signals which can be summarized as the following two tasks: + +- Estimate pedestrian traffic signals that are not subject to be detected by perception pipeline. +- Estimate whether pedestrian traffic signals are flashing and modify the result. + +This module works without `~/input/route`, but its behavior is outputting the subscribed results as is. ## Inputs / Outputs ### Input -| Name | Type | Description | -| ------------------------------------ | ------------------------------------------------ | ------------------ | -| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | -| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | route | -| `~/input/classified/traffic_signals` | `tier4_perception_msgs::msg::TrafficSignalArray` | classified signals | +| Name | Type | Description | +| ------------------------------------ | ----------------------------------------------------- | ------------------ | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/route` | autoware_planning_msgs::msg::LaneletRoute | optional: route | +| `~/input/classified/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | classified signals | ### Output -| Name | Type | Description | -| -------------------------- | ------------------------------------------------------- | --------------------------------------------------------- | -| `~/output/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | output that contains estimated pedestrian traffic signals | +| Name | Type | Description | +| -------------------------- | ----------------------------------------------------- | --------------------------------------------------------- | +| `~/output/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | output that contains estimated pedestrian traffic signals | ## Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `use_last_detect_color` | `bool` | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | `true` | -| `last_detect_color_hold_time` | `double` | The time threshold to hold for last detect color. | `2.0` | +| Name | Type | Description | Default value | +| :---------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `use_last_detect_color` | bool | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | true | +| `last_detect_color_hold_time` | double | The time threshold to hold for last detect color. The unit is second. | 2.0 | +| `last_colors_hold_time` | double | The time threshold to hold for history detected pedestrian traffic light color. The unit is second. | 1.0 | ## Inner-workings / Algorithms +When the pedestrian traffic signals **are detected** by perception pipeline + +- If estimates the pedestrian traffic signals are flashing, overwrite the results +- Prefer the output from perception pipeline, but overwrite it if the pedestrian traffic signals are invalid(`no detection`, `backlight`, or `occlusion`) + +When the pedestrian traffic signals **are NOT detected** by perception pipeline + +- Estimate the color of pedestrian traffic signals based on detected vehicle traffic signals, HDMap, and route + +### Estimate whether pedestrian traffic signals are flashing + +```plantumul +start +if (the pedestrian traffic light classification result exists)then + : update the flashing flag according to the classification result(in_signal) and last_signals + if (the traffic light is flashing?)then(yes) + : update the traffic light state + else(no) + : the traffic light state is the same with the classification result +if (the classification result not exists) + : the traffic light state is the same with the estimation + : output the current traffic light state +end +``` + +#### Update flashing flag + +
+ +
+ +#### Update traffic light status + +
+ +
+ +### Estimate the color of pedestrian traffic signals + ```plantuml start -:subscribe detected traffic signals & HDMap; +:subscribe detected traffic signals, HDMap, and route; :extract crosswalk lanelets from HDMap; -:extract road lanelets that conflicts crosswalk; +:extract road lanelets that conflicts crosswalk from route; :initialize non_red_lanelets(lanelet::ConstLanelets); if (Latest detection result is **GREEN** or **AMBER**?) then (yes) :push back non_red_lanelets; @@ -58,7 +103,7 @@ end If traffic between pedestrians and vehicles is controlled by traffic signals, the crosswalk traffic signal maybe **RED** in order to prevent pedestrian from crossing when the following conditions are satisfied. -### Situation1 +#### Situation1 - crosswalk conflicts **STRAIGHT** lanelet - the lanelet refers **GREEN** or **AMBER** traffic signal (The following pictures show only **GREEN** case) @@ -70,7 +115,7 @@ If traffic between pedestrians and vehicles is controlled by traffic signals, th -### Situation2 +#### Situation2 - crosswalk conflicts different turn direction lanelets (STRAIGHT and LEFT, LEFT and RIGHT, RIGHT and STRAIGHT) - the lanelets refer **GREEN** or **AMBER** traffic signal (The following pictures show only **GREEN** case) diff --git a/perception/autoware_crosswalk_traffic_light_estimator/images/flashing_state.png b/perception/autoware_crosswalk_traffic_light_estimator/images/flashing_state.png new file mode 100644 index 0000000000000..7686f3842e75c Binary files /dev/null and b/perception/autoware_crosswalk_traffic_light_estimator/images/flashing_state.png differ diff --git a/perception/autoware_crosswalk_traffic_light_estimator/images/traffic_light.png b/perception/autoware_crosswalk_traffic_light_estimator/images/traffic_light.png new file mode 100644 index 0000000000000..a03e7a88ee100 Binary files /dev/null and b/perception/autoware_crosswalk_traffic_light_estimator/images/traffic_light.png differ diff --git a/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp b/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp index 39dd3db10cfa9..8bd1deba35ccb 100644 --- a/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp +++ b/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp @@ -89,9 +89,9 @@ class Debugger void startMeasureProcessingTime() { stop_watch_ptr_->tic("processing_time"); } void publishProcessingTime() { - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp index 74b4edbe595ea..8fda0c1395caf 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp @@ -369,9 +369,9 @@ void ScanGroundFilterComponent::faster_filter( if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp index a89ee3d02cd8b..a1d12bcb56384 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -29,8 +30,8 @@ class PointPaintingTRT : public autoware::lidar_centerpoint::CenterPointTRT using autoware::lidar_centerpoint::CenterPointTRT::CenterPointTRT; explicit PointPaintingTRT( - const autoware::lidar_centerpoint::NetworkParam & encoder_param, - const autoware::lidar_centerpoint::NetworkParam & head_param, + const autoware::tensorrt_common::TrtCommonConfig & encoder_param, + const autoware::tensorrt_common::TrtCommonConfig & head_param, const autoware::lidar_centerpoint::DensificationParam & densification_param, const autoware::lidar_centerpoint::CenterPointConfig & config); diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index bcd62383319c1..ef03cf6c3a7cf 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -176,10 +176,10 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt iou_bev_nms_.setParameters(p); } - autoware::lidar_centerpoint::NetworkParam encoder_param( - encoder_onnx_path, encoder_engine_path, trt_precision); - autoware::lidar_centerpoint::NetworkParam head_param( - head_onnx_path, head_engine_path, trt_precision); + autoware::tensorrt_common::TrtCommonConfig encoder_param( + encoder_onnx_path, trt_precision, encoder_engine_path); + autoware::tensorrt_common::TrtCommonConfig head_param( + head_onnx_path, trt_precision, head_engine_path); autoware::lidar_centerpoint::DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); autoware::lidar_centerpoint::CenterPointConfig config( diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index cc3eb17cc32ab..1fe860e68a967 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -27,8 +27,8 @@ namespace autoware::image_projection_based_fusion { PointPaintingTRT::PointPaintingTRT( - const autoware::lidar_centerpoint::NetworkParam & encoder_param, - const autoware::lidar_centerpoint::NetworkParam & head_param, + const autoware::tensorrt_common::TrtCommonConfig & encoder_param, + const autoware::tensorrt_common::TrtCommonConfig & head_param, const autoware::lidar_centerpoint::DensificationParam & densification_param, const autoware::lidar_centerpoint::CenterPointConfig & config) : autoware::lidar_centerpoint::CenterPointTRT( diff --git a/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt index 16ab7ecccae60..0555cedc235d9 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(tf2_eigen REQUIRED) find_package(autoware_universe_utils REQUIRED) -find_package(tier4_debug_msgs REQUIRED) +find_package(autoware_internal_debug_msgs REQUIRED) find_package(tier4_perception_msgs REQUIRED) find_package(autoware_tensorrt_common) @@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME} rclcpp_components::component tf2_eigen::tf2_eigen ${autoware_tensorrt_common_TARGETS} - ${tier4_debug_msgs_TARGETS} + ${autoware_internal_debug_msgs_TARGETS} ${tier4_perception_msgs_TARGETS} ) diff --git a/perception/autoware_lidar_apollo_instance_segmentation/package.xml b/perception/autoware_lidar_apollo_instance_segmentation/package.xml index 8a3d5a7ea6f76..8ff7ce77d943d 100755 --- a/perception/autoware_lidar_apollo_instance_segmentation/package.xml +++ b/perception/autoware_lidar_apollo_instance_segmentation/package.xml @@ -14,6 +14,7 @@ ament_cmake autoware_cuda_utils + autoware_internal_debug_msgs autoware_perception_msgs autoware_tensorrt_common autoware_universe_utils @@ -22,7 +23,6 @@ rclcpp rclcpp_components tf2_eigen - tier4_debug_msgs tier4_perception_msgs ament_lint_auto diff --git a/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp index 60aa09c8e16d7..2e3a7a2243399 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp @@ -18,7 +18,6 @@ #include -#include #include #include @@ -51,12 +50,9 @@ LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node * const auto precision = node_->declare_parameter("precision", "fp32"); trt_common_ = std::make_unique( - onnx_file, precision, nullptr, autoware::tensorrt_common::BatchConfig{1, 1, 1}, 1 << 30); - trt_common_->setup(); - - if (!trt_common_->isInitialized()) { - RCLCPP_ERROR_STREAM(node_->get_logger(), "failed to create tensorrt engine file."); - return; + tensorrt_common::TrtCommonConfig(onnx_file, precision)); + if (!trt_common_->setup()) { + throw std::runtime_error("Failed to setup TensorRT"); } if (node_->declare_parameter("build_only", false)) { @@ -65,11 +61,11 @@ LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node * } // GPU memory allocation - const auto input_dims = trt_common_->getBindingDimensions(0); + const auto input_dims = trt_common_->getTensorShape(0); const auto input_size = std::accumulate(input_dims.d + 1, input_dims.d + input_dims.nbDims, 1, std::multiplies()); input_d_ = autoware::cuda_utils::make_unique(input_size); - const auto output_dims = trt_common_->getBindingDimensions(1); + const auto output_dims = trt_common_->getTensorShape(1); output_size_ = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); output_d_ = autoware::cuda_utils::make_unique(output_size_); @@ -127,10 +123,6 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( const sensor_msgs::msg::PointCloud2 & input, tier4_perception_msgs::msg::DetectedObjectsWithFeature & output) { - if (!trt_common_->isInitialized()) { - return false; - } - // move up pointcloud z_offset in z axis sensor_msgs::msg::PointCloud2 transformed_cloud; transformCloud(input, transformed_cloud, z_offset_); @@ -169,7 +161,10 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( std::vector buffers = {input_d_.get(), output_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); CHECK_CUDA_ERROR(cudaMemcpyAsync( output_h_.get(), output_d_.get(), sizeof(float) * output_size_, cudaMemcpyDeviceToHost, diff --git a/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp index 9a3e13b81120f..c535fc6762e1f 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp @@ -62,9 +62,9 @@ void LidarInstanceSegmentationNode::pointCloudCallback( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_lidar_centerpoint/CMakeLists.txt b/perception/autoware_lidar_centerpoint/CMakeLists.txt index 88244b3153349..322907c67c6ae 100644 --- a/perception/autoware_lidar_centerpoint/CMakeLists.txt +++ b/perception/autoware_lidar_centerpoint/CMakeLists.txt @@ -84,8 +84,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) lib/detection_class_remapper.cpp lib/utils.cpp lib/ros_utils.cpp - lib/network/network_trt.cpp - lib/network/tensorrt_wrapper.cpp lib/postprocess/non_maximum_suppression.cpp lib/preprocess/pointcloud_densification.cpp lib/preprocess/voxel_generator.cpp diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp index 671ff54f8ce0d..4b531e34877d6 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp @@ -16,12 +16,13 @@ #define AUTOWARE__LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_ #include "autoware/lidar_centerpoint/cuda_utils.hpp" -#include "autoware/lidar_centerpoint/network/network_trt.hpp" #include "autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp" #include "autoware/lidar_centerpoint/preprocess/voxel_generator.hpp" #include "pcl/point_cloud.h" #include "pcl/point_types.h" +#include + #include "sensor_msgs/msg/point_cloud2.hpp" #include @@ -31,31 +32,14 @@ namespace autoware::lidar_centerpoint { -class NetworkParam -{ -public: - NetworkParam(std::string onnx_path, std::string engine_path, std::string trt_precision) - : onnx_path_(std::move(onnx_path)), - engine_path_(std::move(engine_path)), - trt_precision_(std::move(trt_precision)) - { - } - - std::string onnx_path() const { return onnx_path_; } - std::string engine_path() const { return engine_path_; } - std::string trt_precision() const { return trt_precision_; } - -private: - std::string onnx_path_; - std::string engine_path_; - std::string trt_precision_; -}; +using autoware::tensorrt_common::ProfileDims; +using autoware::tensorrt_common::TrtCommonConfig; class CenterPointTRT { public: explicit CenterPointTRT( - const NetworkParam & encoder_param, const NetworkParam & head_param, + const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param, const DensificationParam & densification_param, const CenterPointConfig & config); virtual ~CenterPointTRT(); @@ -66,6 +50,7 @@ class CenterPointTRT protected: void initPtr(); + void initTrt(const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param); virtual bool preprocess( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); @@ -75,8 +60,8 @@ class CenterPointTRT void postProcess(std::vector & det_boxes3d); std::unique_ptr vg_ptr_{nullptr}; - std::unique_ptr encoder_trt_ptr_{nullptr}; - std::unique_ptr head_trt_ptr_{nullptr}; + std::unique_ptr encoder_trt_ptr_{nullptr}; + std::unique_ptr head_trt_ptr_{nullptr}; std::unique_ptr post_proc_ptr_{nullptr}; cudaStream_t stream_{nullptr}; diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/network_trt.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/network_trt.hpp deleted file mode 100644 index 17778d77ed798..0000000000000 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/network_trt.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2021 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__LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ -#define AUTOWARE__LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ - -#include "autoware/lidar_centerpoint/centerpoint_config.hpp" -#include "autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp" - -#include - -namespace autoware::lidar_centerpoint -{ -class VoxelEncoderTRT : public TensorRTWrapper -{ -public: - using TensorRTWrapper::TensorRTWrapper; - -protected: - bool setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) override; -}; - -class HeadTRT : public TensorRTWrapper -{ -public: - using TensorRTWrapper::TensorRTWrapper; - - HeadTRT(const std::vector & out_channel_sizes, const CenterPointConfig & config); - -protected: - bool setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) override; - - std::vector out_channel_sizes_; -}; - -} // namespace autoware::lidar_centerpoint - -#endif // AUTOWARE__LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp deleted file mode 100644 index d56ccc699add3..0000000000000 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2021 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__LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ -#define AUTOWARE__LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ - -#include "NvInfer.h" -#include "autoware/lidar_centerpoint/centerpoint_config.hpp" -#include "autoware/tensorrt_common/tensorrt_common.hpp" - -#include -#include -#include - -namespace autoware::lidar_centerpoint -{ - -class TensorRTWrapper -{ -public: - explicit TensorRTWrapper(const CenterPointConfig & config); - - virtual ~TensorRTWrapper(); - - bool init( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision); - - autoware::tensorrt_common::TrtUniquePtr context_{nullptr}; - -protected: - virtual bool setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) = 0; - - CenterPointConfig config_; - autoware::tensorrt_common::Logger logger_; - -private: - bool parseONNX( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision, - std::size_t workspace_size = (1ULL << 30)); - - bool saveEngine(const std::string & engine_path); - - bool loadEngine(const std::string & engine_path); - - bool createContext(); - - autoware::tensorrt_common::TrtUniquePtr runtime_{nullptr}; - autoware::tensorrt_common::TrtUniquePtr plan_{nullptr}; - autoware::tensorrt_common::TrtUniquePtr engine_{nullptr}; -}; - -} // namespace autoware::lidar_centerpoint - -#endif // AUTOWARE__LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ diff --git a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp index 530f59179d25b..c586eeb961716 100644 --- a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp @@ -28,39 +28,22 @@ #include #include #include +#include +#include #include namespace autoware::lidar_centerpoint { CenterPointTRT::CenterPointTRT( - const NetworkParam & encoder_param, const NetworkParam & head_param, + const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param, const DensificationParam & densification_param, const CenterPointConfig & config) : config_(config) { vg_ptr_ = std::make_unique(densification_param, config_); post_proc_ptr_ = std::make_unique(config_); - // encoder - encoder_trt_ptr_ = std::make_unique(config_); - encoder_trt_ptr_->init( - encoder_param.onnx_path(), encoder_param.engine_path(), encoder_param.trt_precision()); - encoder_trt_ptr_->context_->setBindingDimensions( - 0, - nvinfer1::Dims3( - config_.max_voxel_size_, config_.max_point_in_voxel_size_, config_.encoder_in_feature_size_)); - - // head - std::vector out_channel_sizes = { - config_.class_size_, config_.head_out_offset_size_, config_.head_out_z_size_, - config_.head_out_dim_size_, config_.head_out_rot_size_, config_.head_out_vel_size_}; - head_trt_ptr_ = std::make_unique(out_channel_sizes, config_); - head_trt_ptr_->init(head_param.onnx_path(), head_param.engine_path(), head_param.trt_precision()); - head_trt_ptr_->context_->setBindingDimensions( - 0, nvinfer1::Dims4( - config_.batch_size_, config_.encoder_out_feature_size_, config_.grid_size_y_, - config_.grid_size_x_)); - initPtr(); + initTrt(encoder_param, head_param); cudaStreamCreate(&stream_); } @@ -126,6 +109,66 @@ void CenterPointTRT::initPtr() cudaMemcpyHostToDevice, stream_)); } +void CenterPointTRT::initTrt( + const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param) +{ + // encoder input profile + auto enc_in_dims = nvinfer1::Dims{ + 3, + {static_cast(config_.max_voxel_size_), + static_cast(config_.max_point_in_voxel_size_), + static_cast(config_.encoder_in_feature_size_)}}; + std::vector encoder_profile_dims{ + tensorrt_common::ProfileDims(0, enc_in_dims, enc_in_dims, enc_in_dims)}; + auto encoder_profile_dims_ptr = + std::make_unique>(encoder_profile_dims); + + // head input profile + auto head_in_dims = nvinfer1::Dims{ + 4, + {static_cast(config_.batch_size_), + static_cast(config_.encoder_out_feature_size_), + static_cast(config_.grid_size_y_), static_cast(config_.grid_size_x_)}}; + std::vector head_profile_dims{ + tensorrt_common::ProfileDims(0, head_in_dims, head_in_dims, head_in_dims)}; + std::unordered_map out_channel_map = { + {1, static_cast(config_.class_size_)}, + {2, static_cast(config_.head_out_offset_size_)}, + {3, static_cast(config_.head_out_z_size_)}, + {4, static_cast(config_.head_out_dim_size_)}, + {5, static_cast(config_.head_out_rot_size_)}, + {6, static_cast(config_.head_out_vel_size_)}}; + for (const auto & [tensor_name, channel_size] : out_channel_map) { + auto dims = nvinfer1::Dims{ + 4, + {static_cast(config_.batch_size_), channel_size, + static_cast(config_.down_grid_size_y_), + static_cast(config_.down_grid_size_x_)}}; + head_profile_dims.emplace_back(tensor_name, dims, dims, dims); + } + auto head_profile_dims_ptr = + std::make_unique>(head_profile_dims); + + // initialize trt wrappers + encoder_trt_ptr_ = std::make_unique(encoder_param); + + head_trt_ptr_ = std::make_unique(head_param); + + // setup trt engines + if ( + !encoder_trt_ptr_->setup(std::move(encoder_profile_dims_ptr)) || + !head_trt_ptr_->setup(std::move(head_profile_dims_ptr))) { + throw std::runtime_error("Failed to setup TRT engine."); + } + + // set input shapes + if ( + !encoder_trt_ptr_->setInputShape(0, enc_in_dims) || + !head_trt_ptr_->setInputShape(0, head_in_dims)) { + throw std::runtime_error("Failed to set input shape."); + } +} + bool CenterPointTRT::detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, std::vector & det_boxes3d, bool & is_num_pillars_within_range) @@ -210,13 +253,10 @@ bool CenterPointTRT::preprocess( void CenterPointTRT::inference() { - if (!encoder_trt_ptr_->context_ || !head_trt_ptr_->context_) { - throw std::runtime_error("Failed to create tensorrt context."); - } - // pillar encoder network - std::vector encoder_buffers{encoder_in_features_d_.get(), pillar_features_d_.get()}; - encoder_trt_ptr_->context_->enqueueV2(encoder_buffers.data(), stream_, nullptr); + std::vector encoder_tensors = {encoder_in_features_d_.get(), pillar_features_d_.get()}; + encoder_trt_ptr_->setTensorsAddresses(encoder_tensors); + encoder_trt_ptr_->enqueueV3(stream_); // scatter CHECK_CUDA_ERROR(scatterFeatures_launch( @@ -225,11 +265,13 @@ void CenterPointTRT::inference() spatial_features_d_.get(), stream_)); // head network - std::vector head_buffers = {spatial_features_d_.get(), head_out_heatmap_d_.get(), + std::vector head_tensors = {spatial_features_d_.get(), head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), head_out_rot_d_.get(), head_out_vel_d_.get()}; - head_trt_ptr_->context_->enqueueV2(head_buffers.data(), stream_, nullptr); + head_trt_ptr_->setTensorsAddresses(head_tensors); + + head_trt_ptr_->enqueueV3(stream_); } void CenterPointTRT::postProcess(std::vector & det_boxes3d) diff --git a/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp b/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp deleted file mode 100644 index 7767c0b5cbb02..0000000000000 --- a/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2021 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/lidar_centerpoint/network/network_trt.hpp" - -#include -#include - -namespace autoware::lidar_centerpoint -{ -bool VoxelEncoderTRT::setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) -{ - auto profile = builder.createOptimizationProfile(); - auto in_name = network.getInput(0)->getName(); - auto in_dims = nvinfer1::Dims3( - config_.max_voxel_size_, config_.max_point_in_voxel_size_, config_.encoder_in_feature_size_); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kMIN, in_dims); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kOPT, in_dims); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kMAX, in_dims); - - auto out_name = network.getOutput(0)->getName(); - auto out_dims = nvinfer1::Dims2(config_.max_voxel_size_, config_.encoder_out_feature_size_); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kMIN, out_dims); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kOPT, out_dims); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kMAX, out_dims); - config.addOptimizationProfile(profile); - - return true; -} - -HeadTRT::HeadTRT( - const std::vector & out_channel_sizes, const CenterPointConfig & config) -: TensorRTWrapper(config), out_channel_sizes_(out_channel_sizes) -{ -} - -bool HeadTRT::setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) -{ - auto profile = builder.createOptimizationProfile(); - auto in_name = network.getInput(0)->getName(); - auto in_dims = nvinfer1::Dims4( - config_.batch_size_, config_.encoder_out_feature_size_, config_.grid_size_y_, - config_.grid_size_x_); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kMIN, in_dims); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kOPT, in_dims); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kMAX, in_dims); - - for (std::size_t ci = 0; ci < out_channel_sizes_.size(); ci++) { - auto out_name = network.getOutput(ci)->getName(); - - if ( - out_name == std::string("heatmap") && - network.getOutput(ci)->getDimensions().d[1] != static_cast(out_channel_sizes_[ci])) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Expected and actual number of classes do not match" << std::endl; - return false; - } - auto out_dims = nvinfer1::Dims4( - config_.batch_size_, out_channel_sizes_[ci], config_.down_grid_size_y_, - config_.down_grid_size_x_); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kMIN, out_dims); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kOPT, out_dims); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kMAX, out_dims); - } - config.addOptimizationProfile(profile); - - return true; -} - -} // namespace autoware::lidar_centerpoint diff --git a/perception/autoware_lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/autoware_lidar_centerpoint/lib/network/tensorrt_wrapper.cpp deleted file mode 100644 index 6d4c2e053b34f..0000000000000 --- a/perception/autoware_lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ /dev/null @@ -1,169 +0,0 @@ -// Copyright 2021 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/lidar_centerpoint/network/tensorrt_wrapper.hpp" - -#include "NvOnnxParser.h" - -#include -#include -#include - -namespace autoware::lidar_centerpoint -{ -TensorRTWrapper::TensorRTWrapper(const CenterPointConfig & config) : config_(config) -{ -} - -TensorRTWrapper::~TensorRTWrapper() -{ - context_.reset(); - runtime_.reset(); - plan_.reset(); - engine_.reset(); -} - -bool TensorRTWrapper::init( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision) -{ - runtime_ = autoware::tensorrt_common::TrtUniquePtr( - nvinfer1::createInferRuntime(logger_)); - if (!runtime_) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create runtime" << std::endl; - return false; - } - - bool success; - std::ifstream engine_file(engine_path); - if (engine_file.is_open()) { - success = loadEngine(engine_path); - } else { - auto log_thread = logger_.log_throttle( - nvinfer1::ILogger::Severity::kINFO, - "Applying optimizations and building TRT CUDA engine. Please wait a minutes...", 5); - success = parseONNX(onnx_path, engine_path, precision); - logger_.stop_throttle(log_thread); - } - success &= createContext(); - - return success; -} - -bool TensorRTWrapper::createContext() -{ - if (!engine_) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Failed to create context: Engine was not created" << std::endl; - return false; - } - - context_ = autoware::tensorrt_common::TrtUniquePtr( - engine_->createExecutionContext()); - if (!context_) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create context" << std::endl; - return false; - } - - return true; -} - -bool TensorRTWrapper::parseONNX( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision, - const std::size_t workspace_size) -{ - auto builder = autoware::tensorrt_common::TrtUniquePtr( - nvinfer1::createInferBuilder(logger_)); - if (!builder) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create builder" << std::endl; - return false; - } - - auto config = autoware::tensorrt_common::TrtUniquePtr( - builder->createBuilderConfig()); - if (!config) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create config" << std::endl; - return false; - } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 - config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, workspace_size); -#else - config->setMaxWorkspaceSize(workspace_size); -#endif - if (precision == "fp16") { - if (builder->platformHasFastFp16()) { - autoware::tensorrt_common::LOG_INFO(logger_) << "Using TensorRT FP16 Inference" << std::endl; - config->setFlag(nvinfer1::BuilderFlag::kFP16); - } else { - autoware::tensorrt_common::LOG_INFO(logger_) - << "TensorRT FP16 Inference isn't supported in this environment" << std::endl; - } - } - - const auto flag = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - auto network = autoware::tensorrt_common::TrtUniquePtr( - builder->createNetworkV2(flag)); - if (!network) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create network" << std::endl; - return false; - } - - auto parser = autoware::tensorrt_common::TrtUniquePtr( - nvonnxparser::createParser(*network, logger_)); - parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); - - if (!setProfile(*builder, *network, *config)) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to set profile" << std::endl; - return false; - } - - plan_ = autoware::tensorrt_common::TrtUniquePtr( - builder->buildSerializedNetwork(*network, *config)); - if (!plan_) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Failed to create serialized network" << std::endl; - return false; - } - engine_ = autoware::tensorrt_common::TrtUniquePtr( - runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); - if (!engine_) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create engine" << std::endl; - return false; - } - - return saveEngine(engine_path); -} - -bool TensorRTWrapper::saveEngine(const std::string & engine_path) -{ - autoware::tensorrt_common::LOG_INFO(logger_) << "Writing to " << engine_path << std::endl; - std::ofstream file(engine_path, std::ios::out | std::ios::binary); - file.write(reinterpret_cast(plan_->data()), plan_->size()); - return true; -} - -bool TensorRTWrapper::loadEngine(const std::string & engine_path) -{ - std::ifstream engine_file(engine_path); - std::stringstream engine_buffer; - engine_buffer << engine_file.rdbuf(); - std::string engine_str = engine_buffer.str(); - engine_ = - autoware::tensorrt_common::TrtUniquePtr(runtime_->deserializeCudaEngine( - reinterpret_cast(engine_str.data()), engine_str.size())); - autoware::tensorrt_common::LOG_INFO(logger_) << "Loaded engine from " << engine_path << std::endl; - return true; -} - -} // namespace autoware::lidar_centerpoint diff --git a/perception/autoware_lidar_centerpoint/src/node.cpp b/perception/autoware_lidar_centerpoint/src/node.cpp index 825fc40234120..19d1475db9051 100644 --- a/perception/autoware_lidar_centerpoint/src/node.cpp +++ b/perception/autoware_lidar_centerpoint/src/node.cpp @@ -86,8 +86,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti iou_bev_nms_.setParameters(p); } - NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); - NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); + TrtCommonConfig encoder_param(encoder_onnx_path, trt_precision, encoder_engine_path); + TrtCommonConfig head_param(head_onnx_path, trt_precision, head_engine_path); DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); diff --git a/perception/autoware_lidar_transfusion/CMakeLists.txt b/perception/autoware_lidar_transfusion/CMakeLists.txt index c3a56f883fbe5..fdb978c64946f 100644 --- a/perception/autoware_lidar_transfusion/CMakeLists.txt +++ b/perception/autoware_lidar_transfusion/CMakeLists.txt @@ -84,7 +84,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/detection_class_remapper.cpp - lib/network/network_trt.cpp lib/postprocess/non_maximum_suppression.cpp lib/preprocess/voxel_generator.cpp lib/preprocess/pointcloud_densification.cpp diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp deleted file mode 100644 index 71cd08acdb8c8..0000000000000 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp +++ /dev/null @@ -1,87 +0,0 @@ -// 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__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ -#define AUTOWARE__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ - -#include "autoware/lidar_transfusion/transfusion_config.hpp" -#include "autoware/lidar_transfusion/utils.hpp" - -#include - -#include - -#include -#include -#include -#include -#include -#include - -namespace autoware::lidar_transfusion -{ - -struct ProfileDimension -{ - nvinfer1::Dims min; - nvinfer1::Dims opt; - nvinfer1::Dims max; - - bool operator!=(const ProfileDimension & rhs) const - { - return min.nbDims != rhs.min.nbDims || opt.nbDims != rhs.opt.nbDims || - max.nbDims != rhs.max.nbDims || !std::equal(min.d, min.d + min.nbDims, rhs.min.d) || - !std::equal(opt.d, opt.d + opt.nbDims, rhs.opt.d) || - !std::equal(max.d, max.d + max.nbDims, rhs.max.d); - } -}; - -class NetworkTRT -{ -public: - explicit NetworkTRT(const TransfusionConfig & config); - ~NetworkTRT(); - - bool init( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision); - const char * getTensorName(NetworkIO name); - - autoware::tensorrt_common::TrtUniquePtr engine{nullptr}; - autoware::tensorrt_common::TrtUniquePtr context{nullptr}; - -private: - bool parseONNX( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision, - std::size_t workspace_size = (1ULL << 30)); - bool saveEngine(const std::string & engine_path); - bool loadEngine(const std::string & engine_path); - bool createContext(); - bool setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config); - bool validateNetworkIO(); - nvinfer1::Dims validateTensorShape(NetworkIO name, const std::vector shape); - - autoware::tensorrt_common::TrtUniquePtr runtime_{nullptr}; - autoware::tensorrt_common::TrtUniquePtr plan_{nullptr}; - autoware::tensorrt_common::Logger logger_; - TransfusionConfig config_; - std::vector tensors_names_; - - std::array in_profile_dims_; -}; - -} // namespace autoware::lidar_transfusion - -#endif // AUTOWARE__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp index 363ee17a0d6e0..e60ee04150b5c 100644 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp @@ -35,21 +35,21 @@ class TransfusionConfig if (voxels_num.size() == 3) { max_voxels_ = voxels_num[2]; - voxels_num_[0] = voxels_num[0]; - voxels_num_[1] = voxels_num[1]; - voxels_num_[2] = voxels_num[2]; + voxels_num_[0] = static_cast(voxels_num[0]); + voxels_num_[1] = static_cast(voxels_num[1]); + voxels_num_[2] = static_cast(voxels_num[2]); - min_voxel_size_ = voxels_num[0]; - opt_voxel_size_ = voxels_num[1]; - max_voxel_size_ = voxels_num[2]; + min_voxel_size_ = static_cast(voxels_num[0]); + opt_voxel_size_ = static_cast(voxels_num[1]); + max_voxel_size_ = static_cast(voxels_num[2]); - min_points_size_ = voxels_num[0]; - opt_points_size_ = voxels_num[1]; - max_points_size_ = voxels_num[2]; + min_points_size_ = static_cast(voxels_num[0]); + opt_points_size_ = static_cast(voxels_num[1]); + max_points_size_ = static_cast(voxels_num[2]); - min_coors_size_ = voxels_num[0]; - opt_coors_size_ = voxels_num[1]; - max_coors_size_ = voxels_num[2]; + min_coors_size_ = static_cast(voxels_num[0]); + opt_coors_size_ = static_cast(voxels_num[1]); + max_coors_size_ = static_cast(voxels_num[2]); } if (point_cloud_range.size() == 6) { min_x_range_ = static_cast(point_cloud_range[0]); @@ -91,7 +91,7 @@ class TransfusionConfig std::size_t cloud_capacity_{}; ///// KERNEL PARAMETERS ///// const std::size_t threads_for_voxel_{256}; // threads number for a block - const std::size_t points_per_voxel_{20}; + const int32_t points_per_voxel_{20}; const std::size_t warp_size_{32}; // one warp(32 threads) for one pillar const std::size_t pillars_per_block_{64}; // one thread deals with one pillar // and a block has pillars_per_block threads @@ -99,9 +99,9 @@ class TransfusionConfig std::size_t max_voxels_{60000}; ///// NETWORK PARAMETERS ///// - const std::size_t batch_size_{1}; - const std::size_t num_classes_{5}; - const std::size_t num_point_feature_size_{5}; // x, y, z, intensity, lag + const int32_t batch_size_{1}; + const int32_t num_classes_{5}; + const int32_t num_point_feature_size_{5}; // x, y, z, intensity, lag // the dimension of the input cloud float min_x_range_{-76.8}; float max_x_range_{76.8}; @@ -114,9 +114,9 @@ class TransfusionConfig float voxel_y_size_{0.3}; float voxel_z_size_{8.0}; const std::size_t out_size_factor_{4}; - const std::size_t max_num_points_per_pillar_{points_per_voxel_}; - const std::size_t num_point_values_{4}; - std::size_t num_proposals_{200}; + const int32_t max_num_points_per_pillar_{points_per_voxel_}; + const int32_t num_point_values_{4}; + int32_t num_proposals_{200}; // the number of feature maps for pillar scatter const std::size_t num_feature_scatter_{pillar_feature_size_}; // the score threshold for classification @@ -126,7 +126,7 @@ class TransfusionConfig std::size_t max_num_pillars_{max_voxels_}; const std::size_t pillar_points_bev_{max_num_points_per_pillar_ * max_num_pillars_}; // the detected boxes result decode by (x, y, z, w, l, h, yaw) - const std::size_t num_box_values_{8}; + const int32_t num_box_values_{8}; // the input size of the 2D backbone network std::size_t grid_x_size_{512}; std::size_t grid_y_size_{512}; @@ -136,33 +136,33 @@ class TransfusionConfig std::size_t feature_y_size_{grid_y_size_ / out_size_factor_}; ///// RUNTIME DIMENSIONS ///// - std::vector voxels_num_{5000, 30000, 60000}; + std::vector voxels_num_{5000, 30000, 60000}; // voxels - std::size_t min_voxel_size_{voxels_num_[0]}; - std::size_t opt_voxel_size_{voxels_num_[1]}; - std::size_t max_voxel_size_{voxels_num_[2]}; + int32_t min_voxel_size_{voxels_num_[0]}; + int32_t opt_voxel_size_{voxels_num_[1]}; + int32_t max_voxel_size_{voxels_num_[2]}; - std::size_t min_point_in_voxel_size_{points_per_voxel_}; - std::size_t opt_point_in_voxel_size_{points_per_voxel_}; - std::size_t max_point_in_voxel_size_{points_per_voxel_}; + int32_t min_point_in_voxel_size_{points_per_voxel_}; + int32_t opt_point_in_voxel_size_{points_per_voxel_}; + int32_t max_point_in_voxel_size_{points_per_voxel_}; - std::size_t min_network_feature_size_{num_point_feature_size_}; - std::size_t opt_network_feature_size_{num_point_feature_size_}; - std::size_t max_network_feature_size_{num_point_feature_size_}; + int32_t min_network_feature_size_{num_point_feature_size_}; + int32_t opt_network_feature_size_{num_point_feature_size_}; + int32_t max_network_feature_size_{num_point_feature_size_}; // num_points - std::size_t min_points_size_{voxels_num_[0]}; - std::size_t opt_points_size_{voxels_num_[1]}; - std::size_t max_points_size_{voxels_num_[2]}; + int32_t min_points_size_{voxels_num_[0]}; + int32_t opt_points_size_{voxels_num_[1]}; + int32_t max_points_size_{voxels_num_[2]}; // coors - std::size_t min_coors_size_{voxels_num_[0]}; - std::size_t opt_coors_size_{voxels_num_[1]}; - std::size_t max_coors_size_{voxels_num_[2]}; + int32_t min_coors_size_{voxels_num_[0]}; + int32_t opt_coors_size_{voxels_num_[1]}; + int32_t max_coors_size_{voxels_num_[2]}; - std::size_t min_coors_dim_size_{num_point_values_}; - std::size_t opt_coors_dim_size_{num_point_values_}; - std::size_t max_coors_dim_size_{num_point_values_}; + int32_t min_coors_dim_size_{num_point_values_}; + int32_t opt_coors_dim_size_{num_point_values_}; + int32_t max_coors_dim_size_{num_point_values_}; }; } // namespace autoware::lidar_transfusion diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp index 06dd65b4b805f..00bb5e726706c 100644 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp @@ -16,7 +16,6 @@ #define AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ #include "autoware/lidar_transfusion/cuda_utils.hpp" -#include "autoware/lidar_transfusion/network/network_trt.hpp" #include "autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp" #include "autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp" #include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" @@ -24,6 +23,7 @@ #include "autoware/lidar_transfusion/utils.hpp" #include "autoware/lidar_transfusion/visibility_control.hpp" +#include #include #include @@ -34,38 +34,17 @@ #include #include #include -#include #include namespace autoware::lidar_transfusion { -class NetworkParam -{ -public: - NetworkParam(std::string onnx_path, std::string engine_path, std::string trt_precision) - : onnx_path_(std::move(onnx_path)), - engine_path_(std::move(engine_path)), - trt_precision_(std::move(trt_precision)) - { - } - - std::string onnx_path() const { return onnx_path_; } - std::string engine_path() const { return engine_path_; } - std::string trt_precision() const { return trt_precision_; } - -private: - std::string onnx_path_; - std::string engine_path_; - std::string trt_precision_; -}; - class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT { public: explicit TransfusionTRT( - const NetworkParam & network_param, const DensificationParam & densification_param, - const TransfusionConfig & config); + const tensorrt_common::TrtCommonConfig & trt_config, + const DensificationParam & densification_param, const TransfusionConfig config); virtual ~TransfusionTRT(); bool detect( @@ -73,6 +52,8 @@ class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT std::vector & det_boxes3d, std::unordered_map & proc_timing); protected: + void initTrt(const tensorrt_common::TrtCommonConfig & trt_config); + void initPtr(); bool preprocess(const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer); @@ -81,7 +62,7 @@ class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT bool postprocess(std::vector & det_boxes3d); - std::unique_ptr network_trt_ptr_{nullptr}; + std::unique_ptr network_trt_ptr_{nullptr}; std::unique_ptr vg_ptr_{nullptr}; std::unique_ptr> stop_watch_ptr_{ nullptr}; diff --git a/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp b/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp deleted file mode 100644 index 8e3cb8a55ec7e..0000000000000 --- a/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp +++ /dev/null @@ -1,357 +0,0 @@ -// 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/lidar_transfusion/network/network_trt.hpp" - -#include - -#include -#include -#include -#include -#include - -namespace autoware::lidar_transfusion -{ - -inline NetworkIO nameToNetworkIO(const char * name) -{ - static const std::unordered_map name_to_enum = { - {"voxels", NetworkIO::voxels}, {"num_points", NetworkIO::num_points}, - {"coors", NetworkIO::coors}, {"cls_score0", NetworkIO::cls_score}, - {"bbox_pred0", NetworkIO::bbox_pred}, {"dir_cls_pred0", NetworkIO::dir_pred}}; - - auto it = name_to_enum.find(name); - if (it != name_to_enum.end()) { - return it->second; - } - throw std::runtime_error("Invalid input name: " + std::string(name)); -} - -std::ostream & operator<<(std::ostream & os, const ProfileDimension & profile) -{ - std::string delim = ""; - os << "min->["; - for (int i = 0; i < profile.min.nbDims; ++i) { - os << delim << profile.min.d[i]; - delim = ", "; - } - os << "], opt->["; - delim = ""; - for (int i = 0; i < profile.opt.nbDims; ++i) { - os << delim << profile.opt.d[i]; - delim = ", "; - } - os << "], max->["; - delim = ""; - for (int i = 0; i < profile.max.nbDims; ++i) { - os << delim << profile.max.d[i]; - delim = ", "; - } - os << "]"; - return os; -} - -NetworkTRT::NetworkTRT(const TransfusionConfig & config) : config_(config) -{ - ProfileDimension voxels_dims = { - nvinfer1::Dims3( - config_.min_voxel_size_, config_.min_point_in_voxel_size_, config_.min_network_feature_size_), - nvinfer1::Dims3( - config_.opt_voxel_size_, config_.opt_point_in_voxel_size_, config_.opt_network_feature_size_), - nvinfer1::Dims3( - config_.max_voxel_size_, config_.max_point_in_voxel_size_, - config_.max_network_feature_size_)}; - ProfileDimension num_points_dims = { - nvinfer1::Dims{1, {static_cast(config_.min_points_size_)}}, - nvinfer1::Dims{1, {static_cast(config_.opt_points_size_)}}, - nvinfer1::Dims{1, {static_cast(config_.max_points_size_)}}}; - ProfileDimension coors_dims = { - nvinfer1::Dims2(config_.min_coors_size_, config_.min_coors_dim_size_), - nvinfer1::Dims2(config_.opt_coors_size_, config_.opt_coors_dim_size_), - nvinfer1::Dims2(config_.max_coors_size_, config_.max_coors_dim_size_)}; - in_profile_dims_ = {voxels_dims, num_points_dims, coors_dims}; -} - -NetworkTRT::~NetworkTRT() -{ - context.reset(); - runtime_.reset(); - plan_.reset(); - engine.reset(); -} - -bool NetworkTRT::init( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision) -{ - runtime_ = autoware::tensorrt_common::TrtUniquePtr( - nvinfer1::createInferRuntime(logger_)); - if (!runtime_) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create runtime" << std::endl; - return false; - } - - bool success; - std::ifstream engine_file(engine_path); - if (engine_file.is_open()) { - success = loadEngine(engine_path); - } else { - auto log_thread = logger_.log_throttle( - nvinfer1::ILogger::Severity::kINFO, - "Applying optimizations and building TRT CUDA engine. Please wait a minutes...", 5); - success = parseONNX(onnx_path, engine_path, precision); - logger_.stop_throttle(log_thread); - } - success &= createContext(); - - return success; -} - -bool NetworkTRT::setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) -{ - auto profile = builder.createOptimizationProfile(); - - auto voxels_name = network.getInput(NetworkIO::voxels)->getName(); - auto num_points_name = network.getInput(NetworkIO::num_points)->getName(); - auto coors_name = network.getInput(NetworkIO::coors)->getName(); - - profile->setDimensions( - voxels_name, nvinfer1::OptProfileSelector::kMIN, in_profile_dims_[NetworkIO::voxels].min); - profile->setDimensions( - voxels_name, nvinfer1::OptProfileSelector::kOPT, in_profile_dims_[NetworkIO::voxels].opt); - profile->setDimensions( - voxels_name, nvinfer1::OptProfileSelector::kMAX, in_profile_dims_[NetworkIO::voxels].max); - - profile->setDimensions( - num_points_name, nvinfer1::OptProfileSelector::kMIN, - in_profile_dims_[NetworkIO::num_points].min); - profile->setDimensions( - num_points_name, nvinfer1::OptProfileSelector::kOPT, - in_profile_dims_[NetworkIO::num_points].opt); - profile->setDimensions( - num_points_name, nvinfer1::OptProfileSelector::kMAX, - in_profile_dims_[NetworkIO::num_points].max); - - profile->setDimensions( - coors_name, nvinfer1::OptProfileSelector::kMIN, in_profile_dims_[NetworkIO::coors].min); - profile->setDimensions( - coors_name, nvinfer1::OptProfileSelector::kOPT, in_profile_dims_[NetworkIO::coors].opt); - profile->setDimensions( - coors_name, nvinfer1::OptProfileSelector::kMAX, in_profile_dims_[NetworkIO::coors].max); - - config.addOptimizationProfile(profile); - return true; -} - -bool NetworkTRT::createContext() -{ - if (!engine) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Failed to create context: Engine was not created" << std::endl; - return false; - } - - context = autoware::tensorrt_common::TrtUniquePtr( - engine->createExecutionContext()); - if (!context) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create context" << std::endl; - return false; - } - - return true; -} - -bool NetworkTRT::parseONNX( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision, - const size_t workspace_size) -{ - auto builder = autoware::tensorrt_common::TrtUniquePtr( - nvinfer1::createInferBuilder(logger_)); - if (!builder) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create builder" << std::endl; - return false; - } - - auto config = autoware::tensorrt_common::TrtUniquePtr( - builder->createBuilderConfig()); - if (!config) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create config" << std::endl; - return false; - } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 - config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, workspace_size); -#else - config->setMaxWorkspaceSize(workspace_size); -#endif - if (precision == "fp16") { - if (builder->platformHasFastFp16()) { - autoware::tensorrt_common::LOG_INFO(logger_) << "Using TensorRT FP16 Inference" << std::endl; - config->setFlag(nvinfer1::BuilderFlag::kFP16); - } else { - autoware::tensorrt_common::LOG_INFO(logger_) - << "TensorRT FP16 Inference isn't supported in this environment" << std::endl; - } - } - - const auto flag = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - auto network = autoware::tensorrt_common::TrtUniquePtr( - builder->createNetworkV2(flag)); - if (!network) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create network" << std::endl; - return false; - } - - auto parser = autoware::tensorrt_common::TrtUniquePtr( - nvonnxparser::createParser(*network, logger_)); - parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); - - if (!setProfile(*builder, *network, *config)) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to set profile" << std::endl; - return false; - } - - plan_ = autoware::tensorrt_common::TrtUniquePtr( - builder->buildSerializedNetwork(*network, *config)); - if (!plan_) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Failed to create serialized network" << std::endl; - return false; - } - engine = autoware::tensorrt_common::TrtUniquePtr( - runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); - if (!engine) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create engine" << std::endl; - return false; - } - - return saveEngine(engine_path); -} - -bool NetworkTRT::saveEngine(const std::string & engine_path) -{ - autoware::tensorrt_common::LOG_INFO(logger_) << "Writing to " << engine_path << std::endl; - std::ofstream file(engine_path, std::ios::out | std::ios::binary); - file.write(reinterpret_cast(plan_->data()), plan_->size()); - return validateNetworkIO(); -} - -bool NetworkTRT::loadEngine(const std::string & engine_path) -{ - std::ifstream engine_file(engine_path); - std::stringstream engine_buffer; - engine_buffer << engine_file.rdbuf(); - std::string engine_str = engine_buffer.str(); - engine = - autoware::tensorrt_common::TrtUniquePtr(runtime_->deserializeCudaEngine( - reinterpret_cast(engine_str.data()), engine_str.size())); - autoware::tensorrt_common::LOG_INFO(logger_) << "Loaded engine from " << engine_path << std::endl; - return validateNetworkIO(); -} - -bool NetworkTRT::validateNetworkIO() -{ - // Whether the number of IO match the expected size - if (engine->getNbIOTensors() != NetworkIO::ENUM_SIZE) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Invalid network IO. Expected size: " << NetworkIO::ENUM_SIZE - << ". Actual size: " << engine->getNbIOTensors() << "." << std::endl; - throw std::runtime_error("Failed to initialize TRT network."); - } - - // Initialize tensors_names_ with null values - tensors_names_.resize(NetworkIO::ENUM_SIZE, nullptr); - - // Loop over the tensor names and place them in the correct positions - for (int i = 0; i < NetworkIO::ENUM_SIZE; ++i) { - const char * name = engine->getIOTensorName(i); - tensors_names_[nameToNetworkIO(name)] = name; - } - - // Log the network IO - std::string tensors = std::accumulate( - tensors_names_.begin(), tensors_names_.end(), std::string(), - [](const std::string & a, const std::string & b) -> std::string { return a + b + " "; }); - autoware::tensorrt_common::LOG_INFO(logger_) << "Network IO: " << tensors << std::endl; - - // Whether the current engine input profile match the config input profile - for (int i = 0; i <= NetworkIO::coors; ++i) { - ProfileDimension engine_dims{ - engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kMIN), - engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kOPT), - engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kMAX)}; - - autoware::tensorrt_common::LOG_INFO(logger_) - << "Profile for " << tensors_names_[i] << ": " << engine_dims << std::endl; - - if (engine_dims != in_profile_dims_[i]) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Invalid network input dimension. Config: " << in_profile_dims_[i] - << ". Please change the input profile or delete the engine file and build engine again." - << std::endl; - throw std::runtime_error("Failed to initialize TRT network."); - } - } - - // Whether the IO tensor shapes match the network config, -1 for dynamic size - validateTensorShape( - NetworkIO::voxels, {-1, static_cast(config_.points_per_voxel_), - static_cast(config_.num_point_feature_size_)}); - validateTensorShape(NetworkIO::num_points, {-1}); - validateTensorShape(NetworkIO::coors, {-1, static_cast(config_.num_point_values_)}); - auto cls_score = validateTensorShape( - NetworkIO::cls_score, - {static_cast(config_.batch_size_), static_cast(config_.num_classes_), - static_cast(config_.num_proposals_)}); - autoware::tensorrt_common::LOG_INFO(logger_) - << "Network num classes: " << cls_score.d[1] << std::endl; - validateTensorShape( - NetworkIO::dir_pred, - {static_cast(config_.batch_size_), 2, static_cast(config_.num_proposals_)}); // x, y - validateTensorShape( - NetworkIO::bbox_pred, - {static_cast(config_.batch_size_), static_cast(config_.num_box_values_), - static_cast(config_.num_proposals_)}); - - return true; -} - -const char * NetworkTRT::getTensorName(NetworkIO name) -{ - return tensors_names_.at(name); -} - -nvinfer1::Dims NetworkTRT::validateTensorShape(NetworkIO name, const std::vector shape) -{ - auto tensor_shape = engine->getTensorShape(tensors_names_[name]); - if (tensor_shape.nbDims != static_cast(shape.size())) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Invalid tensor shape for " << tensors_names_[name] << ". Expected size: " << shape.size() - << ". Actual size: " << tensor_shape.nbDims << "." << std::endl; - throw std::runtime_error("Failed to initialize TRT network."); - } - for (int i = 0; i < tensor_shape.nbDims; ++i) { - if (tensor_shape.d[i] != static_cast(shape[i])) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Invalid tensor shape for " << tensors_names_[name] << ". Expected: " << shape[i] - << ". Actual: " << tensor_shape.d[i] << "." << std::endl; - throw std::runtime_error("Failed to initialize TRT network."); - } - } - return tensor_shape; -} - -} // namespace autoware::lidar_transfusion diff --git a/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp index 4e100f2e794d5..dd075a45cb29c 100644 --- a/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp +++ b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp @@ -17,6 +17,7 @@ #include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" #include "autoware/lidar_transfusion/transfusion_config.hpp" +#include #include #include @@ -27,25 +28,23 @@ #include #include #include +#include #include namespace autoware::lidar_transfusion { TransfusionTRT::TransfusionTRT( - const NetworkParam & network_param, const DensificationParam & densification_param, - const TransfusionConfig & config) -: config_(config) + const tensorrt_common::TrtCommonConfig & trt_config, + const DensificationParam & densification_param, TransfusionConfig config) +: config_(std::move(config)) { - network_trt_ptr_ = std::make_unique(config_); - - network_trt_ptr_->init( - network_param.onnx_path(), network_param.engine_path(), network_param.trt_precision()); vg_ptr_ = std::make_unique(densification_param, config_, stream_); stop_watch_ptr_ = std::make_unique>(); stop_watch_ptr_->tic("processing/inner"); initPtr(); + initTrt(trt_config); CHECK_CUDA_ERROR(cudaStreamCreate(&stream_)); } @@ -99,6 +98,51 @@ void TransfusionTRT::initPtr() post_ptr_ = std::make_unique(config_, stream_); } +void TransfusionTRT::initTrt(const tensorrt_common::TrtCommonConfig & trt_config) +{ + std::vector network_io{ + autoware::tensorrt_common::NetworkIO( + "voxels", {3, {-1, config_.points_per_voxel_, config_.num_point_feature_size_}}), + autoware::tensorrt_common::NetworkIO("num_points", {1, {-1}}), + autoware::tensorrt_common::NetworkIO("coors", {2, {-1, config_.num_point_values_}}), + autoware::tensorrt_common::NetworkIO( + "cls_score0", {3, {config_.batch_size_, config_.num_classes_, config_.num_proposals_}}), + autoware::tensorrt_common::NetworkIO( + "bbox_pred0", {3, {config_.batch_size_, config_.num_box_values_, config_.num_proposals_}}), + autoware::tensorrt_common::NetworkIO( + "dir_cls_pred0", {3, {config_.batch_size_, 2, config_.num_proposals_}})}; + + std::vector profile_dims{ + autoware::tensorrt_common::ProfileDims( + "voxels", + {3, + {config_.min_voxel_size_, config_.min_point_in_voxel_size_, + config_.min_network_feature_size_}}, + {3, + {config_.opt_voxel_size_, config_.opt_point_in_voxel_size_, + config_.opt_network_feature_size_}}, + {3, + {config_.max_voxel_size_, config_.max_point_in_voxel_size_, + config_.max_network_feature_size_}}), + autoware::tensorrt_common::ProfileDims( + "num_points", {1, {static_cast(config_.min_points_size_)}}, + {1, {static_cast(config_.opt_points_size_)}}, + {1, {static_cast(config_.max_points_size_)}}), + autoware::tensorrt_common::ProfileDims( + "coors", {2, {config_.min_coors_size_, config_.min_coors_dim_size_}}, + {2, {config_.opt_coors_size_, config_.opt_coors_dim_size_}}, + {2, {config_.max_coors_size_, config_.max_coors_dim_size_}})}; + + auto network_io_ptr = + std::make_unique>(network_io); + auto profile_dims_ptr = + std::make_unique>(profile_dims); + + network_trt_ptr_ = std::make_unique(trt_config); + if (!network_trt_ptr_->setup(std::move(profile_dims_ptr), std::move(network_io_ptr))) + throw std::runtime_error("Failed to setup TRT engine."); +} + bool TransfusionTRT::detect( const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer, std::vector & det_boxes3d, std::unordered_map & proc_timing) @@ -166,8 +210,9 @@ bool TransfusionTRT::preprocess( CHECK_CUDA_ERROR(cudaMemcpyAsync( ¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost, stream_)); CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + auto params_input_i32 = static_cast(params_input); - if (params_input < config_.min_voxel_size_) { + if (params_input_i32 < config_.min_voxel_size_) { RCLCPP_WARN_STREAM( rclcpp::get_logger("lidar_transfusion"), "Too few voxels (" << params_input << ") for actual optimization profile (" @@ -185,39 +230,34 @@ bool TransfusionTRT::preprocess( params_input = config_.max_voxels_; } + RCLCPP_DEBUG_STREAM( rclcpp::get_logger("lidar_transfusion"), "Generated input voxels: " << params_input); - network_trt_ptr_->context->setTensorAddress( - network_trt_ptr_->getTensorName(NetworkIO::voxels), voxel_features_d_.get()); - network_trt_ptr_->context->setInputShape( - network_trt_ptr_->getTensorName(NetworkIO::voxels), + bool success = true; + + // Inputs + success &= network_trt_ptr_->setTensor( + "voxels", voxel_features_d_.get(), nvinfer1::Dims3{ - static_cast(params_input), static_cast(config_.max_num_points_per_pillar_), + params_input_i32, config_.max_num_points_per_pillar_, static_cast(config_.num_point_feature_size_)}); - network_trt_ptr_->context->setTensorAddress( - network_trt_ptr_->getTensorName(NetworkIO::num_points), voxel_num_d_.get()); - network_trt_ptr_->context->setInputShape( - network_trt_ptr_->getTensorName(NetworkIO::num_points), - nvinfer1::Dims{1, {static_cast(params_input)}}); - network_trt_ptr_->context->setTensorAddress( - network_trt_ptr_->getTensorName(NetworkIO::coors), voxel_idxs_d_.get()); - network_trt_ptr_->context->setInputShape( - network_trt_ptr_->getTensorName(NetworkIO::coors), - nvinfer1::Dims2{ - static_cast(params_input), static_cast(config_.num_point_values_)}); - network_trt_ptr_->context->setTensorAddress( - network_trt_ptr_->getTensorName(NetworkIO::cls_score), cls_output_d_.get()); - network_trt_ptr_->context->setTensorAddress( - network_trt_ptr_->getTensorName(NetworkIO::bbox_pred), box_output_d_.get()); - network_trt_ptr_->context->setTensorAddress( - network_trt_ptr_->getTensorName(NetworkIO::dir_pred), dir_cls_output_d_.get()); - return true; + success &= network_trt_ptr_->setTensor( + "num_points", voxel_num_d_.get(), nvinfer1::Dims{1, {params_input_i32}}); + success &= network_trt_ptr_->setTensor( + "coors", voxel_idxs_d_.get(), nvinfer1::Dims2{params_input_i32, config_.num_point_values_}); + + // Outputs + success &= network_trt_ptr_->setTensor("cls_score0", cls_output_d_.get()); + success &= network_trt_ptr_->setTensor("bbox_pred0", box_output_d_.get()); + success &= network_trt_ptr_->setTensor("dir_cls_pred0", dir_cls_output_d_.get()); + + return success; } bool TransfusionTRT::inference() { - auto status = network_trt_ptr_->context->enqueueV3(stream_); + auto status = network_trt_ptr_->enqueueV3(stream_); CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); if (!status) { diff --git a/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp index 8bb70a821621f..563abd97698e0 100644 --- a/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp @@ -75,7 +75,6 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) const float score_threshold = static_cast(this->declare_parameter("score_threshold", descriptor)); - NetworkParam network_param(onnx_path, engine_path, trt_precision); DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); TransfusionConfig config( @@ -91,7 +90,9 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); - detector_ptr_ = std::make_unique(network_param, densification_param, config); + auto trt_config = tensorrt_common::TrtCommonConfig(onnx_path, trt_precision, engine_path); + + detector_ptr_ = std::make_unique(trt_config, densification_param, config); cloud_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), @@ -155,7 +156,6 @@ void LidarTransfusionNode::cloudCallback(const sensor_msgs::msg::PointCloud2::Co objects_pub_->publish(output_msg); published_time_pub_->publish_if_subscribed(objects_pub_, output_msg.header.stamp); } - // add processing time for debug if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic", true); diff --git a/perception/autoware_object_merger/src/object_association_merger_node.cpp b/perception/autoware_object_merger/src/object_association_merger_node.cpp index 0e2b88f4aa566..40bdd0a7d6938 100644 --- a/perception/autoware_object_merger/src/object_association_merger_node.cpp +++ b/perception/autoware_object_merger/src/object_association_merger_node.cpp @@ -235,9 +235,9 @@ void ObjectAssociationMergerNode::objectsCallback( merged_object_pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(merged_object_pub_, output_msg.header.stamp); // publish processing time - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } } // namespace autoware::object_merger diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp index eb52b903b90ca..094825056e62c 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp +++ b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp @@ -420,9 +420,9 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index 880dfda03c495..7cde04ca9614b 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -332,11 +332,11 @@ void GridMapFusionNode::publish() std::chrono::duration( std::chrono::nanoseconds((this->get_clock()->now() - latest_stamp).nanoseconds())) .count(); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 4d442ad2c33db..ff836a1ddf760 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -261,11 +261,11 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( std::chrono::nanoseconds( (this->get_clock()->now() - input_raw_msg->header.stamp).nanoseconds())) .count(); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/autoware_shape_estimation/CMakeLists.txt b/perception/autoware_shape_estimation/CMakeLists.txt index d0eb2aa5535a8..d7313111e6ade 100644 --- a/perception/autoware_shape_estimation/CMakeLists.txt +++ b/perception/autoware_shape_estimation/CMakeLists.txt @@ -4,6 +4,9 @@ project(autoware_shape_estimation) find_package(autoware_cmake REQUIRED) autoware_package() +# TODO(amadeuszsz): Remove -Wno-deprecated-declarations once removing implicit quantization +add_compile_options(-Wno-deprecated-declarations) + find_package(PCL REQUIRED COMPONENTS common) find_package(pcl_conversions REQUIRED) diff --git a/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp index d39dce65d224d..4328246ff395f 100644 --- a/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp +++ b/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp @@ -48,11 +48,7 @@ class TrtShapeEstimator { public: TrtShapeEstimator( - const std::string & model_path, const std::string & precision, - const autoware::tensorrt_common::BatchConfig & batch_config, - const size_t max_workspace_size = (1 << 30), - const autoware::tensorrt_common::BuildConfig build_config = - autoware::tensorrt_common::BuildConfig("MinMax", -1, false, false, false, 0.0)); + const std::string & model_path, const std::string & precision, const int batch_size); ~TrtShapeEstimator() = default; diff --git a/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp b/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp index b4b1f18832da7..fa3c3cb09bad3 100644 --- a/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp +++ b/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp @@ -29,47 +29,41 @@ namespace autoware::shape_estimation { TrtShapeEstimator::TrtShapeEstimator( - const std::string & model_path, const std::string & precision, - const autoware::tensorrt_common::BatchConfig & batch_config, const size_t max_workspace_size, - const autoware::tensorrt_common::BuildConfig build_config) + const std::string & model_path, const std::string & precision, const int batch_size) { trt_common_ = std::make_unique( - model_path, precision, nullptr, batch_config, max_workspace_size, build_config); + tensorrt_common::TrtCommonConfig(model_path, precision)); + batch_size_ = batch_size; - trt_common_->setup(); - - if (!trt_common_->isInitialized()) { - std::cerr << "Failed to initialize TensorRT" << std::endl; - return; + if (!trt_common_->setup()) { + throw std::runtime_error("Failed to setup TensorRT"); } - const auto pc_input_dims = trt_common_->getBindingDimensions(0); + const auto pc_input_dims = trt_common_->getTensorShape(0); const auto pc_input_size = std::accumulate( pc_input_dims.d + 1, pc_input_dims.d + pc_input_dims.nbDims, 1, std::multiplies()); - input_pc_d_ = autoware::cuda_utils::make_unique(pc_input_size * batch_config[2]); - batch_size_ = batch_config[2]; - const auto one_hot_input_dims = trt_common_->getBindingDimensions(1); + input_pc_d_ = autoware::cuda_utils::make_unique(pc_input_size * batch_size_); + const auto one_hot_input_dims = trt_common_->getTensorShape(1); const auto one_hot_input_size = std::accumulate( one_hot_input_dims.d + 1, one_hot_input_dims.d + one_hot_input_dims.nbDims, 1, std::multiplies()); - input_one_hot_d_ = - autoware::cuda_utils::make_unique(one_hot_input_size * batch_config[2]); + input_one_hot_d_ = autoware::cuda_utils::make_unique(one_hot_input_size * batch_size_); - const auto stage1_center_out_dims = trt_common_->getBindingDimensions(2); + const auto stage1_center_out_dims = trt_common_->getTensorShape(2); out_s1center_elem_num_ = std::accumulate( stage1_center_out_dims.d + 1, stage1_center_out_dims.d + stage1_center_out_dims.nbDims, 1, std::multiplies()); - out_s1center_elem_num_ = out_s1center_elem_num_ * batch_config[2]; - out_s1center_elem_num_per_batch_ = static_cast(out_s1center_elem_num_ / batch_config[2]); + out_s1center_elem_num_ = out_s1center_elem_num_ * batch_size_; + out_s1center_elem_num_per_batch_ = static_cast(out_s1center_elem_num_ / batch_size_); out_s1center_prob_d_ = autoware::cuda_utils::make_unique(out_s1center_elem_num_); out_s1center_prob_h_ = autoware::cuda_utils::make_unique_host(out_s1center_elem_num_, cudaHostAllocPortable); - const auto pred_out_dims = trt_common_->getBindingDimensions(3); + const auto pred_out_dims = trt_common_->getTensorShape(3); out_pred_elem_num_ = std::accumulate( pred_out_dims.d + 1, pred_out_dims.d + pred_out_dims.nbDims, 1, std::multiplies()); - out_pred_elem_num_ = out_pred_elem_num_ * batch_config[2]; - out_pred_elem_num_per_batch_ = static_cast(out_pred_elem_num_ / batch_config[2]); + out_pred_elem_num_ = out_pred_elem_num_ * batch_size_; + out_pred_elem_num_per_batch_ = static_cast(out_pred_elem_num_ / batch_size_); out_pred_prob_d_ = autoware::cuda_utils::make_unique(out_pred_elem_num_); out_pred_prob_h_ = autoware::cuda_utils::make_unique_host(out_pred_elem_num_, cudaHostAllocPortable); @@ -83,10 +77,6 @@ TrtShapeEstimator::TrtShapeEstimator( bool TrtShapeEstimator::inference( const DetectedObjectsWithFeature & input, DetectedObjectsWithFeature & output) { - if (!trt_common_->isInitialized()) { - return false; - } - bool result = false; for (size_t i = 0; i < input.feature_objects.size(); i += batch_size_) { @@ -111,13 +101,13 @@ bool TrtShapeEstimator::inference( void TrtShapeEstimator::preprocess(const DetectedObjectsWithFeature & input) { - auto input_dims_pc = trt_common_->getBindingDimensions(0); + auto input_dims_pc = trt_common_->getTensorShape(0); int batch_size = static_cast(input.feature_objects.size()); const auto input_chan = static_cast(input_dims_pc.d[1]); const auto input_pc_size = static_cast(input_dims_pc.d[2]); - auto input_dims_one_hot = trt_common_->getBindingDimensions(1); + auto input_dims_one_hot = trt_common_->getTensorShape(1); const auto input_one_hot_size = static_cast(input_dims_one_hot.d[1]); int volume_pc = batch_size * input_chan * input_pc_size; @@ -229,7 +219,10 @@ bool TrtShapeEstimator::feed_forward_and_decode( int batch_size = static_cast(input.feature_objects.size()); std::vector buffers = { input_pc_d_.get(), input_one_hot_d_.get(), out_s1center_prob_d_.get(), out_pred_prob_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); CHECK_CUDA_ERROR(cudaMemcpyAsync( out_s1center_prob_h_.get(), out_s1center_prob_d_.get(), out_s1center_elem_num_ * sizeof(float), diff --git a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp index 67402f68d63ef..28b2a6e398e91 100644 --- a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp +++ b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp @@ -63,9 +63,8 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option min_points_ = declare_parameter("model_params.minimum_points"); std::string precision = declare_parameter("model_params.precision"); int batch_size = declare_parameter("model_params.batch_size"); - autoware::tensorrt_common::BatchConfig batch_config{batch_size, batch_size, batch_size}; tensorrt_shape_estimator_ = - std::make_unique(model_path, precision, batch_config); + std::make_unique(model_path, precision, batch_size); if (this->declare_parameter("model_params.build_only", false)) { RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); @@ -180,9 +179,9 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared // Publish pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(pub_, output_msg.header.stamp); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } } // namespace autoware::shape_estimation diff --git a/perception/autoware_tensorrt_classifier/CMakeLists.txt b/perception/autoware_tensorrt_classifier/CMakeLists.txt index 88747a1e9240c..59ce7b18afcc3 100644 --- a/perception/autoware_tensorrt_classifier/CMakeLists.txt +++ b/perception/autoware_tensorrt_classifier/CMakeLists.txt @@ -6,6 +6,9 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3 -Wno-write-strings -fopen find_package(autoware_cmake REQUIRED) autoware_package() +# TODO(amadeuszsz): Remove -Wno-deprecated-declarations once removing implicit quantization +add_compile_options(-Wno-deprecated-declarations) + find_package(CUDA) find_package(CUDNN) find_package(TENSORRT) diff --git a/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp index ee16343b956d1..4f7a5865c72ae 100644 --- a/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp +++ b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -44,22 +45,18 @@ class TrtClassifier * @param[in] mode_path ONNX model_path * @param[in] precision precision for inference * @param[in] calibration_images path for calibration files (only require for quantization) - * @param[in] batch_config configuration for batched execution - * @param[in] max_workspace_size maximum workspace for building TensorRT engine * @param[in] mean mean for preprocess * @param[in] std std for preprocess - * @param[in] buildConfig configuration including precision, calibration method, dla, remaining - * fp16 for first layer, remaining fp16 for last layer and profiler for builder + * @param[in] calib_config calibration configuration * @param[in] cuda whether use cuda gpu for preprocessing */ TrtClassifier( const std::string & model_path, const std::string & precision, - const autoware::tensorrt_common::BatchConfig & batch_config = {1, 1, 1}, const std::vector & mean = {123.675, 116.28, 103.53}, const std::vector & std = {58.395, 57.12, 57.375}, - const size_t max_workspace_size = (1 << 30), const std::string & calibration_images = "", - const autoware::tensorrt_common::BuildConfig build_config = - autoware::tensorrt_common::BuildConfig("MinMax", -1, false, false, false, 0.0), + const std::string & calibration_images = "", + const tensorrt_common::CalibrationConfig & calibration_config = + tensorrt_common::CalibrationConfig("MinMax", false, false, 0.0), const bool cuda = false); /** * @brief Deconstruct TrtClassifier @@ -84,6 +81,12 @@ class TrtClassifier */ void initPreprocessBuffer(int width, int height); + /** + * @brief get batch size + * @return batch size + */ + [[nodiscard]] int getBatchSize() const; + private: /** * @brief run preprocess including resizing, letterbox, BGR2RGB, NHWC2NCHW and toFloat on CPU @@ -102,7 +105,7 @@ class TrtClassifier const std::vector & images, std::vector & results, std::vector & probabilities); - std::unique_ptr trt_common_; + std::unique_ptr trt_common_; std::vector input_h_; CudaUniquePtr input_d_; diff --git a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp index 4aa008e42c966..7666ae4f9d068 100644 --- a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp +++ b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp @@ -100,11 +100,9 @@ std::vector loadImageList(const std::string & filename, const std:: namespace autoware::tensorrt_classifier { TrtClassifier::TrtClassifier( - const std::string & model_path, const std::string & precision, - const autoware::tensorrt_common::BatchConfig & batch_config, const std::vector & mean, - const std::vector & std, const size_t max_workspace_size, - const std::string & calibration_image_list_path, - autoware::tensorrt_common::BuildConfig build_config, const bool cuda) + const std::string & model_path, const std::string & precision, const std::vector & mean, + const std::vector & std, const std::string & calibration_image_list_path, + const tensorrt_common::CalibrationConfig & calib_config, const bool cuda) { src_width_ = -1; src_height_ = -1; @@ -114,22 +112,36 @@ TrtClassifier::TrtClassifier( for (size_t i = 0; i < inv_std_.size(); i++) { inv_std_[i] = 1.0 / inv_std_[i]; } - batch_size_ = batch_config[2]; + trt_common_ = std::make_unique( + tensorrt_common::TrtCommonConfig(model_path, precision)); + + const auto network_input_dims = trt_common_->getTensorShape(0); + batch_size_ = network_input_dims.d[0]; + const auto input_channel = network_input_dims.d[1]; + const auto input_height = network_input_dims.d[2]; + const auto input_width = network_input_dims.d[3]; + + std::vector profile_dims{ + autoware::tensorrt_common::ProfileDims( + 0, {4, {batch_size_, input_channel, input_height, input_width}}, + {4, {batch_size_, input_channel, input_height, input_width}}, + {4, {batch_size_, input_channel, input_height, input_width}})}; + auto profile_dims_ptr = + std::make_unique>(profile_dims); + if (precision == "int8") { - int max_batch_size = batch_config[2]; - nvinfer1::Dims input_dims = autoware::tensorrt_common::get_input_dims(model_path); std::vector calibration_images; if (calibration_image_list_path != "") { calibration_images = loadImageList(calibration_image_list_path, ""); } autoware::tensorrt_classifier::ImageStream stream( - max_batch_size, input_dims, calibration_images); + batch_size_, network_input_dims, calibration_images); fs::path calibration_table{model_path}; std::string ext = ""; - if (build_config.calib_type_str == "Entropy") { + if (calib_config.calib_type_str == "Entropy") { ext = "EntropyV2-"; } else if ( - build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + calib_config.calib_type_str == "Legacy" || calib_config.calib_type_str == "Percentile") { ext = "Legacy-"; } else { ext = "MinMax-"; @@ -141,11 +153,11 @@ TrtClassifier::TrtClassifier( histogram_table.replace_extension(ext); std::unique_ptr calibrator; - if (build_config.calib_type_str == "Entropy") { + if (calib_config.calib_type_str == "Entropy") { calibrator.reset(new autoware::tensorrt_classifier::Int8EntropyCalibrator( stream, calibration_table, mean_, std_)); } else if ( - build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + calib_config.calib_type_str == "Legacy" || calib_config.calib_type_str == "Percentile") { double quantile = 0.999999; double cutoff = 0.999999; calibrator.reset(new autoware::tensorrt_classifier::Int8LegacyCalibrator( @@ -154,29 +166,27 @@ TrtClassifier::TrtClassifier( calibrator.reset(new autoware::tensorrt_classifier::Int8MinMaxCalibrator( stream, calibration_table, mean_, std_)); } - trt_common_ = std::make_unique( - model_path, precision, std::move(calibrator), batch_config, max_workspace_size, build_config); + if (!trt_common_->setupWithCalibrator( + std::move(calibrator), calib_config, std::move((profile_dims_ptr)))) { + throw std::runtime_error("Failed to setup TensorRT engine with calibrator"); + } } else { - trt_common_ = std::make_unique( - model_path, precision, nullptr, batch_config, max_workspace_size, build_config); - } - trt_common_->setup(); - - if (!trt_common_->isInitialized()) { - return; + if (!trt_common_->setup(std::move(profile_dims_ptr))) { + throw std::runtime_error("Failed to setup TensorRT engine"); + } } // GPU memory allocation - const auto input_dims = trt_common_->getBindingDimensions(0); + const auto input_dims = trt_common_->getTensorShape(0); const auto input_size = std::accumulate(input_dims.d + 1, input_dims.d + input_dims.nbDims, 1, std::multiplies()); - const auto output_dims = trt_common_->getBindingDimensions(1); - input_d_ = autoware::cuda_utils::make_unique(batch_config[2] * input_size); + const auto output_dims = trt_common_->getTensorShape(1); + input_d_ = autoware::cuda_utils::make_unique(batch_size_ * input_size); out_elem_num_ = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); - out_elem_num_ = out_elem_num_ * batch_config[2]; - out_elem_num_per_batch_ = static_cast(out_elem_num_ / batch_config[2]); + out_elem_num_ = out_elem_num_ * batch_size_; + out_elem_num_per_batch_ = static_cast(out_elem_num_ / batch_size_); out_prob_d_ = autoware::cuda_utils::make_unique(out_elem_num_); out_prob_h_ = autoware::cuda_utils::make_unique_host(out_elem_num_, cudaHostAllocPortable); @@ -224,7 +234,7 @@ void TrtClassifier::initPreprocessBuffer(int width, int height) src_width_ = width; src_height_ = height; if (m_cuda) { - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); bool const hasRuntimeDim = std::any_of( input_dims.d, input_dims.d + input_dims.nbDims, [](int32_t input_dim) { return input_dim == -1; }); @@ -232,7 +242,9 @@ void TrtClassifier::initPreprocessBuffer(int width, int height) input_dims.d[0] = batch_size_; } if (!h_img_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } } if (!h_img_) { CHECK_CUDA_ERROR(cudaMallocHost( @@ -245,10 +257,15 @@ void TrtClassifier::initPreprocessBuffer(int width, int height) } } +int TrtClassifier::getBatchSize() const +{ + return batch_size_; +} + void TrtClassifier::preprocessGpu(const std::vector & images) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; for (const auto & image : images) { @@ -272,7 +289,9 @@ void TrtClassifier::preprocessGpu(const std::vector & images) src_height_ = height; } if (!h_img_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); @@ -305,9 +324,11 @@ void TrtClassifier::preprocessGpu(const std::vector & images) void TrtClassifier::preprocess_opt(const std::vector & images) { int batch_size = static_cast(images.size()); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } const float input_chan = static_cast(input_dims.d[1]); const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); @@ -354,9 +375,6 @@ bool TrtClassifier::doInference( const std::vector & images, std::vector & results, std::vector & probabilities) { - if (!trt_common_->isInitialized()) { - return false; - } preprocess_opt(images); return feedforwardAndDecode(images, results, probabilities); @@ -369,7 +387,10 @@ bool TrtClassifier::feedforwardAndDecode( results.clear(); probabilities.clear(); std::vector buffers = {input_d_.get(), out_prob_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); int batch_size = static_cast(images.size()); diff --git a/perception/autoware_tensorrt_common/CMakeLists.txt b/perception/autoware_tensorrt_common/CMakeLists.txt index 3105523a509e3..d7d8026866124 100644 --- a/perception/autoware_tensorrt_common/CMakeLists.txt +++ b/perception/autoware_tensorrt_common/CMakeLists.txt @@ -16,9 +16,14 @@ if(NOT (CUDAToolkit_FOUND AND CUDNN_FOUND AND TENSORRT_FOUND)) return() endif() +if(TENSORRT_VERSION VERSION_LESS 8.5) + message(WARNING "Unsupported version TensorRT ${TENSORRT_VERSION} detected. This package requires TensorRT 8.5 or later.") + return() +endif() + add_library(${PROJECT_NAME} SHARED src/tensorrt_common.cpp - src/simple_profiler.cpp + src/profiler.cpp ) target_link_libraries(${PROJECT_NAME} @@ -42,7 +47,7 @@ set_target_properties(${PROJECT_NAME} CXX_EXTENSIONS NO ) -# TODO(autoware_tensorrt_common): Remove -Wno-deprecated-declarations once upgrading to TensorRT 8.5 is complete +# TODO(amadeuszsz): Remove -Wno-deprecated-declarations once removing implicit quantization target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -Wpedantic -Werror -Wno-deprecated-declarations ) diff --git a/perception/autoware_tensorrt_common/README.md b/perception/autoware_tensorrt_common/README.md index c828be58c1c1c..54bfccdfcb561 100644 --- a/perception/autoware_tensorrt_common/README.md +++ b/perception/autoware_tensorrt_common/README.md @@ -1,6 +1,70 @@ # autoware_tensorrt_common -## Purpose +This package provides a high-level API to work with TensorRT. This library simplifies the process of loading, building, and executing TensorRT inference engines using ONNX models. It also includes utilities for profiling and managing TensorRT execution contexts, making it easier to integrate TensorRT-based packages in Autoware. -This package contains a library of common functions related to TensorRT. -This package may include functions for handling TensorRT engine and calibration algorithm used for quantization +## Usage + +Here is an example usage of the library. For the full API documentation, please refer to the doxygen documentation (see header [file](include/autoware/tensorrt_common/tensorrt_common.hpp)). + +```c++ +#include + +#include +#include +#include + +using autoware::tensorrt_common::TrtCommon; +using autoware::tensorrt_common::TrtCommonConfig; +using autoware::tensorrt_common::TensorInfo; +using autoware::tensorrt_common::NetworkIO; +using autoware::tensorrt_common::ProfileDims; + +std::unique_ptr trt_common_; +``` + +### Create a tensorrt common instance and setup engine + +- With minimal configuration. + +```c++ +trt_common_ = std::make_unique(TrtCommonConfig("/path/to/onnx/model.onnx")); +trt_common_->setup(); +``` + +- With full configuration. + +```c++ +trt_common_ = std::make_unique(TrtCommonConfig("/path/to/onnx/model.onnx", "fp16", "/path/to/engine/model.engine", (1ULL << 30U), -1, false)); + +std::vector network_io{ + NetworkIO("sample_input", {3, {-1, 64, 512}}), NetworkIO("sample_output", {1, {50}})}; +std::vector profile_dims{ + ProfileDims("sample_input", {3, {1, 64, 512}}, {3, {3, 64, 512}}, {3, {9, 64, 512}})}; + +auto network_io_ptr = std::make_unique>(network_io); +auto profile_dims_ptr = std::make_unique>(profile_dims); + +trt_common_->setup(std::move(profile_dims_ptr), std::move(network_io_ptr)); +``` + +By defining network IO names and dimensions, an extra shapes validation will be performed after building / loading engine. This is useful to ensure the model is compatible with current code for preprocessing as it might consists of operations dependent on tensor shapes. + +Profile dimension is used to specify the min, opt, and max dimensions for dynamic shapes. + +Network IO or / and profile dimensions can be omitted if not needed. + +### Setting input and output tensors + +```c++ +bool success = true; +success &= trt_common_->setTensor("sample_input", sample_input_d_.get(), nvinfer1::Dims{3, {var_size, 64, 512}}); +success &= trt_common_->setTensor("sample_output", sample_output_d_.get()); +return success; +``` + +### Execute inference + +```c++ +auto success = trt_common_->enqueueV3(stream_); +return success; +``` diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/conv_profiler.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/conv_profiler.hpp new file mode 100644 index 0000000000000..0b58a797d8b9b --- /dev/null +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/conv_profiler.hpp @@ -0,0 +1,108 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TENSORRT_COMMON__CONV_PROFILER_HPP_ +#define AUTOWARE__TENSORRT_COMMON__CONV_PROFILER_HPP_ + +#include + +#include + +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ +/** + * @struct ConvLayerInfo + * @brief Information of a layer. + */ +struct ConvLayerInfo +{ + //! @brief Input channel. + int in_c; + //! @brief Output channel. + int out_c; + //! @brief Width. + int w; + //! @brief Height. + int h; + //! @brief Kernel size. + int k; + //! @brief Stride. + int stride; + //! @brief Number of groups. + int groups; + //! @brief Layer type. + nvinfer1::LayerType type; +}; + +/** + * @class ConvProfiler + * @brief Collect per-layer profile information, assuming times are reported in the same order. + */ +class ConvProfiler : public tensorrt_common::Profiler +{ +public: + /** + * @brief Construct Profiler for convolutional layers. + * + * @param[in] src_profilers Source profilers to merge. + */ + explicit ConvProfiler( + const std::vector & src_profilers = std::vector()) + : Profiler(src_profilers) + { + } + + /** + * @brief Set per-layer profile information for model. + * + * @param[in] layer Layer to set profile information. + */ + void setProfDict(nvinfer1::ILayer * const layer) noexcept final + { + if (const auto type = layer->getType(); type == nvinfer1::LayerType::kCONVOLUTION) { + const auto name = layer->getName(); + auto conv = dynamic_cast(layer); + + nvinfer1::ITensor * in = layer->getInput(0); + nvinfer1::Dims in_dim = in->getDimensions(); + + nvinfer1::ITensor * out = layer->getOutput(0); + nvinfer1::Dims out_dim = out->getDimensions(); + + nvinfer1::Dims k_dims = conv->getKernelSizeNd(); + nvinfer1::Dims s_dims = conv->getStrideNd(); + + int32_t kernel = k_dims.d[0]; + int32_t stride = s_dims.d[0]; + int32_t groups = conv->getNbGroups(); + + layer_dict_.insert_or_assign( + name, ConvLayerInfo{ + in_dim.d[1], out_dim.d[1], in_dim.d[3], in_dim.d[2], kernel, stride, groups, type}); + } + } + +private: + //! @brief Per-layer information. + std::map layer_dict_; +}; +} // namespace tensorrt_common +} // namespace autoware +#endif // AUTOWARE__TENSORRT_COMMON__CONV_PROFILER_HPP_ diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp index 5aa897983af72..b9b9048cd8204 100644 --- a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp @@ -17,18 +17,19 @@ #ifndef AUTOWARE__TENSORRT_COMMON__LOGGER_HPP_ #define AUTOWARE__TENSORRT_COMMON__LOGGER_HPP_ -#include "NvInferRuntimeCommon.h" +#include #include #include +#include #include #include -#include #include #include #include #include #include +#include namespace autoware { @@ -46,7 +47,7 @@ class LogStreamConsumerBuffer : public std::stringbuf LogStreamConsumerBuffer(LogStreamConsumerBuffer && other) : mOutput(other.mOutput) {} - ~LogStreamConsumerBuffer() + ~LogStreamConsumerBuffer() override { // std::streambuf::pbase() gives a pointer to the beginning of the buffered part of the output // sequence std::streambuf::pptr() gives a pointer to the current position of the output @@ -60,7 +61,7 @@ class LogStreamConsumerBuffer : public std::stringbuf // synchronizes the stream buffer and returns 0 on success // synchronizing the stream buffer consists of inserting the buffer contents into the stream, // resetting the buffer and flushing the stream - virtual int sync() + int sync() override { putOutput(); return 0; @@ -252,6 +253,37 @@ class Logger : public nvinfer1::ILogger // NOLINT } } + void log(Severity severity, const char * msg, ...) const noexcept + { + if (mVerbose) { + va_list args; + va_start(args, msg); + + // Buffer size + va_list args_copy; + va_copy(args_copy, args); + int required_size = std::vsnprintf(nullptr, 0, msg, args_copy); + va_end(args_copy); + + // Formatting error + if (required_size < 0) { + log(Severity::kINTERNAL_ERROR, "Error formatting log message"); + va_end(args); + return; + } + + // Format msg + std::vector buffer(required_size + 1); + std::vsnprintf(buffer.data(), buffer.size(), msg, args); + + va_end(args); // End variadic argument processing + + // Send the formatted message to LogStreamConsumer + LogStreamConsumer(mReportableSeverity, severity) + << "[TRT] " << std::string(buffer.data()) << std::endl; + } + } + /** * @brief Logging with throttle. * diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/profiler.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/profiler.hpp new file mode 100644 index 0000000000000..d45351f97fd01 --- /dev/null +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/profiler.hpp @@ -0,0 +1,96 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TENSORRT_COMMON__PROFILER_HPP_ +#define AUTOWARE__TENSORRT_COMMON__PROFILER_HPP_ + +#include + +#include +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ + +/** + * @class Profiler + * @brief Collect per-layer profile information, assuming times are reported in the same order. + */ +class Profiler : public nvinfer1::IProfiler +{ +public: + /** + * @struct Record + * @brief Record of layer profile information. + */ + struct Record + { + float time{0}; + int count{0}; + float min_time{-1.0}; + int index; + }; + + /** + * @brief Construct Profiler. + * + * @param[in] src_profilers Source profilers to merge. + */ + explicit Profiler(const std::vector & src_profilers = std::vector()); + + /** + * @brief Report layer time. + * + * @param[in] layerName Layer name. + * @param[in] ms Time in milliseconds. + */ + void reportLayerTime(const char * layerName, float ms) noexcept final; + + /** + * @brief Get printable representation of Profiler. + * + * @return String representation for current state of Profiler. + */ + [[nodiscard]] std::string toString() const; + + /** + * @brief Set per-layer profile information for model. + * + * @param[in] layer Layer to set profile information. + */ + virtual void setProfDict([[maybe_unused]] nvinfer1::ILayer * layer) noexcept {}; + + /** + * @brief Output Profiler to ostream. + * + * @param[out] out Output stream. + * @param[in] value Profiler to output. + * @return Output stream. + */ + friend std::ostream & operator<<(std::ostream & out, const Profiler & value); + +private: + //!< @brief Profile information for layers. + std::map profile_; + + //!< @brief Index for layer. + int index_; +}; +} // namespace tensorrt_common +} // namespace autoware +#endif // AUTOWARE__TENSORRT_COMMON__PROFILER_HPP_ diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/simple_profiler.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/simple_profiler.hpp deleted file mode 100644 index a8d504fb2861a..0000000000000 --- a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/simple_profiler.hpp +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__TENSORRT_COMMON__SIMPLE_PROFILER_HPP_ -#define AUTOWARE__TENSORRT_COMMON__SIMPLE_PROFILER_HPP_ - -#include - -#include -#include -#include -#include - -namespace autoware -{ -namespace tensorrt_common -{ -struct LayerInfo -{ - int in_c; - int out_c; - int w; - int h; - int k; - int stride; - int groups; - nvinfer1::LayerType type; -}; - -/** - * @class Profiler - * @brief Collect per-layer profile information, assuming times are reported in the same order - */ -class SimpleProfiler : public nvinfer1::IProfiler -{ -public: - struct Record - { - float time{0}; - int count{0}; - float min_time{-1.0}; - int index; - }; - SimpleProfiler( - std::string name, - const std::vector & src_profilers = std::vector()); - - void reportLayerTime(const char * layerName, float ms) noexcept override; - - void setProfDict(nvinfer1::ILayer * layer) noexcept; - - friend std::ostream & operator<<(std::ostream & out, const SimpleProfiler & value); - -private: - std::string m_name; - std::map m_profile; - int m_index; - std::map m_layer_dict; -}; -} // namespace tensorrt_common -} // namespace autoware -#endif // AUTOWARE__TENSORRT_COMMON__SIMPLE_PROFILER_HPP_ diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp index 2b3b3f02f315f..9355732962e23 100644 --- a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp @@ -15,226 +15,367 @@ #ifndef AUTOWARE__TENSORRT_COMMON__TENSORRT_COMMON_HPP_ #define AUTOWARE__TENSORRT_COMMON__TENSORRT_COMMON_HPP_ -#ifndef YOLOX_STANDALONE -#include - -#include -#endif +#include "autoware/tensorrt_common/logger.hpp" +#include "autoware/tensorrt_common/profiler.hpp" +#include "autoware/tensorrt_common/utils.hpp" #include #include -#if (defined(_MSC_VER) or (defined(__GNUC__) and (7 <= __GNUC_MAJOR__))) -#include -namespace fs = ::std::filesystem; -#else -#include -namespace fs = ::std::experimental::filesystem; -#endif - -#include -#include - #include -#include #include +#include +#include #include namespace autoware { namespace tensorrt_common { -/** - * @struct BuildConfig - * @brief Configuration to provide fine control regarding TensorRT builder - */ -struct BuildConfig -{ - // type for calibration - std::string calib_type_str; - - // DLA core ID that the process uses - int dla_core_id; - - // flag for partial quantization in first layer - bool quantize_first_layer; // For partial quantization - - // flag for partial quantization in last layer - bool quantize_last_layer; // For partial quantization - - // flag for per-layer profiler using IProfiler - bool profile_per_layer; - - // clip value for implicit quantization - double clip_value; // For implicit quantization - - // Supported calibration type - const std::array valid_calib_type = {"Entropy", "Legacy", "Percentile", "MinMax"}; - - BuildConfig() - : calib_type_str("MinMax"), - dla_core_id(-1), - quantize_first_layer(false), - quantize_last_layer(false), - profile_per_layer(false), - clip_value(0.0) - { - } - - explicit BuildConfig( - const std::string & calib_type_str, const int dla_core_id = -1, - const bool quantize_first_layer = false, const bool quantize_last_layer = false, - const bool profile_per_layer = false, const double clip_value = 0.0) - : calib_type_str(calib_type_str), - dla_core_id(dla_core_id), - quantize_first_layer(quantize_first_layer), - quantize_last_layer(quantize_last_layer), - profile_per_layer(profile_per_layer), - clip_value(clip_value) - { -#ifndef YOLOX_STANDALONE - if ( - std::find(valid_calib_type.begin(), valid_calib_type.end(), calib_type_str) == - valid_calib_type.end()) { - std::stringstream message; - message << "Invalid calibration type was specified: " << calib_type_str << std::endl - << "Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]" << std::endl - << "Default calibration type will be used: MinMax" << std::endl; - std::cerr << message.str(); - } -#endif - } -}; - -nvinfer1::Dims get_input_dims(const std::string & onnx_file_path); - -const std::array valid_precisions = {"fp32", "fp16", "int8"}; -bool is_valid_precision_string(const std::string & precision); - template struct InferDeleter // NOLINT { - void operator()(T * obj) const - { - if (obj) { -#if TENSORRT_VERSION_MAJOR >= 8 - delete obj; -#else - obj->destroy(); -#endif - } - } + void operator()(T * obj) const { delete obj; } }; template using TrtUniquePtr = std::unique_ptr>; -using BatchConfig = std::array; +using NetworkIOPtr = std::unique_ptr>; +using ProfileDimsPtr = std::unique_ptr>; +using TensorsVec = std::vector>; +using TensorsMap = std::unordered_map>; /** * @class TrtCommon - * @brief TensorRT common library + * @brief TensorRT common library. */ class TrtCommon // NOLINT { public: /** * @brief Construct TrtCommon. - * @param[in] mode_path ONNX model_path - * @param[in] precision precision for inference - * @param[in] calibrator pointer for any type of INT8 calibrator - * @param[in] batch_config configuration for batched execution - * @param[in] max_workspace_size maximum workspace for building TensorRT engine - * @param[in] buildConfig configuration including precision, calibration method, dla, remaining - * fp16 for first layer, remaining fp16 for last layer and profiler for builder - * @param[in] plugin_paths path for custom plugin + * + * @param[in] trt_config Base configuration with ONNX model path as minimum required. + * parameter. + * @param[in] profiler Per-layer profiler. + * @param[in] plugin_paths Paths for TensorRT plugins. */ TrtCommon( - const std::string & model_path, const std::string & precision, - std::unique_ptr calibrator = nullptr, - const BatchConfig & batch_config = {1, 1, 1}, const size_t max_workspace_size = (16 << 20), - const BuildConfig & buildConfig = BuildConfig(), + const TrtCommonConfig & trt_config, + const std::shared_ptr & profiler = std::make_shared(), const std::vector & plugin_paths = {}); - /** * @brief Deconstruct TrtCommon */ ~TrtCommon(); /** - * @brief Load TensorRT engine - * @param[in] engine_file_path path for a engine file - * @return flag for whether loading are succeeded or failed + * @brief Setup for TensorRT execution including building and loading engine. + * + * @param[in] profile_dims Optimization profile of tensors for dynamic shapes. + * @param[in] network_io Network input/output tensors information. + * @return Whether setup is successful. + */ + [[nodiscard]] virtual bool setup( + ProfileDimsPtr profile_dims = nullptr, NetworkIOPtr network_io = nullptr); + + /** + * @brief Get TensorRT engine precision. + * + * @return string representation of TensorRT engine precision. + */ + [[nodiscard]] std::string getPrecision() const; + + /** + * @brief Get tensor name by index from TensorRT engine with fallback from TensorRT network. + * + * @param[in] index Tensor index. + * @return Tensor name. + */ + [[nodiscard]] const char * getIOTensorName(const int32_t index) const; + + /** + * @brief Get number of IO tensors from TensorRT engine with fallback from TensorRT network. + * + * @return Number of IO tensors. + */ + [[nodiscard]] int32_t getNbIOTensors() const; + + /** + * @brief Get tensor shape by index from TensorRT engine with fallback from TensorRT network. + * + * @param[in] index Tensor index. + * @return Tensor shape. + */ + [[nodiscard]] nvinfer1::Dims getTensorShape(const int32_t index) const; + + /** + * @brief Get tensor shape by name from TensorRT engine. + * + * @param[in] tensor_name Tensor name. + * @return Tensor shape. + */ + [[nodiscard]] nvinfer1::Dims getTensorShape(const char * tensor_name) const; + + /** + * @brief Get input tensor shape by index from TensorRT network. + * + * @param[in] index Tensor index. + * @return Tensor shape. + */ + [[nodiscard]] nvinfer1::Dims getInputDims(const int32_t index) const; + + /** + * @brief Get output tensor shape by index from TensorRT network. + * + * @param[in] index Tensor index. + * @return Tensor shape. + */ + [[nodiscard]] nvinfer1::Dims getOutputDims(const int32_t index) const; + + /** + * @brief Set tensor address by index via TensorRT context. + * + * @param[in] index Tensor index. + * @param[in] data Tensor pointer. + * @return Whether setting tensor address is successful. + */ + bool setTensorAddress(const int32_t index, void * data); + + /** + * @brief Set tensor address by name via TensorRT context. + * + * @param[in] tensor_name Tensor name. + * @param[in] data Tensor pointer. + * @return Whether setting tensor address is successful. + */ + bool setTensorAddress(const char * tensor_name, void * data); + + /** + * @brief Set tensors addresses by indices via TensorRT context. + * + * @param[in] tensors Tensors pointers. + * @return Whether setting tensors addresses is successful. + */ + bool setTensorsAddresses(std::vector & tensors); + + /** + * @brief Set tensors addresses by names via TensorRT context. + * + * @param[in] tensors Tensors pointers. + * @return Whether setting tensors addresses is successful. + */ + bool setTensorsAddresses(std::unordered_map & tensors); + + /** + * @brief Set input shape by index via TensorRT context. + * + * @param[in] index Tensor index. + * @param[in] dimensions Tensor dimensions. + * @return Whether setting input shape is successful. + */ + bool setInputShape(const int32_t index, const nvinfer1::Dims & dimensions); + + /** + * @brief Set input shape by name via TensorRT context. + * + * @param[in] tensor_name Tensor name. + * @param[in] dimensions Tensor dimensions. + * @return Whether setting input shape is successful. + */ + bool setInputShape(const char * tensor_name, const nvinfer1::Dims & dimensions); + + /** + * @brief Set inputs shapes by indices via TensorRT context. + * + * @param[in] dimensions Vector of tensor dimensions with corresponding indices. + * @return Whether setting input shapes is successful. + */ + bool setInputsShapes(const std::vector & dimensions); + + /** + * @brief Set inputs shapes by names via TensorRT context. + * + * @param[in] dimensions Map of tensor dimensions with corresponding names as keys. + * @return Whether setting input shapes is successful. + */ + bool setInputsShapes(const std::unordered_map & dimensions); + + /** + * @brief Set tensor (address and shape) by index via TensorRT context. + * + * @param[in] index Tensor index. + * @param[in] data Tensor pointer. + * @param[in] dimensions Tensor dimensions. + * @return Whether setting tensor is successful. + */ + bool setTensor(const int32_t index, void * data, nvinfer1::Dims dimensions = {}); + + /** + * @brief Set tensor (address and shape) by name via TensorRT context. + * + * @param[in] tensor_name Tensor name. + * @param[in] data Tensor pointer. + * @param[in] dimensions Tensor dimensions. + * @return Whether setting tensor is successful. + */ + bool setTensor(const char * tensor_name, void * data, nvinfer1::Dims dimensions = {}); + + /** + * @brief Set tensors (addresses and shapes) by indices via TensorRT context. + * + * @param[in] tensors Vector of tensor pointers and dimensions with corresponding indices. + * @return Whether setting tensors is successful. */ - bool loadEngine(const std::string & engine_file_path); + bool setTensors(TensorsVec & tensors); /** - * @brief Output layer information including GFLOPs and parameters - * @param[in] onnx_file_path path for a onnx file - * @warning This function is based on darknet log. + * @brief Set tensors (addresses and shapes) by names via TensorRT context. + * + * @param[in] tensors Map of tensor pointers and dimensions with corresponding names as keys. + * @return Whether setting tensors is successful. */ - void printNetworkInfo(const std::string & onnx_file_path); + bool setTensors(TensorsMap & tensors); /** - * @brief build TensorRT engine from ONNX - * @param[in] onnx_file_path path for a onnx file - * @param[in] output_engine_file_path path for a engine file + * @brief Get per-layer profiler for model. + * + * @return Per-layer profiler. */ - bool buildEngineFromOnnx( - const std::string & onnx_file_path, const std::string & output_engine_file_path); + [[nodiscard]] std::shared_ptr getModelProfiler(); /** - * @brief setup for TensorRT execution including building and loading engine + * @brief Get per-layer profiler for host. + * + * @return Per-layer profiler. */ - void setup(); + [[nodiscard]] std::shared_ptr getHostProfiler(); -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 - void setupBindings(std::vector & bindings); -#endif + /** + * @brief Get TensorRT common configuration. + * + * @return TensorRT common configuration. + */ + [[nodiscard]] std::shared_ptr getTrtCommonConfig(); - bool isInitialized(); + /** + * @brief Get TensorRT builder configuration. + * + * @return TensorRT builder configuration. + */ + [[nodiscard]] std::shared_ptr getBuilderConfig(); + + /** + * @brief Get TensorRT network definition. + * + * @return TensorRT network definition. + */ + [[nodiscard]] std::shared_ptr getNetwork(); - nvinfer1::Dims getBindingDimensions(const int32_t index) const; - int32_t getNbBindings(); - bool setBindingDimensions(const int32_t index, const nvinfer1::Dims & dimensions) const; -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 + /** + * @brief Get TensorRT logger. + * + * @return TensorRT logger. + */ + [[nodiscard]] std::shared_ptr getLogger(); + + /** + * @brief Execute inference via TensorRT context. + * + * @param[in] stream CUDA stream. + * @return Whether inference is successful. + */ bool enqueueV3(cudaStream_t stream); -#endif -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH < 10000 - bool enqueueV2(void ** bindings, cudaStream_t stream, cudaEvent_t * input_consumed); -#endif /** - * @brief output per-layer information + * @brief Print per-layer information. + */ + void printProfiling() const; + +private: + /** + * @brief Initialize TensorRT common. + * + * @return Whether initialization is successful. */ - void printProfiling(void); + [[nodiscard]] bool initialize(); -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 /** - * @brief get per-layer information for trt-engine-profiler + * @brief Get per-layer information for trt-engine-profiler. + * + * @param[in] format Format for layer information. + * @return Layer information. */ std::string getLayerInformation(nvinfer1::LayerInformationFormat format); -#endif -private: - Logger logger_; - fs::path model_file_path_; + /** + + * @brief Build TensorRT engine from ONNX. + * + * @return Whether building engine is successful. + */ + bool buildEngineFromOnnx(); + + /** + * @brief Load TensorRT engine. + * + * @return Whether loading engine is successful. + */ + bool loadEngine(); + + /** + * @brief Validate network input/output names and dimensions. + * + * @return Whether network input/output is valid. + */ + bool validateNetworkIO(); + + /** + * @brief Validate optimization profile. + * + * @return Whether optimization profile is valid. + */ + bool validateProfileDims(); + + //! @brief TensorRT runtime. TrtUniquePtr runtime_; + + //! @brief TensorRT engine. TrtUniquePtr engine_; + + //! @brief TensorRT execution context. TrtUniquePtr context_; - std::unique_ptr calibrator_; - std::string precision_; - BatchConfig batch_config_; - size_t max_workspace_size_; - bool is_initialized_{false}; + //! @brief TensorRT builder. + TrtUniquePtr builder_; + + //! @brief TensorRT engine parser. + TrtUniquePtr parser_; + + //! @brief TensorRT builder configuration. + std::shared_ptr builder_config_; + + //! @brief TensorRT network definition. + std::shared_ptr network_; + + //! @brief TrtCommon library base configuration. + std::shared_ptr trt_config_; + + //! @brief TensorRT logger. + std::shared_ptr logger_; + + //! @brief Per-layer profiler for host. + std::shared_ptr host_profiler_; + + //! @brief Per-layer profiler for model. + std::shared_ptr model_profiler_; - // profiler for per-layer - SimpleProfiler model_profiler_; - // profiler for whole model - SimpleProfiler host_profiler_; + //! @brief Model optimization profile. + ProfileDimsPtr profile_dims_; - std::unique_ptr build_config_; + //! @brief Model network input/output tensors information. + NetworkIOPtr network_io_; }; } // namespace tensorrt_common diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_conv_calib.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_conv_calib.hpp new file mode 100644 index 0000000000000..44a944031086e --- /dev/null +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_conv_calib.hpp @@ -0,0 +1,241 @@ +// 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__TENSORRT_COMMON__TENSORRT_CONV_CALIB_HPP_ +#define AUTOWARE__TENSORRT_COMMON__TENSORRT_CONV_CALIB_HPP_ + +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ +template +bool contain(const std::string & s, const T & v) +{ + return s.find(v) != std::string::npos; +} + +using autoware::tensorrt_common::CalibrationConfig; +using autoware::tensorrt_common::NetworkIOPtr; +using autoware::tensorrt_common::ProfileDimsPtr; +using autoware::tensorrt_common::Profiler; +using autoware::tensorrt_common::TrtCommonConfig; + +/** + * @class TrtConvCalib + * @brief TensorRT common library with calibration. + */ +class TrtConvCalib : public tensorrt_common::TrtCommon +{ +public: + /** + * @brief Construct TrtCommonCalib. + * + * @param[in] trt_config base trt common configuration, ONNX model path is mandatory + * @param[in] profiler per-layer profiler + * @param[in] plugin_paths paths for TensorRT plugins + */ + explicit TrtConvCalib( + const TrtCommonConfig & trt_config, + const std::shared_ptr & profiler = std::make_shared(), + const std::vector & plugin_paths = {}) + : TrtCommon(trt_config, profiler, plugin_paths) + { + } + + /** + * @brief Setup for TensorRT execution including calibration, building and loading engine. + * + * @param[in] calibrator Pointer for any type of INT8 calibrator. + * @param[in] calib_config Calibration configuration. + * @param[in] profile_dims Optimization profile of input tensors for dynamic shapes. + * @param[in] network_io Network input/output tensors information. + * @return Whether setup is successful. + */ + [[nodiscard]] bool setupWithCalibrator( + std::unique_ptr calibrator, const CalibrationConfig & calib_config, + ProfileDimsPtr profile_dims = nullptr, NetworkIOPtr network_io = nullptr) + { + calibrator_ = std::move(calibrator); + calib_config_ = std::make_unique(calib_config); + + auto builder_config = getBuilderConfig(); + builder_config->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); + builder_config->setInt8Calibrator(calibrator_.get()); + + // Model specific quantization + auto logger = getLogger(); + auto quantization_log = quantization(); + logger->log(nvinfer1::ILogger::Severity::kINFO, quantization_log.c_str()); + + return setup(std::move(profile_dims), std::move(network_io)); + } + +private: + /** + * @brief Implicit quantization for TensorRT. + * + * @return Output log for TensorRT logger. + */ + std::string quantization() + { + auto network = getNetwork(); + auto trt_config = getTrtCommonConfig(); + auto model_profiler = getModelProfiler(); + + const int num = network->getNbLayers(); + bool first = calib_config_->quantize_first_layer; + bool last = calib_config_->quantize_last_layer; + std::stringstream ss; + + // Partial Quantization + if (getPrecision() == "int8") { + auto builder_config = getBuilderConfig(); + builder_config->setFlag(nvinfer1::BuilderFlag::kFP16); + network->getInput(0)->setDynamicRange(0, 255.0); + for (int i = 0; i < num; i++) { + nvinfer1::ILayer * layer = network->getLayer(i); + auto layer_type = layer->getType(); + std::string name = layer->getName(); + nvinfer1::ITensor * out = layer->getOutput(0); + if (calib_config_->clip_value > 0.0) { + ss << "Set max value for outputs: " << calib_config_->clip_value << " " << name + << std::endl; + out->setDynamicRange(0.0, calib_config_->clip_value); + } + + if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { + if (first) { + layer->setPrecision(nvinfer1::DataType::kHALF); + ss << "Set kHALF in " << name << std::endl; + first = false; + } + if (last) { + // cspell: ignore preds + if ( + contain(name, "reg_preds") || contain(name, "cls_preds") || + contain(name, "obj_preds")) { + layer->setPrecision(nvinfer1::DataType::kHALF); + ss << "Set kHALF in " << name << std::endl; + } + for (int j = num - 1; j >= 0; j--) { + nvinfer1::ILayer * inner_layer = network->getLayer(j); + auto inner_layer_type = inner_layer->getType(); + std::string inner_name = inner_layer->getName(); + if (inner_layer_type == nvinfer1::LayerType::kCONVOLUTION) { + inner_layer->setPrecision(nvinfer1::DataType::kHALF); + ss << "Set kHALF in " << inner_name << std::endl; + break; + } + if (inner_layer_type == nvinfer1::LayerType::kMATRIX_MULTIPLY) { + inner_layer->setPrecision(nvinfer1::DataType::kHALF); + ss << "Set kHALF in " << inner_name << std::endl; + break; + } + } + } + } + } + builder_config->setFlag(nvinfer1::BuilderFlag::kINT8); + } + + // Print layer information + float total_gflops = 0.0; + int total_params = 0; + + for (int i = 0; i < num; i++) { + nvinfer1::ILayer * layer = network->getLayer(i); + auto layer_type = layer->getType(); + if (trt_config->profile_per_layer) { + model_profiler->setProfDict(layer); + } + if (layer_type == nvinfer1::LayerType::kCONSTANT) { + continue; + } + nvinfer1::ITensor * in = layer->getInput(0); + nvinfer1::Dims dim_in = in->getDimensions(); + nvinfer1::ITensor * out = layer->getOutput(0); + nvinfer1::Dims dim_out = out->getDimensions(); + + if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { + auto conv = dynamic_cast(layer); + nvinfer1::Dims k_dims = conv->getKernelSizeNd(); + nvinfer1::Dims s_dims = conv->getStrideNd(); + int groups = conv->getNbGroups(); + int stride = s_dims.d[0]; + int num_weights = (dim_in.d[1] / groups) * dim_out.d[1] * k_dims.d[0] * k_dims.d[1]; + float gflops = (2.0 * num_weights) * (static_cast(dim_in.d[3]) / stride * + static_cast(dim_in.d[2]) / stride / 1e9); + total_gflops += gflops; + total_params += num_weights; + ss << "L" << i << " [conv " << k_dims.d[0] << "x" << k_dims.d[1] << " (" << groups << ") " + << ") " << "/" << s_dims.d[0] << "] " << dim_in.d[3] << "x" << dim_in.d[2] << "x" + << " -> " << dim_out.d[3] << "x" << dim_out.d[2] << "x" << dim_out.d[1]; + ss << " weights:" << num_weights; + ss << " GFLOPs:" << gflops; + ss << std::endl; + } else if (layer_type == nvinfer1::LayerType::kPOOLING) { + auto pool = dynamic_cast(layer); + auto p_type = pool->getPoolingType(); + nvinfer1::Dims dim_stride = pool->getStrideNd(); + nvinfer1::Dims dim_window = pool->getWindowSizeNd(); + + ss << "L" << i << " ["; + if (p_type == nvinfer1::PoolingType::kMAX) { + ss << "max "; + } else if (p_type == nvinfer1::PoolingType::kAVERAGE) { + ss << "avg "; + } else if (p_type == nvinfer1::PoolingType::kMAX_AVERAGE_BLEND) { + ss << "max avg blend "; + } + float gflops = static_cast(dim_in.d[1]) * + (static_cast(dim_window.d[0]) / static_cast(dim_stride.d[0])) * + (static_cast(dim_window.d[1]) / static_cast(dim_stride.d[1])) * + static_cast(dim_in.d[2]) * static_cast(dim_in.d[3]) / 1e9; + total_gflops += gflops; + ss << "pool " << dim_window.d[0] << "x" << dim_window.d[1] << "]"; + ss << " GFLOPs:" << gflops; + ss << std::endl; + } else if (layer_type == nvinfer1::LayerType::kRESIZE) { + ss << "L" << i << " [resize]" << std::endl; + } + } + ss << "Total " << total_gflops << " GFLOPs" << std::endl; + ss << "Total " << total_params / 1000.0 / 1000.0 << " M params" << std::endl; + + return ss.str(); + }; + +private: + //!< Calibration configuration + std::unique_ptr calib_config_; + + //!< Calibrator used for implicit quantization + std::unique_ptr calibrator_; +}; + +} // namespace tensorrt_common +} // namespace autoware + +#endif // AUTOWARE__TENSORRT_COMMON__TENSORRT_CONV_CALIB_HPP_ diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/utils.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/utils.hpp new file mode 100644 index 0000000000000..be640f52147d9 --- /dev/null +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/utils.hpp @@ -0,0 +1,408 @@ +// 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__TENSORRT_COMMON__UTILS_HPP_ +#define AUTOWARE__TENSORRT_COMMON__UTILS_HPP_ + +#include +#include + +#if (defined(_MSC_VER) or (defined(__GNUC__) and (7 <= __GNUC_MAJOR__))) +#include +namespace fs = ::std::filesystem; +#else +#include +namespace fs = ::std::experimental::filesystem; +#endif + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ +constexpr std::array valid_precisions = {"fp32", "fp16", "int8"}; + +/** + * @struct TrtCommonConfig + * @brief Configuration to provide fine control regarding TensorRT builder + */ +struct TrtCommonConfig +{ + /** @brief Construct TrtCommonConfig, ONNX model path is mandatory. + * + * @param[in] onnx_path ONNX model path + * @param[in] precision precision for inference + * @param[in] engine_path TensorRT engine path + * @param[in] max_workspace_size maximum workspace size for TensorRT + * @param[in] dla_core_id DLA core ID + * @param[in] profile_per_layer flag for per-layer profiler using IProfiler + */ + explicit TrtCommonConfig( + const std::string onnx_path, const std::string precision = "fp16", + const std::string engine_path = "", const size_t max_workspace_size = (1ULL << 30U), + const int32_t dla_core_id = -1, const bool profile_per_layer = false) + : onnx_path(onnx_path), + precision(precision), + engine_path(engine_path), + max_workspace_size(max_workspace_size), + dla_core_id(dla_core_id), + profile_per_layer(profile_per_layer) + { + validatePrecision(); + + if (engine_path.empty()) { + this->engine_path = onnx_path; + this->engine_path.replace_extension(".engine"); + } + } + + /** + * @brief Validate configured TensorRT engine precision. + */ + void validatePrecision() const + { + if ( + std::find(valid_precisions.begin(), valid_precisions.end(), precision) == + valid_precisions.end()) { + std::stringstream message; + message << "Invalid precision was specified: " << precision << std::endl + << "Valid string is one of: [" << valid_precisions[0]; + for (size_t i = 1; i < valid_precisions.size(); ++i) { + message << ", " << valid_precisions[i]; + } + message << "] (case sensitive)" << std::endl; + + throw std::runtime_error(message.str()); + } + } + + //!< @brief ONNX model path. + const fs::path onnx_path; + + //!< @brief Precision for inference. + const std::string precision; + + //!< @brief TensorRT engine path. + fs::path engine_path; + + //!< @brief TensorRT max workspace size. + const size_t max_workspace_size; + + //!< @brief DLA core ID that the process uses. + const int32_t dla_core_id; + + //!< @brief Flag for per-layer profiler using IProfiler. + const bool profile_per_layer; +}; + +/** + * @brief Represents a tensor with its name or index. + */ +struct TensorInfo +{ + /** + * @brief Construct TensorInfo with tensor name. + * + * @param[in] name Tensor name. + */ + explicit TensorInfo(std::string name) : tensor_name(std::move(name)), tensor_index(-1) {} + + /** + * @brief Construct TensorInfo with tensor index. + * + * @param[in] index Tensor index. + */ + explicit TensorInfo(int32_t index) : tensor_index(index) {} + + /** + * @brief Check if dimensions are equal. + * + * @param lhs Left-hand side tensor dimensions. + * @param rhs Right-hand side tensor dimensions. + * @return Whether dimensions are equal. + */ + static bool dimsEqual(const nvinfer1::Dims & lhs, const nvinfer1::Dims & rhs) + { + if (lhs.nbDims != rhs.nbDims) return false; + return std::equal(lhs.d, lhs.d + lhs.nbDims, rhs.d); // NOLINT + } + + /** + * @brief Get printable representation of tensor dimensions. + * + * @param[in] dims Tensor dimensions. + * @return String representation of tensor dimensions. + */ + static std::string dimsRepr(const nvinfer1::Dims & dims) + { + if (dims.nbDims == 0) return "[]"; + std::string repr = "[" + std::to_string(dims.d[0]); + for (int i = 1; i < dims.nbDims; ++i) { + repr += ", " + std::to_string(dims.d[i]); + } + repr += "]"; + return repr; + } + + //!< @brief Tensor name. + std::string tensor_name; + + //!< @brief Tensor index. + int32_t tensor_index; +}; + +/** + * @brief Represents a network input/output tensor with its dimensions. + * + * Example usage: + * @code + * nvinfer1::Dims tensor_dims = {3, {1, 2, 3}}; + * NetworkIO input("input_tensor", tensor_dims); + * NetworkIO output("output_tensor", tensor_dims); + * bool is_equal = input == output; // false + * @endcode + */ +struct NetworkIO : public TensorInfo +{ + /** + * @brief Construct NetworkIO with tensor name and dimensions. + * + * @param[in] name Tensor name. + * @param[in] tensor_dims Tensor dimensions. + */ + NetworkIO(std::string name, const nvinfer1::Dims & tensor_dims) + : TensorInfo(std::move(name)), dims(tensor_dims) + { + } + + /** + * @brief Construct NetworkIO with tensor index and dimensions. + * + * @param[in] index Tensor index. + * @param[in] tensor_dims Tensor dimensions. + */ + NetworkIO(int32_t index, const nvinfer1::Dims & tensor_dims) + : TensorInfo(index), dims(tensor_dims) + { + } + + /** + * @brief Get printable representation of NetworkIO. + * + * @return String representation of NetworkIO. + */ + [[nodiscard]] std::string toString() const + { + std::stringstream ss; + ss << tensor_name << " {" << TensorInfo::dimsRepr(dims) << "}"; + return ss.str(); + } + + /** + * @brief Check if NetworkIO is equal to another NetworkIO. + * + * @param rhs Right-hand side NetworkIO. + * @return Whether NetworkIO is equal to another NetworkIO. + */ + bool operator==(const NetworkIO & rhs) const + { + if (tensor_name != rhs.tensor_name) return false; + return dimsEqual(dims, rhs.dims); + } + + /** + * @brief Check if NetworkIO is not equal to another NetworkIO. + * + * @param rhs Right-hand side NetworkIO. + * @return Whether NetworkIO is not equal to another NetworkIO. + */ + bool operator!=(const NetworkIO & rhs) const { return !(*this == rhs); } + + /** + * @brief Output NetworkIO to ostream. + * + * @param os Output stream. + * @param io NetworkIO. + * @return Output stream. + */ + friend std::ostream & operator<<(std::ostream & os, const NetworkIO & io) + { + os << io.toString(); + return os; + } + + //!< @brief Tensor dimensions. + nvinfer1::Dims dims; +}; + +/** + * @brief Represents a tensor optimization profile with minimum, optimal, and maximum dimensions. + * + * Example usage: + * @code + * nvinfer1::Dims min = {3, {1, 2, 3}}; + * nvinfer1::Dims opt = {3, {1, 3, 4}}; + * nvinfer1::Dims max = {3, {1, 4, 5}}; + * ProfileDims entry_1("tensor_name", min, opt, max); + * ProfileDims entry_2("tensor_name", {3, {1, 2, 3}}, {3, {1, 3, 4}}, {3, {1, 4, 5}}); + * bool is_equal = entry_1 == entry_2; // true + * @endcode + */ +struct ProfileDims : public TensorInfo +{ + /** + * @brief Construct ProfileDims with tensor name and dimensions. + * + * @param[in] name Tensor name. + * @param[in] min Minimum dimensions for optimization profile. + * @param[in] opt Optimal dimensions for optimization profile. + * @param[in] max Maximum dimensions for optimization profile. + */ + ProfileDims( + std::string name, const nvinfer1::Dims & min, const nvinfer1::Dims & opt, + const nvinfer1::Dims & max) + : TensorInfo(std::move(name)), min_dims(min), opt_dims(opt), max_dims(max) + { + } + + /** + * @brief Construct ProfileDims with tensor index and dimensions. + * + * @param[in] index Tensor index. + * @param[in] min Minimum dimensions for optimization profile. + * @param[in] opt Optimal dimensions for optimization profile. + * @param[in] max Maximum dimensions for optimization profile. + */ + ProfileDims( + int32_t index, const nvinfer1::Dims & min, const nvinfer1::Dims & opt, + const nvinfer1::Dims & max) + : TensorInfo(index), min_dims(min), opt_dims(opt), max_dims(max) + { + } + + /** + * @brief Get printable representation of ProfileDims. + * + * @return String representation of ProfileDims. + */ + [[nodiscard]] std::string toString() const + { + std::ostringstream oss; + oss << tensor_name << " {min " << TensorInfo::dimsRepr(min_dims) << ", opt " + << TensorInfo::dimsRepr(opt_dims) << ", max " << TensorInfo::dimsRepr(max_dims) << "}"; + return oss.str(); + } + + /** + * @brief Check if ProfileDims is equal to another ProfileDims. + * + * @param rhs Right-hand side ProfileDims. + * @return Whether ProfileDims is equal to another ProfileDims. + */ + bool operator==(const ProfileDims & rhs) const + { + if (tensor_name != rhs.tensor_name) return false; + return dimsEqual(min_dims, rhs.min_dims) && dimsEqual(opt_dims, rhs.opt_dims) && + dimsEqual(max_dims, rhs.max_dims); + } + + /** + * @brief Check if ProfileDims is not equal to another ProfileDims. + * + * @param rhs Right-hand side ProfileDims. + * @return Whether ProfileDims is not equal to another ProfileDims. + */ + bool operator!=(const ProfileDims & rhs) const { return !(*this == rhs); } + + /** + * @brief Output ProfileDims to ostream. + * + * @param os Output stream. + * @param profile ProfileDims. + * @return Output stream. + */ + friend std::ostream & operator<<(std::ostream & os, const ProfileDims & profile) + { + os << profile.toString(); + return os; + } + + //!< @brief Minimum dimensions for optimization profile. + nvinfer1::Dims min_dims; + + //!< @brief Optimal dimensions for optimization profile. + nvinfer1::Dims opt_dims; + + //!< @brief Maximum dimensions for optimization profile. + nvinfer1::Dims max_dims; +}; + +//!< @brief Valid calibration types for TensorRT. +constexpr std::array valid_calib_type = { + "Entropy", "Legacy", "Percentile", "MinMax"}; + +/** + * @brief Configuration for implicit calibration. + */ +struct CalibrationConfig +{ + /** + * @brief Construct CalibrationConfig with its parameters. + * + * @param[in] calib_type_str Calibration type. + * @param[in] quantize_first_layer Flag for partial quantization in first layer. + * @param[in] quantize_last_layer Flag for partial quantization in last layer. + * @param[in] clip_value Clip value for implicit quantization. + */ + explicit CalibrationConfig( + const std::string & calib_type_str = "MinMax", const bool quantize_first_layer = false, + const bool quantize_last_layer = false, const double clip_value = 0.0) + : calib_type_str(calib_type_str), + quantize_first_layer(quantize_first_layer), + quantize_last_layer(quantize_last_layer), + clip_value(clip_value) + { + if ( + std::find(valid_calib_type.begin(), valid_calib_type.end(), calib_type_str) == + valid_calib_type.end()) { + throw std::runtime_error( + "Invalid calibration type was specified: " + std::string(calib_type_str) + + "\nValid value is one of: [Entropy, (Legacy | Percentile), MinMax]"); + } + } + + //!< @brief type of calibration + const std::string calib_type_str; + + //!< @brief flag for partial quantization in first layer + const bool quantize_first_layer; + + //!< @brief flag for partial quantization in last layer + const bool quantize_last_layer; + + //!< @brief clip value for implicit quantization + const double clip_value; +}; + +} // namespace tensorrt_common +} // namespace autoware + +#endif // AUTOWARE__TENSORRT_COMMON__UTILS_HPP_ diff --git a/perception/autoware_tensorrt_common/src/profiler.cpp b/perception/autoware_tensorrt_common/src/profiler.cpp new file mode 100644 index 0000000000000..433cca51f2b4f --- /dev/null +++ b/perception/autoware_tensorrt_common/src/profiler.cpp @@ -0,0 +1,113 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ + +Profiler::Profiler(const std::vector & src_profilers) +{ + index_ = 0; + for (const auto & src_profiler : src_profilers) { + for (const auto & [name, record] : src_profiler.profile_) { + auto it = profile_.find(name); + if (it == profile_.end()) { + profile_.emplace(name, record); + } else { + it->second.time += record.time; + it->second.count += record.count; + } + } + } +} + +void Profiler::reportLayerTime(const char * layerName, float ms) noexcept +{ + if (profile_.find(layerName) == profile_.end()) { + return; + } + profile_[layerName].count++; + profile_[layerName].time += ms; + if (profile_[layerName].min_time == -1.0) { + profile_[layerName].min_time = ms; + profile_[layerName].index = index_; + index_++; + } else if (profile_[layerName].min_time > ms) { + profile_[layerName].min_time = ms; + } +} + +std::string Profiler::toString() const +{ + std::ostringstream out; + float total_time = 0.0; + std::string layer_name = "Operation"; + + int max_layer_name_length = static_cast(layer_name.size()); + for (const auto & elem : profile_) { + total_time += elem.second.time; + max_layer_name_length = std::max(max_layer_name_length, static_cast(elem.first.size())); + } + + auto old_settings = out.flags(); + auto old_precision = out.precision(); + // Output header + { + out << "index, " << std::setw(12); + out << std::setw(max_layer_name_length) << layer_name << " "; + out << std::setw(12) << "Runtime" << "%," << " "; + out << std::setw(12) << "Invocations" << " , "; + out << std::setw(12) << "Runtime[ms]" << " , "; + out << std::setw(12) << "Avg Runtime[ms]" << " ,"; + out << std::setw(12) << "Min Runtime[ms]" << std::endl; + } + int index = index_; + for (int i = 0; i < index; i++) { + for (const auto & elem : profile_) { + if (elem.second.index == i) { + out << i << ", "; + out << std::setw(max_layer_name_length) << elem.first << ","; + out << std::setw(12) << std::fixed << std::setprecision(1) + << (elem.second.time * 100.0F / total_time) << "%" << ","; + out << std::setw(12) << elem.second.count << ","; + out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.time << ", "; + out << std::setw(12) << std::fixed << std::setprecision(2) + << elem.second.time / elem.second.count << ", "; + out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.min_time + << std::endl; + } + } + } + out.flags(old_settings); + out.precision(old_precision); + out << "========== total runtime = " << total_time << " ms ==========" << std::endl; + + return out.str(); +} + +std::ostream & operator<<(std::ostream & out, const Profiler & value) +{ + out << value.toString(); + return out; +} +} // namespace tensorrt_common +} // namespace autoware diff --git a/perception/autoware_tensorrt_common/src/simple_profiler.cpp b/perception/autoware_tensorrt_common/src/simple_profiler.cpp deleted file mode 100644 index 2bcc1c4f9da06..0000000000000 --- a/perception/autoware_tensorrt_common/src/simple_profiler.cpp +++ /dev/null @@ -1,138 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include -#include -#include - -namespace autoware -{ -namespace tensorrt_common -{ - -SimpleProfiler::SimpleProfiler(std::string name, const std::vector & src_profilers) -: m_name(name) -{ - m_index = 0; - for (const auto & src_profiler : src_profilers) { - for (const auto & rec : src_profiler.m_profile) { - auto it = m_profile.find(rec.first); - if (it == m_profile.end()) { - m_profile.insert(rec); - } else { - it->second.time += rec.second.time; - it->second.count += rec.second.count; - } - } - } -} - -void SimpleProfiler::reportLayerTime(const char * layerName, float ms) noexcept -{ - m_profile[layerName].count++; - m_profile[layerName].time += ms; - if (m_profile[layerName].min_time == -1.0) { - m_profile[layerName].min_time = ms; - m_profile[layerName].index = m_index; - m_index++; - } else if (m_profile[layerName].min_time > ms) { - m_profile[layerName].min_time = ms; - } -} - -void SimpleProfiler::setProfDict(nvinfer1::ILayer * layer) noexcept -{ - std::string name = layer->getName(); - m_layer_dict[name]; - m_layer_dict[name].type = layer->getType(); - if (layer->getType() == nvinfer1::LayerType::kCONVOLUTION) { - nvinfer1::IConvolutionLayer * conv = (nvinfer1::IConvolutionLayer *)layer; - nvinfer1::ITensor * in = layer->getInput(0); - nvinfer1::Dims dim_in = in->getDimensions(); - nvinfer1::ITensor * out = layer->getOutput(0); - nvinfer1::Dims dim_out = out->getDimensions(); - nvinfer1::Dims k_dims = conv->getKernelSizeNd(); - nvinfer1::Dims s_dims = conv->getStrideNd(); - int groups = conv->getNbGroups(); - int stride = s_dims.d[0]; - int kernel = k_dims.d[0]; - m_layer_dict[name].in_c = dim_in.d[1]; - m_layer_dict[name].out_c = dim_out.d[1]; - m_layer_dict[name].w = dim_in.d[3]; - m_layer_dict[name].h = dim_in.d[2]; - m_layer_dict[name].k = kernel; - ; - m_layer_dict[name].stride = stride; - m_layer_dict[name].groups = groups; - } -} - -std::ostream & operator<<(std::ostream & out, const SimpleProfiler & value) -{ - out << "========== " << value.m_name << " profile ==========" << std::endl; - float totalTime = 0; - std::string layerNameStr = "Operation"; - - int maxLayerNameLength = static_cast(layerNameStr.size()); - for (const auto & elem : value.m_profile) { - totalTime += elem.second.time; - maxLayerNameLength = std::max(maxLayerNameLength, static_cast(elem.first.size())); - } - - auto old_settings = out.flags(); - auto old_precision = out.precision(); - // Output header - { - out << "index, " << std::setw(12); - out << std::setw(maxLayerNameLength) << layerNameStr << " "; - out << std::setw(12) << "Runtime" - << "%," - << " "; - out << std::setw(12) << "Invocations" - << " , "; - out << std::setw(12) << "Runtime[ms]" - << " , "; - out << std::setw(12) << "Avg Runtime[ms]" - << " ,"; - out << std::setw(12) << "Min Runtime[ms]" << std::endl; - } - int index = value.m_index; - for (int i = 0; i < index; i++) { - for (const auto & elem : value.m_profile) { - if (elem.second.index == i) { - out << i << ", "; - out << std::setw(maxLayerNameLength) << elem.first << ","; - out << std::setw(12) << std::fixed << std::setprecision(1) - << (elem.second.time * 100.0F / totalTime) << "%" - << ","; - out << std::setw(12) << elem.second.count << ","; - out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.time << ", "; - out << std::setw(12) << std::fixed << std::setprecision(2) - << elem.second.time / elem.second.count << ", "; - out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.min_time - << std::endl; - } - } - } - out.flags(old_settings); - out.precision(old_precision); - out << "========== " << value.m_name << " total runtime = " << totalTime - << " ms ==========" << std::endl; - return out; -} -} // namespace tensorrt_common -} // namespace autoware diff --git a/perception/autoware_tensorrt_common/src/tensorrt_common.cpp b/perception/autoware_tensorrt_common/src/tensorrt_common.cpp index 990433ee277a0..ba422277416ab 100644 --- a/perception/autoware_tensorrt_common/src/tensorrt_common.cpp +++ b/perception/autoware_tensorrt_common/src/tensorrt_common.cpp @@ -12,102 +12,38 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "autoware/tensorrt_common/tensorrt_common.hpp" +#include "autoware/tensorrt_common/logger.hpp" +#include "autoware/tensorrt_common/utils.hpp" + +#include #include +#include #include +#include #include -#include #include #include #include +#include #include #include -namespace -{ -template -bool contain(const std::string & s, const T & v) -{ - return s.find(v) != std::string::npos; -} -} // anonymous namespace - namespace autoware { namespace tensorrt_common { -nvinfer1::Dims get_input_dims(const std::string & onnx_file_path) -{ - Logger logger_; - auto builder = TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); - if (!builder) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); - } - - const auto explicitBatch = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - - auto network = - TrtUniquePtr(builder->createNetworkV2(explicitBatch)); - if (!network) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); - } - - auto config = TrtUniquePtr(builder->createBuilderConfig()); - if (!config) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); - } - - auto parser = TrtUniquePtr(nvonnxparser::createParser(*network, logger_)); - if (!parser->parseFromFile( - onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR))) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Failed to parse onnx file"); - } - - const auto input = network->getInput(0); - return input->getDimensions(); -} - -bool is_valid_precision_string(const std::string & precision) -{ - if ( - std::find(valid_precisions.begin(), valid_precisions.end(), precision) == - valid_precisions.end()) { - std::stringstream message; - message << "Invalid precision was specified: " << precision << std::endl - << "Valid string is one of: ["; - for (const auto & s : valid_precisions) { - message << s << ", "; - } - message << "] (case sensitive)" << std::endl; - std::cerr << message.str(); - return false; - } else { - return true; - } -} TrtCommon::TrtCommon( - const std::string & model_path, const std::string & precision, - std::unique_ptr calibrator, const BatchConfig & batch_config, - const size_t max_workspace_size, const BuildConfig & build_config, + const TrtCommonConfig & trt_config, const std::shared_ptr & profiler, const std::vector & plugin_paths) -: model_file_path_(model_path), - calibrator_(std::move(calibrator)), - precision_(precision), - batch_config_(batch_config), - max_workspace_size_(max_workspace_size), - model_profiler_("Model"), - host_profiler_("Host") +: trt_config_(std::make_shared(trt_config)), + host_profiler_(profiler), + model_profiler_(profiler) { - // Check given precision is valid one - if (!is_valid_precision_string(precision)) { - return; - } - build_config_ = std::make_unique(build_config); - + logger_ = std::make_shared(); for (const auto & plugin_path : plugin_paths) { int32_t flags{RTLD_LAZY}; // cspell: ignore asan @@ -120,523 +56,595 @@ TrtCommon::TrtCommon( #endif // ENABLE_ASAN void * handle = dlopen(plugin_path.c_str(), flags); if (!handle) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Could not load plugin library"); + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Could not load plugin library"); + } else { + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Loaded plugin library: %s", plugin_path.c_str()); } } - runtime_ = TrtUniquePtr(nvinfer1::createInferRuntime(logger_)); - if (build_config_->dla_core_id != -1) { - runtime_->setDLACore(build_config_->dla_core_id); + runtime_ = TrtUniquePtr(nvinfer1::createInferRuntime(*logger_)); + if (trt_config_->dla_core_id != -1) { + runtime_->setDLACore(trt_config_->dla_core_id); } - initLibNvInferPlugins(&logger_, ""); -} + initLibNvInferPlugins(&*logger_, ""); -TrtCommon::~TrtCommon() -{ + if (!initialize()) { + throw std::runtime_error("Failed to initialize TensorRT"); + } } -void TrtCommon::setup() +TrtCommon::~TrtCommon() = default; + +bool TrtCommon::setup(ProfileDimsPtr profile_dims, NetworkIOPtr network_io) { - if (!fs::exists(model_file_path_)) { - is_initialized_ = false; - return; - } - // cppcheck-suppress unreadVariable - std::string engine_path = model_file_path_; - if (model_file_path_.extension() == ".engine") { - std::cout << "Load ... " << model_file_path_ << std::endl; - loadEngine(model_file_path_); - } else if (model_file_path_.extension() == ".onnx") { - fs::path cache_engine_path{model_file_path_}; - std::string ext; - std::string calib_name = ""; - if (precision_ == "int8") { - if (build_config_->calib_type_str == "Entropy") { - calib_name = "EntropyV2-"; - } else if ( - build_config_->calib_type_str == "Legacy" || - build_config_->calib_type_str == "Percentile") { - calib_name = "Legacy-"; - } else { - calib_name = "MinMax-"; + profile_dims_ = std::move(profile_dims); + network_io_ = std::move(network_io); + + // Set input profile + if (profile_dims_ && !profile_dims_->empty()) { + auto profile = builder_->createOptimizationProfile(); + for (auto & profile_dim : *profile_dims_) { + if (profile_dim.tensor_name.empty()) { + profile_dim.tensor_name = getIOTensorName(profile_dim.tensor_index); } + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Setting optimization profile for tensor: %s", + profile_dim.toString().c_str()); + profile->setDimensions( + profile_dim.tensor_name.c_str(), nvinfer1::OptProfileSelector::kMIN, profile_dim.min_dims); + profile->setDimensions( + profile_dim.tensor_name.c_str(), nvinfer1::OptProfileSelector::kOPT, profile_dim.opt_dims); + profile->setDimensions( + profile_dim.tensor_name.c_str(), nvinfer1::OptProfileSelector::kMAX, profile_dim.max_dims); } - if (build_config_->dla_core_id != -1) { - ext = "DLA" + std::to_string(build_config_->dla_core_id) + "-" + calib_name + precision_; - if (build_config_->quantize_first_layer) { - ext += "-firstFP16"; - } - if (build_config_->quantize_last_layer) { - ext += "-lastFP16"; - } - ext += "-batch" + std::to_string(batch_config_[0]) + ".engine"; - } else { - ext = calib_name + precision_; - if (build_config_->quantize_first_layer) { - ext += "-firstFP16"; - } - if (build_config_->quantize_last_layer) { - ext += "-lastFP16"; - } - ext += "-batch" + std::to_string(batch_config_[0]) + ".engine"; + builder_config_->addOptimizationProfile(profile); + } + + auto build_engine_with_log = [this]() -> bool { + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Starting to build engine"); + auto log_thread = logger_->log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TensorRT CUDA engine. Please wait for a few minutes...", + 5); + auto success = buildEngineFromOnnx(); + logger_->stop_throttle(log_thread); + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Engine build completed"); + return success; + }; + + // Load engine file if it exists + if (fs::exists(trt_config_->engine_path)) { + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Loading engine"); + if (!loadEngine()) { + return false; } - cache_engine_path.replace_extension(ext); - - // Output Network Information - printNetworkInfo(model_file_path_); - - if (fs::exists(cache_engine_path)) { - std::cout << "Loading... " << cache_engine_path << std::endl; - loadEngine(cache_engine_path); - } else { - std::cout << "Building... " << cache_engine_path << std::endl; - logger_.log(nvinfer1::ILogger::Severity::kINFO, "Start build engine"); - auto log_thread = logger_.log_throttle( - nvinfer1::ILogger::Severity::kINFO, - "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); - buildEngineFromOnnx(model_file_path_, cache_engine_path); - logger_.stop_throttle(log_thread); - logger_.log(nvinfer1::ILogger::Severity::kINFO, "End build engine"); + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Network validation"); + // Validate engine tensor shapes and optimization profile + if (!validateNetworkIO() || !validateProfileDims()) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Network validation failed for loaded engine from file. Rebuilding engine"); + // Rebuild engine if the tensor shapes or optimization profile mismatch + if (!build_engine_with_log()) { + return false; + } } - // cppcheck-suppress unreadVariable - engine_path = cache_engine_path; } else { - is_initialized_ = false; - return; + // Build engine if engine has not been cached + if (!build_engine_with_log()) { + return false; + } } - context_ = TrtUniquePtr(engine_->createExecutionContext()); - if (!context_) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create context"); - is_initialized_ = false; - return; + // Validate engine nevertheless is loaded or rebuilt + if (!validateNetworkIO() || !validateProfileDims()) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Final network validation failed. Possibly the input / output of the currently " + "deployed model has changed. Check your configuration file with the current model."); + return false; } - if (build_config_->profile_per_layer) { - context_->setProfiler(&model_profiler_); + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Engine setup completed"); + return true; +} + +std::string TrtCommon::getPrecision() const +{ + return trt_config_->precision; +} + +const char * TrtCommon::getIOTensorName(const int32_t index) const +{ + if (!engine_) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Engine is not initialized. Retrieving data from network"); + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return nullptr; + } + auto num_inputs = network_->getNbInputs(); + auto num_outputs = network_->getNbOutputs(); + if (index < 0 || index >= num_inputs + num_outputs) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Invalid index for I/O tensor: %d. Total I/O tensors: %d", index, num_inputs + num_outputs); + return nullptr; + } + if (index < num_inputs) { + return network_->getInput(index)->getName(); + } + return network_->getOutput(index - num_inputs)->getName(); } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 - // Write profiles for trt-engine-explorer - // See: https://github.com/NVIDIA/TensorRT/tree/main/tools/experimental/trt-engine-explorer - std::string j_ext = ".json"; - fs::path json_path{engine_path}; - json_path.replace_extension(j_ext); - std::string ret = getLayerInformation(nvinfer1::LayerInformationFormat::kJSON); - std::ofstream os(json_path, std::ofstream::trunc); - os << ret << std::flush; -#endif - is_initialized_ = true; + return engine_->getIOTensorName(index); } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 -void TrtCommon::setupBindings(std::vector & bindings) +int32_t TrtCommon::getNbIOTensors() const { - for (int32_t i = 0, e = engine_->getNbIOTensors(); i < e; i++) { - auto const name = engine_->getIOTensorName(i); - context_->setTensorAddress(name, bindings.at(i)); + if (!engine_) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Engine is not initialized. Retrieving data from network"); + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return 0; + } + return network_->getNbInputs() + network_->getNbOutputs(); } + return engine_->getNbIOTensors(); } -#endif -bool TrtCommon::loadEngine(const std::string & engine_file_path) +nvinfer1::Dims TrtCommon::getTensorShape(const int32_t index) const { - std::ifstream engine_file(engine_file_path); - std::stringstream engine_buffer; - engine_buffer << engine_file.rdbuf(); - std::string engine_str = engine_buffer.str(); - engine_ = TrtUniquePtr(runtime_->deserializeCudaEngine( - reinterpret_cast(engine_str.data()), engine_str.size())); - return true; + if (!engine_) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Engine is not initialized. Retrieving data from network"); + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return nvinfer1::Dims{}; + } + auto num_inputs = network_->getNbInputs(); + auto num_outputs = network_->getNbOutputs(); + if (index < 0 || index >= num_inputs + num_outputs) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Invalid index for I/O tensor: %d. Total I/O tensors: %d", index, num_inputs + num_outputs); + return nvinfer1::Dims{}; + } + if (index < num_inputs) { + return network_->getInput(index)->getDimensions(); + } + return network_->getOutput(index - num_inputs)->getDimensions(); + } + auto const & name = getIOTensorName(index); + return getTensorShape(name); } -void TrtCommon::printNetworkInfo(const std::string & onnx_file_path) +nvinfer1::Dims TrtCommon::getTensorShape(const char * tensor_name) const { - auto builder = TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); - if (!builder) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); - return; + if (!engine_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Engine is not initialized"); + return nvinfer1::Dims{}; } + return engine_->getTensorShape(tensor_name); +} - const auto explicitBatch = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); +nvinfer1::Dims TrtCommon::getInputDims(const int32_t index) const +{ + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return {}; + } + const auto input = network_->getInput(index); + return input->getDimensions(); +} - auto network = - TrtUniquePtr(builder->createNetworkV2(explicitBatch)); - if (!network) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); - return; +nvinfer1::Dims TrtCommon::getOutputDims(const int32_t index) const +{ + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return {}; } + const auto output = network_->getOutput(index); + return output->getDimensions(); +} + +bool TrtCommon::setTensorAddress(const int32_t index, void * data) +{ + auto const & name = getIOTensorName(index); + return setTensorAddress(name, data); +} - auto config = TrtUniquePtr(builder->createBuilderConfig()); - if (!config) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); - return; +bool TrtCommon::setTensorAddress(const char * tensor_name, void * data) +{ + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Context is not initialized"); + return false; + } + auto success = context_->setTensorAddress(tensor_name, data); + if (!success) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Failed to set tensor address for tensor: ", tensor_name); } + return success; +} - if (precision_ == "fp16" || precision_ == "int8") { - config->setFlag(nvinfer1::BuilderFlag::kFP16); +bool TrtCommon::setTensorsAddresses(std::vector & tensors) +{ + bool success = true; + for (std::size_t i = 0, e = tensors.size(); i < e; i++) { + auto const name = getIOTensorName(i); + success &= setTensorAddress(name, tensors.at(i)); } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 - config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, max_workspace_size_); -#else - config->setMaxWorkspaceSize(max_workspace_size_); -#endif + return success; +} - auto parser = TrtUniquePtr(nvonnxparser::createParser(*network, logger_)); - if (!parser->parseFromFile( - onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR))) { - return; - } - int num = network->getNbLayers(); - float total_gflops = 0.0; - int total_params = 0; - for (int i = 0; i < num; i++) { - nvinfer1::ILayer * layer = network->getLayer(i); - auto layer_type = layer->getType(); - if (build_config_->profile_per_layer) { - model_profiler_.setProfDict(layer); - } - if (layer_type == nvinfer1::LayerType::kCONSTANT) { - continue; - } - nvinfer1::ITensor * in = layer->getInput(0); - nvinfer1::Dims dim_in = in->getDimensions(); - nvinfer1::ITensor * out = layer->getOutput(0); - nvinfer1::Dims dim_out = out->getDimensions(); - - if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { - nvinfer1::IConvolutionLayer * conv = (nvinfer1::IConvolutionLayer *)layer; - nvinfer1::Dims k_dims = conv->getKernelSizeNd(); - nvinfer1::Dims s_dims = conv->getStrideNd(); - int groups = conv->getNbGroups(); - int stride = s_dims.d[0]; - int num_weights = (dim_in.d[1] / groups) * dim_out.d[1] * k_dims.d[0] * k_dims.d[1]; - float gflops = (2.0 * num_weights) * (static_cast(dim_in.d[3]) / stride * - static_cast(dim_in.d[2]) / stride / 1e9); - total_gflops += gflops; - total_params += num_weights; - std::cout << "L" << i << " [conv " << k_dims.d[0] << "x" << k_dims.d[1] << " (" << groups - << ") " - << "/" << s_dims.d[0] << "] " << dim_in.d[3] << "x" << dim_in.d[2] << "x" - << dim_in.d[1] << " -> " << dim_out.d[3] << "x" << dim_out.d[2] << "x" - << dim_out.d[1]; - std::cout << " weights:" << num_weights; - std::cout << " GFLOPs:" << gflops; - std::cout << std::endl; - } else if (layer_type == nvinfer1::LayerType::kPOOLING) { - nvinfer1::IPoolingLayer * pool = (nvinfer1::IPoolingLayer *)layer; - auto p_type = pool->getPoolingType(); - nvinfer1::Dims dim_stride = pool->getStrideNd(); - nvinfer1::Dims dim_window = pool->getWindowSizeNd(); - - std::cout << "L" << i << " ["; - if (p_type == nvinfer1::PoolingType::kMAX) { - std::cout << "max "; - } else if (p_type == nvinfer1::PoolingType::kAVERAGE) { - std::cout << "avg "; - } else if (p_type == nvinfer1::PoolingType::kMAX_AVERAGE_BLEND) { - std::cout << "max avg blend "; - } - float gflops = static_cast(dim_in.d[1]) * - (static_cast(dim_window.d[0]) / static_cast(dim_stride.d[0])) * - (static_cast(dim_window.d[1]) / static_cast(dim_stride.d[1])) * - static_cast(dim_in.d[2]) * static_cast(dim_in.d[3]) / 1e9; - total_gflops += gflops; - std::cout << "pool " << dim_window.d[0] << "x" << dim_window.d[1] << "]"; - std::cout << " GFLOPs:" << gflops; - std::cout << std::endl; - } else if (layer_type == nvinfer1::LayerType::kRESIZE) { - std::cout << "L" << i << " [resize]" << std::endl; - } +bool TrtCommon::setTensorsAddresses(std::unordered_map & tensors) +{ + bool success = true; + for (auto const & tensor : tensors) { + success &= setTensorAddress(tensor.first, tensor.second); } - std::cout << "Total " << total_gflops << " GFLOPs" << std::endl; - std::cout << "Total " << total_params / 1000.0 / 1000.0 << " M params" << std::endl; - return; + return success; +} + +bool TrtCommon::setInputShape(const int32_t index, const nvinfer1::Dims & dimensions) +{ + auto const & name = getIOTensorName(index); + return setInputShape(name, dimensions); } -bool TrtCommon::buildEngineFromOnnx( - const std::string & onnx_file_path, const std::string & output_engine_file_path) +bool TrtCommon::setInputShape(const char * tensor_name, const nvinfer1::Dims & dimensions) { - auto builder = TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); - if (!builder) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Context is not initialized"); return false; } + auto success = context_->setInputShape(tensor_name, dimensions); + if (!success) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, "Failed to set input shape for tensor: ", tensor_name); + } + return success; +} - const auto explicitBatch = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); +bool TrtCommon::setInputsShapes(const std::vector & dimensions) +{ + bool success = true; + for (std::size_t i = 0, e = dimensions.size(); i < e; i++) { + success &= setInputShape(i, dimensions.at(i)); + } + return success; +} - auto network = - TrtUniquePtr(builder->createNetworkV2(explicitBatch)); - if (!network) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); - return false; +bool TrtCommon::setInputsShapes(const std::unordered_map & dimensions) +{ + bool success = true; + for (auto const & dim : dimensions) { + success &= setInputShape(dim.first, dim.second); } + return success; +} - auto config = TrtUniquePtr(builder->createBuilderConfig()); - if (!config) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); - return false; +bool TrtCommon::setTensor(const int32_t index, void * data, nvinfer1::Dims dimensions) +{ + auto success = setTensorAddress(index, data); + if (dimensions.nbDims > 0) { + success &= setInputShape(index, dimensions); } + return success; +} - int num_available_dla = builder->getNbDLACores(); - if (build_config_->dla_core_id != -1) { - if (num_available_dla > 0) { - std::cout << "###" << num_available_dla << " DLAs are supported! ###" << std::endl; - } else { - std::cout << "###Warning : " - << "No DLA is supported! ###" << std::endl; - } - config->setDefaultDeviceType(nvinfer1::DeviceType::kDLA); - config->setDLACore(build_config_->dla_core_id); -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 - config->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); -#else - config->setFlag(nvinfer1::BuilderFlag::kSTRICT_TYPES); -#endif - config->setFlag(nvinfer1::BuilderFlag::kGPU_FALLBACK); +bool TrtCommon::setTensor(const char * tensor_name, void * data, nvinfer1::Dims dimensions) +{ + auto success = setTensorAddress(tensor_name, data); + if (dimensions.nbDims > 0) { + success &= setInputShape(tensor_name, dimensions); } - if (precision_ == "fp16" || precision_ == "int8") { - config->setFlag(nvinfer1::BuilderFlag::kFP16); + return success; +} + +bool TrtCommon::setTensors(TensorsVec & tensors) +{ + bool success = true; + for (std::size_t i = 0, e = tensors.size(); i < e; i++) { + success &= setTensor(i, tensors.at(i).first, tensors.at(i).second); } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 - config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, max_workspace_size_); -#else - config->setMaxWorkspaceSize(max_workspace_size_); -#endif + return success; +} - auto parser = TrtUniquePtr(nvonnxparser::createParser(*network, logger_)); - if (!parser->parseFromFile( - onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR))) { - std::cout << "Failed to parse onnx file" << std::endl; - return false; +bool TrtCommon::setTensors(TensorsMap & tensors) +{ + bool success = true; + for (auto const & tensor : tensors) { + success &= setTensor(tensor.first, tensor.second.first, tensor.second.second); } + return success; +} - const int num = network->getNbLayers(); - bool first = build_config_->quantize_first_layer; - bool last = build_config_->quantize_last_layer; - // Partial Quantization - if (precision_ == "int8") { - network->getInput(0)->setDynamicRange(0, 255.0); - for (int i = 0; i < num; i++) { - nvinfer1::ILayer * layer = network->getLayer(i); - auto layer_type = layer->getType(); - std::string name = layer->getName(); - nvinfer1::ITensor * out = layer->getOutput(0); - if (build_config_->clip_value > 0.0) { - std::cout << "Set max value for outputs : " << build_config_->clip_value << " " << name - << std::endl; - out->setDynamicRange(0.0, build_config_->clip_value); - } +std::shared_ptr TrtCommon::getModelProfiler() +{ + return model_profiler_; +} - if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { - if (first) { - layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << name << std::endl; - first = false; - } - if (last) { - // cspell: ignore preds - if ( - contain(name, "reg_preds") || contain(name, "cls_preds") || - contain(name, "obj_preds")) { - layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << name << std::endl; - } - for (int j = num - 1; j >= 0; j--) { - nvinfer1::ILayer * inner_layer = network->getLayer(j); - auto inner_layer_type = inner_layer->getType(); - std::string inner_name = inner_layer->getName(); - if (inner_layer_type == nvinfer1::LayerType::kCONVOLUTION) { - inner_layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << inner_name << std::endl; - break; - } - if (inner_layer_type == nvinfer1::LayerType::kMATRIX_MULTIPLY) { - inner_layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << inner_name << std::endl; - break; - } - } - } - } - } +std::shared_ptr TrtCommon::getHostProfiler() +{ + return host_profiler_; +} + +std::shared_ptr TrtCommon::getTrtCommonConfig() +{ + return trt_config_; +} + +std::shared_ptr TrtCommon::getBuilderConfig() +{ + return builder_config_; +} + +std::shared_ptr TrtCommon::getNetwork() +{ + return network_; +} + +std::shared_ptr TrtCommon::getLogger() +{ + return logger_; +} + +bool TrtCommon::enqueueV3(cudaStream_t stream) +{ + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Context is not initialized"); + return false; + } + if (trt_config_->profile_per_layer) { + auto inference_start = std::chrono::high_resolution_clock::now(); + auto success = context_->enqueueV3(stream); + auto inference_end = std::chrono::high_resolution_clock::now(); + host_profiler_->reportLayerTime( + "inference_host", + std::chrono::duration(inference_end - inference_start).count()); + return success; } + return context_->enqueueV3(stream); +} - const auto input = network->getInput(0); - const auto input_dims = input->getDimensions(); - const auto input_channel = input_dims.d[1]; - const auto input_height = input_dims.d[2]; - const auto input_width = input_dims.d[3]; - const auto input_batch = input_dims.d[0]; +void TrtCommon::printProfiling() const +{ + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Host Profiling\n", host_profiler_->toString().c_str()); + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Model Profiling\n", model_profiler_->toString().c_str()); +} + +std::string TrtCommon::getLayerInformation(nvinfer1::LayerInformationFormat format) +{ + if (!context_ || !engine_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Context or engine are not initialized"); + return {}; + } + auto inspector = std::unique_ptr(engine_->createEngineInspector()); + inspector->setExecutionContext(&(*context_)); + std::string result = inspector->getEngineInformation(format); + return result; +} - if (input_batch > 1) { - batch_config_[0] = input_batch; +bool TrtCommon::initialize() +{ + if (!fs::exists(trt_config_->onnx_path) || trt_config_->onnx_path.extension() != ".onnx") { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Invalid ONNX file path or extension"); + return false; } - if (batch_config_.at(0) > 1 && (batch_config_.at(0) == batch_config_.at(2))) { - // Attention : below API is deprecated in TRT8.4 - builder->setMaxBatchSize(batch_config_.at(2)); - } else { - if (build_config_->profile_per_layer) { - auto profile = builder->createOptimizationProfile(); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMIN, - nvinfer1::Dims4{batch_config_.at(0), input_channel, input_height, input_width}); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kOPT, - nvinfer1::Dims4{batch_config_.at(1), input_channel, input_height, input_width}); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMAX, - nvinfer1::Dims4{batch_config_.at(2), input_channel, input_height, input_width}); - config->addOptimizationProfile(profile); - } + builder_ = TrtUniquePtr(nvinfer1::createInferBuilder(*logger_)); + if (!builder_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); + return false; } - if (precision_ == "int8" && calibrator_) { - config->setFlag(nvinfer1::BuilderFlag::kINT8); -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 - config->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); + +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH < 10000 + const auto explicit_batch = + 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + network_ = TrtUniquePtr(builder_->createNetworkV2(explicit_batch)); #else - config->setFlag(nvinfer1::BuilderFlag::kSTRICT_TYPES); + network_ = TrtUniquePtr(builder_->createNetworkV2(0)); #endif - // QAT requires no calibrator. - // assert((calibrator != nullptr) && "Invalid calibrator for INT8 precision"); - config->setInt8Calibrator(calibrator_.get()); + + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); + return false; } - if (build_config_->profile_per_layer) { -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 - config->setProfilingVerbosity(nvinfer1::ProfilingVerbosity::kDETAILED); -#else - config->setProfilingVerbosity(nvinfer1::ProfilingVerbosity::kVERBOSE); -#endif + + builder_config_ = TrtUniquePtr(builder_->createBuilderConfig()); + if (!builder_config_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); + return false; + } + + auto num_available_dla = builder_->getNbDLACores(); + if (trt_config_->dla_core_id != -1) { + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Number of DLAs supported: %d", num_available_dla); + builder_config_->setDefaultDeviceType(nvinfer1::DeviceType::kDLA); + builder_config_->setDLACore(trt_config_->dla_core_id); + builder_config_->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); + builder_config_->setFlag(nvinfer1::BuilderFlag::kGPU_FALLBACK); } + if (trt_config_->precision == "fp16") { + builder_config_->setFlag(nvinfer1::BuilderFlag::kFP16); + } else if (trt_config_->precision == "int8") { + builder_config_->setFlag(nvinfer1::BuilderFlag::kINT8); + } + builder_config_->setMemoryPoolLimit( + nvinfer1::MemoryPoolType::kWORKSPACE, trt_config_->max_workspace_size); + + parser_ = TrtUniquePtr(nvonnxparser::createParser(*network_, *logger_)); + if (!parser_->parseFromFile( + trt_config_->onnx_path.c_str(), + static_cast(nvinfer1::ILogger::Severity::kERROR))) { + return false; + } + + if (trt_config_->profile_per_layer) { + builder_config_->setProfilingVerbosity(nvinfer1::ProfilingVerbosity::kDETAILED); + } + + return true; +} -#if TENSORRT_VERSION_MAJOR >= 8 - auto plan = - TrtUniquePtr(builder->buildSerializedNetwork(*network, *config)); +bool TrtCommon::buildEngineFromOnnx() +{ + // Build engine + auto plan = TrtUniquePtr( + builder_->buildSerializedNetwork(*network_, *builder_config_)); if (!plan) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create host memory"); + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create host memory"); return false; } engine_ = TrtUniquePtr( runtime_->deserializeCudaEngine(plan->data(), plan->size())); -#else - engine_ = TrtUniquePtr(builder->buildEngineWithConfig(*network, *config)); -#endif if (!engine_) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create engine"); + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create engine"); return false; } - // save engine -#if TENSORRT_VERSION_MAJOR < 8 - auto data = TrtUniquePtr(engine_->serialize()); -#endif + // Save engine std::ofstream file; - file.open(output_engine_file_path, std::ios::binary | std::ios::out); + file.open(trt_config_->engine_path, std::ios::binary | std::ios::out); if (!file.is_open()) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to open engine file"); return false; } -#if TENSORRT_VERSION_MAJOR < 8 - file.write(reinterpret_cast(data->data()), data->size()); -#else - file.write(reinterpret_cast(plan->data()), plan->size()); -#endif - + file.write(reinterpret_cast(plan->data()), plan->size()); // NOLINT file.close(); - return true; -} - -bool TrtCommon::isInitialized() -{ - return is_initialized_; -} - -nvinfer1::Dims TrtCommon::getBindingDimensions(const int32_t index) const -{ -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + (NV_TENSOR_PATCH * 10) >= 8500 - auto const & name = engine_->getIOTensorName(index); - auto dims = context_->getTensorShape(name); - bool const has_runtime_dim = - std::any_of(dims.d, dims.d + dims.nbDims, [](int32_t dim) { return dim == -1; }); + context_ = TrtUniquePtr(engine_->createExecutionContext()); + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create context"); + return false; + } - if (has_runtime_dim) { - return dims; - } else { - return context_->getBindingDimensions(index); + if (trt_config_->profile_per_layer) { + context_->setProfiler(&*model_profiler_); } -#else - return context_->getBindingDimensions(index); -#endif -} -int32_t TrtCommon::getNbBindings() -{ - return engine_->getNbBindings(); -} + fs::path json_path = trt_config_->engine_path.replace_extension(".json"); + auto ret = getLayerInformation(nvinfer1::LayerInformationFormat::kJSON); + std::ofstream os(json_path, std::ofstream::trunc); + os << ret << std::flush; + os.close(); -bool TrtCommon::setBindingDimensions(const int32_t index, const nvinfer1::Dims & dimensions) const -{ - return context_->setBindingDimensions(index, dimensions); + return true; } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 -bool TrtCommon::enqueueV3(cudaStream_t stream) +bool TrtCommon::loadEngine() { - if (build_config_->profile_per_layer) { - auto inference_start = std::chrono::high_resolution_clock::now(); - - bool ret = context_->enqueueV3(stream); + std::ifstream engine_file(trt_config_->engine_path); + std::stringstream engine_buffer; + engine_buffer << engine_file.rdbuf(); + std::string engine_str = engine_buffer.str(); - auto inference_end = std::chrono::high_resolution_clock::now(); - host_profiler_.reportLayerTime( - "inference", - std::chrono::duration(inference_end - inference_start).count()); - return ret; + engine_ = TrtUniquePtr(runtime_->deserializeCudaEngine( + reinterpret_cast( // NOLINT + engine_str.data()), + engine_str.size())); + if (!engine_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create engine"); + return false; } - return context_->enqueueV3(stream); -} -#endif -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH < 10000 -bool TrtCommon::enqueueV2(void ** bindings, cudaStream_t stream, cudaEvent_t * input_consumed) -{ - if (build_config_->profile_per_layer) { - auto inference_start = std::chrono::high_resolution_clock::now(); - bool ret = context_->enqueueV2(bindings, stream, input_consumed); + context_ = TrtUniquePtr(engine_->createExecutionContext()); + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create context"); + return false; + } - auto inference_end = std::chrono::high_resolution_clock::now(); - host_profiler_.reportLayerTime( - "inference", - std::chrono::duration(inference_end - inference_start).count()); - return ret; - } else { - return context_->enqueueV2(bindings, stream, input_consumed); + if (trt_config_->profile_per_layer) { + context_->setProfiler(&*model_profiler_); } + + return true; } -#endif -void TrtCommon::printProfiling() +bool TrtCommon::validateProfileDims() { - std::cout << host_profiler_; - std::cout << std::endl; - std::cout << model_profiler_; + auto success = true; + if (!profile_dims_ || profile_dims_->empty()) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Input profile is empty, skipping validation. If network has dynamic shapes, it might lead " + "to undefined behavior."); + return success; + } + if (engine_->getNbOptimizationProfiles() != 1) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Number of optimization profiles in the engine (%d) is not equal to 1. Selecting the first " + "cached profile.", + engine_->getNbOptimizationProfiles()); + } + + for (const auto & profile_dim : *profile_dims_) { + nvinfer1::Dims min_dims = engine_->getProfileShape( + profile_dim.tensor_name.c_str(), 0, nvinfer1::OptProfileSelector::kMIN); + nvinfer1::Dims opt_dims = engine_->getProfileShape( + profile_dim.tensor_name.c_str(), 0, nvinfer1::OptProfileSelector::kOPT); + nvinfer1::Dims max_dims = engine_->getProfileShape( + profile_dim.tensor_name.c_str(), 0, nvinfer1::OptProfileSelector::kMAX); + ProfileDims profile_from_engine{profile_dim.tensor_name, min_dims, opt_dims, max_dims}; + if (profile_dim != profile_from_engine) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Invalid profile. Current configuration: %s. Cached engine: %s", + profile_dim.toString().c_str(), profile_from_engine.toString().c_str()); + success = false; + } + } + return success; } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 -std::string TrtCommon::getLayerInformation(nvinfer1::LayerInformationFormat format) +bool TrtCommon::validateNetworkIO() { - auto runtime = std::unique_ptr(nvinfer1::createInferRuntime(logger_)); - auto inspector = std::unique_ptr(engine_->createEngineInspector()); - if (context_ != nullptr) { - inspector->setExecutionContext(&(*context_)); + auto success = true; + if (!network_io_ || network_io_->empty()) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Network IO is empty, skipping validation. It might lead to undefined behavior"); + return success; + } + + if (network_io_->size() != static_cast(getNbIOTensors())) { + std::string tensor_names = "[" + std::string(getIOTensorName(0)); + for (int32_t i = 1; i < getNbIOTensors(); ++i) { + tensor_names += ", " + std::string(getIOTensorName(i)); + } + tensor_names += "]"; + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Number of tensors in the engine (%d) does not match number of tensors in the config (%d). " + "Tensors in the built engine: %s", + getNbIOTensors(), network_io_->size(), tensor_names.c_str()); + success = false; + } + for (const auto & io : *network_io_) { + NetworkIO tensor_from_engine{io.tensor_name, getTensorShape(io.tensor_name.c_str())}; + if (io != tensor_from_engine) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Invalid tensor. Current configuration: %s. Cached engine: %s", io.toString().c_str(), + tensor_from_engine.toString().c_str()); + success = false; + } } - std::string result = inspector->getEngineInformation(format); - return result; + + return success; } -#endif } // namespace tensorrt_common } // namespace autoware diff --git a/perception/autoware_tensorrt_yolox/CMakeLists.txt b/perception/autoware_tensorrt_yolox/CMakeLists.txt index 17b31fc7e8df1..cc0d793151781 100644 --- a/perception/autoware_tensorrt_yolox/CMakeLists.txt +++ b/perception/autoware_tensorrt_yolox/CMakeLists.txt @@ -10,6 +10,9 @@ endif() find_package(autoware_cmake REQUIRED) autoware_package() +# TODO(amadeuszsz): Remove -Wno-deprecated-declarations once removing implicit quantization +add_compile_options(-Wno-deprecated-declarations) + find_package(OpenCV REQUIRED) option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) diff --git a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp index 483adfbdf2757..7d49df145b72e 100644 --- a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp +++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include @@ -44,6 +46,13 @@ struct Object using ObjectArray = std::vector; using ObjectArrays = std::vector; +using autoware::tensorrt_common::CalibrationConfig; +using autoware::tensorrt_common::NetworkIOPtr; +using autoware::tensorrt_common::ProfileDimsPtr; +using autoware::tensorrt_common::Profiler; +using autoware::tensorrt_common::TrtCommon; +using autoware::tensorrt_common::TrtCommonConfig; +using autoware::tensorrt_common::TrtConvCalib; struct GridAndStride { @@ -70,31 +79,26 @@ class TrtYoloX public: /** * @brief Construct TrtYoloX. - * @param[in] mode_path ONNX model_path - * @param[in] precision precision for inference + * @param[in] trt_config base trt common configuration * @param[in] num_class classifier-ed num * @param[in] score_threshold threshold for detection * @param[in] nms_threshold threshold for NMS - * @param[in] build_config configuration including precision, calibration method, DLA, remaining - * fp16 for first layer, remaining fp16 for last layer and profiler for builder * @param[in] use_gpu_preprocess whether use cuda gpu for preprocessing - * @param[in] calibration_image_list_file path for calibration files (only require for + * @param[in] gpu_id GPU id for inference + * @param[in] calibration_image_list_path path for calibration files (only require for * quantization) * @param[in] norm_factor scaling factor for preprocess * @param[in] cache_dir unused variable - * @param[in] batch_config configuration for batched execution - * @param[in] max_workspace_size maximum workspace for building TensorRT engine + * @param[in] color_map_path path for colormap for masks + * @param[in] calib_config calibration configuration */ TrtYoloX( - const std::string & model_path, const std::string & precision, const int num_class = 8, - const float score_threshold = 0.3, const float nms_threshold = 0.7, - const autoware::tensorrt_common::BuildConfig build_config = - autoware::tensorrt_common::BuildConfig(), - const bool use_gpu_preprocess = false, const uint8_t gpu_id = 0, - std::string calibration_image_list_file = std::string(), const double norm_factor = 1.0, - [[maybe_unused]] const std::string & cache_dir = "", - const autoware::tensorrt_common::BatchConfig & batch_config = {1, 1, 1}, - const size_t max_workspace_size = (1 << 30), const std::string & color_map_path = ""); + TrtCommonConfig & trt_config, const int num_class = 8, const float score_threshold = 0.3, + const float nms_threshold = 0.7, const bool use_gpu_preprocess = false, + const uint8_t gpu_id = 0, std::string calibration_image_list_path = std::string(), + const double norm_factor = 1.0, [[maybe_unused]] const std::string & cache_dir = "", + const std::string & color_map_path = "", + const CalibrationConfig & calib_config = CalibrationConfig()); /** * @brief Deconstruct TrtYoloX */ @@ -168,6 +172,12 @@ class TrtYoloX cv::Mat & colorized_mask); inline std::vector getColorMap() { return sematic_color_map_; } + /** + * @brief get batch size + * @return batch size + */ + [[nodiscard]] int getBatchSize() const; + private: /** * @brief run preprocess including resizing, letterbox, NHWC2NCHW and toFloat on CPU @@ -266,7 +276,7 @@ class TrtYoloX */ cv::Mat getMaskImageGpu(float * d_prob, nvinfer1::Dims dims, int out_w, int out_h, int b); - std::unique_ptr trt_common_; + std::unique_ptr trt_common_; std::vector input_h_; CudaUniquePtr input_d_; @@ -288,7 +298,7 @@ class TrtYoloX int num_class_; float score_threshold_; float nms_threshold_; - int batch_size_; + int32_t batch_size_; CudaUniquePtrHost out_prob_h_; // flag whether preprocess are performed on GPU diff --git a/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml b/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml index a7952b12414b1..606fd1299cf35 100644 --- a/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml +++ b/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml @@ -11,34 +11,50 @@ + + + + + + + + + + + + + + + + diff --git a/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml index 0096a219c8573..6d40905d78127 100644 --- a/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml +++ b/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -1,6 +1,8 @@ + + @@ -15,13 +17,13 @@ - + - + diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp index 7e2e327bf6f5e..74d8d73ecaed4 100644 --- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp @@ -155,13 +155,11 @@ std::vector get_seg_colormap(const std::stri namespace autoware::tensorrt_yolox { TrtYoloX::TrtYoloX( - const std::string & model_path, const std::string & precision, const int num_class, - const float score_threshold, const float nms_threshold, - autoware::tensorrt_common::BuildConfig build_config, const bool use_gpu_preprocess, - const uint8_t gpu_id, std::string calibration_image_list_path, const double norm_factor, - [[maybe_unused]] const std::string & cache_dir, - const autoware::tensorrt_common::BatchConfig & batch_config, const size_t max_workspace_size, - const std::string & color_map_path) + TrtCommonConfig & trt_config, const int num_class, const float score_threshold, + const float nms_threshold, const bool use_gpu_preprocess, const uint8_t gpu_id, + std::string calibration_image_list_path, const double norm_factor, + [[maybe_unused]] const std::string & cache_dir, const std::string & color_map_path, + const CalibrationConfig & calib_config) : gpu_id_(gpu_id), is_gpu_initialized_(false) { if (!setCudaDeviceId(gpu_id_)) { @@ -172,13 +170,29 @@ TrtYoloX::TrtYoloX( src_width_ = -1; src_height_ = -1; norm_factor_ = norm_factor; - batch_size_ = batch_config[2]; multitask_ = 0; sematic_color_map_ = get_seg_colormap(color_map_path); stream_ = makeCudaStream(); - if (precision == "int8") { - if (build_config.clip_value <= 0.0) { + trt_common_ = std::make_unique(trt_config); + + const auto network_input_dims = trt_common_->getTensorShape(0); + batch_size_ = network_input_dims.d[0]; + const auto input_channel = network_input_dims.d[1]; + const auto input_height = network_input_dims.d[2]; + const auto input_width = network_input_dims.d[3]; + + auto profile_dims_ptr = std::make_unique>(); + + std::vector profile_dims{ + autoware::tensorrt_common::ProfileDims( + 0, {4, {batch_size_, input_channel, input_height, input_width}}, + {4, {batch_size_, input_channel, input_height, input_width}}, + {4, {batch_size_, input_channel, input_height, input_width}})}; + *profile_dims_ptr = profile_dims; + + if (trt_config.precision == "int8") { + if (calib_config.clip_value <= 0.0) { if (calibration_image_list_path.empty()) { throw std::runtime_error( "calibration_image_list_path should be passed to generate int8 engine " @@ -189,19 +203,17 @@ TrtYoloX::TrtYoloX( calibration_image_list_path = ""; } - int max_batch_size = batch_size_; - nvinfer1::Dims input_dims = autoware::tensorrt_common::get_input_dims(model_path); std::vector calibration_images; if (calibration_image_list_path != "") { calibration_images = loadImageList(calibration_image_list_path, ""); } - tensorrt_yolox::ImageStream stream(max_batch_size, input_dims, calibration_images); - fs::path calibration_table{model_path}; + tensorrt_yolox::ImageStream stream(batch_size_, network_input_dims, calibration_images); + fs::path calibration_table{trt_config.onnx_path}; std::string ext = ""; - if (build_config.calib_type_str == "Entropy") { + if (calib_config.calib_type_str == "Entropy") { ext = "EntropyV2-"; } else if ( - build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + calib_config.calib_type_str == "Legacy" || calib_config.calib_type_str == "Percentile") { ext = "Legacy-"; } else { ext = "MinMax-"; @@ -209,17 +221,17 @@ TrtYoloX::TrtYoloX( ext += "calibration.table"; calibration_table.replace_extension(ext); - fs::path histogram_table{model_path}; + fs::path histogram_table{trt_config.onnx_path}; ext = "histogram.table"; histogram_table.replace_extension(ext); std::unique_ptr calibrator; - if (build_config.calib_type_str == "Entropy") { + if (calib_config.calib_type_str == "Entropy") { calibrator.reset( new tensorrt_yolox::Int8EntropyCalibrator(stream, calibration_table, norm_factor_)); } else if ( - build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + calib_config.calib_type_str == "Legacy" || calib_config.calib_type_str == "Percentile") { const double quantile = 0.999999; const double cutoff = 0.999999; calibrator.reset(new tensorrt_yolox::Int8LegacyCalibrator( @@ -228,22 +240,20 @@ TrtYoloX::TrtYoloX( calibrator.reset( new tensorrt_yolox::Int8MinMaxCalibrator(stream, calibration_table, norm_factor_)); } - trt_common_ = std::make_unique( - model_path, precision, std::move(calibrator), batch_config, max_workspace_size, build_config); + if (!trt_common_->setupWithCalibrator( + std::move(calibrator), calib_config, std::move((profile_dims_ptr)))) { + throw std::runtime_error("Failed to setup TensorRT engine with calibrator"); + } } else { - trt_common_ = std::make_unique( - model_path, precision, nullptr, batch_config, max_workspace_size, build_config); - } - trt_common_->setup(); - - if (!trt_common_->isInitialized()) { - return; + if (!trt_common_->setup(std::move(profile_dims_ptr))) { + throw std::runtime_error("Failed to setup TensorRT engine"); + } } // Judge whether decoding output is required // Plain models require decoding, while models with EfficientNMS_TRT module don't. // If getNbBindings == 5, the model contains EfficientNMS_TRT - switch (trt_common_->getNbBindings()) { + switch (trt_common_->getNbIOTensors()) { case 2: // Specified model is plain one. // Bindings are: [input, output] @@ -273,16 +283,16 @@ TrtYoloX::TrtYoloX( */ } // GPU memory allocation - const auto input_dims = trt_common_->getBindingDimensions(0); + const auto input_dims = trt_common_->getTensorShape(0); const auto input_size = std::accumulate(input_dims.d + 1, input_dims.d + input_dims.nbDims, 1, std::multiplies()); if (needs_output_decode_) { - const auto output_dims = trt_common_->getBindingDimensions(1); - input_d_ = autoware::cuda_utils::make_unique(batch_config[2] * input_size); + const auto output_dims = trt_common_->getTensorShape(1); + input_d_ = autoware::cuda_utils::make_unique(batch_size_ * input_size); out_elem_num_ = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); - out_elem_num_ = out_elem_num_ * batch_config[2]; - out_elem_num_per_batch_ = static_cast(out_elem_num_ / batch_config[2]); + out_elem_num_ = out_elem_num_ * batch_size_; + out_elem_num_per_batch_ = static_cast(out_elem_num_ / batch_size_); out_prob_d_ = autoware::cuda_utils::make_unique(out_elem_num_); out_prob_h_ = autoware::cuda_utils::make_unique_host(out_elem_num_, cudaHostAllocPortable); @@ -298,29 +308,27 @@ TrtYoloX::TrtYoloX( output_strides_ = {8, 16, 32, 4}; } } else { - const auto out_scores_dims = trt_common_->getBindingDimensions(3); + const auto out_scores_dims = trt_common_->getTensorShape(3); max_detections_ = out_scores_dims.d[1]; - input_d_ = autoware::cuda_utils::make_unique(batch_config[2] * input_size); - out_num_detections_d_ = autoware::cuda_utils::make_unique(batch_config[2]); - out_boxes_d_ = - autoware::cuda_utils::make_unique(batch_config[2] * max_detections_ * 4); - out_scores_d_ = autoware::cuda_utils::make_unique(batch_config[2] * max_detections_); - out_classes_d_ = - autoware::cuda_utils::make_unique(batch_config[2] * max_detections_); + input_d_ = autoware::cuda_utils::make_unique(batch_size_ * input_size); + out_num_detections_d_ = autoware::cuda_utils::make_unique(batch_size_); + out_boxes_d_ = autoware::cuda_utils::make_unique(batch_size_ * max_detections_ * 4); + out_scores_d_ = autoware::cuda_utils::make_unique(batch_size_ * max_detections_); + out_classes_d_ = autoware::cuda_utils::make_unique(batch_size_ * max_detections_); } if (multitask_) { // Allocate buffer for segmentation segmentation_out_elem_num_ = 0; for (int m = 0; m < multitask_; m++) { const auto output_dims = - trt_common_->getBindingDimensions(m + 2); // 0 : input, 1 : output for detections + trt_common_->getTensorShape(m + 2); // 0 : input, 1 : output for detections size_t out_elem_num = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); - out_elem_num = out_elem_num * batch_config[2]; + out_elem_num = out_elem_num * batch_size_; segmentation_out_elem_num_ += out_elem_num; } segmentation_out_elem_num_per_batch_ = - static_cast(segmentation_out_elem_num_ / batch_config[2]); + static_cast(segmentation_out_elem_num_ / batch_size_); segmentation_out_prob_d_ = autoware::cuda_utils::make_unique(segmentation_out_elem_num_); segmentation_out_prob_h_ = autoware::cuda_utils::make_unique_host( @@ -387,7 +395,7 @@ void TrtYoloX::initPreprocessBuffer(int width, int height) src_width_ = width; src_height_ = height; if (use_gpu_preprocess_) { - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); bool const hasRuntimeDim = std::any_of( input_dims.d, input_dims.d + input_dims.nbDims, [](int32_t input_dim) { return input_dim == -1; }); @@ -395,7 +403,9 @@ void TrtYoloX::initPreprocessBuffer(int width, int height) input_dims.d[0] = batch_size_; } if (!image_buf_h_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } scales_.clear(); } const float input_height = static_cast(input_dims.d[2]); @@ -414,7 +424,7 @@ void TrtYoloX::initPreprocessBuffer(int width, int height) size_t argmax_out_elem_num = 0; for (int m = 0; m < multitask_; m++) { const auto output_dims = - trt_common_->getBindingDimensions(m + 2); // 0 : input, 1 : output for detections + trt_common_->getTensorShape(m + 2); // 0 : input, 1 : output for detections const float scale = std::min( output_dims.d[3] / static_cast(width), output_dims.d[2] / static_cast(height)); @@ -441,7 +451,7 @@ void TrtYoloX::printProfiling(void) void TrtYoloX::preprocessGpu(const std::vector & images) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; for (const auto & image : images) { @@ -469,7 +479,9 @@ void TrtYoloX::preprocessGpu(const std::vector & images) src_height_ = height; } if (!image_buf_h_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } scales_.clear(); } const float input_height = static_cast(input_dims.d[2]); @@ -495,7 +507,7 @@ void TrtYoloX::preprocessGpu(const std::vector & images) if (multitask_) { for (int m = 0; m < multitask_; m++) { const auto output_dims = - trt_common_->getBindingDimensions(m + 2); // 0: input, 1: output for detections + trt_common_->getTensorShape(m + 2); // 0: input, 1: output for detections const float scale = std::min( output_dims.d[3] / static_cast(image.cols), output_dims.d[2] / static_cast(image.rows)); @@ -530,9 +542,11 @@ void TrtYoloX::preprocessGpu(const std::vector & images) void TrtYoloX::preprocess(const std::vector & images) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; @@ -567,9 +581,6 @@ bool TrtYoloX::doInference( if (!setCudaDeviceId(gpu_id_)) { return false; } - if (!trt_common_->isInitialized()) { - return false; - } if (use_gpu_preprocess_) { preprocessGpu(images); @@ -588,7 +599,7 @@ void TrtYoloX::preprocessWithRoiGpu( const std::vector & images, const std::vector & rois) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; for (const auto & image : images) { @@ -610,7 +621,9 @@ void TrtYoloX::preprocessWithRoiGpu( src_height_ = height; } if (!image_buf_h_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); @@ -662,9 +675,11 @@ void TrtYoloX::preprocessWithRoi( const std::vector & images, const std::vector & rois) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; @@ -700,7 +715,7 @@ void TrtYoloX::preprocessWithRoi( void TrtYoloX::multiScalePreprocessGpu(const cv::Mat & image, const std::vector & rois) { const auto batch_size = rois.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; @@ -722,7 +737,9 @@ void TrtYoloX::multiScalePreprocessGpu(const cv::Mat & image, const std::vector< src_height_ = height; if (!image_buf_h_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); @@ -770,9 +787,11 @@ void TrtYoloX::multiScalePreprocessGpu(const cv::Mat & image, const std::vector< void TrtYoloX::multiScalePreprocess(const cv::Mat & image, const std::vector & rois) { const auto batch_size = rois.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; @@ -806,9 +825,6 @@ void TrtYoloX::multiScalePreprocess(const cv::Mat & image, const std::vector & images, ObjectArrays & objects, const std::vector & rois) { - if (!trt_common_->isInitialized()) { - return false; - } if (use_gpu_preprocess_) { preprocessWithRoiGpu(images, rois); } else { @@ -827,9 +843,6 @@ bool TrtYoloX::doInferenceWithRoi( bool TrtYoloX::doMultiScaleInference( const cv::Mat & image, ObjectArrays & objects, const std::vector & rois) { - if (!trt_common_->isInitialized()) { - return false; - } if (use_gpu_preprocess_) { multiScalePreprocessGpu(image, rois); } else { @@ -849,8 +862,10 @@ bool TrtYoloX::feedforward(const std::vector & images, ObjectArrays & o std::vector buffers = { input_d_.get(), out_num_detections_d_.get(), out_boxes_d_.get(), out_scores_d_.get(), out_classes_d_.get()}; - - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); const auto batch_size = images.size(); auto out_num_detections = std::make_unique(batch_size); @@ -903,7 +918,11 @@ bool TrtYoloX::feedforwardAndDecode( if (multitask_) { buffers = {input_d_.get(), out_prob_d_.get(), segmentation_out_prob_d_.get()}; } - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); const auto batch_size = images.size(); @@ -933,7 +952,7 @@ bool TrtYoloX::feedforwardAndDecode( static_cast(segmentation_out_elem_num_ / segmentation_out_elem_num_per_batch_); for (int m = 0; m < multitask_; m++) { const auto output_dims = - trt_common_->getBindingDimensions(m + 2); // 0 : input, 1 : output for detections + trt_common_->getTensorShape(m + 2); // 0 : input, 1 : output for detections size_t out_elem_num = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); out_elem_num = out_elem_num * batch; @@ -972,7 +991,10 @@ bool TrtYoloX::multiScaleFeedforward(const cv::Mat & image, int batch_size, Obje input_d_.get(), out_num_detections_d_.get(), out_boxes_d_.get(), out_scores_d_.get(), out_classes_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); auto out_num_detections = std::make_unique(batch_size); auto out_boxes = std::make_unique(4 * batch_size * max_detections_); @@ -1020,7 +1042,10 @@ bool TrtYoloX::multiScaleFeedforwardAndDecode( const cv::Mat & image, int batch_size, ObjectArrays & objects) { std::vector buffers = {input_d_.get(), out_prob_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); CHECK_CUDA_ERROR(cudaMemcpyAsync( out_prob_h_.get(), out_prob_d_.get(), sizeof(float) * out_elem_num_, cudaMemcpyDeviceToHost, @@ -1042,7 +1067,7 @@ void TrtYoloX::decodeOutputs( float * prob, ObjectArray & objects, float scale, cv::Size & img_size) const { ObjectArray proposals; - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); std::vector grid_strides; @@ -1298,4 +1323,9 @@ void TrtYoloX::getColorizedMask( } } +int TrtYoloX::getBatchSize() const +{ + return batch_size_; +} + } // namespace autoware::tensorrt_yolox diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp index 94a3a37a4d08f..2048279dcf3f1 100644 --- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -83,19 +83,18 @@ TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) roi_overlay_segment_labels_.ANIMAL = declare_parameter("roi_overlay_segment_label.ANIMAL"); replaceLabelMap(); - autoware::tensorrt_common::BuildConfig build_config( - calibration_algorithm, dla_core_id, quantize_first_layer, quantize_last_layer, - profile_per_layer, clip_value); + TrtCommonConfig trt_config( + model_path, precision, "", (1ULL << 30U), dla_core_id, profile_per_layer); + + CalibrationConfig calib_config( + calibration_algorithm, quantize_first_layer, quantize_last_layer, clip_value); const double norm_factor = 1.0; const std::string cache_dir = ""; - const autoware::tensorrt_common::BatchConfig batch_config{1, 1, 1}; - const size_t max_workspace_size = (1 << 30); trt_yolox_ = std::make_unique( - model_path, precision, label_map_.size(), score_threshold, nms_threshold, build_config, - preprocess_on_gpu, gpu_id, calibration_image_list_path, norm_factor, cache_dir, batch_config, - max_workspace_size, color_map_path); + trt_config, label_map_.size(), score_threshold, nms_threshold, preprocess_on_gpu, gpu_id, + calibration_image_list_path, norm_factor, cache_dir, color_map_path, calib_config); if (!trt_yolox_->isGPUInitialized()) { RCLCPP_ERROR(this->get_logger(), "GPU %d does not exist or is not suitable.", gpu_id); diff --git a/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp b/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp index 243e82c65dab9..63c0ee0cd3e8a 100644 --- a/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp +++ b/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp @@ -46,7 +46,9 @@ class YoloXSingleImageInferenceNode : public rclcpp::Node const auto output_image_path = declare_parameter("output_image_path", p.string() + "_detect" + ext); - auto trt_yolox = std::make_unique(model_path, precision); + auto trt_config = tensorrt_common::TrtCommonConfig(model_path, precision); + + auto trt_yolox = std::make_unique(trt_config); auto image = cv::imread(image_path); tensorrt_yolox::ObjectArrays objects; std::vector masks; diff --git a/perception/autoware_traffic_light_classifier/CMakeLists.txt b/perception/autoware_traffic_light_classifier/CMakeLists.txt index d132577743ab0..181037caebfa5 100644 --- a/perception/autoware_traffic_light_classifier/CMakeLists.txt +++ b/perception/autoware_traffic_light_classifier/CMakeLists.txt @@ -34,7 +34,6 @@ if(NVINFER AND NVONNXPARSER AND NVINFER_PLUGIN) if(CUDA_VERBOSE) message(STATUS "TensorRT is available!") message(STATUS "NVINFER: ${NVINFER}") - message(STATUS "NVPARSERS: ${NVPARSERS}") message(STATUS "NVINFER_PLUGIN: ${NVINFER_PLUGIN}") message(STATUS "NVONNXPARSER: ${NVONNXPARSER}") endif() diff --git a/perception/autoware_traffic_light_classifier/README.md b/perception/autoware_traffic_light_classifier/README.md index 6e720aabc7593..7dcd4a73380bb 100644 --- a/perception/autoware_traffic_light_classifier/README.md +++ b/perception/autoware_traffic_light_classifier/README.md @@ -1,22 +1,33 @@ -# traffic_light_classifier +# autoware_traffic_light_classifier ## Purpose -traffic_light_classifier is a package for classifying traffic light labels using cropped image around a traffic light. This package has two classifier models: `cnn_classifier` and `hsv_classifier`. +`autoware_traffic_light_classifier` is a package for classifying traffic light labels using cropped image around a traffic light. This package has two classifier models: `cnn_classifier` and `hsv_classifier`. ## Inner-workings / Algorithms +If height and width of `~/input/rois` is `0`, color, shape, and confidence of `~/output/traffic_signals` become `UNKNOWN`, `CIRCLE`, and `0.0`. +If `~/input/rois` is judged as backlight, color, shape, and confidence of `~/output/traffic_signals` become `UNKNOWN`, `UNKNOWN`, and `0.0`. + ### cnn_classifier -Traffic light labels are classified by EfficientNet-b1 or MobileNet-v2. -Totally 83400 (58600 for training, 14800 for evaluation and 10000 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. -The information of the models is listed here: +Traffic light labels are classified by EfficientNet-b1 or MobileNet-v2. +We trained classifiers for vehicular signals and pedestrian signals separately. +For vehicular signals, a total of 83400 (58600 for training, 14800 for evaluation and 10000 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. | Name | Input Size | Test Accuracy | | --------------- | ---------- | ------------- | | EfficientNet-b1 | 128 x 128 | 99.76% | | MobileNet-v2 | 224 x 224 | 99.81% | +For pedestrian signals, a total of 21199 (17860 for training, 2114 for evaluation and 1225 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. +The information of the models is listed here: + +| Name | Input Size | Test Accuracy | +| --------------- | ---------- | ------------- | +| EfficientNet-b1 | 128 x 128 | 97.89% | +| MobileNet-v2 | 224 x 224 | 99.10% | + ### hsv_classifier Traffic light colors (green, yellow and red) are classified in HSV model. @@ -52,11 +63,12 @@ These colors and shapes are assigned to the message as follows: ### Node Parameters -| Name | Type | Description | -| --------------------- | ----- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `classifier_type` | int | if the value is `1`, cnn_classifier is used | -| `data_path` | str | packages data and artifacts directory path | -| `backlight_threshold` | float | If the intensity get grater than this overwrite with UNKNOWN in corresponding RoI. Note that, if the value is much higher, the node only overwrites in the harsher backlight situations. Therefore, If you wouldn't like to use this feature set this value to `1.0`. The value can be `[0.0, 1.0]`. The confidence of overwritten signal is set to `0.0`. | +| Name | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `classifier_type` | int | If the value is `1`, cnn_classifier is used | +| `data_path` | str | Packages data and artifacts directory path | +| `backlight_threshold` | double | If the intensity of light is grater than this threshold, the color and shape of the corresponding ROI will be overwritten with UNKNOWN, and the confidence of the overwritten signal will be set to `0.0`. The value should be set in the range of `[0.0, 1.0]`. If you wouldn't like to use this feature, please set it to `1.0`. | +| `classify_traffic_light_type` | int | If the value is `0`, vehicular signals are classified. If the value is `1`, pedestrian signals are classified. | ### Core Parameters diff --git a/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp index f9bf2509aa2ae..d47cb1500fffd 100644 --- a/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp +++ b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp @@ -51,13 +51,10 @@ CNNClassifier::CNNClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) } readLabelfile(label_file_path, labels_); - nvinfer1::Dims input_dim = autoware::tensorrt_common::get_input_dims(model_file_path); - assert(input_dim.d[0] > 0); - batch_size_ = input_dim.d[0]; - autoware::tensorrt_common::BatchConfig batch_config{batch_size_, batch_size_, batch_size_}; classifier_ = std::make_unique( - model_file_path, precision, batch_config, mean_, std_); + model_file_path, precision, mean_, std_); + batch_size_ = classifier_->getBatchSize(); if (node_ptr_->declare_parameter("build_only", false)) { RCLCPP_INFO(node_ptr_->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); diff --git a/perception/autoware_traffic_light_fine_detector/CMakeLists.txt b/perception/autoware_traffic_light_fine_detector/CMakeLists.txt index 80d6e43c098ed..dffbab0e77681 100644 --- a/perception/autoware_traffic_light_fine_detector/CMakeLists.txt +++ b/perception/autoware_traffic_light_fine_detector/CMakeLists.txt @@ -39,7 +39,6 @@ if(NVINFER AND NVONNXPARSER AND NVINFER_PLUGIN) if(CUDA_VERBOSE) message(STATUS "TensorRT is available!") message(STATUS "NVINFER: ${NVINFER}") - message(STATUS "NVPARSERS: ${NVPARSERS}") message(STATUS "NVINFER_PLUGIN: ${NVINFER_PLUGIN}") message(STATUS "NVONNXPARSER: ${NVONNXPARSER}") endif() diff --git a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp index 37ffca4a9526c..d9e0471a43d62 100644 --- a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp +++ b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp @@ -75,21 +75,18 @@ TrafficLightFineDetectorNode::TrafficLightFineDetectorNode(const rclcpp::NodeOpt RCLCPP_ERROR(this->get_logger(), "Could not find tlr id"); } - const autoware::tensorrt_common::BuildConfig build_config = - autoware::tensorrt_common::BuildConfig("MinMax", -1, false, false, false, 0.0); - const bool cuda_preprocess = true; const std::string calib_image_list = ""; const double scale = 1.0; const std::string cache_dir = ""; - nvinfer1::Dims input_dim = autoware::tensorrt_common::get_input_dims(model_path); - assert(input_dim.d[0] > 0); - batch_size_ = input_dim.d[0]; - const autoware::tensorrt_common::BatchConfig batch_config{batch_size_, batch_size_, batch_size_}; + + auto trt_config = autoware::tensorrt_common::TrtCommonConfig(model_path, precision); trt_yolox_ = std::make_unique( - model_path, precision, num_class, score_thresh_, nms_threshold, build_config, cuda_preprocess, - gpu_id, calib_image_list, scale, cache_dir, batch_config); + trt_config, num_class, score_thresh_, nms_threshold, cuda_preprocess, gpu_id, calib_image_list, + scale, cache_dir); + + batch_size_ = trt_yolox_->getBatchSize(); if (!trt_yolox_->isGPUInitialized()) { RCLCPP_ERROR(this->get_logger(), "GPU %d does not exist or is not suitable.", gpu_id); diff --git a/perception/autoware_traffic_light_multi_camera_fusion/README.md b/perception/autoware_traffic_light_multi_camera_fusion/README.md index f7ee294cda147..b54c623f5d750 100644 --- a/perception/autoware_traffic_light_multi_camera_fusion/README.md +++ b/perception/autoware_traffic_light_multi_camera_fusion/README.md @@ -1,29 +1,36 @@ -# The `traffic_light_multi_camera_fusion` Package +# autoware_traffic_light_multi_camera_fusion ## Overview -`traffic_light_multi_camera_fusion` performs traffic light signal fusion which can be summarized as the following two tasks: +`autoware_traffic_light_multi_camera_fusion` performs traffic light signal fusion which can be summarized as the following two tasks: -1. Multi-Camera-Fusion: performed on single traffic light signal detected by different cameras. -2. Group-Fusion: performed on traffic light signals within the same group, which means traffic lights sharing the same regulatory element id defined in lanelet2 map. +1. Multi-Camera-Fusion: fusion each traffic light signal detected by different cameras. +2. Group-Fusion: Fusion each traffic light signal within the same group, which means traffic lights share the same regulatory element ID defined in lanelet2 map. + +The fusion method is below. + +1. Use the results of the new timestamp if the results are from the same sensor +2. Use the results that are not `elements.size() == 1 && color == UNKNOWN && shape == UNKNOWN` +3. Use the results that each vertex of ROI is not at the edge of the image +4. Use the results of high confidence ## Input topics For every camera, the following three topics are subscribed: -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------- | --------------------------------------------------- | -| `~//camera_info` | sensor_msgs::CameraInfo | camera info from traffic_light_map_based_detector | -| `~//rois` | tier4_perception_msgs::TrafficLightRoiArray | detection roi from traffic_light_fine_detector | -| `~//traffic_signals` | tier4_perception_msgs::TrafficLightSignalArray | classification result from traffic_light_classifier | +| Name | Type | Description | +| ----------------------------------------------------- | ------------------------------------------------ | ------------------------------------- | +| `~//camera_info` | sensor_msgs::msg::CameraInfo | camera info from map_based_detector | +| `~//detection/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | detection roi from fine_detector | +| `~//classification/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | classification result from classifier | You don't need to configure these topics manually. Just provide the `camera_namespaces` parameter and the node will automatically extract the `` and create the subscribers. ## Output topics -| Name | Type | Description | -| -------------------------- | ------------------------------------------------- | ---------------------------------- | -| `~/output/traffic_signals` | autoware_perception_msgs::TrafficLightSignalArray | traffic light signal fusion result | +| Name | Type | Description | +| -------------------------- | ----------------------------------------------------- | ---------------------------------- | +| `~/output/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light signal fusion result | ## Node parameters diff --git a/perception/autoware_traffic_light_occlusion_predictor/README.md b/perception/autoware_traffic_light_occlusion_predictor/README.md index bc57dbea76c97..5d2e320dfc72a 100644 --- a/perception/autoware_traffic_light_occlusion_predictor/README.md +++ b/perception/autoware_traffic_light_occlusion_predictor/README.md @@ -1,8 +1,10 @@ -# The `autoware_traffic_light_occlusion_predictor` Package +# autoware_traffic_light_occlusion_predictor ## Overview `autoware_traffic_light_occlusion_predictor` receives the detected traffic lights rois and calculates the occlusion ratios of each roi with point cloud. +If that rois is judged as occlusion, color, shape, and confidence of `~/output/traffic_signals` become `UNKNOWN`, `UNKNOWN`, and `0.0`. +This node publishes when each car and pedestrian `traffic_signals` is arrived and processed. For each traffic light roi, hundreds of pixels would be selected and projected into the 3D space. Then from the camera point of view, the number of projected pixels that are occluded by the point cloud is counted and used for calculating the occlusion ratio for the roi. As shown in follow image, the red pixels are occluded and the occlusion ratio is the number of red pixels divided by the total pixel numbers. @@ -12,18 +14,20 @@ If no point cloud is received or all point clouds have very large stamp differen ## Input topics -| Name | Type | Description | -| -------------------- | ---------------------------------------------- | ------------------------ | -| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | -| `~/input/rois` | autoware_perception_msgs::TrafficLightRoiArray | traffic light detections | -| `~input/camera_info` | sensor_msgs::CameraInfo | target camera parameter | -| `~/input/cloud` | sensor_msgs::PointCloud2 | LiDAR point cloud | +| Name | Type | Description | +| ------------------------------------ | ------------------------------------------------ | -------------------------------- | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/car/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | vehicular traffic light signals | +| `~/input/pedestrian/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | pedestrian traffic light signals | +| `~/input/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | traffic light detections | +| `~/input/camera_info` | sensor_msgs::msg::CameraInfo | target camera parameter | +| `~/input/cloud` | sensor_msgs::msg::PointCloud2 | LiDAR point cloud | ## Output topics -| Name | Type | Description | -| -------------------- | ---------------------------------------------------- | ---------------------------- | -| `~/output/occlusion` | autoware_perception_msgs::TrafficLightOcclusionArray | occlusion ratios of each roi | +| Name | Type | Description | +| -------------------------- | --------------------------------------------- | -------------------------------------------------------------- | +| `~/output/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | traffic light signals which occluded image results overwritten | ## Node parameters @@ -34,3 +38,4 @@ If no point cloud is received or all point clouds have very large stamp differen | `max_valid_pt_dist` | double | The points within this distance would be used for calculation | | `max_image_cloud_delay` | double | The maximum delay between LiDAR point cloud and camera image | | `max_wait_t` | double | The maximum time waiting for the LiDAR point cloud | +| `max_occlusion_ratio` | int | The maximum occlusion ratio for setting signal as unknown | 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 4fed5bcb6cff3..b6749117a6177 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 @@ -58,10 +58,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -99,7 +99,8 @@ 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::Publisher::SharedPtr + pub_processing_time_ms_; rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; autoware::universe_utils::InterProcessPollingSubscriber diff --git a/planning/autoware_costmap_generator/src/costmap_generator.cpp b/planning/autoware_costmap_generator/src/costmap_generator.cpp index 081a6ead91cda..70b8051bcdc12 100644 --- a/planning/autoware_costmap_generator/src/costmap_generator.cpp +++ b/planning/autoware_costmap_generator/src/costmap_generator.cpp @@ -176,7 +176,8 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) 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); + this->create_publisher( + "~/debug/processing_time_ms", 1); // Timer const auto period_ns = rclcpp::Rate(param_->update_rate).period(); @@ -269,7 +270,7 @@ void CostmapGenerator::onTimer() if (!isActive()) { // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_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); @@ -485,7 +486,7 @@ void CostmapGenerator::publishCostmap( pub_costmap_->publish(*out_gridmap_msg); // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_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); diff --git a/planning/autoware_external_velocity_limit_selector/CMakeLists.txt b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt index ca758d1262b48..aa8d07d91135e 100644 --- a/planning/autoware_external_velocity_limit_selector/CMakeLists.txt +++ b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt @@ -21,6 +21,13 @@ rclcpp_components_register_node(external_velocity_limit_selector_node EXECUTABLE external_velocity_limit_selector ) +if(BUILD_TESTING) + add_launch_test( + test/test_external_velocity_limit_selector_node_launch.py + TIMEOUT "30" + ) +endif() + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/planning/autoware_external_velocity_limit_selector/package.xml b/planning/autoware_external_velocity_limit_selector/package.xml index b75a4fab72d7c..5bfd3fa936d50 100644 --- a/planning/autoware_external_velocity_limit_selector/package.xml +++ b/planning/autoware_external_velocity_limit_selector/package.xml @@ -28,6 +28,7 @@ ament_lint_auto autoware_lint_common + ros_testing ament_cmake diff --git a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index cf42763fc6b60..cc2b134900a31 100644 --- a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -14,7 +14,6 @@ #include "autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" -#include #include #include #include diff --git a/planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py b/planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py new file mode 100644 index 0000000000000..1c7e882fbc81f --- /dev/null +++ b/planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python3 + +# 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.logging import get_logger +import launch_testing +import pytest +from rcl_interfaces.msg import Parameter +from rcl_interfaces.msg import ParameterType +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters +import rclpy +import rclpy.qos +from tier4_planning_msgs.msg import VelocityLimit +from tier4_planning_msgs.msg import VelocityLimitClearCommand + +logger = get_logger(__name__) + + +@pytest.mark.launch_test +def generate_test_description(): + test_external_velocity_limit_selector_launch_file = os.path.join( + get_package_share_directory("autoware_external_velocity_limit_selector"), + "launch", + "external_velocity_limit_selector.launch.xml", + ) + external_velocity_limit_selector = IncludeLaunchDescription( + AnyLaunchDescriptionSource(test_external_velocity_limit_selector_launch_file), + ) + + return launch.LaunchDescription( + [ + external_velocity_limit_selector, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestExternalVelocityLimitSelector(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def velocity_limit_callback(self, msg): + self.msg_buffer_ = msg + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + qos = rclpy.qos.QoSProfile(depth=1, durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL) + self.pub_api_limit_ = self.test_node.create_publisher( + VelocityLimit, "/planning/scenario_planning/max_velocity_default", qos + ) + self.pub_internal_limit_ = self.test_node.create_publisher( + VelocityLimit, "/planning/scenario_planning/max_velocity_candidates", qos + ) + self.pub_clear_limit_ = self.test_node.create_publisher( + VelocityLimitClearCommand, "/planning/scenario_planning/clear_velocity_limit", qos + ) + self.msg_buffer_ = None + self.velocity_limit_output_ = None + self.test_node.create_subscription( + VelocityLimit, + "/planning/scenario_planning/max_velocity", + self.velocity_limit_callback, + 1, + ) + + def wait_for_output(self): + while not self.msg_buffer_: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + self.velocity_limit_output_ = self.msg_buffer_ + self.msg_buffer_ = None + + def tearDown(self): + self.test_node.destroy_node() + + def update_max_vel_param(self, max_vel): + set_params_client = self.test_node.create_client( + SetParameters, "/external_velocity_limit_selector/set_parameters" + ) + while not set_params_client.wait_for_service(timeout_sec=1.0): + continue + set_params_request = SetParameters.Request() + set_params_request.parameters = [ + Parameter( + name="max_vel", + value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=max_vel), + ), + ] + future = set_params_client.call_async(set_params_request) + rclpy.spin_until_future_complete(self.test_node, future) + + if future.result() is None: + self.test_node.get_logger().error( + "Exception while calling service: %r" % future.exception() + ) + raise self.failureException("setting of initial parameters failed") + + @staticmethod + def make_velocity_limit_msg(vel): + msg = VelocityLimit() + msg.use_constraints = False + msg.max_velocity = vel + return msg + + def test_external_velocity_limit_selector_node(self): + self.update_max_vel_param(15.0) + # clear velocity limit to trigger first output + clear_cmd = VelocityLimitClearCommand(command=True) + self.pub_clear_limit_.publish(clear_cmd) + self.wait_for_output() + # velocity limit is 0 before any limit is set + self.assertEqual(self.velocity_limit_output_.max_velocity, 0.0) + + # Send velocity limits + # new API velocity limit: higher than the node param -> limit is set to the param value + api_limit = self.make_velocity_limit_msg(20.0) + self.pub_api_limit_.publish(api_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 15.0) + # new API velocity limit + api_limit = self.make_velocity_limit_msg(10.0) + self.pub_api_limit_.publish(api_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 10.0) + # new INTERNAL velocity limit + internal_limit = self.make_velocity_limit_msg(5.0) + self.pub_internal_limit_.publish(internal_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 5.0) + # CLEAR: back to API velocity limit + self.pub_clear_limit_.publish(clear_cmd) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 10.0) + # lower the max_vel node parameter + self.update_max_vel_param(2.5) + self.pub_clear_limit_.publish(clear_cmd) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 2.5) + # velocity limit set by internal limit is no longer applied since above max_vel parameter + internal_limit = self.make_velocity_limit_msg(5.0) + self.pub_internal_limit_.publish(internal_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 2.5) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp index 1f5a52ca040a0..bd431372b70dc 100644 --- a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp @@ -39,13 +39,13 @@ #include #include +#include #include #include #include #include #include #include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -112,7 +112,8 @@ class FreespacePlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pose_array_pub_; rclcpp::Publisher::SharedPtr debug_partial_pose_array_pub_; rclcpp::Publisher::SharedPtr parking_state_pub_; - rclcpp::Publisher::SharedPtr processing_time_pub_; + rclcpp::Publisher::SharedPtr + processing_time_pub_; rclcpp::Subscription::SharedPtr route_sub_; diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index a937114e653c6..2749408bedb7b 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -95,8 +95,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti debug_pose_array_pub_ = create_publisher("~/debug/pose_array", qos); debug_partial_pose_array_pub_ = create_publisher("~/debug/partial_pose_array", qos); parking_state_pub_ = create_publisher("is_completed", qos); - processing_time_pub_ = - create_publisher("~/debug/processing_time_ms", 1); + processing_time_pub_ = create_publisher( + "~/debug/processing_time_ms", 1); } // TF @@ -362,7 +362,7 @@ void FreespacePlannerNode::onTimer() is_new_parking_cycle_ = false; // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); processing_time_pub_->publish(processing_time_msg); diff --git a/planning/autoware_mission_planner/README.md b/planning/autoware_mission_planner/README.md index b5993d0106add..720ecd976f65f 100644 --- a/planning/autoware_mission_planner/README.md +++ b/planning/autoware_mission_planner/README.md @@ -53,6 +53,7 @@ It distributes route requests and planning results according to current MRM oper | `input/vector_map` | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 | | `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | | `input/operation_mode_state` | autoware_adapi_v1_msgs/OperationModeState | operation mode state | +| `input/odometry` | nav_msgs/msg/Odometry | vehicle odometry | ### Publications diff --git a/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt b/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt index d29a153a0ce5d..31fc4b1572f33 100644 --- a/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt +++ b/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt @@ -4,10 +4,22 @@ project(autoware_remaining_distance_time_calculator) find_package(autoware_cmake REQUIRED) autoware_package() +generate_parameter_library(remaining_distance_time_calculator_parameters + param/remaining_distance_time_calculator_parameters.yaml +) + ament_auto_add_library(${PROJECT_NAME} SHARED src/remaining_distance_time_calculator_node.cpp ) +target_link_libraries(${PROJECT_NAME} + remaining_distance_time_calculator_parameters +) + +target_compile_options(${PROJECT_NAME} PRIVATE + -Wno-error=deprecated-declarations +) + rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::remaining_distance_time_calculator::RemainingDistanceTimeCalculatorNode" EXECUTABLE ${PROJECT_NAME}_node diff --git a/planning/autoware_remaining_distance_time_calculator/README.md b/planning/autoware_remaining_distance_time_calculator/README.md index 694c6764de91c..c227b73b87448 100644 --- a/planning/autoware_remaining_distance_time_calculator/README.md +++ b/planning/autoware_remaining_distance_time_calculator/README.md @@ -8,6 +8,7 @@ This package aims to provide mission remaining distance and remaining time calcu - The calculations are activated once we have a route planned for a mission in Autoware. - The calculations are triggered timely based on the `update_rate` parameter. +- The calculations are skipped if the scenario is PARKING, and the remaining time and distance values are set to 0.0. ### Module Parameters diff --git a/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml b/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml index cfb456c57ca41..be7898bdd8913 100644 --- a/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml +++ b/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml @@ -4,6 +4,7 @@ + @@ -13,6 +14,7 @@ + diff --git a/planning/autoware_remaining_distance_time_calculator/package.xml b/planning/autoware_remaining_distance_time_calculator/package.xml index 924e6d62d6aba..568f677a0c13d 100644 --- a/planning/autoware_remaining_distance_time_calculator/package.xml +++ b/planning/autoware_remaining_distance_time_calculator/package.xml @@ -18,6 +18,7 @@ autoware_route_handler autoware_test_utils autoware_universe_utils + generate_parameter_library geometry_msgs nav_msgs rclcpp diff --git a/planning/autoware_remaining_distance_time_calculator/param/remaining_distance_time_calculator_parameters.yaml b/planning/autoware_remaining_distance_time_calculator/param/remaining_distance_time_calculator_parameters.yaml new file mode 100644 index 0000000000000..0f1b1b9efbb6d --- /dev/null +++ b/planning/autoware_remaining_distance_time_calculator/param/remaining_distance_time_calculator_parameters.yaml @@ -0,0 +1,4 @@ +remaining_distance_time_calculator: + update_rate: + type: double + description: Timer callback period. [Hz] diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp index 383e85731604e..74f3f2f43077d 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp @@ -37,6 +37,7 @@ RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode( : Node("remaining_distance_time_calculator", options), is_graph_ready_{false}, has_received_route_{false}, + has_received_scenario_{false}, velocity_limit_{99.99}, remaining_distance_{0.0}, remaining_time_{0.0} @@ -57,14 +58,18 @@ RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode( sub_planning_velocity_ = create_subscription( "/planning/scenario_planning/current_max_velocity", qos_transient_local, std::bind(&RemainingDistanceTimeCalculatorNode::on_velocity_limit, this, _1)); + sub_scenario_ = this->create_subscription( + "~/input/scenario", 1, std::bind(&RemainingDistanceTimeCalculatorNode::on_scenario, this, _1)); pub_mission_remaining_distance_time_ = create_publisher( "~/output/mission_remaining_distance_time", rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable()); - node_param_.update_rate = declare_parameter("update_rate"); + param_listener_ = std::make_shared<::remaining_distance_time_calculator::ParamListener>( + this->get_node_parameters_interface()); + const auto param = param_listener_->get_params(); - const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); + const auto period_ns = rclcpp::Rate(param.update_rate).period(); timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&RemainingDistanceTimeCalculatorNode::on_timer, this)); } @@ -100,9 +105,25 @@ void RemainingDistanceTimeCalculatorNode::on_velocity_limit( } } +void RemainingDistanceTimeCalculatorNode::on_scenario( + const tier4_planning_msgs::msg::Scenario::ConstSharedPtr & msg) +{ + scenario_ = msg; + has_received_scenario_ = true; +} + void RemainingDistanceTimeCalculatorNode::on_timer() { - if (is_graph_ready_ && has_received_route_) { + if (!has_received_scenario_) { + return; + } + + // check if the scenario is parking or not + if (scenario_->current_scenario == tier4_planning_msgs::msg::Scenario::PARKING) { + remaining_distance_ = 0.0; + remaining_time_ = 0.0; + publish_mission_remaining_distance_time(); + } else if (is_graph_ready_ && has_received_route_) { calculate_remaining_distance(); calculate_remaining_time(); publish_mission_remaining_distance_time(); diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp index d6c900af4dfed..8a38941d383ee 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -24,6 +25,7 @@ #include #include #include +#include #include #include @@ -38,11 +40,6 @@ namespace autoware::remaining_distance_time_calculator { -struct NodeParam -{ - double update_rate{0.0}; -}; - class RemainingDistanceTimeCalculatorNode : public rclcpp::Node { public: @@ -59,6 +56,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr sub_planning_velocity_; + rclcpp::Subscription::SharedPtr sub_scenario_; rclcpp::Publisher::SharedPtr pub_mission_remaining_distance_time_; @@ -75,14 +73,16 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node geometry_msgs::msg::Pose current_vehicle_pose_; geometry_msgs::msg::Vector3 current_vehicle_velocity_; geometry_msgs::msg::Pose goal_pose_; + tier4_planning_msgs::msg::Scenario::ConstSharedPtr scenario_; bool has_received_route_; + bool has_received_scenario_; double velocity_limit_; double remaining_distance_; double remaining_time_; // Parameter - NodeParam node_param_; + std::shared_ptr<::remaining_distance_time_calculator::ParamListener> param_listener_; // Callbacks void on_timer(); @@ -90,6 +90,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node void on_route(const LaneletRoute::ConstSharedPtr & msg); void on_map(const HADMapBin::ConstSharedPtr & msg); void on_velocity_limit(const VelocityLimit::ConstSharedPtr & msg); + void on_scenario(const tier4_planning_msgs::msg::Scenario::ConstSharedPtr & msg); /** * @brief calculate mission remaining distance diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 47bc278c30564..7fcd0596b6b32 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -80,9 +80,7 @@ Module getModuleType(const std::string & module_name) module.type = Module::OCCLUSION_SPOT; } else if (module_name == "stop_line") { module.type = Module::NONE; - } else if (module_name == "traffic_light") { - module.type = Module::TRAFFIC_LIGHT; - } else if (module_name == "virtual_traffic_light") { + } else if (module_name == "traffic_light" || module_name == "virtual_traffic_light") { module.type = Module::TRAFFIC_LIGHT; } else if (module_name == "lane_change_left") { module.type = Module::LANE_CHANGE_LEFT; 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 244876ee2cb22..7e215fcfef8dd 100644 --- a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -19,13 +19,13 @@ #include #include +#include #include #include #include #include #include #include -#include #include #include @@ -97,7 +97,8 @@ 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_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; // polling subscribers universe_utils::InterProcessPollingSubscriber< diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index defbde4f6aabc..91e86bfea6dba 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -390,7 +390,7 @@ void ScenarioSelectorNode::onTimer() pub_scenario_->publish(scenario); // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_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); @@ -490,8 +490,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); + 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 0adca312ca733..b3881a8f4fbd9 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -81,8 +81,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); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( @@ -231,7 +231,7 @@ void SurroundObstacleCheckerNode::onTimer() debug_ptr_->pushPose(odometry_ptr_->pose.pose, PoseType::NoStart); } - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_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); diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index f84ad3a8f5987..b6d6bffefed1a 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -24,12 +24,12 @@ #include #include +#include #include #include #include #include #include -#include #include #include #include @@ -106,7 +106,8 @@ class SurroundObstacleCheckerNode : public rclcpp::Node this, "~/input/objects"}; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; // parameter callback result OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp index 46d3b97b71e58..6172fb75978cd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp @@ -564,13 +564,12 @@ int main(int argc, char ** argv) node.get(), "goal_planner."); goal_planner_parameter.bus_stop_area.use_bus_stop_area = true; goal_planner_parameter.lane_departure_check_expansion_margin = 0.2; - const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo(); - autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info); autoware::lane_departure_checker::Param lane_departure_checker_params; lane_departure_checker_params.footprint_extra_margin = goal_planner_parameter.lane_departure_check_expansion_margin; - lane_departure_checker.setParam(lane_departure_checker_params); + autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker( + lane_departure_checker_params, vehicle_info); + const auto footprint = vehicle_info.createFootprint(); autoware::behavior_path_planner::GoalSearcher goal_searcher(goal_planner_parameter, footprint); auto goal_candidates = goal_searcher.search(planner_data); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp index 3518b5041be53..f7fefa8ba9dc0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp @@ -573,12 +573,11 @@ int main(int argc, char ** argv) autoware::behavior_path_planner::GoalPlannerModuleManager::initGoalPlannerParameters( node.get(), "goal_planner."); const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo(); - autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info); autoware::lane_departure_checker::Param lane_departure_checker_params; lane_departure_checker_params.footprint_extra_margin = goal_planner_parameter.lane_departure_check_expansion_margin; - lane_departure_checker.setParam(lane_departure_checker_params); + autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker( + lane_departure_checker_params, vehicle_info); const auto footprint = vehicle_info.createFootprint(); autoware::behavior_path_planner::GoalSearcher goal_searcher(goal_planner_parameter, footprint); auto goal_candidates = goal_searcher.search(planner_data); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 0765147cbeb80..41524b1b8193c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -23,8 +23,6 @@ #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include - #include #include @@ -40,7 +38,6 @@ namespace autoware::behavior_path_planner { -using autoware::lane_departure_checker::LaneDepartureChecker; using autoware_vehicle_msgs::msg::HazardLightsCommand; using geometry_msgs::msg::PoseArray; using nav_msgs::msg::OccupancyGrid; @@ -135,10 +132,8 @@ bool isOnModifiedGoal( bool hasPreviousModulePathShapeChanged( const BehaviorModuleOutput & upstream_module_output, const BehaviorModuleOutput & last_upstream_module_output); -bool hasDeviatedFromLastPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & last_upstream_module_output); -bool hasDeviatedFromCurrentPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & upstream_module_output); +bool hasDeviatedFromPath( + const Point & ego_position, const BehaviorModuleOutput & upstream_module_output); bool needPathUpdate( const Pose & current_pose, const double path_update_duration, const rclcpp::Time & now, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp index fabbd667298bb..9b97f87c933a8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp @@ -29,9 +29,7 @@ using autoware::lane_departure_checker::LaneDepartureChecker; class BezierPullOver : public PullOverPlannerBase { public: - BezierPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker); + BezierPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::BEZIER; } std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index 37c82ea904a55..06ff47224dd44 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -30,9 +30,7 @@ using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; class FreespacePullOver : public PullOverPlannerBase { public: - FreespacePullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + FreespacePullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp index d15c1e796bbe7..e12a759eb2137 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -32,8 +32,7 @@ class GeometricPullOver : public PullOverPlannerBase { public: GeometricPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker, const bool is_forward); + rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward); PullOverPlannerType getPlannerType() const override { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index 2c6aec919bfbb..994d8283fe36c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -31,9 +31,7 @@ using autoware::lane_departure_checker::LaneDepartureChecker; class ShiftPullOver : public PullOverPlannerBase { public: - ShiftPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker); + ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 8272ddde0189e..0a6185127fcf0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -101,7 +101,7 @@ GoalPlannerModule::GoalPlannerModule( // freespace parking if (parameters_->enable_freespace_parking) { - auto freespace_planner = std::make_shared(node, *parameters, vehicle_info); + auto freespace_planner = std::make_shared(node, *parameters); const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period(); freespace_parking_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -155,7 +155,7 @@ bool hasPreviousModulePathShapeChanged( { // Calculate the lateral distance between each point of the current path and the nearest point of // the last path - constexpr double LATERAL_DEVIATION_THRESH = 0.3; + constexpr double LATERAL_DEVIATION_THRESH = 0.1; for (const auto & p : upstream_module_output.path.points) { const size_t nearest_seg_idx = autoware::motion_utils::findNearestSegmentIndex( last_upstream_module_output.path.points, p.point.pose.position); @@ -184,21 +184,12 @@ bool hasPreviousModulePathShapeChanged( return false; } -bool hasDeviatedFromLastPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & last_upstream_module_output) +bool hasDeviatedFromPath( + const Point & ego_position, const BehaviorModuleOutput & upstream_module_output) { + constexpr double LATERAL_DEVIATION_THRESH = 0.1; return std::abs(autoware::motion_utils::calcLateralOffset( - last_upstream_module_output.path.points, - planner_data.self_odometry->pose.pose.position)) > 0.3; -} - -bool hasDeviatedFromCurrentPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & upstream_module_output) -{ - constexpr double LATERAL_DEVIATION_THRESH = 0.3; - return std::abs(autoware::motion_utils::calcLateralOffset( - upstream_module_output.path.points, planner_data.self_odometry->pose.pose.position)) > - LATERAL_DEVIATION_THRESH; + upstream_module_output.path.points, ego_position)) > LATERAL_DEVIATION_THRESH; } bool needPathUpdate( @@ -289,24 +280,15 @@ LaneParkingPlanner::LaneParkingPlanner( is_lane_parking_cb_running_(is_lane_parking_cb_running), logger_(logger) { - const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); - LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info); - lane_departure_checker::Param lane_departure_checker_params; - lane_departure_checker_params.footprint_extra_margin = - parameters.lane_departure_check_expansion_margin; - lane_departure_checker.setParam(lane_departure_checker_params); - for (const std::string & planner_type : parameters.efficient_path_order) { if (planner_type == "SHIFT" && parameters.enable_shift_parking) { - pull_over_planners_.push_back( - std::make_shared(node, parameters, lane_departure_checker)); + pull_over_planners_.push_back(std::make_shared(node, parameters)); } else if (planner_type == "ARC_FORWARD" && parameters.enable_arc_forward_parking) { - pull_over_planners_.push_back(std::make_shared( - node, parameters, lane_departure_checker, /*is_forward*/ true)); + pull_over_planners_.push_back( + std::make_shared(node, parameters, /*is_forward*/ true)); } else if (planner_type == "ARC_BACKWARD" && parameters.enable_arc_backward_parking) { - pull_over_planners_.push_back(std::make_shared( - node, parameters, lane_departure_checker, /*is_forward*/ false)); + pull_over_planners_.push_back( + std::make_shared(node, parameters, /*is_forward*/ false)); } } @@ -373,7 +355,8 @@ void LaneParkingPlanner::onTimer() local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters_)) { return false; } - if (hasDeviatedFromCurrentPreviousModulePath(*local_planner_data, upstream_module_output)) { + if (hasDeviatedFromPath( + local_planner_data->self_odometry->pose.pose.position, upstream_module_output)) { RCLCPP_DEBUG(getLogger(), "has deviated from current previous module path"); return false; } @@ -383,8 +366,8 @@ void LaneParkingPlanner::onTimer() return true; } if ( - hasDeviatedFromLastPreviousModulePath( - *local_planner_data, original_upstream_module_output_) && + hasDeviatedFromPath( + local_planner_data->self_odometry->pose.pose.position, original_upstream_module_output_) && current_state != PathDecisionState::DecisionKind::DECIDED) { RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; @@ -1673,7 +1656,10 @@ PathWithLaneId GoalPlannerModule::generateStopPath( const auto reference_path = std::invoke([&]() -> PathWithLaneId { const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); - const double s_end = s_current + common_parameters.forward_path_length; + const double s_end = std::clamp( + lanelet::utils::getArcCoordinates(current_lanes, route_handler->getGoalPose()).length, + s_current + std::numeric_limits::epsilon(), + s_current + common_parameters.forward_path_length); return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); }); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp index ad42ccf1582ab..ec3984a6d9443 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp @@ -31,15 +31,13 @@ #include namespace autoware::behavior_path_planner { -BezierPullOver::BezierPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker) -: PullOverPlannerBase{node, parameters}, - lane_departure_checker_{lane_departure_checker}, - left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} +BezierPullOver::BezierPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) +: PullOverPlannerBase(node, parameters), + lane_departure_checker_( + lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_), + left_side_parking_(parameters.parking_policy == ParkingPolicy::LEFT_SIDE) { } - std::optional BezierPullOver::plan( [[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id, [[maybe_unused]] const std::shared_ptr planner_data, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index 473f76b5904d6..6605d258a8d5e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -32,9 +32,7 @@ namespace autoware::behavior_path_planner using autoware::freespace_planning_algorithms::AstarSearch; using autoware::freespace_planning_algorithms::RRTStar; -FreespacePullOver::FreespacePullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +FreespacePullOver::FreespacePullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) : PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE}, @@ -45,7 +43,7 @@ FreespacePullOver::FreespacePullOver( } { autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( - vehicle_info, parameters.vehicle_shape_margin); + vehicle_info_, parameters.vehicle_shape_margin); if (parameters.freespace_parking_algorithm == "astar") { planner_ = std::make_unique( parameters.freespace_parking_common_parameters, vehicle_shape, parameters.astar_parameters, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index 74b3fe149fd1d..44dca13ccfd66 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -26,11 +26,11 @@ namespace autoware::behavior_path_planner { GeometricPullOver::GeometricPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker, const bool is_forward) + rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward) : PullOverPlannerBase{node, parameters}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, - lane_departure_checker_{lane_departure_checker}, + lane_departure_checker_{ + lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_}, is_forward_{is_forward}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { 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 9a3e2d6b13db9..8350252369cc6 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 @@ -30,11 +30,10 @@ namespace autoware::behavior_path_planner { -ShiftPullOver::ShiftPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker) +ShiftPullOver::ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) : PullOverPlannerBase{node, parameters}, - lane_departure_checker_{lane_departure_checker}, + lane_departure_checker_{ + lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 0508dc753a2e8..20fd564049133 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -722,47 +722,174 @@ If the ego vehicle gets stuck, to avoid stuck, it enables lane change in crosswa If the ego vehicle stops more than `stuck_detection.stop_time` seconds, it is regarded as a stuck. If the ego vehicle velocity is smaller than `stuck_detection.velocity`, it is regarded as stopping. -### Aborting lane change +## Lane change completion checks + +To determine if the ego vehicle has successfully changed lanes, one of two criteria must be met: either the longitudinal or the lateral criteria. -The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise. +For the longitudinal criteria, the ego vehicle must pass the lane-changing end pose and be within the `finish_judge_buffer` distance from it. The module then checks if the ego vehicle is in the target lane. If true, the module returns success. This check ensures that the planner manager updates the root lanelet correctly based on the ego vehicle's current pose. Without this check, if the ego vehicle is changing lanes while avoiding an obstacle and its current pose is in the original lane, the planner manager might set the root lanelet as the original lane. This would force the ego vehicle to perform the lane change again. With the target lane check, the ego vehicle is confirmed to be in the target lane, and the planner manager can correctly update the root lanelets. -The following depicts the flow of the abort lane change check. +If the longitudinal criteria are not met, the module evaluates the lateral criteria. For the lateral criteria, the ego vehicle must be within `finish_judge_lateral_threshold` distance from the target lane's centerline, and the angle deviation must be within `finish_judge_lateral_angle_deviation` degrees. The angle deviation check ensures there is no sudden steering. If the angle deviation is set too high, the ego vehicle's orientation could deviate significantly from the centerline, causing the trajectory follower to aggressively correct the steering to return to the centerline. Keeping the angle deviation value as small as possible avoids this issue. + +The process of determining lane change completion is shown in the following diagram. ```plantuml @startuml -skinparam monochrome true skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title Lane change completion judge + +start + +:Calculate distance from current ego pose to lane change end pose; + +if (Is ego velocity < 1.0?) then (YES) + :Set finish_judge_buffer to 0.0; +else (NO) + :Set finish_judge_buffer to lane_change_finish_judge_buffer; +endif + +if (ego has passed the end_pose and ego is finish_judge_buffer meters away from end_pose?) then (YES) + if (Current ego pose is in target lanes' polygon?) then (YES) + :Lane change is completed; + stop + else (NO) +:Lane change is NOT completed; +stop + endif +else (NO) +endif + +if (ego's yaw deviation to centerline exceeds finish_judge_lateral_angle_deviation?) then (YES) + :Lane change is NOT completed; + stop +else (NO) + :Calculate distance to the target lanes' centerline; + if (abs(distance to the target lanes' centerline) is less than finish_judge_lateral_threshold?) then (YES) + :Lane change is completed; + stop + else (NO) + :Lane change is NOT completed; + stop + endif +endif + +@enduml +``` + +## Terminal Lane Change Path + +Depending on the space configuration around the Ego vehicle, it is possible that a valid LC path cannot be generated. If that happens, then Ego will get stuck at `terminal_start` and not be able to proceed. Therefore we introduced the terminal LC path feature; when ego gets near to the terminal point (dist to `terminal_start` is less than the maximum lane change length) a terminal lane changing path will be computed starting from the terminal start point on the current lane and connects to the target lane. The terminal path only needs to be computed once in each execution of LC module. If no valid candidate paths are found in the path generation process, then the terminal path will be used as a fallback candidate path, the safety of the terminal path is not ensured and therefore it can only be force approved. The following images illustrate the expected behavior without and with the terminal path feature respectively: + +![no terminal path](./images/lane_change-no_terminal_path.png) + +![terminal path](./images/lane_change-terminal_path.png) + +Additionally if terminal path feature is enabled and path is computed, stop point placement can be configured to be at the edge of the current lane instead of at the `terminal_start` position, as indicated by the dashed red line in the image above. + +## Generating Path Using Frenet Planner + +!!! warning + + Generating path using Frenet planner applies only when ego is near terminal start + +If the ego vehicle is far from the terminal, the lane change module defaults to using the [path shifter](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design/). This ensures that the lane change is completed while the target lane remains a neighbor of the current lane. However, this approach may result in high curvature paths near the terminal, potentially causing long vehicles to deviate from the lane. + +To address this, the lane change module provides an option to choose between the path shifter and the [Frenet planner](https://autowarefoundation.github.io/autoware.universe/main/planning/sampling_based_planner/autoware_frenet_planner/). The Frenet planner allows for some flexibility in the lane change endpoint, extending the lane changing end point slightly beyond the current lane's neighbors. + +The following table provides comparisons between the planners + +
+ + + + + + + + + + + + + + + + + +
With Path ShifterWith Frenet Planner
Path shifter result at straight laneletsFrenet planner result at straight lanelets
Path shifter result at branching laneletsFrenet planner result at branching lanelets
Path shifter result at curved laneletsFrenet planner result at curved lanelets
+
+ +!!! note + + The planner can be enabled or disabled using the `frenet.enable` flag. + +!!! note + + Since only a segment of the target lane is used as input to generate the lane change path, the end pose of the lane change segment may not smoothly connect to the target lane centerline. To address this, increase the value of `frenet.th_curvature_smoothing` to improve the smoothness. + +!!! note + + The yaw difference threshold (`frenet.th_yaw_diff`) limits the maximum curvature difference between the end of the prepare segment and the lane change segment. This threshold might prevent the generation of a lane change path when the lane curvature is high. In such cases, you can increase the frenet.th_yaw_diff value. However, note that if the prepare path was initially shifted by other modules, the resultant steering may not be continuous. + +## Aborting a Previously Approved Lane Change + +Once the lane change path is approved, there are several situations where we may need to abort the maneuver. The abort process is triggered when any of the following conditions is met + +1. The ego vehicle is near a traffic light, crosswalk, or intersection, and it is possible to complete the lane change after the ego vehicle passes these areas. +2. The target object list is updated, requiring us to [delay lane change](#delay-lane-change-check) +3. The lane change is forcefully canceled via [RTC](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/features/cooperation/). +4. The path has become unsafe. + +Furthermore, if the path has become unsafe, there are three possible outcomes for the maneuver: + +1. **CANCEL**: The approved lane change path is canceled, and the ego vehicle resumes its previous maneuver. +2. **ABORT**: The lane change module generates a return path to bring the ego vehicle back to its current lane. +3. **CRUISE** or **STOP**: If aborting is not feasible, the ego vehicle continues with the lane change. [Another module](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_obstacle_cruise_planner/) should decide whether the ego vehicle should cruise or stop in this scenario. + +**CANCEL** can be enabled by setting the `cancel.enable_on_prepare_phase` flag to `true`, and **ABORT** can be enabled by setting the `cancel.enable_on_lane_changing_phase` flag to true. -title Abort Lane Change +!!! warning + + Enabling **CANCEL** is a prerequisite for enabling **ABORT**. + +!!! warning + + When **CANCEL** is disabled, all maneuvers will default to either **CRUISE** or **STOP**. + +The chart shows the high level flow of the lane change abort process. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title High-Level Flow of Lane Change Abort Process while(Lane Following) if (Lane Change required) then (**YES**) - if (Safe to change lane) then (**SAFE**) - while(Lane Changing) - if (Lane Change Completed) then (**YES**) - break - else (**NO**) - if (Is Abort Condition Satisfied) then (**NO**) + if (Safe to change lane) then (SAFE) + :Approve safe path; + while(Lane change maneuver is completed?) is (**NO**) + if (Is cancel/abort Condition satisfied) then (**NO**) else (**YES**) if (Is Enough margin to retry lane change) then (**YES**) - if (Ego is on lane change prepare phase) then (**YES**) - :Cancel lane change; + if (Ego is preparing to change lane) then (**YES**) + #LightPink:CANCEL; break else (**NO**) - if (Will the overhang to target lane be less than threshold?) then (**YES**) - :Perform abort maneuver; + if (Overhang from current lanes is less than threshold?) then (**YES**) + #Cyan:ABORT; break else (NO) - :Stop or Cruise depending on the situation; endif endif else (**NO**) endif + #Yellow:CRUISE/STOP; endif - endif - :Stop and wait; - endwhile - else (**UNSAFE**) + endwhile (**YES**) + else (UNSAFE) endif else (**NO**) endif @@ -772,117 +899,137 @@ detach @enduml ``` -During a lane change, a safety check is made in consideration of the deceleration of the ego vehicle, and a safety check is made for `cancel.deceleration_sampling_num` deceleration patterns, and the lane change will be canceled if the abort condition is satisfied for all deceleration patterns. +### Preventing Oscillating Paths When Unsafe -To preventive measure for lane change path oscillations caused by alternating safe and unsafe conditions, an additional hysteresis count check is implemented before executing an abort or cancel maneuver. If unsafe, the `unsafe_hysteresis_count_` is incremented and compared against `unsafe_hysteresis_threshold`; exceeding it prompts an abort condition check, ensuring decisions are made with consideration to recent safety assessments as shown in flow chart above. This mechanism stabilizes decision-making, preventing abrupt changes due to transient unsafe conditions. +Lane change paths can oscillate when conditions switch between safe and unsafe. To address this, a hysteresis count check is added before executing an abort maneuver. When the path is unsafe, the `unsafe_hysteresis_count_` increases. If it exceeds the `unsafe_hysteresis_threshold`, an abort condition check is triggered. This logic stabilizes the path approval process and prevents abrupt changes caused by temporary unsafe conditions. ```plantuml @startuml skinparam defaultTextAlignment center skinparam backgroundColor #WHITE -title Abort Lane Change +title Hysteresis count flow for oscillation prevention +while (lane changing completed?) is (FALSE) if (Perform collision check?) then (SAFE) :Reset unsafe_hysteresis_count_; else (UNSAFE) :Increase unsafe_hysteresis_count_; - if (unsafe_hysteresis_count_ > unsafe_hysteresis_threshold?) then (FALSE) + if (unsafe_hysteresis_count_ is more than\n unsafe_hysteresis_threshold?) then (FALSE) else (TRUE) #LightPink:Check abort condition; - stop + -[hidden]-> + detach endif endif -:Continue lane changing; +endwhile (TRUE) +stop @enduml ``` -#### Cancel +### Evaluating Ego Vehicle's Position to Prevent Abrupt Maneuvers -Suppose the lane change trajectory is evaluated as unsafe. In that case, if the ego vehicle has not departed from the current lane yet, the trajectory will be reset, and the ego vehicle will resume the lane following the maneuver. +To avoid abrupt maneuvers during **CANCEL** or **ABORT**, the lane change module ensures the ego vehicle can safely return to the original lane. This is done through geometric checks that verify whether the ego vehicle remains within the lane boundaries. -The function can be enabled by setting `enable_on_prepare_phase` to `true`. +The edges of the ego vehicle’s footprint are compared against the boundary of the current lane to determine if they exceed the overhang tolerance, `cancel.overhang_tolerance`. If the distance from any edge of the footprint to the boundary exceeds this threshold, the vehicle is considered to be diverging. -The following image illustrates the cancel process. +The footprints checked against the lane boundary include: -![cancel](./images/lane_change-cancel.png) +1. Current Footprint: Based on the ego vehicle's current position. +2. Future Footprint: Based on the ego vehicle's estimated position after traveling a distance, calculated as $𝑑_{est}=𝑣_{ego} \cdot \Delta_{𝑡}$, where -#### Abort + - $v_{ego}$ is ego vehicle's current velocity + - $\Delta_{t}$ is parameterized time constant value, `cancel.delta_time`. -Assume the ego vehicle has already departed from the current lane. In that case, it is dangerous to cancel the path, and it will cause the ego vehicle to change the heading direction abruptly. In this case, planning a trajectory that allows the ego vehicle to return to the current path while minimizing the heading changes is necessary. In this case, the lane change module will generate an abort path. The following images show an example of the abort path. Do note that the function DOESN'T GUARANTEE a safe abort process, as it didn't check the presence of the surrounding objects and/or their reactions. The function can be enable manually by setting both `enable_on_prepare_phase` and `enable_on_lane_changing_phase` to `true`. The parameter `max_lateral_jerk` need to be set to a high value in order for it to work. + as depicted in the following diagram -![abort](./images/lane_change-abort.png) + ![can_return](./images/check_able_to_return.png) -#### Stop/Cruise +!!! note -The last behavior will also occur if the ego vehicle has departed from the current lane. If the abort function is disabled or the abort is no longer possible, the ego vehicle will attempt to stop or transition to the obstacle cruise mode. Do note that the module DOESN'T GUARANTEE safe maneuver due to the unexpected behavior that might've occurred during these critical scenarios. The following images illustrate the situation. + The ego vehicle is considered capable of safely returning to the current lane only if **BOTH** the current and future footprint checks are `true`. -![stop](./images/lane_change-cant_cancel_no_abort.png) +### Checking Approved Path Safety -## Lane change completion checks +The lane change module samples accelerations along the path and recalculates velocity to perform safety checks. The motivation for this feature is explained in the [Limitation](#limitation) section. -To determine if the ego vehicle has successfully changed lanes, one of two criteria must be met: either the longitudinal or the lateral criteria. +The computation of sampled accelerations is as follows: -For the longitudinal criteria, the ego vehicle must pass the lane-changing end pose and be within the `finish_judge_buffer` distance from it. The module then checks if the ego vehicle is in the target lane. If true, the module returns success. This check ensures that the planner manager updates the root lanelet correctly based on the ego vehicle's current pose. Without this check, if the ego vehicle is changing lanes while avoiding an obstacle and its current pose is in the original lane, the planner manager might set the root lanelet as the original lane. This would force the ego vehicle to perform the lane change again. With the target lane check, the ego vehicle is confirmed to be in the target lane, and the planner manager can correctly update the root lanelets. +Let -If the longitudinal criteria are not met, the module evaluates the lateral criteria. For the lateral criteria, the ego vehicle must be within `finish_judge_lateral_threshold` distance from the target lane's centerline, and the angle deviation must be within `finish_judge_lateral_angle_deviation` degrees. The angle deviation check ensures there is no sudden steering. If the angle deviation is set too high, the ego vehicle's orientation could deviate significantly from the centerline, causing the trajectory follower to aggressively correct the steering to return to the centerline. Keeping the angle deviation value as small as possible avoids this issue. +$$ +\text{resolution} = \frac{a_{\text{min}} - a_{\text{LC}}}{N} +$$ -The process of determining lane change completion is shown in the following diagram. +The sequence of sampled accelerations is then given as -```plantuml -@startuml -skinparam defaultTextAlignment center -skinparam backgroundColor #WHITE +$$ +\text{acc} = a_{\text{LC}} + k \cdot \text{resolution}, \quad k = [0, N] +$$ -title Lane change completion judge +where -start +- $a_{\text{min}}$, is the minimum of the parameterized [global acceleration constant](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/common.param.yaml) `normal.min_acc` or the [parameterized constant](#essential-lane-change-parameters) `trajectory.min_longitudinal_acceleration`. +- $a_{\text{LC}}$ is the acceleration used to generate the approved path. +- $N$ is the parameterized constant `cancel.deceleration_sampling` -:Calculate distance from current ego pose to lane change end pose; +If none of the sampled accelerations pass the safety check, the lane change path will be canceled, subject to the [hysteresis check](#preventing-oscillating-paths-when-unsafe). -if (Is ego velocity < 1.0?) then (YES) - :Set finish_judge_buffer to 0.0; -else (NO) - :Set finish_judge_buffer to lane_change_finish_judge_buffer; -endif +### Cancel -if (ego has passed the end_pose and ego is finish_judge_buffer meters away from end_pose?) then (YES) - if (Current ego pose is in target lanes' polygon?) then (YES) - :Lane change is completed; - stop - else (NO) -:Lane change is NOT completed; -stop - endif -else (NO) -endif +When lane change is canceled, the approved path is reset. After the reset, the ego vehicle will return to following the original reference path (the last approved path before the lane change started), as illustrated in the following image -if (ego's yaw deviation to centerline exceeds finish_judge_lateral_angle_deviation?) then (YES) - :Lane change is NOT completed; - stop -else (NO) - :Calculate distance to the target lanes' centerline; - if (abs(distance to the target lanes' centerline) is less than finish_judge_lateral_threshold?) then (YES) - :Lane change is completed; - stop - else (NO) - :Lane change is NOT completed; - stop - endif -endif +![cancel](./images/lane_change-cancel.png) -@enduml -``` +The following parameters can be configured to tune the behavior of the cancel process: -## Terminal Lane Change Path +1. [Safety constraints](#safety-constraints-to-cancel-lane-change-path) for cancel. +2. [Safety constraints](#safety-constraints-specifically-for-stopped-or-parked-vehicles) for parked vehicle. -Depending on the space configuration around the Ego vehicle, it is possible that a valid LC path cannot be generated. If that happens, then Ego will get stuck at `terminal_start` and will be not be able to proceed. Therefore we introduced the terminal LC path feature; when ego gets near to the terminal point (dist to `terminal_start` is less than the maximum lane change length) a terminal lane changing path will be computed starting from the terminal start point on the current lane and connects to the target lane. The terminal path only needs to be computed once in each execution of LC module. If no valid candidate paths are found in the path generation process, then the terminal path will be used as a fallback candidate path, the safety of the terminal path is not ensured and therefore it can only be force approved. The following images illustrate the expected behavior without and with the terminal path feature respectively: +!!! note -![no terminal path](./images/lane_change-no_terminal_path.png) + To ensure feasible behavior, all safety constraint values must be equal to or less than their corresponding parameters in the [execution](#safety-constraints-during-lane-change-path-is-computed) settings. -![terminal path](./images/lane_change-terminal_path.png) + - The closer the values, the more conservative the lane change behavior will be. This means it will be easier to cancel the lane change but harder for the ego vehicle to complete a lane change. + - The larger the difference, the more aggressive the lane change behavior will be. This makes it harder to cancel the lane change but easier for the ego vehicle to change lanes. -Additionally if terminal path feature is enabled and path is computed, stop point placement can be configured to be at the edge of the current lane instead of at the `terminal_start` position, as indicated by the dashed red line in the image above. +### Abort + +During the prepare phase, the ego vehicle follows the previously approved path. However, once the ego vehicle begins the lane change, its heading starts to diverge from this path. Resetting to the previously approved path in this situation would cause abrupt steering, as the controller would attempt to rapidly realign the vehicle with the reference trajectory. + +Instead, the lane change module generates an abort path. This return path is specifically designed to guide the ego vehicle back to the current lane, avoiding any sudden maneuvers. The following image provides an illustration of the abort process. + +![abort](./images/lane_change-abort.png) + +The abort path is generated by shifting the approved lane change path using the path shifter. This ensures the continuity in lateral velocity, and prevents abrupt changes in the vehicle’s movement. The abort start shift and abort end shift are computed as follows: + +1. Start Shift: $d_{start}^{abort} = v_{ego} \cdot \Delta_{t}$ +2. End Shift: $d_{end}^{abort} = v_{ego} \cdot ( \Delta_{t} + t_{end} )$ + +- $v_{ego}$ is ego vehicle's current velocity +- $\Delta_{t}$ is parameterized time constant value, `cancel.delta_time`. +- $t_{end}$ is the parameterized time constant value, `cancel.duration`. + +as depicted in the following diagram + +![abort_computation](./images/lane_change-abort_computation.png) + +!!! note + + When executing the abort process, comfort is not a primary concern. However, due to safety considerations, limited real-world testing has been conducted to tune or validate this parameter. Currently, the maximum lateral jerk is set to an arbitrary value. To avoid generating a path with excessive lateral jerk, this value can be configured using `cancel.max_lateral_jerk`. + +!!! note + + Lane change module returns `ModuleStatus::FAILURE` once abort is completed. + +### Stop/Cruise + +Once canceling or aborting the lane change is no longer an option, the ego vehicle will proceed with the lane change. This can happen in the following situations: + +- The ego vehicle is performing a lane change near a terminal or dead-end, making it impossible to return to the original lane. In such cases, completing the lane change is necessary. +- If safety parameters are tuned too aggressively, it becomes harder to cancel or abort the lane change. This reduces tolerance for unexpected behaviors from surrounding vehicles, such as a trailing vehicle in the target lane suddenly accelerating or a leading vehicle suddenly decelerating. Aggressive settings leave less room for error during the maneuver. + +![stop](./images/lane_change-cant_cancel_no_abort.png) ## Parameters @@ -892,6 +1039,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | Name | Unit | Type | Description | Default value | | :------------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | +| `time_limit` | [ms] | double | Time limit for lane change candidate path generation | 50.0 | | `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | | `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | | `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | @@ -906,6 +1054,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `trajectory.max_longitudinal_acc` | [m/s2] | double | maximum longitudinal acceleration for lane change | 1.0 | | `trajectory.min_longitudinal_acc` | [m/s2] | double | maximum longitudinal deceleration for lane change | -1.0 | | `trajectory.lane_changing_decel_factor` | [-] | double | longitudinal deceleration factor during lane changing phase | 0.5 | +| `trajectory.th_prepare_curvature` | [-] | double | If the maximum curvature of the prepare segment exceeds the threshold, the prepare segment is invalid. | 0.03 | | `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | | `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | | `lateral_acceleration.min_values` | [m/s2] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | @@ -955,6 +1104,18 @@ The following parameters are used to configure terminal lane change path feature | `terminal_path.disable_near_goal` | [-] | bool | Flag to disable terminal path feature if ego is near goal | true | | `terminal_path.stop_at_boundary` | [-] | bool | If true, ego will stop at current lane boundary instead of middle of lane | false | +### Generating Lane Changing Path using Frenet Planner + +!!! warning + + Only applicable when ego is near terminal start + +| Name | Unit | Type | Description | Default value | +| :------------------------------ | ----- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `frenet.enable` | [-] | bool | Flag to enable/disable frenet planner when ego is near terminal start. | true | +| `frenet.th_yaw_diff` | [deg] | double | If the yaw diff between of the prepare segment's end and lane changing segment's start exceed the threshold , the lane changing segment is invalid. | 10.0 | +| `frenet.th_curvature_smoothing` | [-] | double | Filters and appends target path points with curvature below the threshold to candidate path. | 0.1 | + ### Collision checks #### Target Objects @@ -1095,3 +1256,10 @@ Available information 3. Object is safe or not, shown by the color of the polygon (Green = Safe, Red = unsafe) 4. Valid candidate paths. 5. Position when lane changing start and end. + +## Limitation + +1. When a lane change is canceled, the lane change module returns `ModuleStatus::FAILURE`. As the module is removed from the approved module stack (see [Failure modules](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design/#failure-modules)), a new instance of the lane change module is initiated. Due to this, any information stored prior to the reset is lost. For example, the `lane_change_prepare_duration` in the `TransientData` is reset to its maximum value. +2. The lane change module has no knowledge of any velocity modifications introduced to the path after it is approved. This is because other modules may add deceleration points after subscribing to the behavior path planner output, and the final velocity is managed by the [velocity smoother](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_velocity_smoother/). Since this limitation affects **CANCEL**, the lane change module mitigates it by [sampling accelerations along the approved lane change path](#checking-approved-path-safety). These sampled accelerations are used during safety checks to estimate the velocity that might occur if the ego vehicle decelerates. +3. Ideally, the abort path should account for whether its execution would affect trailing vehicles in the current lane. However, the lane change module does not evaluate such interactions or assess whether the abort path is safe. As a result, **the abort path is not guaranteed to be safe**. To minimize the risk of unsafe situations, the abort maneuver is only permitted if the ego vehicle has not yet diverged from the current lane. +4. Due to limited resources, the abort path logic is not fully optimized. The generated path may overshoot, causing the return trajectory to slightly shift toward the opposite lane. This can be dangerous, especially if the opposite lane has traffic moving in the opposite direction. Furthermore, the logic does not account for different vehicle types, which can lead to varying effects. For instance, the behavior might differ significantly between a bus and a small passenger car. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 91b296f8bd40f..15eb23daecf95 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: lane_change: + time_limit: 50.0 # [ms] backward_lane_length: 200.0 backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] @@ -29,6 +30,7 @@ lon_acc_sampling_num: 5 lat_acc_sampling_num: 3 lane_changing_decel_factor: 0.5 + th_prepare_curvature: 0.03 # [/] # delay lane change delay_lane_change: @@ -37,6 +39,12 @@ min_road_shoulder_width: 0.5 # [m] th_parked_vehicle_shift_ratio: 0.6 + # trajectory generation near terminal using frenet planner + frenet: + enable: true + th_yaw_diff: 10.0 # [deg] + th_curvature_smoothing: 0.1 # [/] + # safety check safety_check: allow_loose_check_for_cancel: true diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/check_able_to_return.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/check_able_to_return.png new file mode 100644 index 0000000000000..1f01b6df5464a Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/check_able_to_return.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png index 0913912b6a332..41e2bddbdafba 100644 Binary files a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort_computation.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort_computation.png new file mode 100644 index 0000000000000..47d4ba81b3354 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort_computation.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png index 10b7beb6ad7d0..91e44e609bf25 100644 Binary files a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png index c3780540f8d00..f142b42c8a644 100644 Binary files a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_frenet.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_frenet.png new file mode 100644 index 0000000000000..51f3a2fc46500 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_frenet.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_path_shifter.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_path_shifter.png new file mode 100644 index 0000000000000..4af931348d6ca Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_path_shifter.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_frenet.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_frenet.png new file mode 100644 index 0000000000000..f834a64867e6c Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_frenet.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_path_shifter.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_path_shifter.png new file mode 100644 index 0000000000000..22ba8d2d7c47d Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_path_shifter.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_frenet.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_frenet.png new file mode 100644 index 0000000000000..9a36642b2d545 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_frenet.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_path_shifter.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_path_shifter.png new file mode 100644 index 0000000000000..7a4e3a453304b Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_path_shifter.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index a69ae0d92647a..4b97fb0d069a0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -35,6 +35,7 @@ #include #include #include +#include namespace autoware::behavior_path_planner { @@ -120,6 +121,8 @@ class LaneChangeInterface : public SceneModuleInterface } } + std::pair check_transit_failure(); + void updateDebugMarker() const; void updateSteeringFactorPtr(const BehaviorModuleOutput & output); @@ -134,6 +137,8 @@ class LaneChangeInterface : public SceneModuleInterface bool is_abort_path_approved_{false}; bool is_abort_approval_requested_{false}; + + lane_change::InterfaceDebug interface_debug_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index fe1d18ea6ea0c..c11839f0356c6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -135,10 +135,23 @@ class NormalLaneChange : public LaneChangeBase std::vector get_prepare_metrics() const; std::vector get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, - const double shift_length, const double dist_to_reg_element) const; + const double shift_length, const double dist_to_reg_element, + lane_change::MetricsDebug & debug_metrics) const; bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; + bool get_path_using_frenet( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const; + + bool get_path_using_path_shifter( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const; + bool check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp index d5bbfe25fe1e9..c844b6b218d10 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp @@ -48,6 +48,7 @@ enum class States { Cancel, Abort, Stop, + Warning, }; struct PhaseInfo @@ -268,6 +269,7 @@ struct CommonData LanesPolygonPtr lanes_polygon_ptr; TransientData transient_data; PathWithLaneId current_lanes_path; + PathWithLaneId target_lanes_path; ModuleType lc_type; Direction direction; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp index 90b13f86976b2..eb07d2f0d3704 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp @@ -34,11 +34,25 @@ using utils::path_safety_checker::CollisionCheckDebugMap; struct MetricsDebug { LaneChangePhaseMetrics prep_metric; - std::vector lc_metrics; + std::vector> lc_metrics; double max_prepare_length; double max_lane_changing_length; }; +struct FrenetStateDebug +{ + LaneChangePhaseMetrics prep_metric; + frenet_planner::SamplingParameter sampling_parameter; + double max_lane_changing_length; + + FrenetStateDebug( + LaneChangePhaseMetrics prep_metric, frenet_planner::SamplingParameter sampling_param, + const double max_len) + : prep_metric(prep_metric), sampling_parameter(sampling_param), max_lane_changing_length(max_len) + { + } +}; + struct Debug { std::string module_type; @@ -52,13 +66,13 @@ struct Debug lanelet::ConstLanelets target_lanes; lanelet::ConstLanelets target_backward_lanes; std::vector lane_change_metrics; + std::vector frenet_states; double collision_check_object_debug_lifetime{0.0}; double distance_to_end_of_current_lane{std::numeric_limits::max()}; double distance_to_lane_change_finished{std::numeric_limits::max()}; double distance_to_abort_finished{std::numeric_limits::max()}; bool is_able_to_return_to_current_lane{true}; bool is_stuck{false}; - bool is_abort{false}; void reset() { @@ -83,9 +97,14 @@ struct Debug distance_to_abort_finished = std::numeric_limits::max(); is_able_to_return_to_current_lane = true; is_stuck = false; - is_abort = false; } }; + +struct InterfaceDebug +{ + std::string_view failing_reason; + LaneChangeStates lc_state; +}; } // namespace autoware::behavior_path_planner::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index b25dbc99189e8..60386f535fc64 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -113,6 +113,13 @@ struct SafetyParameters CollisionCheckParameters collision_check{}; }; +struct FrenetPlannerParameters +{ + bool enable{true}; + double th_yaw_diff_deg{10.0}; + double th_curvature_smoothing{0.1}; +}; + struct TrajectoryParameters { double max_prepare_duration{4.0}; @@ -124,6 +131,7 @@ struct TrajectoryParameters double th_lane_changing_length_diff{0.5}; double min_lane_changing_velocity{5.6}; double lane_changing_decel_factor{0.5}; + double th_prepare_curvature{0.03}; int lon_acc_sampling_num{10}; int lat_acc_sampling_num{10}; LateralAccelerationMap lat_acc_map{}; @@ -151,8 +159,10 @@ struct Parameters CancelParameters cancel{}; DelayParameters delay{}; TerminalPathParameters terminal_path{}; + FrenetPlannerParameters frenet{}; // lane change parameters + double time_limit{50.0}; double backward_lane_length{200.0}; double backward_length_buffer_for_end_of_lane{0.0}; double backward_length_buffer_for_blocking_object{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp index 0fa0a9d977a26..0fa2c6cc8dfbc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp @@ -19,19 +19,53 @@ #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include + #include +#include #include namespace autoware::behavior_path_planner::lane_change { +enum class PathType { ConstantJerk = 0, FrenetPlanner }; + using autoware::behavior_path_planner::TurnSignalInfo; using tier4_planning_msgs::msg::PathWithLaneId; +struct TrajectoryGroup +{ + PathWithLaneId prepare; + PathWithLaneId target_lane_ref_path; + std::vector target_lane_ref_path_dist; + LaneChangePhaseMetrics prepare_metric; + frenet_planner::Trajectory lane_changing; + frenet_planner::FrenetState initial_state; + double max_lane_changing_length{0.0}; + + TrajectoryGroup() = default; + TrajectoryGroup( + PathWithLaneId prepare, PathWithLaneId target_lane_ref_path, + std::vector target_lane_ref_path_dist, LaneChangePhaseMetrics prepare_metric, + frenet_planner::Trajectory lane_changing, frenet_planner::FrenetState initial_state, + const double max_lane_changing_length) + : prepare(std::move(prepare)), + target_lane_ref_path(std::move(target_lane_ref_path)), + target_lane_ref_path_dist(std::move(target_lane_ref_path_dist)), + prepare_metric(prepare_metric), + lane_changing(std::move(lane_changing)), + initial_state(initial_state), + max_lane_changing_length(max_lane_changing_length) + { + } +}; + struct Path { + Info info; PathWithLaneId path; ShiftedPath shifted_path; - Info info; + TrajectoryGroup frenet_path; + PathType type = PathType::ConstantJerk; }; struct Status @@ -39,7 +73,6 @@ struct Status Path lane_change_path; bool is_safe{false}; bool is_valid_path{false}; - double start_distance{0.0}; }; } // namespace autoware::behavior_path_planner::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp index 059db8e38300f..a9ea35cb9b83e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp @@ -32,6 +32,7 @@ namespace marker_utils::lane_change_markers using autoware::behavior_path_planner::FilteredLanesObjects; using autoware::behavior_path_planner::LaneChangePath; using autoware::behavior_path_planner::lane_change::Debug; +using autoware::behavior_path_planner::lane_change::InterfaceDebug; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; MarkerArray showAllValidLaneChangePath( @@ -42,9 +43,12 @@ MarkerArray createLaneChangingVirtualWallMarker( MarkerArray showFilteredObjects( const FilteredLanesObjects & filtered_objects, const std::string & ns); MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area); -MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); +MarkerArray showExecutionInfo( + const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data, + const geometry_msgs::msg::Pose & ego_pose); MarkerArray createDebugMarkerArray( - const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); + const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data, + const geometry_msgs::msg::Pose & ego_pose); } // namespace marker_utils::lane_change_markers #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp index 77ba8fe68a653..42586e7b1df92 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp @@ -11,7 +11,6 @@ // 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__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ @@ -26,6 +25,7 @@ namespace autoware::behavior_path_planner::utils::lane_change { using behavior_path_planner::LaneChangePath; using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::TrajectoryGroup; /** * @brief Generates a prepare segment for a lane change maneuver. @@ -98,5 +98,65 @@ LaneChangePath construct_candidate_path( const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids); + +/** + * @brief Generates candidate trajectories in the Frenet frame for a lane change maneuver. + * + * This function computes a set of candidate trajectories for the ego vehicle in the Frenet frame, + * based on the provided metrics, target lane reference path, and preparation segments. It ensures + * that the generated trajectories adhere to specified constraints, such as lane boundaries and + * velocity limits, while optimizing for smoothness and curvature. + * + * @param common_data_ptr Shared pointer to CommonData containing route, lane, and transient + * information. + * @param prev_module_path The previous module's path used as a base for preparation segments. + * @param prep_metric Metrics for the preparation phase, including path length and velocity. + * + * @return std::vector A vector of trajectory groups representing + * valid lane change candidates, each containing the prepare segment, target reference path, and + * Frenet trajectory. + */ +std::vector generate_frenet_candidates( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const std::vector & prep_metrics); + +/** + * @brief Constructs a lane change path candidate based on a Frenet trajectory group. + * + * This function generates a candidate lane change path by converting a Frenet trajectory group + * into a shifted path and combining it with a prepare segment. It validates the path's feasibility + * by ensuring yaw differences are within allowable thresholds and calculates lane change + * information, such as acceleration, velocity, and duration. + * + * @param trajectory_group A Frenet trajectory group containing the prepared path and Frenet + * trajectory data. + * @param common_data_ptr Shared pointer to CommonData providing parameters and constraints for lane + * changes. + * @param sorted_lane_ids A vector of sorted lane IDs used to update the lane IDs in the path. + * + * @return std::optional The constructed candidate lane change path if valid, or + * std::nullopt if the path is not feasible. + * + * @throws std::logic_error If the yaw difference exceeds the threshold, or other critical errors + * occur. + */ +std::optional get_candidate_path( + const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr, + const std::vector> & sorted_lane_ids); + +/** + * @brief Appends the target lane reference path to the candidate lane change path. + * + * This function extends the lane change candidate path by appending points from the + * target lane reference path beyond the lane change end position. The appending process + * is constrained by a curvature threshold to ensure smooth transitions and avoid sharp turns. + * + * @param frenet_candidate The candidate lane change path to which the target reference path is + * appended. This includes the lane change path and associated Frenet trajectory data. + * @param th_curvature_smoothing A threshold for the allowable curvature during the extension + * process. Points with curvature exceeding this threshold are excluded. + */ +void append_target_ref_to_candidate(LaneChangePath & frenet_candidate, const double th_curvature); } // namespace autoware::behavior_path_planner::utils::lane_change + #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 69362994dbbac..422c392cac462 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -24,6 +24,8 @@ #include #include +#include +#include #include #include @@ -56,6 +58,7 @@ using behavior_path_planner::lane_change::LCParamPtr; using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; using behavior_path_planner::lane_change::TargetLaneLeadingObjects; +using behavior_path_planner::lane_change::TrajectoryGroup; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -69,9 +72,27 @@ bool is_mandatory_lane_change(const ModuleType lc_type); void set_prepare_velocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); -std::vector replaceWithSortedIds( - const std::vector & original_lane_ids, - const std::vector> & sorted_lane_ids); +/** + * @brief Replaces the current lane IDs with a sorted set of IDs based on a predefined mapping. + * + * This function checks if the current lane IDs match the previously processed lane IDs. + * If they do, it returns the previously sorted IDs for efficiency. Otherwise, it matches + * the current lane IDs to the appropriate sorted IDs from the provided mapping and updates + * the cached values. + * + * @param current_lane_ids The current lane IDs to be replaced or verified. + * @param sorted_lane_ids A vector of sorted lane ID groups, each representing a predefined + * order of IDs for specific conditions. + * @param prev_lane_ids Reference to the previously processed lane IDs for caching purposes. + * @param prev_sorted_lane_ids Reference to the previously sorted lane IDs for caching purposes. + * + * @return std::vector The sorted lane IDs if a match is found, or the original + * `current_lane_ids` if no match exists. + */ +std::vector replace_with_sorted_ids( + const std::vector & current_lane_ids, + const std::vector> & sorted_lane_ids, std::vector & prev_lane_ids, + std::vector & prev_sorted_lane_ids); std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr); @@ -432,5 +453,24 @@ std::vector> convert_to_predicted_paths( * @return true if the pose is within the target or target neighbor polygons, false otherwise. */ bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose); + +/** + * @brief Converts a lane change frenet candidate into a predicted path for the ego vehicle. + * + * This function generates a predicted path based on the provided Frenet candidate, + * simulating the vehicle's trajectory during the preparation and lane-changing phases. + * It interpolates poses and velocities over the duration of the prediction, considering + * the ego vehicle's initial conditions and the candidate's trajectory data. + * + * @param common_data_ptr Shared pointer to CommonData containing parameters and ego vehicle state. + * @param frenet_candidate A Frenet trajectory group representing the lane change candidate. + * @param deceleration_sampling_num Unused parameter for deceleration sampling count. + * + * @return std::vector The predicted path as a series of stamped poses + * with associated velocities over the prediction time. + */ +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const lane_change::TrajectoryGroup & frenet_candidate, + [[maybe_unused]] const size_t deceleration_sampling_num); } // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index fad98ecf8f8e1..cf7600556080e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -23,6 +23,7 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_frenet_planner autoware_motion_utils autoware_rtc_interface autoware_universe_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 9f3c6c9ef48bf..ebcd4eb4809fc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -139,9 +139,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() const auto force_activated = std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); - const bool safe = force_activated ? false : true; updateRTCStatus( - path.start_distance_to_path_change, path.finish_distance_to_path_change, safe, + path.start_distance_to_path_change, path.finish_distance_to_path_change, !force_activated, State::RUNNING); } } @@ -220,9 +219,7 @@ bool LaneChangeInterface::canTransitSuccessState() if (module_type_->specialExpiredCheck() && isWaitingApproval()) { log_debug_throttled("Run specialExpiredCheck."); - if (isWaitingApproval()) { - return true; - } + return true; } if (module_type_->hasFinishedLaneChange()) { @@ -237,13 +234,6 @@ bool LaneChangeInterface::canTransitSuccessState() bool LaneChangeInterface::canTransitFailureState() { - auto log_debug_throttled = [&](std::string_view message) -> void { - RCLCPP_DEBUG(getLogger(), "%s", message.data()); - }; - - updateDebugMarker(); - log_debug_throttled(__func__); - const auto force_activated = std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); @@ -253,121 +243,106 @@ bool LaneChangeInterface::canTransitFailureState() return false; } - if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) { - log_debug_throttled("Abort process has on going."); - return false; - } + const auto [state, reason] = check_transit_failure(); - if (isWaitingApproval()) { - if (module_type_->is_near_regulatory_element()) { - log_debug_throttled("Ego is close to regulatory element. Cancel lane change"); - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, - State::FAILED); - return true; - } - log_debug_throttled("Can't transit to failure state. Module is WAITING_FOR_APPROVAL"); - return false; - } + interface_debug_.failing_reason = reason; + interface_debug_.lc_state = state; - if (!module_type_->isValidPath()) { - log_debug_throttled("Transit to failure state due not to find valid path"); + updateDebugMarker(); + + if (state == LaneChangeStates::Cancel) { updateRTCStatus( std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, State::FAILED); + module_type_->toCancelState(); return true; } - if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) { - log_debug_throttled("Abort process has completed."); - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, - State::FAILED); - return true; + if (state == LaneChangeStates::Abort) { + module_type_->toAbortState(); + return false; } - if (module_type_->is_near_terminal()) { - log_debug_throttled("Unsafe, but ego is approaching terminal. Continue lane change"); + // Note: Ideally, if it is unsafe, but for some reason, we can't abort or cancel, then we should + // stop. Note: This feature is not working properly for now. + const auto [is_safe, unsafe_trailing_obj] = post_process_safety_status_; + if (!is_safe && module_type_->isRequiredStop(unsafe_trailing_obj)) { + module_type_->toStopState(); + return false; + } + + module_type_->toNormalState(); + return false; +} - if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) { - log_debug_throttled("Module require stopping"); +std::pair LaneChangeInterface::check_transit_failure() +{ + if (module_type_->isAbortState()) { + if (module_type_->hasFinishedAbort()) { + return {LaneChangeStates::Cancel, "Aborted"}; } - return false; + return {LaneChangeStates::Abort, "Aborting"}; } - if (module_type_->isCancelEnabled() && module_type_->isEgoOnPreparePhase()) { - if (module_type_->isStoppedAtRedTrafficLight()) { - log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change"); - module_type_->toCancelState(); - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, - State::FAILED); - return true; + if (isWaitingApproval()) { + if (module_type_->is_near_regulatory_element()) { + return {LaneChangeStates::Cancel, "CloseToRegElement"}; } + return {LaneChangeStates::Normal, "WaitingForApproval"}; + } + if (!module_type_->isValidPath()) { + return {LaneChangeStates::Cancel, "InvalidPath"}; + } + + const auto is_preparing = module_type_->isEgoOnPreparePhase(); + const auto can_return_to_current = module_type_->isAbleToReturnCurrentLane(); + + // regardless of safe and unsafe, we want to cancel lane change. + if (is_preparing) { const auto force_deactivated = std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { return rtc.second->isForceDeactivated(uuid_map_.at(rtc.first)); }); - if (force_deactivated && module_type_->isAbleToReturnCurrentLane()) { - log_debug_throttled("Cancel lane change due to force deactivation"); - module_type_->toCancelState(); - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, - State::FAILED); - return true; - } - - if (post_process_safety_status_.is_safe) { - log_debug_throttled("Can't transit to failure state. Ego is on prepare, and it's safe."); - return false; - } - - if (module_type_->isAbleToReturnCurrentLane()) { - log_debug_throttled("It's possible to return to current lane. Cancel lane change."); - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, - State::FAILED); - return true; + if (force_deactivated && can_return_to_current) { + return {LaneChangeStates::Cancel, "ForceDeactivation"}; } } if (post_process_safety_status_.is_safe) { - log_debug_throttled("Can't transit to failure state. Ego is lane changing, and it's safe."); - return false; + return {LaneChangeStates::Normal, "SafeToLaneChange"}; } - if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) { - log_debug_throttled("Module require stopping"); + if (!module_type_->isCancelEnabled()) { + return {LaneChangeStates::Warning, "CancelDisabled"}; } - if (!module_type_->isCancelEnabled()) { - log_debug_throttled( - "Lane change path is unsafe but cancel was not enabled. Continue lane change."); - return false; + // We also check if the ego can return to the current lane, as prepare segment might be out of the + // lane, for example, during an evasive maneuver around a static object. + if (is_preparing && can_return_to_current) { + return {LaneChangeStates::Cancel, "SafeToCancel"}; + } + + if (module_type_->is_near_terminal()) { + return {LaneChangeStates::Warning, "TooNearTerminal"}; } if (!module_type_->isAbortEnabled()) { - log_debug_throttled( - "Lane change path is unsafe but abort was not enabled. Continue lane change."); - return false; + return {LaneChangeStates::Warning, "AbortDisabled"}; } - if (!module_type_->isAbleToReturnCurrentLane()) { - log_debug_throttled("It's is not possible to return to original lane. Continue lane change."); - return false; + // To prevent the lane module from having to check rear objects in the current lane, we limit the + // abort maneuver to cases where the ego vehicle is still in the current lane. + if (!can_return_to_current) { + return {LaneChangeStates::Warning, "TooLateToAbort"}; } const auto found_abort_path = module_type_->calcAbortPath(); if (!found_abort_path) { - log_debug_throttled( - "Lane change path is unsafe but abort path not found. Continue lane change."); - return false; + return {LaneChangeStates::Warning, "AbortPathNotFound"}; } - log_debug_throttled("Lane change path is unsafe. Abort lane change."); - module_type_->toAbortState(); - return false; + return {LaneChangeStates::Abort, "SafeToAbort"}; } void LaneChangeInterface::updateDebugMarker() const @@ -377,7 +352,8 @@ void LaneChangeInterface::updateDebugMarker() const return; } using marker_utils::lane_change_markers::createDebugMarkerArray; - debug_marker_ = createDebugMarkerArray(module_type_->getDebugData(), module_type_->getEgoPose()); + debug_marker_ = createDebugMarkerArray( + interface_debug_, module_type_->getDebugData(), module_type_->getEgoPose()); } MarkerArray LaneChangeInterface::getModuleVirtualWall() diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index ec000b8fee97c..f26cf79e8b120 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -63,6 +63,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("trajectory.min_lane_changing_velocity")); p.trajectory.lane_changing_decel_factor = getOrDeclareParameter(*node, parameter("trajectory.lane_changing_decel_factor")); + p.trajectory.th_prepare_curvature = + getOrDeclareParameter(*node, parameter("trajectory.th_prepare_curvature")); p.trajectory.lon_acc_sampling_num = getOrDeclareParameter(*node, parameter("trajectory.lon_acc_sampling_num")); p.trajectory.lat_acc_sampling_num = @@ -188,6 +190,7 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s } // lane change parameters + p.time_limit = getOrDeclareParameter(*node, parameter("time_limit")); p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); p.backward_length_buffer_for_end_of_lane = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); @@ -213,6 +216,12 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s p.delay.th_parked_vehicle_shift_ratio = getOrDeclareParameter( *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio")); + // trajectory generation near terminal using frenet planner + p.frenet.enable = getOrDeclareParameter(*node, parameter("frenet.enable")); + p.frenet.th_yaw_diff_deg = getOrDeclareParameter(*node, parameter("frenet.th_yaw_diff")); + p.frenet.th_curvature_smoothing = + getOrDeclareParameter(*node, parameter("frenet.th_curvature_smoothing")); + // lane change cancel p.cancel.enable_on_prepare_phase = getOrDeclareParameter(*node, parameter("cancel.enable_on_prepare_phase")); @@ -305,6 +314,17 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortime_limit; + updateParam(parameters, ns + "time_limit", time_limit); + if (time_limit >= 10.0) { + p->time_limit = time_limit; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'time_limit' is not updated because the value (%.3f ms) is not valid, " + "keep current value (%.3f ms)", + time_limit, p->time_limit); + } updateParam(parameters, ns + "backward_lane_length", p->backward_lane_length); updateParam( parameters, ns + "backward_length_buffer_for_end_of_lane", @@ -316,10 +336,12 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorlane_change_finish_judge_buffer); updateParam( parameters, ns + "enable_stopped_vehicle_buffer", p->enable_stopped_vehicle_buffer); - updateParam( parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff); updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); + updateParam( + parameters, ns + "min_length_for_turn_signal_activation", + p->min_length_for_turn_signal_activation); } { @@ -330,24 +352,37 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.min_prepare_duration); updateParam(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk); updateParam( - parameters, ns + ".min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); - // longitudinal acceleration + parameters, ns + "min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); updateParam( parameters, ns + "min_longitudinal_acc", p->trajectory.min_longitudinal_acc); updateParam( parameters, ns + "max_longitudinal_acc", p->trajectory.max_longitudinal_acc); updateParam( parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor); - int longitudinal_acc_sampling_num = 0; + updateParam( + parameters, ns + "th_prepare_curvature", p->trajectory.th_prepare_curvature); + int longitudinal_acc_sampling_num = p->trajectory.lon_acc_sampling_num; updateParam(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); if (longitudinal_acc_sampling_num > 0) { p->trajectory.lon_acc_sampling_num = longitudinal_acc_sampling_num; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'lon_acc_sampling_num' is not updated because the value (%d) is not " + "positive", + longitudinal_acc_sampling_num); } - int lateral_acc_sampling_num = 0; + int lateral_acc_sampling_num = p->trajectory.lat_acc_sampling_num; updateParam(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num); if (lateral_acc_sampling_num > 0) { p->trajectory.lat_acc_sampling_num = lateral_acc_sampling_num; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'lat_acc_sampling_num' is not updated because the value (%d) is not " + "positive", + lateral_acc_sampling_num); } updateParam( @@ -356,12 +391,71 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.th_lane_changing_length_diff); } + { + const std::string ns = "lane_change.frenet."; + updateParam(parameters, ns + "enable", p->frenet.enable); + updateParam(parameters, ns + "th_yaw_diff", p->frenet.th_yaw_diff_deg); + updateParam( + parameters, ns + "th_curvature_smoothing", p->frenet.th_curvature_smoothing); + } + { const std::string ns = "lane_change.safety_check.lane_expansion."; updateParam(parameters, ns + "left_offset", p->safety.lane_expansion_left_offset); updateParam(parameters, ns + "right_offset", p->safety.lane_expansion_right_offset); } + { + const std::string ns = "lane_change.lateral_acceleration."; + std::vector velocity = p->trajectory.lat_acc_map.base_vel; + std::vector min_values = p->trajectory.lat_acc_map.base_min_acc; + std::vector max_values = p->trajectory.lat_acc_map.base_max_acc; + + updateParam>(parameters, ns + "velocity", velocity); + updateParam>(parameters, ns + "min_values", min_values); + updateParam>(parameters, ns + "max_values", max_values); + if ( + velocity.size() >= 2 && velocity.size() == min_values.size() && + velocity.size() == max_values.size()) { + LateralAccelerationMap lat_acc_map; + for (size_t i = 0; i < velocity.size(); ++i) { + lat_acc_map.add(velocity.at(i), min_values.at(i), max_values.at(i)); + } + p->trajectory.lat_acc_map = lat_acc_map; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "Mismatched size for lateral acceleration. Expected size: %lu, but velocity: %lu, " + "min_values: %lu, max_values: %lu", + std::max(2ul, velocity.size()), velocity.size(), min_values.size(), max_values.size()); + } + } + + { + const std::string ns = "lane_change.collision_check."; + updateParam( + parameters, ns + "enable_for_prepare_phase.general_lanes", + p->safety.collision_check.enable_for_prepare_phase_in_general_lanes); + updateParam( + parameters, ns + "enable_for_prepare_phase.intersection", + p->safety.collision_check.enable_for_prepare_phase_in_intersection); + updateParam( + parameters, ns + "enable_for_prepare_phase.turns", + p->safety.collision_check.enable_for_prepare_phase_in_turns); + updateParam( + parameters, ns + "check_current_lanes", p->safety.collision_check.check_current_lane); + updateParam( + parameters, ns + "check_other_lanes", p->safety.collision_check.check_other_lanes); + updateParam( + parameters, ns + "use_all_predicted_paths", + p->safety.collision_check.use_all_predicted_paths); + updateParam( + parameters, ns + "prediction_time_resolution", + p->safety.collision_check.prediction_time_resolution); + updateParam( + parameters, ns + "yaw_diff_threshold", p->safety.collision_check.th_yaw_diff); + } + { const std::string ns = "lane_change.target_object."; updateParam(parameters, ns + "car", p->safety.target_object_types.check_car); @@ -389,23 +483,83 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "stop_time", p->th_stop_time); } + auto update_rss_params = [¶meters](const std::string & prefix, auto & params) { + using autoware::universe_utils::updateParam; + updateParam( + parameters, prefix + "longitudinal_distance_min_threshold", + params.longitudinal_distance_min_threshold); + updateParam( + parameters, prefix + "longitudinal_velocity_delta_time", + params.longitudinal_velocity_delta_time); + updateParam( + parameters, prefix + "expected_front_deceleration", params.front_vehicle_deceleration); + updateParam( + parameters, prefix + "expected_rear_deceleration", params.rear_vehicle_deceleration); + updateParam( + parameters, prefix + "rear_vehicle_reaction_time", params.rear_vehicle_reaction_time); + updateParam( + parameters, prefix + "rear_vehicle_safety_time_margin", + params.rear_vehicle_safety_time_margin); + updateParam( + parameters, prefix + "lateral_distance_max_threshold", params.lateral_distance_max_threshold); + }; + + update_rss_params("lane_change.safety_check.execution.", p->safety.rss_params); + update_rss_params("lane_change.safety_check.parked.", p->safety.rss_params_for_parked); + update_rss_params("lane_change.safety_check.cancel.", p->safety.rss_params_for_abort); + update_rss_params("lane_change.safety_check.stuck.", p->safety.rss_params_for_stuck); + + { + const std::string ns = "lane_change.delay_lane_change."; + updateParam(parameters, ns + "enable", p->delay.enable); + updateParam( + parameters, ns + "check_only_parked_vehicle", p->delay.check_only_parked_vehicle); + updateParam( + parameters, ns + "min_road_shoulder_width", p->delay.min_road_shoulder_width); + updateParam( + parameters, ns + "th_parked_vehicle_shift_ratio", p->delay.th_parked_vehicle_shift_ratio); + } + + { + const std::string ns = "lane_change.terminal_path."; + updateParam(parameters, ns + "enable", p->terminal_path.enable); + updateParam(parameters, ns + "disable_near_goal", p->terminal_path.disable_near_goal); + updateParam(parameters, ns + "stop_at_boundary", p->terminal_path.stop_at_boundary); + } + { const std::string ns = "lane_change.cancel."; - bool enable_on_prepare_phase = true; + bool enable_on_prepare_phase = p->cancel.enable_on_prepare_phase; updateParam(parameters, ns + "enable_on_prepare_phase", enable_on_prepare_phase); if (!enable_on_prepare_phase) { - RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change cancel function is disabled."); + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Lane Change cancel function is disabled."); p->cancel.enable_on_prepare_phase = enable_on_prepare_phase; } - bool enable_on_lane_changing_phase = true; + bool enable_on_lane_changing_phase = p->cancel.enable_on_lane_changing_phase; updateParam( parameters, ns + "enable_on_lane_changing_phase", enable_on_lane_changing_phase); if (!enable_on_lane_changing_phase) { - RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change abort function is disabled."); + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Lane Change abort function is disabled."); p->cancel.enable_on_lane_changing_phase = enable_on_lane_changing_phase; } + int deceleration_sampling_num = p->cancel.deceleration_sampling_num; + updateParam(parameters, ns + "deceleration_sampling_num", deceleration_sampling_num); + if (deceleration_sampling_num > 0) { + p->cancel.deceleration_sampling_num = deceleration_sampling_num; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "Parameter 'deceleration_sampling_num' is not updated because the value (%d) is not " + "positive", + deceleration_sampling_num); + } + updateParam(parameters, ns + "delta_time", p->cancel.delta_time); updateParam(parameters, ns + "duration", p->cancel.duration); updateParam(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk); @@ -413,6 +567,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( parameters, ns + "unsafe_hysteresis_threshold", p->cancel.th_unsafe_hysteresis); } + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { if (!observer.expired()) observer.lock()->updateModuleParams(p); }); 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 2719239baaed8..45fcd6fa99086 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 @@ -28,7 +28,10 @@ #include #include #include +#include #include +#include +#include #include #include #include @@ -36,8 +39,11 @@ #include #include +#include + #include #include +#include #include #include @@ -104,6 +110,9 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->current_lanes_path = route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + common_data_ptr_->target_lanes_path = + route_handler_ptr->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::get_target_neighbor_lanes( *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); @@ -232,8 +241,6 @@ void NormalLaneChange::updateLaneChangeStatus() // Update status status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - - status_.start_distance = common_data_ptr_->transient_data.target_lanes_ego_arc.length; status_.lane_change_path.path.header = getRouteHeader(); } @@ -892,7 +899,6 @@ bool NormalLaneChange::isAbleToStopSafely() const bool NormalLaneChange::hasFinishedAbort() const { if (!abort_path_) { - lane_change_debug_.is_abort = true; return true; } @@ -901,7 +907,6 @@ bool NormalLaneChange::hasFinishedAbort() const lane_change_debug_.distance_to_abort_finished = distance_to_finish; const auto has_finished_abort = distance_to_finish < 0.0; - lane_change_debug_.is_abort = has_finished_abort; return has_finished_abort; } @@ -916,12 +921,7 @@ bool NormalLaneChange::isAbortState() const return false; } - if (!abort_path_) { - return false; - } - - lane_change_debug_.is_abort = true; - return true; + return abort_path_ != nullptr; } int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const @@ -1090,7 +1090,8 @@ std::vector NormalLaneChange::get_prepare_metrics() cons std::vector NormalLaneChange::get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metric, - const double shift_length, const double dist_to_reg_element) const + const double shift_length, const double dist_to_reg_element, + lane_change::MetricsDebug & debug_metrics) const { const auto & transient_data = common_data_ptr_->transient_data; const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( @@ -1107,15 +1108,11 @@ std::vector NormalLaneChange::get_lane_changing_metrics( return max_length; }); + debug_metrics.max_lane_changing_length = max_lane_changing_length; const auto max_path_velocity = prep_segment.points.back().point.longitudinal_velocity_mps; - const auto lc_metrics = calculation::calc_shift_phase_metrics( + return calculation::calc_shift_phase_metrics( common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, prep_metric.sampled_lon_accel, max_lane_changing_length); - - const auto max_prep_length = common_data_ptr_->transient_data.dist_to_terminal_start; - lane_change_debug_.lane_change_metrics.push_back( - {prep_metric, lc_metrics, max_prep_length, max_lane_changing_length}); - return lc_metrics; } bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const @@ -1134,15 +1131,97 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) } const auto & current_lanes = get_current_lanes(); - const auto & target_lanes = get_target_lanes(); - const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); const auto target_objects = get_target_objects(filtered_objects_, current_lanes); const auto prepare_phase_metrics = get_prepare_metrics(); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); + if ( + common_data_ptr_->lc_param_ptr->frenet.enable && + common_data_ptr_->transient_data.is_ego_near_current_terminal_start) { + return get_path_using_frenet( + prepare_phase_metrics, target_objects, sorted_lane_ids, candidate_paths); + } + + return get_path_using_path_shifter( + prepare_phase_metrics, target_objects, sorted_lane_ids, candidate_paths); +} + +bool NormalLaneChange::get_path_using_frenet( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const +{ + stop_watch_.tic(__func__); + constexpr auto found_safe_path = true; + const auto frenet_candidates = utils::lane_change::generate_frenet_candidates( + common_data_ptr_, prev_module_output_.path, prepare_metrics); + RCLCPP_DEBUG( + logger_, "Generated %lu candidate paths in %2.2f[us]", frenet_candidates.size(), + stop_watch_.toc(__func__)); + + candidate_paths.reserve(frenet_candidates.size()); + lane_change_debug_.frenet_states.clear(); + lane_change_debug_.frenet_states.reserve(frenet_candidates.size()); + for (const auto & frenet_candidate : frenet_candidates) { + if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) { + break; + } + + lane_change_debug_.frenet_states.emplace_back( + frenet_candidate.prepare_metric, frenet_candidate.lane_changing.sampling_parameter, + frenet_candidate.max_lane_changing_length); + + std::optional candidate_path_opt; + try { + candidate_path_opt = + utils::lane_change::get_candidate_path(frenet_candidate, common_data_ptr_, sorted_lane_ids); + } catch (const std::exception & e) { + RCLCPP_DEBUG(logger_, "%s", e.what()); + } + + if (!candidate_path_opt) { + continue; + } + + try { + if (check_candidate_path_safety(*candidate_path_opt, target_objects)) { + RCLCPP_DEBUG( + logger_, "Found safe path after %lu candidate(s). Total time: %2.2f[us]", + frenet_candidates.size(), stop_watch_.toc("__func__")); + utils::lane_change::append_target_ref_to_candidate( + *candidate_path_opt, common_data_ptr_->lc_param_ptr->frenet.th_curvature_smoothing); + candidate_paths.push_back(*candidate_path_opt); + return found_safe_path; + } + } catch (const std::exception & e) { + RCLCPP_DEBUG(logger_, "%s", e.what()); + } + + // appending all paths affect performance + if (candidate_paths.empty()) { + candidate_paths.push_back(*candidate_path_opt); + } + } + + RCLCPP_DEBUG( + logger_, "No safe path after %lu candidate(s). Total time: %2.2f[us]", frenet_candidates.size(), + stop_watch_.toc(__func__)); + return !found_safe_path; +} + +bool NormalLaneChange::get_path_using_path_shifter( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const +{ + stop_watch_.tic(__func__); + const auto & target_lanes = get_target_lanes(); candidate_paths.reserve( - prepare_phase_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); + prepare_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); const bool only_tl = getStopTime() >= lane_change_parameters_->th_stop_time; const auto dist_to_next_regulatory_element = @@ -1161,7 +1240,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) return lc_diff > lane_change_parameters_->trajectory.th_lane_changing_length_diff; }; - for (const auto & prep_metric : prepare_phase_metrics) { + for (const auto & prep_metric : prepare_metrics) { const auto debug_print = [&](const std::string & s) { RCLCPP_DEBUG( logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s.c_str(), @@ -1192,8 +1271,12 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + lane_change_debug_.lane_change_metrics.emplace_back(); + auto & debug_metrics = lane_change_debug_.lane_change_metrics.back(); + debug_metrics.prep_metric = prep_metric; + debug_metrics.max_prepare_length = common_data_ptr_->transient_data.dist_to_terminal_start; const auto lane_changing_metrics = get_lane_changing_metrics( - prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element); + prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element, debug_metrics); // set_prepare_velocity must only be called after computing lane change metrics, as lane change // metrics rely on the prepare segment's original velocity as max_path_velocity. @@ -1201,6 +1284,13 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity); for (const auto & lc_metric : lane_changing_metrics) { + if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) { + RCLCPP_DEBUG(logger_, "Time limit reached and no safe path was found."); + return false; + } + + debug_metrics.lc_metrics.emplace_back(lc_metric, -1); + const auto debug_print_lat = [&](const std::string & s) { RCLCPP_DEBUG( logger_, "%s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s.c_str(), @@ -1222,6 +1312,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) } candidate_paths.push_back(candidate_path); + debug_metrics.lc_metrics.back().second = static_cast(candidate_paths.size()) - 1; try { if (check_candidate_path_safety(candidate_path, target_objects)) { @@ -1491,14 +1582,8 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if ( - common_data_ptr_->transient_data.is_ego_near_current_terminal_start && isAbleToStopSafely() && - is_trailing_object) { - current_lane_change_state_ = LaneChangeStates::Stop; - return true; - } - current_lane_change_state_ = LaneChangeStates::Normal; - return false; + return common_data_ptr_->transient_data.is_ego_near_current_terminal_start && + isAbleToStopSafely() && is_trailing_object; } bool NormalLaneChange::calcAbortPath() diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index da9ee52b038c6..bf0af0d133dc7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -90,8 +91,10 @@ MarkerArray showAllValidLaneChangePath( text_marker.id = ++id; text_marker.pose = points.at(lc_idx).point.pose; text_marker.text = fmt::format( - "vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | lat_acc: {lat_acc:.3f}[m/s2] | t: " + "type: {type} | vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | lat_acc: " + "{lat_acc:.3f}[m/s2] | t: " "{time:.3f}[s] | L: {length:.3f}[m]", + fmt::arg("type", magic_enum::enum_name(lc_path.type)), fmt::arg("vel", info.velocity.lane_changing), fmt::arg("lon_acc", info.longitudinal_acceleration.lane_changing), fmt::arg("lat_acc", info.lateral_acceleration), fmt::arg("time", info.duration.lane_changing), @@ -164,7 +167,9 @@ MarkerArray showFilteredObjects( return marker_array; } -MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose) +MarkerArray showExecutionInfo( + const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data, + const geometry_msgs::msg::Pose & ego_pose) { auto default_text_marker = [&]() { return createDefaultMarker( @@ -177,11 +182,15 @@ MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg auto safety_check_info_text = default_text_marker(); safety_check_info_text.pose = ego_pose; safety_check_info_text.pose.position.z += 4.0; + const auto lc_state = interface_debug_data.lc_state; + const auto & failing_reason = interface_debug_data.failing_reason; safety_check_info_text.text = fmt::format( - "{stuck} | {return_lane} | {abort}", fmt::arg("stuck", debug_data.is_stuck ? "is stuck" : ""), - fmt::arg("return_lane", debug_data.is_able_to_return_to_current_lane ? "" : "can't return"), - fmt::arg("abort", debug_data.is_abort ? "aborting" : "")); + "{stuck} | {return_lane} | {state} : {reason}", + fmt::arg("stuck", scene_debug_data.is_stuck ? "is stuck" : ""), + fmt::arg( + "return_lane", scene_debug_data.is_able_to_return_to_current_lane ? "" : "can't return"), + fmt::arg("state", magic_enum::enum_name(lc_state)), fmt::arg("reason", failing_reason)); marker_array.markers.push_back(safety_check_info_text); return marker_array; } @@ -190,44 +199,84 @@ MarkerArray ShowLaneChangeMetricsInfo( const Debug & debug_data, const geometry_msgs::msg::Pose & pose) { MarkerArray marker_array; - if (debug_data.lane_change_metrics.empty()) { - return marker_array; - } auto text_marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "sampling_metrics", 0, Marker::TEXT_VIEW_FACING, createMarkerScale(0.6, 0.6, 0.6), colors::yellow()); text_marker.pose = autoware::universe_utils::calcOffsetPose(pose, 10.0, 15.0, 0.0); - text_marker.text = fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lat_accel[m/s2]") + - fmt::format("{:^18}|", "lon_accel[m/s2]") + - fmt::format("{:^17}|", "velocity[m/s]") + - fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + - fmt::format("{:^20}\n", "max_length_th[m]"); - for (const auto & metrics : debug_data.lane_change_metrics) { - text_marker.text += fmt::format("{:-<170}\n", ""); - const auto & p_m = metrics.prep_metric; - text_marker.text += - fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^10.3f}", p_m.lat_accel) + - fmt::format("{:^21.3f}", p_m.actual_lon_accel) + fmt::format("{:^12.3f}", p_m.velocity) + - fmt::format("{:^15.3f}", p_m.duration) + fmt::format("{:^15.3f}", p_m.length) + - fmt::format("{:^17.3f}\n", metrics.max_prepare_length); - text_marker.text += fmt::format("{:<20}\n", "lc_metrics:"); - for (const auto lc_m : metrics.lc_metrics) { + if (!debug_data.lane_change_metrics.empty()) { + text_marker.text = + fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lat_accel[m/s2]") + + fmt::format("{:^18}|", "lon_accel[m/s2]") + fmt::format("{:^17}|", "velocity[m/s]") + + fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + + fmt::format("{:^20}|", "max_length_th[m]") + fmt::format("{:^15}\n", "path_index"); + for (const auto & metrics : debug_data.lane_change_metrics) { + text_marker.text += fmt::format("{:-<190}\n", ""); + const auto & p_m = metrics.prep_metric; text_marker.text += - fmt::format("{:<15}", "") + fmt::format("{:^10.3f}", lc_m.lat_accel) + - fmt::format("{:^21.3f}", lc_m.actual_lon_accel) + fmt::format("{:^12.3f}", lc_m.velocity) + - fmt::format("{:^15.3f}", lc_m.duration) + fmt::format("{:^15.3f}", lc_m.length) + - fmt::format("{:^17.3f}\n", metrics.max_lane_changing_length); + fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^10.3f}", p_m.lat_accel) + + fmt::format("{:^21.3f}", p_m.actual_lon_accel) + fmt::format("{:^12.3f}", p_m.velocity) + + fmt::format("{:^15.3f}", p_m.duration) + fmt::format("{:^15.3f}", p_m.length) + + fmt::format("{:^17.3f}", metrics.max_prepare_length) + fmt::format("{:^15}\n", "-"); + text_marker.text += fmt::format("{:<20}\n", "lc_metrics:"); + for (const auto & lc_m : metrics.lc_metrics) { + const auto & metric = lc_m.first; + const auto path_index = lc_m.second < 0 ? "-" : std::to_string(lc_m.second); + text_marker.text += fmt::format("{:<15}", "") + fmt::format("{:^10.3f}", metric.lat_accel) + + fmt::format("{:^21.3f}", metric.actual_lon_accel) + + fmt::format("{:^12.3f}", metric.velocity) + + fmt::format("{:^15.3f}", metric.duration) + + fmt::format("{:^15.3f}", metric.length) + + fmt::format("{:^17.3f}", metrics.max_lane_changing_length) + + fmt::format("{:^15}\n", path_index); + } } + marker_array.markers.push_back(text_marker); + } + + if (!debug_data.frenet_states.empty()) { + text_marker.text = fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lon_accel[m/s2]") + + fmt::format("{:^17}|", "lon_vel[m/s]") + + fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + + fmt::format("{:^17}|", "lat_accel[m/s2]") + + fmt::format("{:^15}|", "lat_vel[m/s2]") + fmt::format("{:^15}|", "s[m]") + + fmt::format("{:^15}|", "d[m]") + fmt::format("{:^20}\n", "max_length_th[m]"); + for (const auto & metrics : debug_data.frenet_states) { + text_marker.text += fmt::format("{:-<250}\n", ""); + const auto & p_m = metrics.prep_metric; + const auto max_len = metrics.max_lane_changing_length; + text_marker.text += + fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^13.3f}", p_m.actual_lon_accel) + + fmt::format("{:^15.3f}", p_m.velocity) + fmt::format("{:^15.3f}", p_m.duration) + + fmt::format("{:^12.3f}", p_m.length) + + fmt::format("{:^13}", "") + // Empty string for lat_accel + fmt::format("{:^13}", "") + // Empty string for lat_vel + fmt::format("{:^15}", "") + // Empty string for s + fmt::format("{:^15}", "") + // Empty string for d // Empty string for d + fmt::format("{:^20.3f}\n", max_len); // Empty string for max_length_t + const auto & lc_m = metrics.sampling_parameter.target_state; // Assuming lc_metric exists + const auto duration = metrics.sampling_parameter.target_duration; + text_marker.text += + fmt::format("{:<17}", "frenet_state:") + + fmt::format("{:^15.3f}", lc_m.longitudinal_acceleration) + + fmt::format("{:^13.3f}", lc_m.longitudinal_velocity) + fmt::format("{:^17.3f}", duration) + + fmt::format("{:^10.3f}", lc_m.position.s) + + fmt::format("{:^19.3f}", lc_m.lateral_acceleration) + + fmt::format("{:^10.3f}", lc_m.lateral_velocity) + + fmt::format("{:^18.3f}", lc_m.position.s) + fmt::format("{:^15.3f}", lc_m.position.d) + + fmt::format("{:^16.3f}\n", max_len); // Empty string for max_length_t + } + + marker_array.markers.push_back(text_marker); } - marker_array.markers.push_back(text_marker); return marker_array; } MarkerArray createDebugMarkerArray( - const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose) + const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data, + const geometry_msgs::msg::Pose & ego_pose) { using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::showPolygon; @@ -236,31 +285,32 @@ MarkerArray createDebugMarkerArray( using marker_utils::lane_change_markers::showAllValidLaneChangePath; using marker_utils::lane_change_markers::showFilteredObjects; - const auto & debug_collision_check_object = debug_data.collision_check_objects; + const auto & debug_collision_check_object = scene_debug_data.collision_check_objects; const auto & debug_collision_check_object_after_approval = - debug_data.collision_check_objects_after_approval; - const auto & debug_valid_paths = debug_data.valid_paths; - const auto & debug_filtered_objects = debug_data.filtered_objects; + scene_debug_data.collision_check_objects_after_approval; + const auto & debug_valid_paths = scene_debug_data.valid_paths; + const auto & debug_filtered_objects = scene_debug_data.filtered_objects; MarkerArray debug_marker; const auto add = [&debug_marker](const MarkerArray & added) { autoware::universe_utils::appendMarkerArray(added, &debug_marker); }; - if (!debug_data.execution_area.points.empty()) { + if (!scene_debug_data.execution_area.points.empty()) { add(createPolygonMarkerArray( - debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1)); + scene_debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1)); } - add(showExecutionInfo(debug_data, ego_pose)); - add(ShowLaneChangeMetricsInfo(debug_data, ego_pose)); + add(showExecutionInfo(interface_debug_data, scene_debug_data, ego_pose)); + add(ShowLaneChangeMetricsInfo(scene_debug_data, ego_pose)); // lanes add(laneletsAsTriangleMarkerArray( - "current_lanes", debug_data.current_lanes, colors::light_yellow(0.2))); - add(laneletsAsTriangleMarkerArray("target_lanes", debug_data.target_lanes, colors::aqua(0.2))); + "current_lanes", scene_debug_data.current_lanes, colors::light_yellow(0.2))); + add(laneletsAsTriangleMarkerArray( + "target_lanes", scene_debug_data.target_lanes, colors::aqua(0.2))); add(laneletsAsTriangleMarkerArray( - "target_backward_lanes", debug_data.target_backward_lanes, colors::blue(0.2))); + "target_backward_lanes", scene_debug_data.target_backward_lanes, colors::blue(0.2))); add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths")); add(showFilteredObjects(debug_filtered_objects, "object_filtered")); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp index 44ee1624f0f51..df45da3f08fa1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp @@ -15,17 +15,22 @@ #include "autoware/behavior_path_lane_change_module/utils/path.hpp" #include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include #include +#include #include +#include #include #include #include +#include + #include #include #include @@ -42,8 +47,22 @@ using autoware::behavior_path_planner::lane_change::LCParamPtr; using autoware::behavior_path_planner::LaneChangePhaseMetrics; using autoware::behavior_path_planner::ShiftLine; +using autoware::behavior_path_planner::lane_change::TrajectoryGroup; +using autoware::frenet_planner::FrenetState; +using autoware::frenet_planner::SamplingParameters; +using autoware::route_handler::Direction; +using autoware::sampler_common::FrenetPoint; +using autoware::sampler_common::transform::Spline2D; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; using geometry_msgs::msg::Pose; +template +T sign(const Direction direction) +{ + return static_cast(direction == Direction::LEFT ? 1.0 : -1.0); +} + double calc_resample_interval( const double lane_changing_length, const double lane_changing_velocity) { @@ -53,7 +72,7 @@ double calc_resample_interval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } -PathWithLaneId get_reference_path_from_target_Lane( +PathWithLaneId get_reference_path_from_target_lane( const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, const double lane_changing_length, const double resample_interval) { @@ -146,11 +165,197 @@ std::optional exceed_yaw_threshold( } return std::nullopt; } + +Spline2D init_reference_spline(const std::vector & target_lanes_ref_path) +{ + std::vector xs; + std::vector ys; + xs.reserve(target_lanes_ref_path.size()); + ys.reserve(target_lanes_ref_path.size()); + for (const auto & p : target_lanes_ref_path) { + xs.push_back(p.point.pose.position.x); + ys.push_back(p.point.pose.position.y); + } + + return {xs, ys}; +} + +FrenetState init_frenet_state( + const FrenetPoint & start_position, const LaneChangePhaseMetrics & prepare_metrics) +{ + FrenetState initial_state; + initial_state.position = start_position; + initial_state.longitudinal_velocity = prepare_metrics.velocity; + initial_state.lateral_velocity = + 0.0; // TODO(Maxime): this can be sampled if we want but it would impact the LC duration + initial_state.longitudinal_acceleration = prepare_metrics.sampled_lon_accel; + initial_state.lateral_acceleration = prepare_metrics.lat_accel; + return initial_state; +} + +std::optional init_sampling_parameters( + const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prepare_metrics, + const FrenetState & initial_state, const Spline2D & ref_spline, const Pose & lc_start_pose) +{ + const auto & trajectory = common_data_ptr->lc_param_ptr->trajectory; + const auto min_lc_vel = trajectory.min_lane_changing_velocity; + const auto [min_lateral_acc, max_lateral_acc] = + trajectory.lat_acc_map.find(std::max(min_lc_vel, prepare_metrics.velocity)); + const auto duration = autoware::motion_utils::calc_shift_time_from_jerk( + std::abs(initial_state.position.d), trajectory.lateral_jerk, max_lateral_acc); + const auto final_velocity = + std::max(min_lc_vel, prepare_metrics.velocity + prepare_metrics.sampled_lon_accel * duration); + const auto lc_length = duration * (prepare_metrics.velocity + final_velocity) * 0.5; + const auto target_s = initial_state.position.s + lc_length; + const auto initial_yaw = tf2::getYaw(lc_start_pose.orientation); + const auto target_lat_vel = + (1.0 - ref_spline.curvature(target_s + 1e-3) * initial_state.position.d) * + std::tan(initial_yaw - ref_spline.yaw(target_s)); + + if (std::isnan(target_lat_vel)) { + return std::nullopt; + } + + SamplingParameters sampling_parameters; + const auto & safety = common_data_ptr->lc_param_ptr->safety; + sampling_parameters.resolution = safety.collision_check.prediction_time_resolution; + sampling_parameters.parameters.emplace_back(); + sampling_parameters.parameters.back().target_duration = duration; + sampling_parameters.parameters.back().target_state.position = {target_s, 0.0}; + // TODO(Maxime): not sure if we should use curvature at initial or target s + sampling_parameters.parameters.back().target_state.lateral_velocity = + sign(common_data_ptr->direction) * target_lat_vel; + sampling_parameters.parameters.back().target_state.lateral_acceleration = 0.0; + sampling_parameters.parameters.back().target_state.longitudinal_velocity = final_velocity; + sampling_parameters.parameters.back().target_state.longitudinal_acceleration = + prepare_metrics.sampled_lon_accel; + return sampling_parameters; +} + +std::vector calc_curvatures( + const std::vector & points, const Pose & current_pose) +{ + const auto nearest_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(points, current_pose.position); + + // Ignore all path points behind ego vehicle. + if (points.size() <= nearest_segment_idx + 2) { + return {}; + } + + std::vector curvatures; + curvatures.reserve(points.size() - nearest_segment_idx + 2); + for (const auto & [p1, p2, p3] : ranges::views::zip( + points | ranges::views::drop(nearest_segment_idx), + points | ranges::views::drop(nearest_segment_idx + 1), + points | ranges::views::drop(nearest_segment_idx + 2))) { + const auto point1 = autoware::universe_utils::getPoint(p1); + const auto point2 = autoware::universe_utils::getPoint(p2); + const auto point3 = autoware::universe_utils::getPoint(p3); + + curvatures.push_back(autoware::universe_utils::calcCurvature(point1, point2, point3)); + } + + return curvatures; +} + +double calc_average_curvature(const std::vector & curvatures) +{ + const auto filter_zeros = [](const auto & k) { return k != 0.0; }; + auto filtered_k = curvatures | ranges::views::filter(filter_zeros); + if (filtered_k.empty()) { + return 0.0; + } + const auto sums_of_curvatures = [](float sum, const double k) { return sum + std::abs(k); }; + const auto sum_of_k = ranges::accumulate(filtered_k, 0.0, sums_of_curvatures); + const auto count_k = static_cast(ranges::distance(filtered_k)); + return sum_of_k / count_k; +} + +LineString2d get_linestring_bound(const lanelet::ConstLanelets & lanes, const Direction direction) +{ + LineString2d line_string; + const auto & get_bound = [&](const lanelet::ConstLanelet & lane) { + return (direction == Direction::LEFT) ? lane.leftBound2d() : lane.rightBound2d(); + }; + + const auto reserve_size = ranges::accumulate( + lanes, 0UL, + [&](auto sum, const lanelet::ConstLanelet & lane) { return sum + get_bound(lane).size(); }); + line_string.reserve(reserve_size); + for (const auto & lane : lanes) { + ranges::for_each(get_bound(lane), [&line_string](const auto & point) { + boost::geometry::append(line_string, Point2d(point.x(), point.y())); + }); + } + return line_string; +} + +Point2d shift_point(const Point2d & p1, const Point2d & p2, const double offset) +{ + // Calculate the perpendicular vector + double dx = p2.x() - p1.x(); + double dy = p2.y() - p1.y(); + double length = std::sqrt(dx * dx + dy * dy); + + // Normalize and find the perpendicular direction + double nx = -dy / length; + double ny = dx / length; + + return {p1.x() + nx * offset, p1.y() + ny * offset}; +} + +bool check_out_of_bound_paths( + const CommonDataPtr & common_data_ptr, const std::vector & lane_changing_poses, + const LineString2d & lane_boundary, const Direction direction) +{ + const auto distance = (0.5 * common_data_ptr->bpp_param_ptr->vehicle_width + 0.1); + const auto offset = sign(direction) * distance; // invert direction + if (lane_changing_poses.size() <= 2) { + return true; // Remove candidates with insufficient poses + } + + LineString2d path_ls; + path_ls.reserve(lane_changing_poses.size()); + + const auto segments = lane_changing_poses | ranges::views::sliding(2); + ranges::for_each(segments | ranges::views::drop(1), [&](const auto & segment) { + const auto & p1 = segment[0].position; + const auto & p2 = segment[1].position; + boost::geometry::append(path_ls, shift_point({p2.x, p2.y}, {p1.x, p1.y}, offset)); + }); + + return boost::geometry::disjoint(path_ls, lane_boundary); // Remove if disjoint +} + +double calc_limit(const CommonDataPtr & common_data_ptr, const Pose & lc_end_pose) +{ + const auto dist_to_target_end = std::invoke([&]() { + if (common_data_ptr->lanes_ptr->target_lane_in_goal_section) { + return autoware::motion_utils::calcSignedArcLength( + common_data_ptr->target_lanes_path.points, lc_end_pose.position, + common_data_ptr->route_handler_ptr->getGoalPose().position); + } + return autoware::motion_utils::calcSignedArcLength( + common_data_ptr->target_lanes_path.points, lc_end_pose.position, + common_data_ptr->target_lanes_path.points.back().point.pose.position); + }); + + // v2 = u2 + 2ad + // u = sqrt(2ad) + return std::clamp( + std::sqrt( + std::abs(2.0 * common_data_ptr->bpp_param_ptr->min_acc * std::max(dist_to_target_end, 0.0))), + common_data_ptr->lc_param_ptr->trajectory.min_lane_changing_velocity, + common_data_ptr->bpp_param_ptr->max_vel); +} + }; // namespace namespace autoware::behavior_path_planner::utils::lane_change { using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::PathType; bool get_prepare_segment( const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, @@ -187,7 +392,17 @@ bool get_prepare_segment( throw std::logic_error("lane change start is behind target lanelet!"); } - return true; + const auto curvatures = calc_curvatures(prepare_segment.points, common_data_ptr->get_ego_pose()); + + // curvatures may be empty if ego is near the terminal start of the path. + if (curvatures.empty()) { + return true; + } + + const auto average_curvature = calc_average_curvature(curvatures); + + RCLCPP_DEBUG(get_logger(), "average curvature: %.3f", average_curvature); + return average_curvature <= common_data_ptr->lc_param_ptr->trajectory.th_prepare_curvature; } LaneChangePath get_candidate_path( @@ -205,7 +420,7 @@ LaneChangePath get_candidate_path( } const auto & lc_start_pose = prep_segment.points.back().point.pose; - const auto target_lane_reference_path = get_reference_path_from_target_Lane( + const auto target_lane_reference_path = get_reference_path_from_target_lane( common_data_ptr, lc_start_pose, lc_metric.length, resample_interval); if (target_lane_reference_path.points.empty()) { @@ -259,10 +474,14 @@ LaneChangePath construct_candidate_path( throw std::logic_error("Lane change end idx not found on target path."); } + std::vector prev_ids; + std::vector prev_sorted_ids; for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { auto & point = shifted_path.path.points.at(i); if (i < *lc_end_idx_opt) { - point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids); + const auto & current_ids = point.lane_ids; + point.lane_ids = + replace_with_sorted_ids(current_ids, sorted_lane_ids, prev_ids, prev_sorted_ids); point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, static_cast(terminal_lane_changing_velocity)); continue; @@ -286,7 +505,251 @@ LaneChangePath construct_candidate_path( candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); candidate_path.shifted_path = shifted_path; candidate_path.info = lane_change_info; + candidate_path.type = PathType::ConstantJerk; return candidate_path; } + +std::vector generate_frenet_candidates( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const std::vector & prep_metrics) +{ + std::vector trajectory_groups; + universe_utils::StopWatch sw; + + const auto & transient_data = common_data_ptr->transient_data; + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto direction = common_data_ptr->direction; + const auto current_lane_boundary = get_linestring_bound(current_lanes, direction); + + for (const auto & metric : prep_metrics) { + PathWithLaneId prepare_segment; + try { + if (!utils::lane_change::get_prepare_segment( + common_data_ptr, prev_module_path, metric.length, prepare_segment)) { + RCLCPP_DEBUG(get_logger(), "Reject: failed to get valid prepare segment!"); + continue; + } + } catch (const std::exception & e) { + RCLCPP_WARN(get_logger(), "%s", e.what()); + break; + } + const auto lc_start_pose = prepare_segment.points.back().point.pose; + + const auto dist_to_end_from_lc_start = + calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr, target_lanes, lc_start_pose) - + common_data_ptr->lc_param_ptr->lane_change_finish_judge_buffer; + const auto max_lc_len = transient_data.lane_changing_length.max; + const auto max_lane_changing_length = std::min(dist_to_end_from_lc_start, max_lc_len); + + constexpr auto resample_interval = 0.5; + const auto target_lane_reference_path = get_reference_path_from_target_lane( + common_data_ptr, lc_start_pose, max_lane_changing_length, resample_interval); + if (target_lane_reference_path.points.empty()) { + continue; + } + + std::vector target_ref_path_sums{0.0}; + target_ref_path_sums.reserve(target_lane_reference_path.points.size() - 1); + double ref_s = 0.0; + for (auto it = target_lane_reference_path.points.begin(); + std::next(it) != target_lane_reference_path.points.end(); ++it) { + ref_s += universe_utils::calcDistance2d(*it, *std::next(it)); + target_ref_path_sums.push_back(ref_s); + } + + const auto reference_spline = init_reference_spline(target_lane_reference_path.points); + + const auto initial_state = init_frenet_state( + reference_spline.frenet({lc_start_pose.position.x, lc_start_pose.position.y}), metric); + + RCLCPP_DEBUG( + get_logger(), "Initial state [s=%2.2f, d=%2.2f, s'=%2.2f, d'=%2.2f, s''=%2.2f, d''=%2.2f]", + initial_state.position.s, initial_state.position.d, initial_state.longitudinal_velocity, + initial_state.lateral_velocity, initial_state.longitudinal_acceleration, + initial_state.lateral_acceleration); + + const auto sampling_parameters_opt = init_sampling_parameters( + common_data_ptr, metric, initial_state, reference_spline, lc_start_pose); + + if (!sampling_parameters_opt) { + continue; + } + + auto frenet_trajectories = frenet_planner::generateTrajectories( + reference_spline, initial_state, *sampling_parameters_opt); + + trajectory_groups.reserve(trajectory_groups.size() + frenet_trajectories.size()); + for (const auto & traj : frenet_trajectories) { + if (!trajectory_groups.empty()) { + const auto diff = std::abs( + traj.frenet_points.back().s - + trajectory_groups.back().lane_changing.frenet_points.back().s); + if (diff < common_data_ptr->lc_param_ptr->trajectory.th_lane_changing_length_diff) { + continue; + } + } + + const auto out_of_bound = + check_out_of_bound_paths(common_data_ptr, traj.poses, current_lane_boundary, direction); + + if (out_of_bound) { + continue; + } + + trajectory_groups.emplace_back( + prepare_segment, target_lane_reference_path, target_ref_path_sums, metric, traj, + initial_state, max_lane_changing_length); + } + } + + const auto limit_vel = [&](TrajectoryGroup & group) { + const auto max_vel = calc_limit(common_data_ptr, group.lane_changing.poses.back()); + for (auto & vel : group.lane_changing.longitudinal_velocities) { + vel = std::clamp(vel, 0.0, max_vel); + } + }; + + ranges::for_each(trajectory_groups, limit_vel); + + ranges::sort(trajectory_groups, [&](const auto & p1, const auto & p2) { + return calc_average_curvature(p1.lane_changing.curvatures) < + calc_average_curvature(p2.lane_changing.curvatures); + }); + + return trajectory_groups; +} + +std::optional get_candidate_path( + const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr, + const std::vector> & sorted_lane_ids) +{ + if (trajectory_group.lane_changing.frenet_points.empty()) { + return std::nullopt; + } + + ShiftedPath shifted_path; + std::vector prev_ids; + std::vector prev_sorted_ids; + const auto & lane_changing_candidate = trajectory_group.lane_changing; + const auto & target_lane_ref_path = trajectory_group.target_lane_ref_path; + const auto & prepare_segment = trajectory_group.prepare; + const auto & prepare_metric = trajectory_group.prepare_metric; + const auto & initial_state = trajectory_group.initial_state; + const auto & target_ref_sums = trajectory_group.target_lane_ref_path_dist; + auto zipped_candidates = ranges::views::zip( + lane_changing_candidate.poses, lane_changing_candidate.frenet_points, + lane_changing_candidate.longitudinal_velocities, lane_changing_candidate.lateral_velocities, + lane_changing_candidate.curvatures); + + shifted_path.path.points.reserve(zipped_candidates.size()); + shifted_path.shift_length.reserve(zipped_candidates.size()); + for (const auto & [pose, frenet_point, longitudinal_velocity, lateral_velocity, curvature] : + zipped_candidates) { + // Find the reference index + const auto & s = frenet_point.s; + auto ref_i_itr = std::find_if( + target_ref_sums.begin(), target_ref_sums.end(), + [s](const double ref_s) { return ref_s > s; }); + auto ref_i = std::distance(target_ref_sums.begin(), ref_i_itr); + + PathPointWithLaneId point; + point.point.pose = pose; + point.point.longitudinal_velocity_mps = static_cast(longitudinal_velocity); + point.point.lateral_velocity_mps = static_cast(lateral_velocity); + point.point.heading_rate_rps = static_cast(curvature); + point.point.pose.position.z = target_lane_ref_path.points[ref_i].point.pose.position.z; + const auto & current_ids = target_lane_ref_path.points[ref_i].lane_ids; + point.lane_ids = + replace_with_sorted_ids(current_ids, sorted_lane_ids, prev_ids, prev_sorted_ids); + + // Add to shifted path + shifted_path.shift_length.push_back(frenet_point.d); + shifted_path.path.points.push_back(point); + } + + if (shifted_path.path.points.empty()) { + return std::nullopt; + } + + for (auto & point : shifted_path.path.points) { + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + static_cast(shifted_path.path.points.back().point.longitudinal_velocity_mps)); + } + + const auto th_yaw_diff_deg = common_data_ptr->lc_param_ptr->frenet.th_yaw_diff_deg; + if ( + const auto yaw_diff_opt = exceed_yaw_threshold( + prepare_segment, shifted_path.path, autoware::universe_utils::deg2rad(th_yaw_diff_deg))) { + const auto yaw_diff_deg = autoware::universe_utils::rad2deg(yaw_diff_opt.value()); + const auto err_msg = fmt::format( + "Excessive yaw difference {yaw_diff:2.2f}[deg]. The threshold is {th_yaw_diff:2.2f}[deg].", + fmt::arg("yaw_diff", yaw_diff_deg), fmt::arg("th_yaw_diff", th_yaw_diff_deg)); + throw std::logic_error(err_msg); + } + + LaneChangeInfo info; + info.longitudinal_acceleration = { + prepare_metric.actual_lon_accel, lane_changing_candidate.longitudinal_accelerations.front()}; + info.velocity = {prepare_metric.velocity, initial_state.longitudinal_velocity}; + info.duration = { + prepare_metric.duration, lane_changing_candidate.sampling_parameter.target_duration}; + info.length = {prepare_metric.length, lane_changing_candidate.lengths.back()}; + info.lane_changing_start = prepare_segment.points.back().point.pose; + info.lane_changing_end = lane_changing_candidate.poses.back(); + + ShiftLine sl; + + sl.start = lane_changing_candidate.poses.front(); + sl.end = lane_changing_candidate.poses.back(); + sl.start_shift_length = 0.0; + sl.end_shift_length = initial_state.position.d; + sl.start_idx = 0UL; + sl.end_idx = shifted_path.shift_length.size() - 1; + + info.shift_line = sl; + info.terminal_lane_changing_velocity = lane_changing_candidate.longitudinal_velocities.back(); + info.lateral_acceleration = lane_changing_candidate.lateral_accelerations.front(); + + LaneChangePath candidate_path; + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); + candidate_path.info = info; + candidate_path.shifted_path = shifted_path; + candidate_path.frenet_path = trajectory_group; + candidate_path.type = PathType::FrenetPlanner; + + return candidate_path; +} + +void append_target_ref_to_candidate( + LaneChangePath & frenet_candidate, const double th_curvature_smoothing) +{ + const auto & target_lane_ref_path = frenet_candidate.frenet_path.target_lane_ref_path.points; + const auto & lc_end_pose = frenet_candidate.info.lane_changing_end; + const auto lc_end_idx = + motion_utils::findNearestIndex(target_lane_ref_path, lc_end_pose.position); + auto & candidate_path = frenet_candidate.path.points; + if (target_lane_ref_path.size() <= lc_end_idx + 2) { + return; + } + const auto add_size = target_lane_ref_path.size() - (lc_end_idx + 1); + candidate_path.reserve(candidate_path.size() + add_size); + const auto & points = target_lane_ref_path; + for (const auto & [p2, p3] : ranges::views::zip( + points | ranges::views::drop(lc_end_idx + 1), + points | ranges::views::drop(lc_end_idx + 2))) { + const auto point1 = autoware::universe_utils::getPoint(candidate_path.back().point.pose); + const auto point2 = autoware::universe_utils::getPoint(p2); + const auto point3 = autoware::universe_utils::getPoint(p3); + const auto curvature = + std::abs(autoware::universe_utils::calcCurvature(point1, point2, point3)); + if (curvature < th_curvature_smoothing) { + candidate_path.push_back(p2); + } + } + candidate_path.push_back(target_lane_ref_path.back()); +} } // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 9360435891632..9002270185847 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -24,12 +24,18 @@ #include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +// for the geometry types +#include +#include +// for the svg mapper #include #include #include #include #include #include +#include +#include #include #include #include @@ -40,9 +46,10 @@ #include #include -#include - +#include #include +#include +#include #include #include @@ -64,9 +71,11 @@ namespace autoware::behavior_path_planner::utils::lane_change { using autoware::route_handler::RouteHandler; using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::lane_change::PathType; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; @@ -382,18 +391,26 @@ std::vector> get_sorted_lane_ids(const CommonDataPtr & comm return sorted_lane_ids; } -std::vector replaceWithSortedIds( - const std::vector & original_lane_ids, - const std::vector> & sorted_lane_ids) +std::vector replace_with_sorted_ids( + const std::vector & current_lane_ids, + const std::vector> & sorted_lane_ids, std::vector & prev_lane_ids, + std::vector & prev_sorted_lane_ids) { - for (const auto original_id : original_lane_ids) { + if (current_lane_ids == prev_lane_ids) { + return prev_sorted_lane_ids; + } + + for (const auto original_id : current_lane_ids) { for (const auto & sorted_id : sorted_lane_ids) { if (std::find(sorted_id.cbegin(), sorted_id.cend(), original_id) != sorted_id.cend()) { - return sorted_id; + prev_lane_ids = current_lane_ids; + prev_sorted_lane_ids = sorted_id; + return prev_sorted_lane_ids; } } } - return original_lane_ids; + + return current_lane_ids; } CandidateOutput assignToCandidate( @@ -475,6 +492,7 @@ std::vector convert_to_predicted_path( 0.5 * lane_changing_acceleration * delta_t * delta_t + offset; const auto pose = autoware::motion_utils::calcInterpolatedPose( path.points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); } @@ -1127,6 +1145,10 @@ std::vector> convert_to_predicted_paths( const auto acc_resolution = (min_acc - lane_changing_acc) / static_cast(sampling_num); const auto ego_predicted_path = [&](size_t n) { + if (lane_change_path.type == PathType::FrenetPlanner) { + return convert_to_predicted_path( + common_data_ptr, lane_change_path.frenet_path, deceleration_sampling_num); + } auto acc = lane_changing_acc + static_cast(n) * acc_resolution; return utils::lane_change::convert_to_predicted_path(common_data_ptr, lane_change_path, acc); }; @@ -1135,6 +1157,48 @@ std::vector> convert_to_predicted_paths( ranges::to(); } +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const lane_change::TrajectoryGroup & frenet_candidate, + [[maybe_unused]] const size_t deceleration_sampling_num) +{ + const auto initial_velocity = common_data_ptr->get_ego_speed(); + const auto prepare_time = frenet_candidate.prepare_metric.duration; + const auto resolution = + common_data_ptr->lc_param_ptr->safety.collision_check.prediction_time_resolution; + const auto prepare_acc = frenet_candidate.prepare_metric.sampled_lon_accel; + std::vector predicted_path; + const auto & path = frenet_candidate.prepare.points; + const auto & vehicle_pose = common_data_ptr->get_ego_pose(); + const auto & bpp_param_ptr = common_data_ptr->bpp_param_ptr; + const auto nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path, vehicle_pose, bpp_param_ptr->ego_nearest_dist_threshold, + bpp_param_ptr->ego_nearest_yaw_threshold); + + const auto vehicle_pose_frenet = + convertToFrenetPoint(path, vehicle_pose.position, nearest_seg_idx); + + for (double t = 0.0; t < prepare_time; t += resolution) { + const auto velocity = + std::clamp(initial_velocity + prepare_acc * t, 0.0, frenet_candidate.prepare_metric.velocity); + const auto length = initial_velocity * t + 0.5 * prepare_acc * t * t; + const auto pose = + autoware::motion_utils::calcInterpolatedPose(path, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + const auto & poses = frenet_candidate.lane_changing.poses; + const auto & velocities = frenet_candidate.lane_changing.longitudinal_velocities; + const auto & times = frenet_candidate.lane_changing.times; + + for (const auto [t, pose, velocity] : + ranges::views::zip(times, poses, velocities) | ranges::views::drop(1)) { + predicted_path.emplace_back(prepare_time + t, pose, velocity); + } + + return predicted_path; +} + bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) { const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/README.md b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md index ab8d02dc83f92..81cb945802595 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md @@ -4,11 +4,11 @@ The Behavior Path Planner's main objective is to significantly enhance the safet The module begins by thoroughly analyzing the ego vehicle's current situation, including its position, speed, and surrounding environment. This analysis leads to essential driving decisions about lane changes or stopping and subsequently generates a path that is both safe and efficient. It considers road geometry, traffic rules, and dynamic conditions while also incorporating obstacle avoidance to respond to static and dynamic obstacles such as other vehicles, pedestrians, or unexpected roadblocks, ensuring safe navigation. -Moreover, the planner actively interacts with other traffic participants, predicting their actions and accordingly adjusting the vehicle's path. This ensures not only the safety of the autonomous vehicle but also contributes to smooth traffic flow. Its adherence to traffic laws, including speed limits and traffic signals, further guarantees lawful and predictable driving behavior. The planner is also designed to minimize sudden or abrupt maneuvers, aiming for a comfortable and natural driving experience. +Moreover, the planner responds to the behavior of other traffic participants, predicting their actions and accordingly adjusting the vehicle's path. This ensures not only the safety of the autonomous vehicle but also contributes to smooth traffic flow. Its adherence to traffic laws, including speed limits and compliance with traffic signals, further guarantees lawful and predictable driving behavior. The planner is also designed to minimize sudden or abrupt maneuvers, aiming for a comfortable and natural driving experience. !!! note - The [Planning Component Design](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/) Document outlines the foundational philosophy guiding the design and future development of the Behavior Path Planner module. We strongly encourage readers to consult this document to understand the rationale behind its current configuration and the direction of its ongoing development. + The [Planning Component Design](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/) documentation outlines the foundational philosophy guiding the design and future development of the Behavior Path Planner module. We strongly encourage readers to consult this document to understand the rationale behind its current configuration and the direction of its ongoing development. ## Purpose / Use Cases @@ -22,23 +22,23 @@ Essentially, the module has three primary responsibilities: ### Supported Scene Modules -Behavior Path Planner has following scene modules +Behavior Path Planner has the following scene modules -| Name | Description | Details | -| :------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------------- | -| Lane Following | this module generates reference path from lanelet centerline. | LINK | -| Static Obstacle Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](../autoware_behavior_path_static_obstacle_avoidance_module/README.md) | -| Dynamic Obstacle Avoidance | WIP | [LINK](../autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md) | -| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | -| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../autoware_behavior_path_lane_change_module/README.md) | -| External Lane Change | WIP | LINK | -| Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../autoware_behavior_path_goal_planner_module/README.md) | -| Start Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../autoware_behavior_path_start_planner_module/README.md) | -| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../autoware_behavior_path_side_shift_module/README.md) | +| Name | Description | Details | +| :------------------------- | :--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------------- | +| Lane Following | This module generates a reference path from lanelet centerline. | LINK | +| Static Obstacle Avoidance | This module generates an avoidance path when there are objects that should be avoided. | [LINK](../autoware_behavior_path_static_obstacle_avoidance_module/README.md) | +| Dynamic Obstacle Avoidance | WIP | [LINK](../autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md) | +| Avoidance By Lane Change | This module generates a lane change path when there are objects that should be avoided. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | +| Lane Change | This module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../autoware_behavior_path_lane_change_module/README.md) | +| External Lane Change | WIP | LINK | +| Goal Planner | This module is performed when the ego vehicle is in a driving lane and the goal is in the shoulder lane. The ego vehicle will stop at the goal. | [LINK](../autoware_behavior_path_goal_planner_module/README.md) | +| Start Planner | This module is performed when the ego vehicle is stationary and the footprint of the ego vehicle is included in the shoulder lane. This module ends when the ego vehicle merges into the road. | [LINK](../autoware_behavior_path_start_planner_module/README.md) | +| Side Shift | This module shifts the path to the left or right based on external instructions, intended for remote control applications. | [LINK](../autoware_behavior_path_side_shift_module/README.md) | !!! Note - click on the following images to view the video of their execution + Click on the following images to view videos of their execution
@@ -59,13 +59,13 @@ Behavior Path Planner has following scene modules Users can refer to [Planning component design](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/#supported-functions) for some additional behavior. -#### How to add or implement new module? +#### How to add or implement new module -All scene modules are implemented by inheriting base class `scene_module_interface.hpp`. +All scene modules are implemented by inheriting the base class `scene_module_interface.hpp`. !!! Warning - The remainder of this subsection is work in progress (WIP). + The remainder of this subsection is a work in progress (WIP). ### Planner Manager @@ -77,65 +77,65 @@ The Planner Manager's responsibilities include: !!! note - To check the scene module's transition, i.e.: registered, approved and candidate modules, set `verbose: true` in the [behavior path planner configuration file](https://github.com/autowarefoundation/autoware_launch/blob/0cd5d891a36ac34a32a417205905c109f2bafe7b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml#L3). + To check the scene module's transition – i.e., registered, approved and candidate modules – set `verbose: true` in the [Behavior Path Planner configuration file](https://github.com/autowarefoundation/autoware_launch/blob/0cd5d891a36ac34a32a417205905c109f2bafe7b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml#L3). ![Scene module's transition table](./image/checking_module_transition.png) !!! note - For more in-depth information, refer to [Manager design](./docs/behavior_path_planner_manager_design.md) document. + For more in-depth information, refer to the [Manager design](./docs/behavior_path_planner_manager_design.md) document. ## Inputs / Outputs / API ### Input -| Name | Required? | Type | Description | -| :---------------------------- | :-------: | :------------------------------------------------------ | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | for ego velocity. | -| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | for ego acceleration. | -| ~/input/objects | ○ | `autoware_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | -| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | -| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficLightGroupArray` | traffic signals information from the perception module | -| ~/input/vector_map | ○ | `autoware_map_msgs::msg::LaneletMapBin` | vector map information. | -| ~/input/route | ○ | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal. | -| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | -| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | -| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows planning module to know if vehicle is in autonomous mode or can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/autoware_default_adapi/document/operation-mode.md) | - -- ○ Mandatory: Planning Module would not work if anyone of this is not present. -- △ Optional: Some module would not work, but Planning Module can still be operated. +| Name | Required? | Type | Description | +| :---------------------------- | :-------: | :------------------------------------------------------ | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | For ego velocity | +| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | For ego acceleration | +| ~/input/objects | ○ | `autoware_perception_msgs::msg::PredictedObjects` | Dynamic objects from the perception module | +| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | Occupancy grid map from the perception module. This is used for only the Goal Planner module | +| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficLightGroupArray` | Traffic signal information from the perception module | +| ~/input/vector_map | ○ | `autoware_map_msgs::msg::LaneletMapBin` | Vector map information | +| ~/input/route | ○ | `autoware_planning_msgs::msg::LaneletRoute` | Current route from start to goal | +| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches Behavior Path Planner if current scenario == `Scenario:LaneDriving` | +| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | Lateral offset to trigger side shift | +| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows the planning module to know if vehicle is in autonomous mode or if it can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/autoware_default_adapi/document/operation-mode.md) | + +- ○ Mandatory: The planning module would not work if anyone of these were not present. +- △ Optional: Some modules would not work, but the planning module can still be operated. ### Output -| Name | Type | Description | QoS Durability | -| :---------------------------- | :-------------------------------------------------- | :--------------------------------------------------------------------------------------------- | ----------------- | -| ~/output/path | `tier4_planning_msgs::msg::PathWithLaneId` | the path generated by modules. | `volatile` | -| ~/output/turn_indicators_cmd | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command. | `volatile` | -| ~/output/hazard_lights_cmd | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command. | `volatile` | -| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | output modified goal commands. | `transient_local` | -| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | the path the module is about to take. to be executed as soon as external approval is obtained. | `volatile` | +| Name | Type | Description | QoS Durability | +| :---------------------------- | :-------------------------------------------------- | :-------------------------------------------------------------------------------------------- | ----------------- | +| ~/output/path | `tier4_planning_msgs::msg::PathWithLaneId` | The path generated by modules | `volatile` | +| ~/output/turn_indicators_cmd | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | Turn indicators command | `volatile` | +| ~/output/hazard_lights_cmd | `autoware_vehicle_msgs::msg::HazardLightsCommand` | Hazard lights command | `volatile` | +| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | Output modified goal commands | `transient_local` | +| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | The path the module is about to take. To be executed as soon as external approval is obtained | `volatile` | ### Debug -| Name | Type | Description | QoS Durability | -| :-------------------------------------- | :-------------------------------------------------- | :---------------------------------------------------------------------------------------- | -------------- | -| ~/debug/avoidance_debug_message_array | `tier4_planning_msgs::msg::AvoidanceDebugMsgArray` | debug message for avoidance. notify users reasons for avoidance path cannot be generated. | `volatile` | -| ~/debug/lane_change_debug_message_array | `tier4_planning_msgs::msg::LaneChangeDebugMsgArray` | debug message for lane change. notify users unsafe reason during lane changing process | `volatile` | -| ~/debug/maximum_drivable_area | `visualization_msgs::msg::MarkerArray` | shows maximum static drivable area. | `volatile` | -| ~/debug/turn_signal_info | `visualization_msgs::msg::MarkerArray` | TBA | `volatile` | -| ~/debug/bound | `visualization_msgs::msg::MarkerArray` | debug for static drivable area | `volatile` | -| ~/planning/path_candidate/\* | `autoware_planning_msgs::msg::Path` | the path before approval. | `volatile` | -| ~/planning/path_reference/\* | `autoware_planning_msgs::msg::Path` | reference path generated by each modules. | `volatile` | +| Name | Type | Description | QoS Durability | +| :-------------------------------------- | :-------------------------------------------------- | :---------------------------------------------------------------------------------------------- | -------------- | +| ~/debug/avoidance_debug_message_array | `tier4_planning_msgs::msg::AvoidanceDebugMsgArray` | Debug message for avoidance. Notifies users of reasons avoidance path cannot be generated | `volatile` | +| ~/debug/lane_change_debug_message_array | `tier4_planning_msgs::msg::LaneChangeDebugMsgArray` | Debug message for lane change. Notifies users of unsafe conditions during lane-changing process | `volatile` | +| ~/debug/maximum_drivable_area | `visualization_msgs::msg::MarkerArray` | Shows maximum static drivable area | `volatile` | +| ~/debug/turn_signal_info | `visualization_msgs::msg::MarkerArray` | TBA | `volatile` | +| ~/debug/bound | `visualization_msgs::msg::MarkerArray` | Debug for static drivable area | `volatile` | +| ~/planning/path_candidate/\* | `autoware_planning_msgs::msg::Path` | The path before approval | `volatile` | +| ~/planning/path_reference/\* | `autoware_planning_msgs::msg::Path` | Reference path generated by each module | `volatile` | !!! note - For specific information of which topics are being subscribed and published, refer to [behavior_path_planner.xml](https://github.com/autowarefoundation/autoware.universe/blob/9000f430c937764c14e43109539302f1f878ed70/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml#L36-L49). + For specific information about which topics are being subscribed to and published, refer to [behavior_path_planner.xml](https://github.com/autowarefoundation/autoware.universe/blob/9000f430c937764c14e43109539302f1f878ed70/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml#L36-L49). -## How to enable or disable the modules +## How to Enable or Disable Modules -Enabling and disabling the modules in the behavior path planner is primarily managed through two key files: `default_preset.yaml` and `behavior_path_planner.launch.xml`. +Enabling and disabling the modules in the Behavior Path Planner is primarily managed through two key files: `default_preset.yaml` and `behavior_path_planner.launch.xml`. -The `default_preset.yaml` file acts as a configuration file for enabling or disabling specific modules within the planner. It contains a series of arguments which represent the behavior path planner's modules or features. For example: +The `default_preset.yaml` file acts as a configuration file for enabling or disabling specific modules within the planner. It contains a series of arguments which represent the Behavior Path Planner's modules or features. For example: - `launch_static_obstacle_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. @@ -143,14 +143,12 @@ The `default_preset.yaml` file acts as a configuration file for enabling or disa Click [here](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/preset/default_preset.yaml) to view the `default_preset.yaml`. -The `behavior_path_planner.launch.xml` file references the settings defined in `default_preset.yaml` to apply the configurations when the behavior path planner's node is running. For instance, the parameter `static_obstacle_avoidance.enable_module` in +The `behavior_path_planner.launch.xml` file references the settings defined in `default_preset.yaml` to apply the configurations when the Behavior Path Planner's node is running. For instance, the parameter `static_obstacle_avoidance.enable_module` in the following segment corresponds to launch_static_obstacle_avoidance_module from `default_preset.yaml`: ```xml ``` -corresponds to launch_static_obstacle_avoidance_module from `default_preset.yaml`. - Therefore, to enable or disable a module, simply set the corresponding module in `default_preset.yaml` to `true` or `false`. These changes will be applied upon the next launch of Autoware. ## Generating Path @@ -163,13 +161,13 @@ The `ShiftLine` struct (as seen [here](https://github.com/autowarefoundation/aut Furthermore, the design and its implementation incorporate various equations and mathematical models to calculate essential parameters for the path shift. These include the total distance of the lateral shift, the maximum allowable lateral acceleration and jerk, and the total time required for the shift. Practical considerations are also noted, such as simplifying assumptions in the absence of a specific time interval for most lane change and avoidance cases. -The shifted path generation logic enables the behavior path planner to dynamically generate safe and efficient paths, precisely controlling the vehicle’s lateral movements to ensure the smooth execution of lane changes and avoidance maneuvers. This careful planning and execution adhere to the vehicle's dynamic capabilities and safety constraints, maximizing efficiency and safety in autonomous vehicle navigation. +The shifted path generation logic enables the Behavior Path Planner to dynamically generate safe and efficient paths, precisely controlling the vehicle’s lateral movements to ensure the smooth execution of lane changes and avoidance maneuvers. This careful planning and execution adhere to the vehicle's dynamic capabilities and safety constraints, maximizing efficiency and safety in autonomous vehicle navigation. !!! note If you're a math lover, refer to [Path Generation Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md) for the nitty-gritty. -## Collision Assessment / Safety check +## Collision Assessment / Safety Check The purpose of the collision assessment function in the Behavior Path Planner is to evaluate the potential for collisions with target objects across all modules. It is utilized in two scenarios: @@ -207,7 +205,7 @@ Static drivable area expansion operates under assumptions about the correct arra !!! note - Further details can is provided in [Drivable Area Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md). + Further details can be found in [Drivable Area Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md). ### Dynamic Drivable Area Logic @@ -233,7 +231,7 @@ The `TurnIndicatorsCommand` message structure has a command field that can take !!! warning - Rerouting is a feature that was still under progress. Further information will be included on a later date. + The rerouting feature is under development. Further information will be included at a later date. ## Parameters and Configuration @@ -278,5 +276,5 @@ preset ## Limitations & Future Work -1. Goal Planner module cannot be simultaneously executed together with other modules. -2. Module is not designed as plugin. Integrating custom module is not straightforward and user have to modify some part of the behavior path planner main code. +1. The Goal Planner module cannot be simultaneously executed together with other modules. +2. The module is not designed as a plugin. Integrating a custom module is not straightforward. Users have to modify part of the Behavior Path Planner's main code. diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt index e28339fb51a6c..5961b98bdda08 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt @@ -19,12 +19,22 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + # Add test helper library + ament_auto_add_library(start_planner_test_helper SHARED + test/src/start_planner_test_helper.cpp + ) + target_include_directories(start_planner_test_helper PUBLIC + test/include + ) + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${TEST_SOURCES} ) - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} + start_planner_test_helper + ) endif() - ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp index 364d2d31a2577..e1a7ae455e374 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -40,7 +40,11 @@ class FreespacePullOut : public PullOutPlannerBase PlannerType getPlannerType() const override { return PlannerType::FREESPACE; } std::optional plan( - const Pose & start_pose, const Pose & end_pose, PlannerDebugData & planner_debug_data) override; + const Pose & start_pose, const Pose & end_pose, + const std::shared_ptr & planner_data, + PlannerDebugData & planner_debug_data) override; + + friend class TestFreespacePullOut; protected: std::unique_ptr planner_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp index 78eb72183cdf5..5d36c996cca3a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -33,14 +33,13 @@ class GeometricPullOut : public PullOutPlannerBase public: explicit GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const std::shared_ptr - lane_departure_checker, std::shared_ptr time_keeper = std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; std::optional plan( const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) override; GeometricParallelParking planner_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index dfd972aff9be0..0316d6a14a4f9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -42,33 +42,30 @@ class PullOutPlannerBase rclcpp::Node & node, const StartPlannerParameters & parameters, std::shared_ptr time_keeper = std::make_shared()) - : parameters_(parameters), - vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()), - vehicle_footprint_(vehicle_info_.createFootprint()), + : parameters_{parameters}, + vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, + vehicle_footprint_{vehicle_info_.createFootprint()}, time_keeper_(time_keeper) { } virtual ~PullOutPlannerBase() = default; - void setPlannerData(const std::shared_ptr & planner_data) - { - planner_data_ = planner_data; - } - void setCollisionCheckMargin(const double collision_check_margin) { collision_check_margin_ = collision_check_margin; }; virtual PlannerType getPlannerType() const = 0; virtual std::optional plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) = 0; + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, + PlannerDebugData & planner_debug_data) = 0; protected: bool isPullOutPathCollided( autoware::behavior_path_planner::PullOutPath & pull_out_path, + const std::shared_ptr & planner_data, double collision_check_distance_from_end) const; - std::shared_ptr planner_data_; StartPlannerParameters parameters_; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp index 8da104940d327..1000437bdea56 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp @@ -35,18 +35,19 @@ class ShiftPullOut : public PullOutPlannerBase public: explicit ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker, std::shared_ptr time_keeper = std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::SHIFT; }; std::optional plan( const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) override; std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose); + const Pose & start_pose, const Pose & goal_pose, + const BehaviorPathPlannerParameters & behavior_path_parameters); double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index 47eecb54e2b2a..cf9227c2387f8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -27,7 +27,6 @@ #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp" #include "autoware/behavior_path_start_planner_module/shift_pull_out.hpp" -#include #include #include @@ -51,7 +50,6 @@ using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilter using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; -using autoware::lane_departure_checker::LaneDepartureChecker; using geometry_msgs::msg::PoseArray; using PriorityOrder = std::vector>>; @@ -310,8 +308,6 @@ ego pose. std::vector searchPullOutStartPoseCandidates( const PathWithLaneId & back_path_from_start_pose) const; - std::shared_ptr lane_departure_checker_; - // turn signal TurnSignalInfo calcTurnSignalInfo(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index a089f6a8a83fc..6893c7d11d09d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -47,13 +47,14 @@ FreespacePullOut::FreespacePullOut(rclcpp::Node & node, const StartPlannerParame } std::optional FreespacePullOut::plan( - const Pose & start_pose, const Pose & end_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & end_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - const auto & route_handler = planner_data_->route_handler; - const double backward_path_length = planner_data_->parameters.backward_path_length; - const double forward_path_length = planner_data_->parameters.forward_path_length; + const auto & route_handler = planner_data->route_handler; + const double backward_path_length = planner_data->parameters.backward_path_length; + const double forward_path_length = planner_data->parameters.forward_path_length; - planner_->setMap(*planner_data_->costmap); + planner_->setMap(*planner_data->costmap); try { if (!planner_->makePlan(start_pose, end_pose)) { @@ -65,11 +66,11 @@ std::optional FreespacePullOut::plan( } const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); // find candidate paths const auto pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + start_planner_utils::getPullOutLanes(planner_data, backward_path_length); const auto lanes = utils::combineLanelets(road_lanes, pull_out_lanes); const PathWithLaneId path = diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index bbc6c05725434..69c5c41405932 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -36,35 +36,37 @@ using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const std::shared_ptr - lane_departure_checker, std::shared_ptr time_keeper) : PullOutPlannerBase{node, parameters, time_keeper}, - parallel_parking_parameters_{parameters.parallel_parking_parameters}, - lane_departure_checker_(lane_departure_checker) + parallel_parking_parameters_{parameters.parallel_parking_parameters} { + lane_departure_checker_ = + std::make_shared( + autoware::lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, + vehicle_info_, time_keeper_); planner_.setParameters(parallel_parking_parameters_); } std::optional GeometricPullOut::plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { PullOutPath output; // combine road lane and pull out lane const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_.max_back_distance; + planner_data->parameters.backward_path_length + parameters_.max_back_distance; const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); - const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); + const auto pull_out_lanes = getPullOutLanes(planner_data, backward_path_length); // check if the ego is at left or right side of road lane center const bool left_side_start = 0 < getArcCoordinates(road_lanes, start_pose).distance; planner_.setTurningRadius( - planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); - planner_.setPlannerData(planner_data_); + planner_data->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); + planner_.setPlannerData(planner_data); const bool found_valid_path = planner_.planPullOut( start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, lane_departure_checker_); if (!found_valid_path) { @@ -122,7 +124,8 @@ std::optional GeometricPullOut::plan( output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; - if (isPullOutPathCollided(output, parameters_.geometric_collision_check_distance_from_end)) { + if (isPullOutPathCollided( + output, planner_data, parameters_.geometric_collision_check_distance_from_end)) { planner_debug_data.conditions_evaluation.emplace_back("collision"); return {}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp index 42c5bead33604..a475b3ad582cb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp @@ -14,21 +14,24 @@ #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include + namespace autoware::behavior_path_planner { bool PullOutPlannerBase::isPullOutPathCollided( autoware::behavior_path_planner::PullOutPath & pull_out_path, + const std::shared_ptr & planner_data, double collision_check_distance_from_end) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // check for collisions - const auto & dynamic_objects = planner_data_->dynamic_object; + const auto & dynamic_objects = planner_data->dynamic_object; if (!dynamic_objects) { return false; } const auto pull_out_lanes = start_planner_utils::getPullOutLanes( - planner_data_, planner_data_->parameters.backward_path_length + parameters_.max_back_distance); + planner_data, planner_data->parameters.backward_path_length + parameters_.max_back_distance); // extract stop objects in pull out lane for collision check const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *dynamic_objects, parameters_.th_moving_object_velocity); 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 4759559619870..8f4de1d3636de 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 @@ -41,31 +41,36 @@ using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker, std::shared_ptr time_keeper) -: PullOutPlannerBase{node, parameters, time_keeper}, lane_departure_checker_{lane_departure_checker} +: PullOutPlannerBase{node, parameters, time_keeper} { + lane_departure_checker_ = + std::make_shared( + autoware::lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, + vehicle_info_, time_keeper_); } std::optional ShiftPullOut::plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - const auto & route_handler = planner_data_->route_handler; - const auto & common_parameters = planner_data_->parameters; + const auto & route_handler = planner_data->route_handler; + const auto & common_parameters = planner_data->parameters; const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_.max_back_distance; + planner_data->parameters.backward_path_length + parameters_.max_back_distance; const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); // find candidate paths - auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose); + auto pull_out_paths = + calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose, common_parameters); if (pull_out_paths.empty()) { planner_debug_data.conditions_evaluation.emplace_back("no path found"); return std::nullopt; } - const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + const auto lanelet_map_ptr = planner_data->route_handler->getLaneletMapPtr(); std::vector fused_id_start_to_end{}; std::optional fused_polygon_start_to_end = std::nullopt; @@ -159,9 +164,10 @@ std::optional ShiftPullOut::plan( continue; } shift_path.points = cropped_path.points; - shift_path.header = planner_data_->route_handler->getRouteHeader(); + shift_path.header = planner_data->route_handler->getRouteHeader(); - if (isPullOutPathCollided(pull_out_path, parameters_.shift_collision_check_distance_from_end)) { + if (isPullOutPathCollided( + pull_out_path, planner_data, parameters_.shift_collision_check_distance_from_end)) { planner_debug_data.conditions_evaluation.emplace_back("collision"); continue; } @@ -227,7 +233,8 @@ bool ShiftPullOut::refineShiftedPathToStartPose( std::vector ShiftPullOut::calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose) + const Pose & start_pose, const Pose & goal_pose, + const BehaviorPathPlannerParameters & behavior_path_parameters) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -238,9 +245,8 @@ std::vector ShiftPullOut::calcPullOutPaths( } // rename parameter - const auto & common_parameters = planner_data_->parameters; - const double forward_path_length = common_parameters.forward_path_length; - const double backward_path_length = common_parameters.backward_path_length; + const double forward_path_length = behavior_path_parameters.forward_path_length; + const double backward_path_length = behavior_path_parameters.backward_path_length; const double lateral_jerk = parameters_.lateral_jerk; const double minimum_lateral_acc = parameters_.minimum_lateral_acc; const double maximum_lateral_acc = parameters_.maximum_lateral_acc; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index f45924b9542dc..0bac50a37fdbf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -68,22 +68,12 @@ StartPlannerModule::StartPlannerModule( vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_freespace_planner_cb_running_{false} { - lane_departure_checker_ = std::make_shared(time_keeper_); - lane_departure_checker_->setVehicleInfo(vehicle_info_); - autoware::lane_departure_checker::Param lane_departure_checker_params{}; - lane_departure_checker_params.footprint_extra_margin = - parameters->lane_departure_check_expansion_margin; - - lane_departure_checker_->setParam(lane_departure_checker_params); - // set enabled planner if (parameters_->enable_shift_pull_out) { - start_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_, time_keeper_)); + start_planners_.push_back(std::make_shared(node, *parameters, time_keeper_)); } if (parameters_->enable_geometric_pull_out) { - start_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_, time_keeper_)); + start_planners_.push_back(std::make_shared(node, *parameters, time_keeper_)); } if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); @@ -985,17 +975,16 @@ bool StartPlannerModule::findPullOutPath( const bool backward_is_unnecessary = backwards_distance < epsilon; planner->setCollisionCheckMargin(collision_check_margin); - planner->setPlannerData(planner_data_); PlannerDebugData debug_data{ planner->getPlannerType(), {}, collision_check_margin, backwards_distance}; - const auto pull_out_path = planner->plan(start_pose_candidate, goal_pose, debug_data); + const auto pull_out_path = + planner->plan(start_pose_candidate, goal_pose, planner_data_, debug_data); debug_data_vector.push_back(debug_data); // If no path is found, return false if (!pull_out_path) { return false; } - if (backward_is_unnecessary) { updateStatusWithCurrentPath(*pull_out_path, start_pose_candidate, planner->getPlannerType()); return true; @@ -1594,9 +1583,9 @@ std::optional StartPlannerModule::planFreespacePath( for (const auto & p : center_line_path.points) { const Pose end_pose = p.point.pose; - freespace_planner_->setPlannerData(planner_data); PlannerDebugData debug_data{freespace_planner_->getPlannerType(), {}, 0.0, 0.0}; - auto freespace_path = freespace_planner_->plan(current_pose, end_pose, debug_data); + auto freespace_path = + freespace_planner_->plan(current_pose, end_pose, planner_data, debug_data); DEBUG_PRINT( "\nFreespace Pull out path search results\n%s%s", debug_data.header_str().c_str(), debug_data.str().c_str()); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/include/start_planner_test_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/include/start_planner_test_helper.hpp new file mode 100644 index 0000000000000..a1d294b279ff6 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/include/start_planner_test_helper.hpp @@ -0,0 +1,39 @@ +// 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. +#pragma once + +#include +#include + +#include + +namespace autoware::behavior_path_planner::testing +{ + +class StartPlannerTestHelper +{ +public: + static rclcpp::NodeOptions make_node_options(); + + static void set_odometry( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose); + static void set_route( + std::shared_ptr & planner_data, const int route_start_lane_id, + const int route_goal_lane_id); + static void set_costmap( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose, + const double grid_resolution, const double grid_length_x, const double grid_length_y); +}; + +} // namespace autoware::behavior_path_planner::testing diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/src/start_planner_test_helper.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/src/start_planner_test_helper.cpp new file mode 100644 index 0000000000000..0f0720edc4a15 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/src/start_planner_test_helper.cpp @@ -0,0 +1,98 @@ +// 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. +// pull_out_test_utils.cpp +#include "start_planner_test_helper.hpp" + +#include +#include + +#include +#include + +namespace autoware::behavior_path_planner::testing +{ +using autoware::test_utils::get_absolute_path_to_config; +using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; + +rclcpp::NodeOptions StartPlannerTestHelper::make_node_options() +{ + // Load common configuration files + auto node_options = rclcpp::NodeOptions{}; + + const auto common_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_common.param.yaml"); + const auto nearest_search_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_nearest_search.param.yaml"); + const auto vehicle_info_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_vehicle_info.param.yaml"); + const auto behavior_path_planner_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "behavior_path_planner.param.yaml"); + const auto drivable_area_expansion_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "drivable_area_expansion.param.yaml"); + const auto scene_module_manager_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "scene_module_manager.param.yaml"); + const auto start_planner_param_path = get_absolute_path_to_config( + "autoware_behavior_path_start_planner_module", "start_planner.param.yaml"); + + autoware::test_utils::updateNodeOptions( + node_options, {common_param_path, nearest_search_param_path, vehicle_info_param_path, + behavior_path_planner_param_path, drivable_area_expansion_param_path, + scene_module_manager_param_path, start_planner_param_path}); + + return node_options; +} + +void StartPlannerTestHelper::set_odometry( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose) +{ + auto odometry = std::make_shared(); + odometry->pose.pose = start_pose; + odometry->header.frame_id = "map"; + planner_data->self_odometry = odometry; +} + +void StartPlannerTestHelper::set_route( + std::shared_ptr & planner_data, const int route_start_lane_id, + const int route_goal_lane_id) +{ + const auto shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); + const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5); + auto route_handler = std::make_shared(map_bin_msg); + + const auto route = makeBehaviorRouteFromLaneId( + route_start_lane_id, route_goal_lane_id, "autoware_test_utils", + "road_shoulder/lanelet2_map.osm"); + route_handler->setRoute(route); + planner_data->route_handler = route_handler; +} + +void StartPlannerTestHelper::set_costmap( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose, + const double grid_resolution, const double grid_length_x, const double grid_length_y) +{ + nav_msgs::msg::OccupancyGrid costmap; + costmap.header.frame_id = "map"; + costmap.info.width = static_cast(grid_length_x / grid_resolution); + costmap.info.height = static_cast(grid_length_y / grid_resolution); + costmap.info.resolution = grid_resolution; + + costmap.info.origin.position.x = start_pose.position.x - grid_length_x / 2; + costmap.info.origin.position.y = start_pose.position.y - grid_length_y / 2; + costmap.data = std::vector(costmap.info.width * costmap.info.height, 0); + + planner_data->costmap = std::make_shared(costmap); +} + +} // namespace autoware::behavior_path_planner::testing diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp new file mode 100644 index 0000000000000..79c58c37f0dcf --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp @@ -0,0 +1,113 @@ +// 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 "start_planner_test_helper.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using autoware::behavior_path_planner::FreespacePullOut; +using autoware::behavior_path_planner::StartPlannerParameters; +using autoware::test_utils::get_absolute_path_to_config; +using autoware_planning_msgs::msg::LaneletRoute; +using RouteSections = std::vector; +using autoware::behavior_path_planner::testing::StartPlannerTestHelper; +using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; + +namespace autoware::behavior_path_planner +{ + +class TestFreespacePullOut : public ::testing::Test +{ +public: + std::optional call_plan( + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) + { + return freespace_pull_out_->plan(start_pose, goal_pose, planner_data, planner_debug_data); + } + +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + node_ = + rclcpp::Node::make_shared("freespace_pull_out", StartPlannerTestHelper::make_node_options()); + + planner_data_ = std::make_shared(); + planner_data_->init_parameters(*node_); + + initialize_freespace_pull_out_planner(); + } + + void TearDown() override { rclcpp::shutdown(); } + + // Member variables + std::shared_ptr node_; + std::shared_ptr freespace_pull_out_; + std::shared_ptr planner_data_; + +private: + void initialize_freespace_pull_out_planner() + { + auto parameters = StartPlannerParameters::init(*node_); + freespace_pull_out_ = std::make_shared(*node_, parameters); + } +}; + +TEST_F(TestFreespacePullOut, GenerateValidFreespacePullOutPath) +{ + const auto start_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(299.796).y(303.529).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(-0.748629).w( + 0.662990)); + + const auto goal_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(280.721).y(301.025).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.991718).w( + 0.128435)); + + StartPlannerTestHelper::set_odometry(planner_data_, start_pose); + StartPlannerTestHelper::set_route(planner_data_, 508, 720); + StartPlannerTestHelper::set_costmap(planner_data_, start_pose, 0.3, 70.0, 70.0); + + // Plan the pull out path + PlannerDebugData debug_data; + auto result = call_plan(start_pose, goal_pose, planner_data_, debug_data); + + // Assert that a valid Freespace pull out path is generated + ASSERT_TRUE(result.has_value()) << "Freespace pull out path generation failed."; + // EXPECT_EQ(result->partial_paths.size(), 2UL) + // << "Freespace pull out path does not have the expected number of partial paths."; + EXPECT_EQ(debug_data.conditions_evaluation.back(), "success") + << "Freespace pull out path planning did not succeed."; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp index dd8bb02c97dea..4548c058b871d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "start_planner_test_helper.hpp" + #include #include #include #include -#include #include #include #include @@ -24,18 +25,16 @@ #include -#include #include -#include #include #include using autoware::behavior_path_planner::GeometricPullOut; using autoware::behavior_path_planner::StartPlannerParameters; -using autoware::lane_departure_checker::LaneDepartureChecker; using autoware::test_utils::get_absolute_path_to_config; using autoware_planning_msgs::msg::LaneletRoute; using RouteSections = std::vector; +using autoware::behavior_path_planner::testing::StartPlannerTestHelper; using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; namespace autoware::behavior_path_planner @@ -44,142 +43,35 @@ namespace autoware::behavior_path_planner class TestGeometricPullOut : public ::testing::Test { public: - std::optional plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + std::optional call_plan( + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - return geometric_pull_out_->plan(start_pose, goal_pose, planner_debug_data); + return geometric_pull_out_->plan(start_pose, goal_pose, planner_data, planner_debug_data); } protected: void SetUp() override { rclcpp::init(0, nullptr); - node_ = rclcpp::Node::make_shared("geometric_pull_out", get_node_options()); + node_ = + rclcpp::Node::make_shared("geometric_pull_out", StartPlannerTestHelper::make_node_options()); - load_parameters(); - initialize_vehicle_info(); - initialize_lane_departure_checker(); - initialize_routeHandler(); initialize_geometric_pull_out_planner(); - initialize_planner_data(); } - void TearDown() override { rclcpp::shutdown(); } + // Member variables std::shared_ptr node_; - std::shared_ptr route_handler_; - autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::shared_ptr geometric_pull_out_; - std::shared_ptr lane_departure_checker_; - PlannerData planner_data_; private: - rclcpp::NodeOptions get_node_options() const - { - // Load common configuration files - auto node_options = rclcpp::NodeOptions{}; - - const auto common_param_path = - get_absolute_path_to_config("autoware_test_utils", "test_common.param.yaml"); - const auto nearest_search_param_path = - get_absolute_path_to_config("autoware_test_utils", "test_nearest_search.param.yaml"); - const auto vehicle_info_param_path = - get_absolute_path_to_config("autoware_test_utils", "test_vehicle_info.param.yaml"); - const auto behavior_path_planner_param_path = get_absolute_path_to_config( - "autoware_behavior_path_planner", "behavior_path_planner.param.yaml"); - const auto drivable_area_expansion_param_path = get_absolute_path_to_config( - "autoware_behavior_path_planner", "drivable_area_expansion.param.yaml"); - const auto scene_module_manager_param_path = get_absolute_path_to_config( - "autoware_behavior_path_planner", "scene_module_manager.param.yaml"); - const auto start_planner_param_path = get_absolute_path_to_config( - "autoware_behavior_path_start_planner_module", "start_planner.param.yaml"); - - autoware::test_utils::updateNodeOptions( - node_options, {common_param_path, nearest_search_param_path, vehicle_info_param_path, - behavior_path_planner_param_path, drivable_area_expansion_param_path, - scene_module_manager_param_path, start_planner_param_path}); - - return node_options; - } - - void load_parameters() - { - const auto dp_double = [&](const std::string & s) { - return node_->declare_parameter(s); - }; - const auto dp_bool = [&](const std::string & s) { return node_->declare_parameter(s); }; - // Load parameters required for planning - const std::string ns = "start_planner."; - lane_departure_check_expansion_margin_ = - dp_double(ns + "lane_departure_check_expansion_margin"); - pull_out_max_steer_angle_ = dp_double(ns + "pull_out_max_steer_angle"); - pull_out_arc_path_interval_ = dp_double(ns + "arc_path_interval"); - center_line_path_interval_ = dp_double(ns + "center_line_path_interval"); - th_moving_object_velocity_ = dp_double(ns + "th_moving_object_velocity"); - divide_pull_out_path_ = dp_bool(ns + "divide_pull_out_path"); - backward_path_length_ = dp_double("backward_path_length"); - forward_path_length_ = dp_double("forward_path_length"); - } - - void initialize_vehicle_info() - { - vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); - } - - void initialize_lane_departure_checker() - { - lane_departure_checker_ = std::make_shared(); - lane_departure_checker_->setVehicleInfo(vehicle_info_); - - autoware::lane_departure_checker::Param lane_departure_checker_params{}; - lane_departure_checker_params.footprint_extra_margin = lane_departure_check_expansion_margin_; - lane_departure_checker_->setParam(lane_departure_checker_params); - } - - void initialize_routeHandler() - { - // Load a sample lanelet map and create a route handler - const auto shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( - "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); - const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5); - - route_handler_ = std::make_shared(map_bin_msg); - } - void initialize_geometric_pull_out_planner() { - auto parameters = std::make_shared(); - parameters->parallel_parking_parameters.pull_out_max_steer_angle = pull_out_max_steer_angle_; - parameters->parallel_parking_parameters.pull_out_arc_path_interval = - pull_out_arc_path_interval_; - parameters->parallel_parking_parameters.center_line_path_interval = center_line_path_interval_; - parameters->th_moving_object_velocity = th_moving_object_velocity_; - parameters->divide_pull_out_path = divide_pull_out_path_; - - geometric_pull_out_ = - std::make_shared(*node_, *parameters, lane_departure_checker_); - } + auto parameters = StartPlannerParameters::init(*node_); - void initialize_planner_data() - { - planner_data_.parameters.backward_path_length = backward_path_length_; - planner_data_.parameters.forward_path_length = forward_path_length_; - planner_data_.parameters.wheel_base = vehicle_info_.wheel_base_m; - planner_data_.parameters.wheel_tread = vehicle_info_.wheel_tread_m; - planner_data_.parameters.front_overhang = vehicle_info_.front_overhang_m; - planner_data_.parameters.left_over_hang = vehicle_info_.left_overhang_m; - planner_data_.parameters.right_over_hang = vehicle_info_.right_overhang_m; + geometric_pull_out_ = std::make_shared(*node_, parameters); } - - // Parameter variables - double lane_departure_check_expansion_margin_{0.0}; - double pull_out_max_steer_angle_{0.0}; - double pull_out_arc_path_interval_{0.0}; - double center_line_path_interval_{0.0}; - double th_moving_object_velocity_{0.0}; - double backward_path_length_{0.0}; - double forward_path_length_{0.0}; - bool divide_pull_out_path_{false}; }; TEST_F(TestGeometricPullOut, GenerateValidGeometricPullOutPath) @@ -198,26 +90,16 @@ TEST_F(TestGeometricPullOut, GenerateValidGeometricPullOutPath) geometry_msgs::build().x(0.0).y(0.0).z(0.705897).w( 0.708314)); - // Set up current odometry at start pose - auto odometry = std::make_shared(); - odometry->pose.pose = start_pose; - odometry->header.frame_id = "map"; - planner_data_.self_odometry = odometry; - - // Setup route - const auto route = makeBehaviorRouteFromLaneId( - 4619, 4635, "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); - route_handler_->setRoute(route); - - // Update planner data with the route handler - planner_data_.route_handler = route_handler_; - geometric_pull_out_->setPlannerData(std::make_shared(planner_data_)); + auto planner_data = std::make_shared(); + planner_data->init_parameters(*node_); + StartPlannerTestHelper::set_odometry(planner_data, start_pose); + StartPlannerTestHelper::set_route(planner_data, 4619, 4635); // Plan the pull out path PlannerDebugData debug_data; - auto result = plan(start_pose, goal_pose, debug_data); + auto result = call_plan(start_pose, goal_pose, planner_data, debug_data); - // Assert that a valid geometric geometric pull out path is generated + // Assert that a valid geometric pull out path is generated ASSERT_TRUE(result.has_value()) << "Geometric pull out path generation failed."; EXPECT_EQ(result->partial_paths.size(), 2UL) << "Generated geometric pull out path does not have the expected number of partial paths."; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp index 474da055b4de7..d2c7c60e1a4ca 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "start_planner_test_helper.hpp" + #include #include #include #include -#include #include #include #include @@ -25,15 +26,16 @@ #include #include +#include #include #include using autoware::behavior_path_planner::ShiftPullOut; using autoware::behavior_path_planner::StartPlannerParameters; -using autoware::lane_departure_checker::LaneDepartureChecker; using autoware::test_utils::get_absolute_path_to_config; using autoware_planning_msgs::msg::LaneletRoute; using RouteSections = std::vector; +using autoware::behavior_path_planner::testing::StartPlannerTestHelper; using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; namespace autoware::behavior_path_planner @@ -43,103 +45,34 @@ class TestShiftPullOut : public ::testing::Test { public: std::optional call_plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - return shift_pull_out_->plan(start_pose, goal_pose, planner_debug_data); + return shift_pull_out_->plan(start_pose, goal_pose, planner_data, planner_debug_data); } protected: void SetUp() override { rclcpp::init(0, nullptr); - node_ = rclcpp::Node::make_shared("shift_pull_out", make_node_options()); + node_ = + rclcpp::Node::make_shared("shift_pull_out", StartPlannerTestHelper::make_node_options()); - initialize_lane_departure_checker(); initialize_shift_pull_out_planner(); } void TearDown() override { rclcpp::shutdown(); } - PlannerData make_planner_data( - const Pose & start_pose, const int route_start_lane_id, const int route_goal_lane_id) - { - PlannerData planner_data; - planner_data.init_parameters(*node_); - - // Load a sample lanelet map and create a route handler - const auto shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( - "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); - const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5); - auto route_handler = std::make_shared(map_bin_msg); - - // Set up current odometry at start pose - auto odometry = std::make_shared(); - odometry->pose.pose = start_pose; - odometry->header.frame_id = "map"; - planner_data.self_odometry = odometry; - - // Setup route - const auto route = makeBehaviorRouteFromLaneId( - route_start_lane_id, route_goal_lane_id, "autoware_test_utils", - "road_shoulder/lanelet2_map.osm"); - route_handler->setRoute(route); - - // Update planner data with the route handler - planner_data.route_handler = route_handler; - - return planner_data; - } - // Member variables std::shared_ptr node_; std::shared_ptr shift_pull_out_; - std::shared_ptr lane_departure_checker_; private: - rclcpp::NodeOptions make_node_options() const - { - // Load common configuration files - auto node_options = rclcpp::NodeOptions{}; - - const auto common_param_path = - get_absolute_path_to_config("autoware_test_utils", "test_common.param.yaml"); - const auto nearest_search_param_path = - get_absolute_path_to_config("autoware_test_utils", "test_nearest_search.param.yaml"); - const auto vehicle_info_param_path = - get_absolute_path_to_config("autoware_test_utils", "test_vehicle_info.param.yaml"); - const auto behavior_path_planner_param_path = get_absolute_path_to_config( - "autoware_behavior_path_planner", "behavior_path_planner.param.yaml"); - const auto drivable_area_expansion_param_path = get_absolute_path_to_config( - "autoware_behavior_path_planner", "drivable_area_expansion.param.yaml"); - const auto scene_module_manager_param_path = get_absolute_path_to_config( - "autoware_behavior_path_planner", "scene_module_manager.param.yaml"); - const auto start_planner_param_path = get_absolute_path_to_config( - "autoware_behavior_path_start_planner_module", "start_planner.param.yaml"); - - autoware::test_utils::updateNodeOptions( - node_options, {common_param_path, nearest_search_param_path, vehicle_info_param_path, - behavior_path_planner_param_path, drivable_area_expansion_param_path, - scene_module_manager_param_path, start_planner_param_path}); - - return node_options; - } - - void initialize_lane_departure_checker() - { - const auto vehicle_info = - autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); - lane_departure_checker_ = std::make_shared(); - lane_departure_checker_->setVehicleInfo(vehicle_info); - - autoware::lane_departure_checker::Param lane_departure_checker_params{}; - lane_departure_checker_->setParam(lane_departure_checker_params); - } - void initialize_shift_pull_out_planner() { auto parameters = StartPlannerParameters::init(*node_); - shift_pull_out_ = std::make_shared(*node_, parameters, lane_departure_checker_); + shift_pull_out_ = std::make_shared(*node_, parameters); } }; @@ -158,13 +91,14 @@ TEST_F(TestShiftPullOut, GenerateValidShiftPullOutPath) .orientation( geometry_msgs::build().x(0.0).y(0.0).z(0.705897).w( 0.708314)); - const auto planner_data = make_planner_data(start_pose, 4619, 4635); - - shift_pull_out_->setPlannerData(std::make_shared(planner_data)); + auto planner_data = std::make_shared(); + planner_data->init_parameters(*node_); + StartPlannerTestHelper::set_odometry(planner_data, start_pose); + StartPlannerTestHelper::set_route(planner_data, 4619, 4635); // Plan the pull out path PlannerDebugData debug_data; - auto result = call_plan(start_pose, goal_pose, debug_data); + auto result = call_plan(start_pose, goal_pose, planner_data, debug_data); // Assert that a valid shift pull out path is generated ASSERT_TRUE(result.has_value()) << "shift pull out path generation failed."; 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 d8d640a078267..a891b011dbab8 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 @@ -228,6 +228,7 @@ class SceneModuleManagerInterface auto itr = scene_modules_.begin(); while (itr != scene_modules_.end()) { if (isModuleExpired(*itr)) { + registered_module_id_set_.erase((*itr)->getModuleId()); itr = scene_modules_.erase(itr); } else { itr++; diff --git a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index cab4acfbb8556..54d4bc2e90714 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -197,6 +197,15 @@ void calculateCartesian( trajectory.longitudinal_accelerations.push_back(0.0); trajectory.lateral_accelerations.push_back(0.0); } + for (auto i = 0UL; i < trajectory.points.size(); ++i) { + geometry_msgs::msg::Pose pose; + pose.position.x = trajectory.points[i].x(); + pose.position.y = trajectory.points[i].y(); + pose.position.z = 0.0; + pose.orientation = + autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, trajectory.yaws[i]); + trajectory.poses.push_back(pose); + } } } diff --git a/sensing/autoware_image_diagnostics/README.md b/sensing/autoware_image_diagnostics/README.md index 03858088564b3..42fcbdc8dc612 100644 --- a/sensing/autoware_image_diagnostics/README.md +++ b/sensing/autoware_image_diagnostics/README.md @@ -28,13 +28,13 @@ After all image's blocks state are evaluated, the whole image status is summariz ### Output -| Name | Type | Description | -| ----------------------------------- | --------------------------------------- | ------------------------------------- | -| `image_diag/debug/gray_image` | `sensor_msgs::msg::Image` | gray image | -| `image_diag/debug/dft_image` | `sensor_msgs::msg::Image` | discrete Fourier transformation image | -| `image_diag/debug/diag_block_image` | `sensor_msgs::msg::Image` | each block state colorization | -| `image_diag/image_state_diag` | `tier4_debug_msgs::msg::Int32Stamped` | image diagnostics status value | -| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | +| Name | Type | Description | +| ----------------------------------- | ------------------------------------------------- | ------------------------------------- | +| `image_diag/debug/gray_image` | `sensor_msgs::msg::Image` | gray image | +| `image_diag/debug/dft_image` | `sensor_msgs::msg::Image` | discrete Fourier transformation image | +| `image_diag/debug/diag_block_image` | `sensor_msgs::msg::Image` | each block state colorization | +| `image_diag/image_state_diag` | `autoware_internal_debug_msgs::msg::Int32Stamped` | image diagnostics status value | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | ## Parameters diff --git a/sensing/autoware_image_diagnostics/package.xml b/sensing/autoware_image_diagnostics/package.xml index 2e4556de2b92b..3140b87e1f1b9 100644 --- a/sensing/autoware_image_diagnostics/package.xml +++ b/sensing/autoware_image_diagnostics/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto + autoware_internal_debug_msgs autoware_universe_utils cv_bridge diagnostic_updater @@ -19,7 +20,6 @@ rclcpp rclcpp_components sensor_msgs - tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp index 1625cba5b72aa..57e53aee46328 100644 --- a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp +++ b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp @@ -33,7 +33,7 @@ ImageDiagNode::ImageDiagNode(const rclcpp::NodeOptions & node_options) dft_image_pub_ = image_transport::create_publisher(this, "image_diag/debug/dft_image"); gray_image_pub_ = image_transport::create_publisher(this, "image_diag/debug/gray_image"); - image_state_pub_ = create_publisher( + image_state_pub_ = create_publisher( "image_diag/image_state_diag", rclcpp::SensorDataQoS()); updater_.setHardwareID("Image_Diagnostics"); @@ -225,7 +225,7 @@ void ImageDiagNode::ImageChecker(const sensor_msgs::msg::Image::ConstSharedPtr i } else { params_.diagnostic_status = 0; } - tier4_debug_msgs::msg::Int32Stamped image_state_out; + autoware_internal_debug_msgs::msg::Int32Stamped image_state_out; image_state_out.data = params_.diagnostic_status; image_state_pub_->publish(image_state_out); diff --git a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp index 325624062b90b..9383da33160f9 100644 --- a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp +++ b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp @@ -21,10 +21,10 @@ #include #include +#include +#include +#include #include -#include -#include -#include #if __has_include() #include @@ -95,8 +95,9 @@ class ImageDiagNode : public rclcpp::Node image_transport::Publisher block_diag_image_pub_; image_transport::Publisher dft_image_pub_; image_transport::Publisher gray_image_pub_; - rclcpp::Publisher::SharedPtr average_pub_; - rclcpp::Publisher::SharedPtr image_state_pub_; + rclcpp::Publisher::SharedPtr + average_pub_; + rclcpp::Publisher::SharedPtr image_state_pub_; }; } // namespace autoware::image_diagnostics diff --git a/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md b/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md index 911cda3823021..a9dcb8437ec67 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md @@ -38,16 +38,16 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Output -| Name | Type | Description | -| :-------------------------------------------------------- | :-------------------------------------- | ------------------------------------------------------------------------------------------------ | -| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | -| `~/output/blockage_diag/debug/ground_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | -| `~/output/blockage_diag/debug/sky_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | -| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | -| `~/output/blockage_diag/debug/single_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of detected dusty area in latest single frame | -| `~/output/blockage_diag/debug/multi_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of continuous detected dusty area | -| `~/output/blockage_diag/debug/blockage_dust_merged_image` | `sensor_msgs::msg::Image` | The merged image of blockage detection(red) and multi frame dusty area detection(yellow) results | -| `~/output/blockage_diag/debug/ground_dust_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The ratio of dusty area divided by area where ray usually returns from the ground. | +| Name | Type | Description | +| :-------------------------------------------------------- | :-------------------------------------------------- | ------------------------------------------------------------------------------------------------ | +| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | +| `~/output/blockage_diag/debug/ground_blockage_ratio` | `autoware_internal_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | +| `~/output/blockage_diag/debug/sky_blockage_ratio` | `autoware_internal_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | +| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | +| `~/output/blockage_diag/debug/single_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of detected dusty area in latest single frame | +| `~/output/blockage_diag/debug/multi_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of continuous detected dusty area | +| `~/output/blockage_diag/debug/blockage_dust_merged_image` | `sensor_msgs::msg::Image` | The merged image of blockage detection(red) and multi frame dusty area detection(yellow) results | +| `~/output/blockage_diag/debug/ground_dust_ratio` | `autoware_internal_debug_msgs::msg::Float32Stamped` | The ratio of dusty area divided by area where ray usually returns from the ground. | ## Parameters diff --git a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md index 8f4da273861a3..0d0423d40ab83 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -36,11 +36,11 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Output -| Name | Type | Description | -| ---------------------------------------------- | --------------------------------------- | ------------------------------------------------------- | -| `/dual_return_outlier_filter/frequency_image` | `sensor_msgs::msg::Image` | The histogram image that represent visibility | -| `/dual_return_outlier_filter/visibility` | `tier4_debug_msgs::msg::Float32Stamped` | A representation of visibility with a value from 0 to 1 | -| `/dual_return_outlier_filter/pointcloud_noise` | `sensor_msgs::msg::Pointcloud2` | The pointcloud removed as noise | +| Name | Type | Description | +| ---------------------------------------------- | --------------------------------------------------- | ------------------------------------------------------- | +| `/dual_return_outlier_filter/frequency_image` | `sensor_msgs::msg::Image` | The histogram image that represent visibility | +| `/dual_return_outlier_filter/visibility` | `autoware_internal_debug_msgs::msg::Float32Stamped` | A representation of visibility with a value from 0 to 1 | +| `/dual_return_outlier_filter/pointcloud_noise` | `sensor_msgs::msg::Pointcloud2` | The pointcloud removed as noise | ## Parameters diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp index 3601b492c4fe3..2dbda770f6cec 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp @@ -22,10 +22,10 @@ #include #include +#include #include #include #include -#include #if __has_include() #include @@ -58,10 +58,13 @@ class BlockageDiagComponent : public autoware::pointcloud_preprocessor::Filter image_transport::Publisher single_frame_dust_mask_pub; image_transport::Publisher multi_frame_dust_mask_pub; image_transport::Publisher blockage_dust_merged_pub; - rclcpp::Publisher::SharedPtr ground_blockage_ratio_pub_; - rclcpp::Publisher::SharedPtr sky_blockage_ratio_pub_; - rclcpp::Publisher::SharedPtr ground_dust_ratio_pub_; - rclcpp::Publisher::SharedPtr blockage_type_pub_; + rclcpp::Publisher::SharedPtr + ground_blockage_ratio_pub_; + rclcpp::Publisher::SharedPtr + sky_blockage_ratio_pub_; + rclcpp::Publisher::SharedPtr + ground_dust_ratio_pub_; + rclcpp::Publisher::SharedPtr blockage_type_pub_; private: void onBlockageChecker(DiagnosticStatusWrapper & stat); diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index 0e959eedae1b6..d57e44431a8d2 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -68,13 +68,13 @@ #include #include +#include +#include #include #include #include #include #include -#include -#include #include #include diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp index ef33e88ef5c98..fa55222fb6530 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp @@ -22,9 +22,9 @@ #include #include +#include #include #include -#include #if __has_include() #include @@ -54,7 +54,7 @@ class DualReturnOutlierFilterComponent : public autoware::pointcloud_preprocesso /** \brief Parameter service callback */ rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); image_transport::Publisher image_pub_; - rclcpp::Publisher::SharedPtr visibility_pub_; + rclcpp::Publisher::SharedPtr visibility_pub_; rclcpp::Publisher::SharedPtr noise_cloud_pub_; private: diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp index c898cbacf33ea..96bd2fb2411a0 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp @@ -53,7 +53,7 @@ class RingOutlierFilterComponent : public autoware::pointcloud_preprocessor::Fil const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, const TransformInfo & transform_info) override; - rclcpp::Publisher::SharedPtr visibility_pub_; + rclcpp::Publisher::SharedPtr visibility_pub_; private: /** \brief publisher of excluded pointcloud for debug reason. **/ diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp index d36fcb36a7be1..f4921be66a7e2 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp @@ -68,14 +68,14 @@ #include #include +#include +#include #include #include #include #include #include #include -#include -#include #include #include diff --git a/sensing/autoware_pointcloud_preprocessor/package.xml b/sensing/autoware_pointcloud_preprocessor/package.xml index 4511c5497a3a0..525b1f3eb0699 100644 --- a/sensing/autoware_pointcloud_preprocessor/package.xml +++ b/sensing/autoware_pointcloud_preprocessor/package.xml @@ -26,6 +26,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_lanelet2_extension autoware_pcl_extensions autoware_point_types @@ -51,7 +52,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp index 5021a3551c939..e05d5f789f680 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp @@ -68,7 +68,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options std::string(this->get_namespace()) + ": dust_validation", this, &BlockageDiagComponent::dustChecker); - ground_dust_ratio_pub_ = create_publisher( + ground_dust_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_dust_ratio", rclcpp::SensorDataQoS()); if (publish_debug_image_) { single_frame_dust_mask_pub = @@ -86,9 +86,9 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image"); } - ground_blockage_ratio_pub_ = create_publisher( + ground_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); - sky_blockage_ratio_pub_ = create_publisher( + sky_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/sky_blockage_ratio", rclcpp::SensorDataQoS()); using std::placeholders::_1; set_param_res_ = this->add_on_set_parameters_callback( @@ -315,7 +315,7 @@ void BlockageDiagComponent::filter( cv::Mat ground_mask(cv::Size(ideal_horizontal_bins, horizontal_ring_id_), CV_8UC1); cv::vconcat(sky_blank, single_dust_ground_img, single_dust_img); - tier4_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg; + autoware_internal_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg; ground_dust_ratio_ = static_cast(cv::countNonZero(single_dust_ground_img)) / (single_dust_ground_img.cols * single_dust_ground_img.rows); ground_dust_ratio_msg.data = ground_dust_ratio_; @@ -386,12 +386,12 @@ void BlockageDiagComponent::filter( } } - tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; + autoware_internal_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; ground_blockage_ratio_msg.data = ground_blockage_ratio_; ground_blockage_ratio_msg.stamp = now(); ground_blockage_ratio_pub_->publish(ground_blockage_ratio_msg); - tier4_debug_msgs::msg::Float32Stamped sky_blockage_ratio_msg; + autoware_internal_debug_msgs::msg::Float32Stamped sky_blockage_ratio_msg; sky_blockage_ratio_msg.data = sky_blockage_ratio_; sky_blockage_ratio_msg.stamp = now(); sky_blockage_ratio_pub_->publish(sky_blockage_ratio_msg); diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index ba29389b88bf7..ba5e2eb51997a 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -271,7 +271,7 @@ void PointCloudConcatenationComponent::publish() std::chrono::nanoseconds( (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); } } @@ -297,9 +297,9 @@ void PointCloudConcatenationComponent::publish() if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp index ecbc5b8fd13ef..54c940d414db8 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp @@ -194,9 +194,9 @@ void CropBoxFilterComponent::faster_filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); auto pipeline_latency_ms = @@ -204,7 +204,7 @@ void CropBoxFilterComponent::faster_filter( std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index 9eaafeae9dbc7..cc05a6cc2765c 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -123,7 +123,7 @@ void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr po std::chrono::nanoseconds( (this->get_clock()->now() - pointcloud_msg->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } @@ -133,9 +133,9 @@ void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr po if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp index 3d2bc0a206c94..0c467234d6591 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp @@ -142,9 +142,9 @@ void PickupBasedVoxelGridDownsampleFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); auto pipeline_latency_ms = @@ -152,7 +152,7 @@ void PickupBasedVoxelGridDownsampleFilterComponent::filter( std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp index a8024e02de2c1..bac3a59e09528 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp @@ -65,7 +65,7 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( image_pub_ = image_transport::create_publisher(this, "dual_return_outlier_filter/debug/frequency_image"); - visibility_pub_ = create_publisher( + visibility_pub_ = create_publisher( "dual_return_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); { rclcpp::PublisherOptions pub_options; @@ -322,7 +322,7 @@ void DualReturnOutlierFilterComponent::filter( frequency_image_msg->header = input->header; // Publish histogram image image_pub_.publish(frequency_image_msg); - tier4_debug_msgs::msg::Float32Stamped visibility_msg; + autoware_internal_debug_msgs::msg::Float32Stamped visibility_msg; visibility_msg.data = (1.0f - filled); visibility_msg.stamp = now(); visibility_pub_->publish(visibility_msg); diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp index fae9043143ba4..fcea299ba5aff 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp @@ -41,7 +41,7 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions outlier_pointcloud_publisher_ = this->create_publisher("debug/ring_outlier_filter", 1, pub_options); } - visibility_pub_ = create_publisher( + visibility_pub_ = create_publisher( "ring_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); @@ -262,7 +262,7 @@ void RingOutlierFilterComponent::faster_filter( outlier.header = input->header; outlier_pointcloud_publisher_->publish(outlier); - tier4_debug_msgs::msg::Float32Stamped visibility_msg; + autoware_internal_debug_msgs::msg::Float32Stamped visibility_msg; visibility_msg.data = calculateVisibilityScore(outlier); visibility_msg.stamp = input->header.stamp; visibility_pub_->publish(visibility_msg); @@ -272,9 +272,9 @@ void RingOutlierFilterComponent::faster_filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); auto pipeline_latency_ms = @@ -282,7 +282,7 @@ void RingOutlierFilterComponent::faster_filter( std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp index 42170fd88ee36..e5bd4a955590f 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp @@ -378,7 +378,7 @@ void PointCloudDataSynchronizerComponent::publish() std::chrono::nanoseconds( (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); } auto output = std::make_unique(*e.second); @@ -400,9 +400,9 @@ void PointCloudDataSynchronizerComponent::publish() if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/simulator/learning_based_vehicle_model/src/interconnected_model.cpp b/simulator/learning_based_vehicle_model/src/interconnected_model.cpp index d2fef15aa4e88..e495aceb8da9c 100644 --- a/simulator/learning_based_vehicle_model/src/interconnected_model.cpp +++ b/simulator/learning_based_vehicle_model/src/interconnected_model.cpp @@ -79,8 +79,7 @@ void InterconnectedModel::addSubmodel( std::tuple submodel_desc) { const auto [lib_path, param_path, class_name] = submodel_desc; - auto new_model = new SimplePyModel(lib_path, param_path, class_name); - submodels.push_back(std::unique_ptr(new_model)); + submodels.emplace_back(std::make_unique(lib_path, param_path, class_name)); } void InterconnectedModel::initState(std::vector new_state) diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index fdc782182f9a5..5bd7a2f74eacc 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -10,6 +10,7 @@ Mamoru Sobue Zulfaqar Azmi Temkei Kem + Kotaro Yoshimoto Apache License 2.0 ament_cmake_auto diff --git a/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp b/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp index 485d85c926268..4760c9366da46 100644 --- a/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp +++ b/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp @@ -24,7 +24,6 @@ using namespace std::literals; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::nanoseconds; -using std::placeholders::_1; namespace dummy_infrastructure { @@ -84,6 +83,7 @@ boost::optional findCommand( DummyInfrastructureNode::DummyInfrastructureNode(const rclcpp::NodeOptions & node_options) : Node("dummy_infrastructure", node_options) { + using std::placeholders::_1; // Parameter Server set_param_res_ = this->add_on_set_parameters_callback(std::bind(&DummyInfrastructureNode::onSetParam, this, _1)); diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 4022bdaadebef..b5f4bbaeefc46 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -115,12 +115,7 @@ void MrmHandler::publishHazardCmd() HazardLightsCommand msg; msg.stamp = this->now(); - if (is_emergency_holding_) { - // turn hazard on during emergency holding - msg.command = HazardLightsCommand::ENABLE; - } else if (isEmergency() && param_.turning_hazard_on.emergency) { - // turn hazard on if vehicle is in emergency state and - // turning hazard on if emergency flag is true + if (param_.turning_hazard_on.emergency && isEmergency()) { msg.command = HazardLightsCommand::ENABLE; } else { msg.command = HazardLightsCommand::NO_COMMAND;