diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index f9983f0fccc3f..9de27f82ffcef 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -56,7 +56,7 @@ common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.j common/traffic_light_utils/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp yusuke.muramatsu@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp +control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp control/external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -66,6 +66,7 @@ control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/pid_longitudinal_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/predicted_path_checker/** berkay@leodrive.ai control/pure_pursuit/** takamasa.horibe@tier4.jp control/shift_decider/** takamasa.horibe@tier4.jp control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -140,6 +141,7 @@ perception/simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4. perception/tensorrt_classifier/** mingyu.li@tier4.jp perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp +perception/tracking_object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp perception/traffic_light_classifier/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/traffic_light_fine_detector/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp @@ -181,7 +183,6 @@ planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp -planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/sampling_based_planner/bezier_sampler/** maxime.clement@tier4.jp @@ -215,6 +216,7 @@ system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp kahhoo system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/dummy_diag_publisher/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp +system/duplicated_node_checker/** shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp system/emergency_handler/** makoto.kurihara@tier4.jp system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp diff --git a/common/kalman_filter/CMakeLists.txt b/common/kalman_filter/CMakeLists.txt index dd59da727605d..6cbdf67a0affa 100644 --- a/common/kalman_filter/CMakeLists.txt +++ b/common/kalman_filter/CMakeLists.txt @@ -19,4 +19,11 @@ ament_auto_add_library(kalman_filter SHARED include/kalman_filter/time_delay_kalman_filter.hpp ) +if(BUILD_TESTING) + file(GLOB_RECURSE test_files test/*.cpp) + ament_add_ros_isolated_gtest(test_kalman_filter ${test_files}) + + target_link_libraries(test_kalman_filter kalman_filter) +endif() + ament_auto_package() diff --git a/common/kalman_filter/package.xml b/common/kalman_filter/package.xml index ea061f3f23bb8..200440b5774c7 100644 --- a/common/kalman_filter/package.xml +++ b/common/kalman_filter/package.xml @@ -18,7 +18,9 @@ eigen3_cmake_module ament_cmake_cppcheck + ament_cmake_ros ament_lint_auto + autoware_lint_common ament_cmake diff --git a/common/kalman_filter/test/test_kalman_filter.cpp b/common/kalman_filter/test/test_kalman_filter.cpp new file mode 100644 index 0000000000000..4f4e9ce44669b --- /dev/null +++ b/common/kalman_filter/test/test_kalman_filter.cpp @@ -0,0 +1,94 @@ +// 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 "kalman_filter/kalman_filter.hpp" + +#include + +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/kalman_filter/test/test_time_delay_kalman_filter.cpp b/common/kalman_filter/test/test_time_delay_kalman_filter.cpp new file mode 100644 index 0000000000000..32fefd8ceff70 --- /dev/null +++ b/common/kalman_filter/test/test_time_delay_kalman_filter.cpp @@ -0,0 +1,121 @@ +// 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 "kalman_filter/time_delay_kalman_filter.hpp" + +#include + +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/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index dcdefe61e4000..5d773c5be32d9 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -177,8 +177,7 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) for (size_t i = start_idx + 1; i < points.size(); ++i) { const auto prev_p = tier4_autoware_utils::getPoint(dst.back()); const auto curr_p = tier4_autoware_utils::getPoint(points.at(i)); - const double dist = tier4_autoware_utils::calcDistance2d(prev_p, curr_p); - if (dist < eps) { + if (std::abs(prev_p.x - curr_p.x) < eps && std::abs(prev_p.y - curr_p.y) < eps) { continue; } dst.push_back(points.at(i)); diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 17ce7e1f4de5a..6106e3a15f4c7 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -261,7 +261,7 @@ inline void setOrientation(const geometry_msgs::msg::Quaternion & orientation, T } template -void setLongitudinalVelocity([[maybe_unused]] const double velocity, [[maybe_unused]] T & p) +void setLongitudinalVelocity([[maybe_unused]] const float velocity, [[maybe_unused]] T & p) { static_assert(sizeof(T) == 0, "Only specializations of getLongitudinalVelocity can be used."); throw std::logic_error("Only specializations of getLongitudinalVelocity can be used."); @@ -269,21 +269,21 @@ void setLongitudinalVelocity([[maybe_unused]] const double velocity, [[maybe_unu template <> inline void setLongitudinalVelocity( - const double velocity, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) + const float velocity, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) { p.longitudinal_velocity_mps = velocity; } template <> inline void setLongitudinalVelocity( - const double velocity, autoware_auto_planning_msgs::msg::PathPoint & p) + const float velocity, autoware_auto_planning_msgs::msg::PathPoint & p) { p.longitudinal_velocity_mps = velocity; } template <> inline void setLongitudinalVelocity( - const double velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) + const float velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) { p.point.longitudinal_velocity_mps = velocity; } diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml index da5cc757682c5..97c3104242026 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -14,6 +14,10 @@ behavior_path_planner_avoidance: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance +behavior_path_planner_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change + behavior_velocity_planner: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner diff --git a/common/tier4_system_rviz_plugin/README.md b/common/tier4_system_rviz_plugin/README.md index 098844c8b4091..61260ecfda723 100644 --- a/common/tier4_system_rviz_plugin/README.md +++ b/common/tier4_system_rviz_plugin/README.md @@ -1 +1,11 @@ # tier4_system_rviz_plugin + +## Purpose + +This plugin display the Hazard information from Autoware; and output notices when emergencies are from initial localization and route setting. + +## Input + +| Name | Type | Description | +| --------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------ | +| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | The topic represents the emergency information from Autoware | diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp index b80f06f645632..61c2bd378dab1 100644 --- a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp @@ -50,6 +50,7 @@ #include #include +#include #include #include @@ -126,6 +127,7 @@ MrmSummaryOverlayDisplay::~MrmSummaryOverlayDisplay() void MrmSummaryOverlayDisplay::onInitialize() { RTDClass::onInitialize(); + static int count = 0; rviz_common::UniformStringStream ss; ss << "MrmSummaryOverlayDisplayObject" << count++; @@ -160,9 +162,11 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) // MRM summary std::vector mrm_comfortable_stop_summary_list = {}; std::vector mrm_emergency_stop_summary_list = {}; + int hazard_level = autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT; { std::lock_guard message_lock(mutex_); if (last_msg_ptr_) { + hazard_level = last_msg_ptr_->status.level; for (const auto & diag_status : last_msg_ptr_->status.diag_latent_fault) { const std::optional msg = generateMrmMessage(diag_status); if (msg != std::nullopt) { @@ -201,16 +205,32 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) painter.setFont(font); std::ostringstream output_text; - output_text << std::fixed - << "Comfortable Stop MRM Summary: " << int(mrm_comfortable_stop_summary_list.size()) - << std::endl; - for (const auto & mrm_element : mrm_comfortable_stop_summary_list) { - output_text << mrm_element << std::endl; - } - output_text << "Emergency Stop MRM Summary: " << int(mrm_emergency_stop_summary_list.size()) - << std::endl; - for (const auto & mrm_element : mrm_emergency_stop_summary_list) { - output_text << mrm_element << std::endl; + + // Display the MRM Summary only when there is a fault + if (hazard_level != autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT) { + // Broadcasting the Basic Error Infos + int number_of_comfortable_stop_messages = + static_cast(mrm_comfortable_stop_summary_list.size()); + if (number_of_comfortable_stop_messages > 0) // Only Display when there are errors + { + output_text << std::fixed + << "Comfortable Stop MRM Summary: " << number_of_comfortable_stop_messages + << std::endl; + for (const auto & mrm_element : mrm_comfortable_stop_summary_list) { + output_text << mrm_element << std::endl; + } + } + + int number_of_emergency_stop_messages = + static_cast(mrm_emergency_stop_summary_list.size()); + if (number_of_emergency_stop_messages > 0) // Only Display when there are some errors + { + output_text << "Emergency Stop MRM Summary: " << int(mrm_emergency_stop_summary_list.size()) + << std::endl; + for (const auto & mrm_element : mrm_emergency_stop_summary_list) { + output_text << mrm_element << std::endl; + } + } } // same as above, but align on right side diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index 4f2ad6c50a9b3..5dc65cb243bfc 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -6,6 +6,8 @@ Autonomous Emergency Braking package as a ROS 2 node Yutaka Shimizu Takamasa Horibe + Tomoya Kimura + Apache License 2.0 ament_cmake diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index e68dff6b0ec1d..0ccc9bc4a3dae 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -432,6 +432,10 @@ void AEB::generateEgoPath( const Trajectory & predicted_traj, Path & path, std::vector & polygons) { + if (predicted_traj.points.empty()) { + return; + } + geometry_msgs::msg::TransformStamped transform_stamped{}; try { transform_stamped = tf_buffer_.lookupTransform( diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index 7579e3da265c4..50e9d44a0a7a4 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -67,7 +67,7 @@ Set the following from the [controller_node](../trajectory_follower_node/README. - `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry -- `autoware_auto_vehicle_msgs/SteeringReport` current steering +- `autoware_auto_vehicle_msgs/SteeringReport`: current steering ### Outputs @@ -89,83 +89,108 @@ can be calculated by providing the current steer, velocity, and pose to function The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the AutonomouStuff Lexus RX 450h for under 40 km/h driving. -| Name | Type | Description | Default value | -| :------------------------------------------- | :----- | :--------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| show_debug_info | bool | display debug info | false | -| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | -| enable_path_smoothing | bool | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | true | -| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 35 | -| path_smoothing_times | int | number of times of applying path smoothing filter | 1 | -| curvature_smoothing_num_ref_steer | double | index distance of points used in curvature calculation for reference steering command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 35 | -| curvature_smoothing_num_traj | double | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 1 | -| extend_trajectory_for_end_yaw_control | bool | trajectory extending flag for end yaw control. | true | -| steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 | -| admissible_position_error | double | stop vehicle when following position error is larger than this value [m]. | 5.0 | -| admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad]. | 1.57 | -| stop_state_entry_ego_speed \*1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.0 | -| stop_state_entry_target_speed \*1 | double | threshold value of the target speed used to the stop state entry condition | 0.0 | -| converged_steer_rad | double | threshold value of the steer convergence | 0.1 | -| keep_steer_control_until_converged | bool | keep steer control until steer is converged | true | -| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | -| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | -| mpc_converged_threshold_rps | double | threshold value to be sure output of the optimization is converged, it is used in stopped state | 0.3 | - -(\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state. - -#### MPC algorithm - -| Name | Type | Description | Default value | -| :-------------------------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------- | -| qp_solver_type | string | QP solver option. described below in detail. | unconstraint_fast | -| mpc_vehicle_model_type | string | vehicle model option. described below in detail. | kinematics | -| mpc_prediction_horizon | int | total prediction step for MPC | 70 | -| mpc_prediction_sampling_time | double | prediction period for one step [s] | 0.1 | -| mpc_weight_lat_error | double | weight for lateral error | 0.1 | -| mpc_weight_heading_error | double | weight for heading error | 0.0 | -| mpc_weight_heading_error_squared_vel_coeff | double | weight for heading error \* velocity | 5.0 | -| mpc_weight_steering_input | double | weight for steering error (steer command - reference steer) | 1.0 | -| mpc_weight_steering_input_squared_vel_coeff | double | weight for steering error (steer command - reference steer) \* velocity | 0.25 | -| mpc_weight_lat_jerk | double | weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.0 | -| mpc_weight_terminal_lat_error | double | terminal cost weight for lateral error | 1.0 | -| mpc_weight_steer_rate | double | weight for steering rate [rad/s] | 0.0 | -| mpc_weight_steer_acc | double | weight for derivatives of the steering rate [rad/ss] | 0.0 | -| mpc_weight_terminal_heading_error | double | terminal cost weight for heading error | 0.1 | -| mpc_low_curvature_thresh_curvature | double | trajectory curvature threshold to change the weight. If the curvature is lower than this value, the `low_curvature_weight_**` values will be used. | 0.0 | -| mpc_low_curvature_weight_lat_error | double | [used in a low curvature trajectory] weight for lateral error | 0.0 | -| mpc_low_curvature_weight_heading_error | double | [used in a low curvature trajectory] weight for heading error | 0.0 | -| mpc_low_curvature_weight_heading_error_squared_vel | double | [used in a low curvature trajectory] weight for heading error \* velocity | 0.0 | -| mpc_low_curvature_weight_steering_input | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) | 0.0 | -| mpc_low_curvature_weight_steering_input_squared_vel | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) \* velocity | 0.0 | -| mpc_low_curvature_weight_lat_jerk | double | [used in a low curvature trajectory] weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.0 | -| mpc_low_curvature_weight_steer_rate | double | [used in a low curvature trajectory] weight for steering rate [rad/s] | 0.0 | -| mpc_low_curvature_weight_steer_acc | double | [used in a low curvature trajectory] weight for derivatives of the steering rate [rad/ss] | 0.0 | -| mpc_zero_ff_steer_deg | double | threshold of feedforward angle [deg]. feedforward angle smaller than this value is set to zero. | 2.0 | - -#### Steering offset remover - -Defined in the `steering_offset` namespace. This logic is designed as simple as possible, with minimum design parameters. - -| Name | Type | Description | Default value | -| :---------------------------------- | :----- | :----------------------------------------------------------------------------------------------- | :------------ | -| enable_auto_steering_offset_removal | bool | Estimate the steering offset and apply compensation | true | -| update_vel_threshold | double | If the velocity is smaller than this value, the data is not used for the offset estimation. | 5.56 | -| update_steer_threshold | double | If the steering angle is larger than this value, the data is not used for the offset estimation. | 0.035 | -| average_num | double | The average of this number of data is used as a steering offset. | 1000 | -| steering_offset_limit | double | The angle limit to be applied to the offset compensation. | 0.02 | - -#### Vehicle model +#### System + +| Name | Type | Description | Default value | +| :------------------------ | :------ | :-------------------------------------------------------------------------- | :------------ | +| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | +| use_steer_prediction | boolean | flag for using steer prediction (do not use steer measurement) | false | +| admissible_position_error | double | stop vehicle when following position error is larger than this value [m] | 5.0 | +| admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad] | 1.57 | + +#### Path Smoothing + +| Name | Type | Description | Default value | +| :-------------------------------- | :------ | :--------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_path_smoothing | boolean | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | false | +| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 25 | +| curvature_smoothing_num_traj | int | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 15 | +| curvature_smoothing_num_ref_steer | int | index distance of points used in curvature calculation for reference steering command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 15 | + +#### Trajectory Extending + +| Name | Type | Description | Default value | +| :------------------------------------ | :------ | :-------------------------------------------- | :------------ | +| extend_trajectory_for_end_yaw_control | boolean | trajectory extending flag for end yaw control | true | + +#### MPC Optimization + +| Name | Type | Description | Default value | +| :-------------------------------------------------- | :----- | :----------------------------------------------------------------------------------------------------------- | :------------ | +| qp_solver_type | string | QP solver option. described below in detail. | "osqp" | +| mpc_prediction_horizon | int | total prediction step for MPC | 50 | +| mpc_prediction_dt | double | prediction period for one step [s] | 0.1 | +| mpc_weight_lat_error | double | weight for lateral error | 1.0 | +| mpc_weight_heading_error | double | weight for heading error | 0.0 | +| mpc_weight_heading_error_squared_vel | double | weight for heading error \* velocity | 0.3 | +| mpc_weight_steering_input | double | weight for steering error (steer command - reference steer) | 1.0 | +| mpc_weight_steering_input_squared_vel | double | weight for steering error (steer command - reference steer) \* velocity | 0.25 | +| mpc_weight_lat_jerk | double | weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.1 | +| mpc_weight_steer_rate | double | weight for steering rate [rad/s] | 0.0 | +| mpc_weight_steer_acc | double | weight for derivatives of the steering rate [rad/ss] | 0.000001 | +| mpc_low_curvature_weight_lat_error | double | [used in a low curvature trajectory] weight for lateral error | 0.1 | +| mpc_low_curvature_weight_heading_error | double | [used in a low curvature trajectory] weight for heading error | 0.0 | +| mpc_low_curvature_weight_heading_error_squared_vel | double | [used in a low curvature trajectory] weight for heading error \* velocity | 0.3 | +| mpc_low_curvature_weight_steering_input | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) | 1.0 | +| mpc_low_curvature_weight_steering_input_squared_vel | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) \* velocity | 0.25 | +| mpc_low_curvature_weight_lat_jerk | double | [used in a low curvature trajectory] weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.0 | +| mpc_low_curvature_weight_steer_rate | double | [used in a low curvature trajectory] weight for steering rate [rad/s] | 0.0 | +| mpc_low_curvature_weight_steer_acc | double | [used in a low curvature trajectory] weight for derivatives of the steering rate [rad/ss] | 0.000001 | +| mpc_low_curvature_thresh_curvature | double | threshold of curvature to use "low_curvature" parameter | 0.0 | +| mpc_weight_terminal_lat_error | double | terminal lateral error weight in matrix Q to improve mpc stability | 1.0 | +| mpc_weight_terminal_heading_error | double | terminal heading error weight in matrix Q to improve mpc stability | 0.1 | +| mpc_zero_ff_steer_deg | double | threshold that feed-forward angle becomes zero | 0.5 | +| mpc_acceleration_limit | double | limit on the vehicle's acceleration | 2.0 | +| mpc_velocity_time_constant | double | time constant used for velocity smoothing | 0.3 | +| mpc_min_prediction_length | double | minimum prediction length | 5.0 | + +#### Vehicle Model | Name | Type | Description | Default value | | :----------------------------------- | :------- | :--------------------------------------------------------------------------------- | :------------------- | +| vehicle_model_type | string | vehicle model type for mpc prediction | "kinematics" | | input_delay | double | steering input delay time for delay compensation | 0.24 | -| vehicle_model_steer_tau | double | steering dynamics time constant | 0.3 | -| steer_rate_lim_dps_list_by_curvature | [double] | steering angle rate limit list depending on curvature [deg/s] | [10.0, 20.0, 30.0] | +| vehicle_model_steer_tau | double | steering dynamics time constant (1d approximation) [s] | 0.3 | +| steer_rate_lim_dps_list_by_curvature | [double] | steering angle rate limit list depending on curvature [deg/s] | [40.0, 50.0, 60.0] | | curvature_list_for_steer_rate_lim | [double] | curvature list for steering angle rate limit interpolation in ascending order [/m] | [0.001, 0.002, 0.01] | -| steer_rate_lim_dps_list_by_velocity | [double] | steering angle rate limit list depending on velocity [deg/s] | [40.0, 30.0, 20.0] | +| steer_rate_lim_dps_list_by_velocity | [double] | steering angle rate limit list depending on velocity [deg/s] | [60.0, 50.0, 40.0] | | velocity_list_for_steer_rate_lim | [double] | velocity list for steering angle rate limit interpolation in ascending order [m/s] | [10.0, 15.0, 20.0] | | acceleration_limit | double | acceleration limit for trajectory velocity modification [m/ss] | 2.0 | | velocity_time_constant | double | velocity dynamics time constant for trajectory velocity modification [s] | 0.3 | +#### Lowpass Filter for Noise Reduction + +| Name | Type | Description | Default value | +| :------------------------ | :----- | :------------------------------------------------------------------ | :------------ | +| steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 | +| error_deriv_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for error derivative [Hz] | 5.0 | + +#### Stop State + +| Name | Type | Description | Default value | +| :------------------------------------------- | :------ | :---------------------------------------------------------------------------------------------- | :------------ | +| stop_state_entry_ego_speed \*1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.001 | +| stop_state_entry_target_speed \*1 | double | threshold value of the target speed used to the stop state entry condition | 0.001 | +| converged_steer_rad | double | threshold value of the steer convergence | 0.1 | +| keep_steer_control_until_converged | boolean | keep steer control until steer is converged | true | +| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | +| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | +| mpc_converged_threshold_rps | double | threshold value to be sure output of the optimization is converged, it is used in stopped state | 0.01 | + +(\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state. + +#### Steer Offset + +Defined in the `steering_offset` namespace. This logic is designed as simple as possible, with minimum design parameters. + +| Name | Type | Description | Default value | +| :---------------------------------- | :------ | :----------------------------------------------------------------------------------------------- | :------------ | +| enable_auto_steering_offset_removal | boolean | Estimate the steering offset and apply compensation | true | +| update_vel_threshold | double | If the velocity is smaller than this value, the data is not used for the offset estimation | 5.56 | +| update_steer_threshold | double | If the steering angle is larger than this value, the data is not used for the offset estimation. | 0.035 | +| average_num | int | The average of this number of data is used as a steering offset. | 1000 | +| steering_offset_limit | double | The angle limit to be applied to the offset compensation. | 0.02 | + ##### For dynamics model (WIP) | Name | Type | Description | Default value | @@ -215,10 +240,8 @@ If you want to adjust the effect only in the high-speed range, you can use `weig - `weight_steering_input`: Reduce oscillation of tracking. - `weight_steering_input_squared_vel_coeff`: Reduce oscillation of tracking in high speed range. - `weight_lat_jerk`: Reduce lateral jerk. -- `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for - stability. -- `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` - for stability. +- `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for stability. +- `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` for stability. #### Other tips for tuning diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index b80874a8a2e57..a134fd775155b 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -501,9 +501,26 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged; const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; - if ( - std::fabs(current_vel) > p.stopped_state_entry_vel || - std::fabs(current_acc) > p.stopped_state_entry_acc) { + + const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel && + std::abs(current_acc) < p.stopped_state_entry_acc; + // Case where the ego slips in the opposite direction of the gear due to e.g. a slope is also + // considered as a stop + const bool is_not_running = [&]() { + if (control_data.shift == Shift::Forward) { + if (is_stopped || current_vel < 0.0) { + // NOTE: Stopped or moving backward + return true; + } + } else { + if (is_stopped || 0.0 < current_vel) { + // NOTE: Stopped or moving forward + return true; + } + } + return false; + }(); + if (!is_not_running) { m_last_running_time = std::make_shared(clock_->now()); } const bool stopped_condition = diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 67e123ea5b018..59fe3f13f1231 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -192,7 +192,7 @@ - + @@ -329,19 +329,24 @@ + + + + - + - + + @@ -356,16 +361,20 @@ + + - + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index fbe5f21c041b6..11deaad5d06cc 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -51,6 +51,10 @@ + + + + - + @@ -363,21 +367,26 @@ + + + + - + - + + - + @@ -390,29 +399,36 @@ + + - - + + + + - - + + - - - - - - - - - - + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index c844db39f4e9d..822c251ddad33 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -58,7 +58,7 @@ - + @@ -128,18 +128,23 @@ + + + + - + - + + @@ -154,16 +159,20 @@ + + - + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml index 711e6374f5786..fa343f49840ad 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml @@ -1,12 +1,13 @@ - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 77de1e5995ee0..07b53fb671732 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -3,29 +3,54 @@ + + + + + + + + - + + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index bbb7ebb2d8d25..347606d91759f 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -96,6 +96,16 @@ + + + + + + @@ -181,6 +191,14 @@ + + + + + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index aef1e45f517b1..4da6720b0af47 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -65,7 +65,16 @@ - + + + + + + + diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 016d613cfa72d..2de6a61547498 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -3,6 +3,7 @@ + @@ -84,6 +85,12 @@ + + + + + + diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt new file mode 100644 index 0000000000000..9490ffb67723b --- /dev/null +++ b/localization/localization_util/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) +project(localization_util) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(localization_util SHARED + src/util_func.cpp + src/pose_array_interpolator.cpp + src/tf2_listener_module.cpp +) + +ament_auto_package( + INSTALL_TO_SHARE +) diff --git a/localization/localization_util/README.md b/localization/localization_util/README.md new file mode 100644 index 0000000000000..a3e980464d0c6 --- /dev/null +++ b/localization/localization_util/README.md @@ -0,0 +1,5 @@ +# localization_util + +`localization_util`` is a localization utility package. + +This package does not have a node, it is just a library. diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp b/localization/localization_util/include/localization_util/matrix_type.hpp similarity index 84% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp rename to localization/localization_util/include/localization_util/matrix_type.hpp index 038ed4549eb2f..d9ec9b369127a 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp +++ b/localization/localization_util/include/localization_util/matrix_type.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__MATRIX_TYPE_HPP_ -#define NDT_SCAN_MATCHER__MATRIX_TYPE_HPP_ +#ifndef LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ +#define LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ #include using Matrix6d = Eigen::Matrix; using RowMatrixXd = Eigen::Matrix; -#endif // NDT_SCAN_MATCHER__MATRIX_TYPE_HPP_ +#endif // LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp b/localization/localization_util/include/localization_util/pose_array_interpolator.hpp similarity index 90% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp rename to localization/localization_util/include/localization_util/pose_array_interpolator.hpp index f1e7dfb3f544b..998d8465733f5 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp +++ b/localization/localization_util/include/localization_util/pose_array_interpolator.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_ -#define NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_ +#ifndef LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ +#define LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ -#include "ndt_scan_matcher/util_func.hpp" +#include "localization_util/util_func.hpp" #include @@ -59,4 +59,4 @@ class PoseArrayInterpolator const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; }; -#endif // NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_ +#endif // LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/tf2_listener_module.hpp b/localization/localization_util/include/localization_util/tf2_listener_module.hpp similarity index 88% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/tf2_listener_module.hpp rename to localization/localization_util/include/localization_util/tf2_listener_module.hpp index 159acbd75ce3d..b332f9effe0e0 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/tf2_listener_module.hpp +++ b/localization/localization_util/include/localization_util/tf2_listener_module.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__TF2_LISTENER_MODULE_HPP_ -#define NDT_SCAN_MATCHER__TF2_LISTENER_MODULE_HPP_ +#ifndef LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ +#define LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ #include @@ -40,4 +40,4 @@ class Tf2ListenerModule tf2_ros::TransformListener tf2_listener_; }; -#endif // NDT_SCAN_MATCHER__TF2_LISTENER_MODULE_HPP_ +#endif // LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp b/localization/localization_util/include/localization_util/util_func.hpp similarity index 96% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp rename to localization/localization_util/include/localization_util/util_func.hpp index 5c37cef36c422..bd9a2b9305e25 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp +++ b/localization/localization_util/include/localization_util/util_func.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ -#define NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ +#ifndef LOCALIZATION_UTIL__UTIL_FUNC_HPP_ +#define LOCALIZATION_UTIL__UTIL_FUNC_HPP_ #include #include @@ -91,4 +91,4 @@ void output_pose_with_cov_to_log( const rclcpp::Logger logger, const std::string & prefix, const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); -#endif // NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ +#endif // LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/localization_util/package.xml b/localization/localization_util/package.xml new file mode 100644 index 0000000000000..20f9b160212b5 --- /dev/null +++ b/localization/localization_util/package.xml @@ -0,0 +1,40 @@ + + + + localization_util + 0.1.0 + The localization_util package + Yamato Ando + Masahiro Sakamoto + Shintaro Sakoda + Kento Yabuuchi + Apache License 2.0 + Yamato Ando + + ament_cmake_auto + autoware_cmake + + fmt + geometry_msgs + nav_msgs + rclcpp + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + tier4_autoware_utils + tier4_debug_msgs + tier4_localization_msgs + visualization_msgs + + ament_cmake_cppcheck + ament_lint_auto + + + ament_cmake + + diff --git a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp b/localization/localization_util/src/pose_array_interpolator.cpp similarity index 98% rename from localization/ndt_scan_matcher/src/pose_array_interpolator.cpp rename to localization/localization_util/src/pose_array_interpolator.cpp index f09b71523e804..ebc904fcf8d38 100644 --- a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp +++ b/localization/localization_util/src/pose_array_interpolator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/pose_array_interpolator.hpp" +#include "localization_util/pose_array_interpolator.hpp" PoseArrayInterpolator::PoseArrayInterpolator( rclcpp::Node * node, const rclcpp::Time & target_ros_time, diff --git a/localization/ndt_scan_matcher/src/tf2_listener_module.cpp b/localization/localization_util/src/tf2_listener_module.cpp similarity index 97% rename from localization/ndt_scan_matcher/src/tf2_listener_module.cpp rename to localization/localization_util/src/tf2_listener_module.cpp index 364952a631f06..8a19ceec9f30d 100644 --- a/localization/ndt_scan_matcher/src/tf2_listener_module.cpp +++ b/localization/localization_util/src/tf2_listener_module.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/tf2_listener_module.hpp" +#include "localization_util/tf2_listener_module.hpp" #include diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/localization_util/src/util_func.cpp similarity index 99% rename from localization/ndt_scan_matcher/src/util_func.cpp rename to localization/localization_util/src/util_func.cpp index 52cb7844ab241..865cc02cff293 100644 --- a/localization/ndt_scan_matcher/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/util_func.hpp" +#include "localization_util/util_func.hpp" -#include "ndt_scan_matcher/matrix_type.hpp" +#include "localization_util/matrix_type.hpp" static std::random_device seed_gen; diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 1d5a9d5ac5320..79491a85a0a66 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -29,9 +29,6 @@ ament_auto_add_executable(ndt_scan_matcher src/debug.cpp src/ndt_scan_matcher_node.cpp src/ndt_scan_matcher_core.cpp - src/util_func.cpp - src/pose_array_interpolator.cpp - src/tf2_listener_module.cpp src/map_module.cpp src/map_update_module.cpp ) @@ -39,6 +36,13 @@ ament_auto_add_executable(ndt_scan_matcher link_directories(${PCL_LIBRARY_DIRS}) target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES}) +if(BUILD_TESTING) + add_ros_test( + test/test_ndt_scan_matcher_launch.py + TIMEOUT "30" + ) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index a1b9339b9780a..e210d3a28796f 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -38,6 +38,7 @@ One optional function is regularization. Please see the regularization chapter i | `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | | `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on de-grounded LiDAR scan | | `iteration_num` | `tier4_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | +| `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | | `initial_to_result_distance` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | | `initial_to_result_distance_old` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m] | | `initial_to_result_distance_new` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the newer of the two initial points used in linear interpolation and the convergence point [m] | @@ -68,6 +69,7 @@ One optional function is regularization. Please see the regularization chapter i | `converged_param_transform_probability` | double | TP threshold for deciding whether to trust the estimation result (when converged_param_type = 0) | | `converged_param_nearest_voxel_transformation_likelihood` | double | NVTL threshold for deciding whether to trust the estimation result (when converged_param_type = 1) | | `initial_estimate_particles_num` | int | The number of particles to estimate initial pose | +| `n_startup_trials` | int | The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). | | `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud | | `initial_pose_timeout_sec` | int | Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] | | `initial_pose_distance_tolerance_m` | double | Tolerance of distance difference between two initial poses used for linear interpolation. [m] | diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index cf41a4cf55d0b..0ba1b1a2e15f4 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -43,6 +43,11 @@ # The number of particles to estimate initial pose initial_estimate_particles_num: 200 + # The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). + # This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. + # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search. + n_startup_trials: 20 + # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] lidar_topic_timeout_sec: 1.0 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 4f630bb8c5898..13721c03ca22e 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -15,10 +15,10 @@ #ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#include "localization_util/tf2_listener_module.hpp" +#include "localization_util/util_func.hpp" #include "ndt_scan_matcher/debug.hpp" #include "ndt_scan_matcher/particle.hpp" -#include "ndt_scan_matcher/tf2_listener_module.hpp" -#include "ndt_scan_matcher/util_func.hpp" #include #include diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 011cda6ba3356..e5f46b5022b51 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -17,9 +17,9 @@ #define FMT_HEADER_ONLY +#include "localization_util/tf2_listener_module.hpp" #include "ndt_scan_matcher/map_module.hpp" #include "ndt_scan_matcher/map_update_module.hpp" -#include "ndt_scan_matcher/tf2_listener_module.hpp" #include @@ -92,8 +92,7 @@ class NDTScanMatcher : public rclcpp::Node void callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr); - geometry_msgs::msg::PoseWithCovarianceStamped align_using_monte_carlo( - const std::shared_ptr & ndt_ptr, + geometry_msgs::msg::PoseWithCovarianceStamped align_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); void transform_sensor_measurement( @@ -111,7 +110,7 @@ class NDTScanMatcher : public rclcpp::Node const pcl::shared_ptr> & sensor_points_in_map_ptr); void publish_marker( const rclcpp::Time & sensor_ros_time, const std::vector & pose_array); - void publish_initial_to_result_distances( + void publish_initial_to_result( const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_cov_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_old_msg, @@ -150,6 +149,8 @@ class NDTScanMatcher : public rclcpp::Node rclcpp::Publisher::SharedPtr no_ground_nearest_voxel_transformation_likelihood_pub_; rclcpp::Publisher::SharedPtr iteration_num_pub_; + rclcpp::Publisher::SharedPtr + initial_to_result_relative_pose_pub_; rclcpp::Publisher::SharedPtr initial_to_result_distance_pub_; rclcpp::Publisher::SharedPtr @@ -179,6 +180,7 @@ class NDTScanMatcher : public rclcpp::Node double converged_param_nearest_voxel_transformation_likelihood_; int initial_estimate_particles_num_; + int n_startup_trials_; double lidar_topic_timeout_sec_; double initial_pose_timeout_sec_; double initial_pose_distance_tolerance_m_; diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 92c690a708492..a9083c4ae0ae4 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -19,6 +19,7 @@ fmt geometry_msgs libpcl-all-dev + localization_util nav_msgs ndt_omp pcl_conversions @@ -34,10 +35,12 @@ tier4_autoware_utils tier4_debug_msgs tier4_localization_msgs + tree_structured_parzen_estimator visualization_msgs ament_cmake_cppcheck ament_lint_auto + ros_testing ament_cmake diff --git a/localization/ndt_scan_matcher/src/debug.cpp b/localization/ndt_scan_matcher/src/debug.cpp index 9c82e06d89b80..941f682de5803 100644 --- a/localization/ndt_scan_matcher/src/debug.cpp +++ b/localization/ndt_scan_matcher/src/debug.cpp @@ -14,7 +14,7 @@ #include "ndt_scan_matcher/debug.hpp" -#include "ndt_scan_matcher/util_func.hpp" +#include "localization_util/util_func.hpp" visualization_msgs::msg::MarkerArray make_debug_markers( const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_, diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 9c94467dc94c0..7512ae2e77a21 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -55,12 +55,6 @@ MapUpdateModule::MapUpdateModule( pcd_loader_client_ = node->create_client("pcd_loader_service"); - while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - RCLCPP_INFO( - logger_, - "Waiting for pcd loader service. Check if the enable_differential_load in " - "pointcloud_map_loader is set `true`."); - } double map_update_dt = 1.0; auto period_ns = std::chrono::duration_cast( @@ -117,7 +111,14 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) request->area.radius = static_cast(dynamic_map_loading_map_radius_); request->cached_ids = ndt_ptr_->getCurrentMapIDs(); - // // send a request to map_loader + while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_INFO( + logger_, + "Waiting for pcd loader service. Check if the enable_differential_load in " + "pointcloud_map_loader is set `true`."); + } + + // send a request to map_loader auto result{pcd_loader_client_->async_send_request( request, [](rclcpp::Client::SharedFuture) {})}; diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index d9054d77d2256..b38e3f0826184 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -14,14 +14,17 @@ #include "ndt_scan_matcher/ndt_scan_matcher_core.hpp" -#include "ndt_scan_matcher/matrix_type.hpp" +#include "localization_util/matrix_type.hpp" +#include "localization_util/pose_array_interpolator.hpp" +#include "localization_util/util_func.hpp" #include "ndt_scan_matcher/particle.hpp" -#include "ndt_scan_matcher/pose_array_interpolator.hpp" -#include "ndt_scan_matcher/util_func.hpp" +#include "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" #include #include +#include + #include #ifdef ROS_DISTRO_GALACTIC @@ -145,6 +148,7 @@ NDTScanMatcher::NDTScanMatcher() } initial_estimate_particles_num_ = this->declare_parameter("initial_estimate_particles_num"); + n_startup_trials_ = this->declare_parameter("n_startup_trials"); estimate_scores_for_degrounded_scan_ = this->declare_parameter("estimate_scores_for_degrounded_scan"); @@ -201,6 +205,8 @@ NDTScanMatcher::NDTScanMatcher() "no_ground_nearest_voxel_transformation_likelihood", 10); iteration_num_pub_ = this->create_publisher("iteration_num", 10); + initial_to_result_relative_pose_pub_ = + this->create_publisher("initial_to_result_relative_pose", 10); initial_to_result_distance_pub_ = this->create_publisher("initial_to_result_distance", 10); initial_to_result_distance_old_pub_ = @@ -442,20 +448,8 @@ void NDTScanMatcher::callback_sensor_points( } // perform several validations - /***************************************************************************** - The reason the add 2 to the ndt_ptr_->getMaximumIterations() is that there are bugs in - implementation of ndt. - 1. gradient descent method ends when the iteration is greater than max_iteration if it does not - converge (be careful it's 'greater than' instead of 'greater equal than'.) - https://github.com/tier4/autoware.iv/blob/2323e5baa0b680d43a9219f5fb3b7a11dd9edc82/localization/pose_estimator/ndt_scan_matcher/ndt_omp/include/ndt_omp/ndt_omp_impl.hpp#L212 - 2. iterate iteration count when end of gradient descent function. - https://github.com/tier4/autoware.iv/blob/2323e5baa0b680d43a9219f5fb3b7a11dd9edc82/localization/pose_estimator/ndt_scan_matcher/ndt_omp/include/ndt_omp/ndt_omp_impl.hpp#L217 - - These bugs are now resolved in original pcl implementation. - https://github.com/PointCloudLibrary/pcl/blob/424c1c6a0ca97d94ca63e5daff4b183a4db8aae4/registration/include/pcl/registration/impl/ndt.hpp#L73-L180 - *****************************************************************************/ bool is_ok_iteration_num = - validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations() + 2); + validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations()); bool is_local_optimal_solution_oscillation = false; if (!is_ok_iteration_num) { is_local_optimal_solution_oscillation = validate_local_optimal_solution_oscillation( @@ -483,7 +477,7 @@ void NDTScanMatcher::callback_sensor_points( publish_tf(sensor_ros_time, result_pose_msg); publish_pose(sensor_ros_time, result_pose_msg, is_converged); publish_marker(sensor_ros_time, transformation_msg_array); - publish_initial_to_result_distances( + publish_initial_to_result( sensor_ros_time, result_pose_msg, interpolator.get_current_pose(), interpolator.get_old_pose(), interpolator.get_new_pose()); @@ -624,12 +618,19 @@ void NDTScanMatcher::publish_marker( ndt_marker_pub_->publish(marker_array); } -void NDTScanMatcher::publish_initial_to_result_distances( +void NDTScanMatcher::publish_initial_to_result( const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_cov_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_old_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_new_msg) { + geometry_msgs::msg::PoseStamped initial_to_result_relative_pose_stamped; + initial_to_result_relative_pose_stamped.pose = + tier4_autoware_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); + initial_to_result_relative_pose_stamped.header.stamp = sensor_ros_time; + initial_to_result_relative_pose_stamped.header.frame_id = map_frame_; + initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped); + const auto initial_to_result_distance = static_cast(norm(initial_pose_cov_msg.pose.pose.position, result_pose_msg.position)); initial_to_result_distance_pub_->publish( @@ -751,51 +752,95 @@ void NDTScanMatcher::service_ndt_align( map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); } + // mutex Map + std::lock_guard lock(ndt_ptr_mtx_); + if (ndt_ptr_->getInputTarget() == nullptr) { res->success = false; - RCLCPP_WARN(get_logger(), "No InputTarget"); + RCLCPP_WARN( + get_logger(), "No InputTarget. Please check the map file and the map_loader service"); return; } if (ndt_ptr_->getInputSource() == nullptr) { res->success = false; - RCLCPP_WARN(get_logger(), "No InputSource"); + RCLCPP_WARN(get_logger(), "No InputSource. Please check the input lidar topic"); return; } - // mutex Map - std::lock_guard lock(ndt_ptr_mtx_); - (*state_ptr_)["state"] = "Aligning"; - res->pose_with_covariance = align_using_monte_carlo(ndt_ptr_, initial_pose_msg_in_map_frame); + res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame); (*state_ptr_)["state"] = "Sleeping"; res->success = true; res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; } -geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_carlo( - const std::shared_ptr & ndt_ptr, +geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) { - if (ndt_ptr->getInputTarget() == nullptr || ndt_ptr->getInputSource() == nullptr) { - RCLCPP_WARN(get_logger(), "No Map or Sensor PointCloud"); - return geometry_msgs::msg::PoseWithCovarianceStamped(); - } - - output_pose_with_cov_to_log(get_logger(), "align_using_monte_carlo_input", initial_pose_with_cov); - - // generateParticle - const auto initial_poses = - create_random_pose_array(initial_pose_with_cov, initial_estimate_particles_num_); + output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov); + + const auto base_rpy = get_rpy(initial_pose_with_cov); + const Eigen::Map covariance = { + initial_pose_with_cov.pose.covariance.data(), 6, 6}; + const double stddev_x = std::sqrt(covariance(0, 0)); + const double stddev_y = std::sqrt(covariance(1, 1)); + const double stddev_z = std::sqrt(covariance(2, 2)); + const double stddev_roll = std::sqrt(covariance(3, 3)); + const double stddev_pitch = std::sqrt(covariance(4, 4)); + + // Let phi be the cumulative distribution function of the standard normal distribution. + // It has the following relationship with the error function (erf). + // phi(x) = 1/2 (1 + erf(x / sqrt(2))) + // so, 2 * phi(x) - 1 = erf(x / sqrt(2)). + // The range taken by 2 * phi(x) - 1 is [-1, 1], so it can be used as a uniform distribution in + // TPE. Let u = 2 * phi(x) - 1, then x = sqrt(2) * erf_inv(u). Computationally, it is not a good + // to give erf_inv -1 and 1, so it is rounded off at (-1 + eps, 1 - eps). + const double SQRT2 = std::sqrt(2); + auto uniform_to_normal = [&SQRT2](const double uniform) { + assert(-1.0 <= uniform && uniform <= 1.0); + constexpr double epsilon = 1.0e-6; + const double clamped = std::clamp(uniform, -1.0 + epsilon, 1.0 - epsilon); + return boost::math::erf_inv(clamped) * SQRT2; + }; + + auto normal_to_uniform = [&SQRT2](const double normal) { + return boost::math::erf(normal / SQRT2); + }; + + // Optimizing (x, y, z, roll, pitch, yaw) 6 dimensions. + // The last dimension (yaw) is a loop variable. + // Although roll and pitch are also angles, they are considered non-looping variables that follow + // a normal distribution with a small standard deviation. This assumes that the initial pose of + // the ego vehicle is aligned with the ground to some extent about roll and pitch. + const std::vector is_loop_variable = {false, false, false, false, false, true}; + TreeStructuredParzenEstimator tpe( + TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials_, is_loop_variable); std::vector particle_array; auto output_cloud = std::make_shared>(); - for (unsigned int i = 0; i < initial_poses.size(); i++) { - const auto & initial_pose = initial_poses[i]; + for (int i = 0; i < initial_estimate_particles_num_; i++) { + const TreeStructuredParzenEstimator::Input input = tpe.get_next_input(); + + geometry_msgs::msg::Pose initial_pose; + initial_pose.position.x = + initial_pose_with_cov.pose.pose.position.x + uniform_to_normal(input[0]) * stddev_x; + initial_pose.position.y = + initial_pose_with_cov.pose.pose.position.y + uniform_to_normal(input[1]) * stddev_y; + initial_pose.position.z = + initial_pose_with_cov.pose.pose.position.z + uniform_to_normal(input[2]) * stddev_z; + geometry_msgs::msg::Vector3 init_rpy; + init_rpy.x = base_rpy.x + uniform_to_normal(input[3]) * stddev_roll; + init_rpy.y = base_rpy.y + uniform_to_normal(input[4]) * stddev_pitch; + init_rpy.z = base_rpy.z + input[5] * M_PI; + tf2::Quaternion tf_quaternion; + tf_quaternion.setRPY(init_rpy.x, init_rpy.y, init_rpy.z); + initial_pose.orientation = tf2::toMsg(tf_quaternion); + const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose); - ndt_ptr->align(*output_cloud, initial_pose_matrix); - const pclomp::NdtResult ndt_result = ndt_ptr->getResult(); + ndt_ptr_->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); Particle particle( initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, @@ -806,9 +851,30 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ particle, i); ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); + const geometry_msgs::msg::Pose pose = matrix4f_to_pose(ndt_result.pose); + const geometry_msgs::msg::Vector3 rpy = get_rpy(pose); + + const double diff_x = pose.position.x - initial_pose_with_cov.pose.pose.position.x; + const double diff_y = pose.position.y - initial_pose_with_cov.pose.pose.position.y; + const double diff_z = pose.position.z - initial_pose_with_cov.pose.pose.position.z; + const double diff_roll = rpy.x - base_rpy.x; + const double diff_pitch = rpy.y - base_rpy.y; + const double diff_yaw = rpy.z - base_rpy.z; + + // Only yaw is a loop_variable, so only simple normalization is performed. + // All other variables are converted from normal distribution to uniform distribution. + TreeStructuredParzenEstimator::Input result(is_loop_variable.size()); + result[0] = normal_to_uniform(diff_x / stddev_x); + result[1] = normal_to_uniform(diff_y / stddev_y); + result[2] = normal_to_uniform(diff_z / stddev_z); + result[3] = normal_to_uniform(diff_roll / stddev_roll); + result[4] = normal_to_uniform(diff_pitch / stddev_pitch); + result[5] = diff_yaw / M_PI; + tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability}); + auto sensor_points_in_map_ptr = std::make_shared>(); tier4_autoware_utils::transformPointCloud( - *ndt_ptr->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); + *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_in_map_ptr); } @@ -821,8 +887,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ result_pose_with_cov_msg.header.frame_id = map_frame_; result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; - output_pose_with_cov_to_log( - get_logger(), "align_using_monte_carlo_output", result_pose_with_cov_msg); + output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); RCLCPP_INFO_STREAM(get_logger(), "best_score," << best_particle_ptr->score); return result_pose_with_cov_msg; diff --git a/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py b/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py new file mode 100644 index 0000000000000..2dc4958c4704f --- /dev/null +++ b/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 + +# 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. + +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 +import rclpy +from std_srvs.srv import SetBool + +logger = get_logger(__name__) + + +@pytest.mark.launch_test +def generate_test_description(): + test_ndt_scan_matcher_launch_file = os.path.join( + get_package_share_directory("ndt_scan_matcher"), + "launch", + "ndt_scan_matcher.launch.xml", + ) + ndt_scan_matcher = IncludeLaunchDescription( + AnyLaunchDescriptionSource(test_ndt_scan_matcher_launch_file), + ) + + return launch.LaunchDescription( + [ + ndt_scan_matcher, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestNDTScanMatcher(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 setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + + def tearDown(self): + self.test_node.destroy_node() + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_launch(self): + # Trigger ndt_scan_matcher to activate the node + cli_trigger = self.test_node.create_client(SetBool, "/trigger_node") + while not cli_trigger.wait_for_service(timeout_sec=1.0): + continue + + request = SetBool.Request() + request.data = True + future = cli_trigger.call_async(request) + rclpy.spin_until_future_complete(self.test_node, future) + + if future.result() is not None: + self.test_node.get_logger().info("Result of bool service: %s" % future.result().message) + self.assertTrue(future.result().success, "ndt_scan_matcher is not activated") + else: + self.test_node.get_logger().error( + "Exception while calling service: %r" % future.exception() + ) + raise self.failureException("service trigger failed") + + +@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/localization/tree_structured_parzen_estimator/CMakeLists.txt b/localization/tree_structured_parzen_estimator/CMakeLists.txt new file mode 100644 index 0000000000000..7b687a12a003c --- /dev/null +++ b/localization/tree_structured_parzen_estimator/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(tree_structured_parzen_estimator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(tree_structured_parzen_estimator SHARED + src/tree_structured_parzen_estimator.cpp +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_tpe + test/test_tpe.cpp + src/tree_structured_parzen_estimator.cpp + ) + target_include_directories(test_tpe PRIVATE include) + target_link_libraries(test_tpe) +endif() + +ament_auto_package( + INSTALL_TO_SHARE +) diff --git a/localization/tree_structured_parzen_estimator/README.md b/localization/tree_structured_parzen_estimator/README.md new file mode 100644 index 0000000000000..2b21e65929485 --- /dev/null +++ b/localization/tree_structured_parzen_estimator/README.md @@ -0,0 +1,5 @@ +# tree_structured_parzen_estimator + +`tree_structured_parzen_estimator`` is a package for black-box optimization. + +This package does not have a node, it is just a library. diff --git a/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp b/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp new file mode 100644 index 0000000000000..b7b522b4e6b76 --- /dev/null +++ b/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp @@ -0,0 +1,80 @@ +// 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 TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#define TREE_STRUCTURED_PARZEN_ESTIMATOR__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 + +class TreeStructuredParzenEstimator +{ +public: + using Input = std::vector; + using Score = double; + struct Trial + { + Input input; + Score score; + }; + + enum Direction { + MINIMIZE = 0, + MAXIMIZE = 1, + }; + + TreeStructuredParzenEstimator() = delete; + TreeStructuredParzenEstimator( + const Direction direction, const int64_t n_startup_trials, std::vector is_loop_variable); + void add_trial(const Trial & trial); + Input get_next_input() const; + +private: + static constexpr double BASE_STDDEV_COEFF = 0.2; + static constexpr double MAX_GOOD_RATE = 0.10; + static constexpr double MAX_VALUE = 1.0; + static constexpr double MIN_VALUE = -1.0; + static constexpr double VALUE_WIDTH = MAX_VALUE - MIN_VALUE; + static constexpr int64_t N_EI_CANDIDATES = 100; + static constexpr double PRIOR_WEIGHT = 0.0; + + static std::mt19937_64 engine; + static std::uniform_real_distribution dist_uniform; + static std::normal_distribution dist_normal; + + double compute_log_likelihood_ratio(const Input & input) const; + double log_gaussian_pdf(const Input & input, const Input & mu, const Input & sigma) const; + static std::vector get_weights(const int64_t n); + static double normalize_loop_variable(const double value); + + std::vector trials_; + int64_t above_num_; + const Direction direction_; + const int64_t n_startup_trials_; + const int64_t input_dimension_; + const std::vector is_loop_variable_; + const Input base_stddev_; +}; + +#endif // TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/tree_structured_parzen_estimator/package.xml b/localization/tree_structured_parzen_estimator/package.xml new file mode 100644 index 0000000000000..b699d5c69e610 --- /dev/null +++ b/localization/tree_structured_parzen_estimator/package.xml @@ -0,0 +1,23 @@ + + + + tree_structured_parzen_estimator + 0.1.0 + The tree_structured_parzen_estimator package + Yamato Ando + Masahiro Sakamoto + Shintaro Sakoda + Kento Yabuuchi + Apache License 2.0 + Shintaro Sakoda + + ament_cmake_auto + autoware_cmake + + ament_cmake_cppcheck + ament_lint_auto + + + ament_cmake + + diff --git a/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp b/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp new file mode 100644 index 0000000000000..99c70a844f331 --- /dev/null +++ b/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp @@ -0,0 +1,217 @@ +// 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 "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" + +#include +#include +#include +#include + +// random number generator +std::mt19937_64 TreeStructuredParzenEstimator::engine(std::random_device{}()); +std::uniform_real_distribution TreeStructuredParzenEstimator::dist_uniform( + TreeStructuredParzenEstimator::MIN_VALUE, TreeStructuredParzenEstimator::MAX_VALUE); +std::normal_distribution TreeStructuredParzenEstimator::dist_normal(0.0, 1.0); + +TreeStructuredParzenEstimator::TreeStructuredParzenEstimator( + const Direction direction, const int64_t n_startup_trials, std::vector is_loop_variable) +: above_num_(0), + direction_(direction), + n_startup_trials_(n_startup_trials), + input_dimension_(is_loop_variable.size()), + is_loop_variable_(is_loop_variable), + base_stddev_(input_dimension_, VALUE_WIDTH) +{ +} + +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(25), static_cast(trials_.size() * MAX_GOOD_RATE)); +} + +TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const +{ + 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_); + for (int64_t j = 0; j < input_dimension_; j++) { + input[j] = dist_uniform(engine); + } + return input; + } + + Input best_input; + double best_log_likelihood_ratio = std::numeric_limits::lowest(); + const double coeff = BASE_STDDEV_COEFF * std::pow(above_num_, -1.0 / (4 + input_dimension_)); + std::vector weights = get_weights(above_num_); + weights.push_back(PRIOR_WEIGHT); + std::discrete_distribution dist(weights.begin(), weights.end()); + for (int64_t i = 0; i < N_EI_CANDIDATES; i++) { + Input mu, sigma; + const int64_t index = dist(engine); + if (index == above_num_) { + mu = Input(input_dimension_, 0.0); + sigma = base_stddev_; + } else { + mu = trials_[index].input; + sigma = base_stddev_; + for (int64_t j = 0; j < input_dimension_; j++) { + sigma[j] *= coeff; + } + } + // sample from the normal distribution + Input input(input_dimension_); + for (int64_t j = 0; j < input_dimension_; j++) { + input[j] = mu[j] + dist_normal(engine) * sigma[j]; + input[j] = + (is_loop_variable_[j] ? normalize_loop_variable(input[j]) + : std::clamp(input[j], MIN_VALUE, MAX_VALUE)); + } + 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 int64_t n = 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; + + // Scott's rule + const double coeff_above = + BASE_STDDEV_COEFF * std::pow(above_num_, -1.0 / (4 + input_dimension_)); + const double coeff_below = + BASE_STDDEV_COEFF * std::pow(n - above_num_, -1.0 / (4 + input_dimension_)); + Input sigma_above = base_stddev_; + Input sigma_below = base_stddev_; + for (int64_t j = 0; j < input_dimension_; j++) { + sigma_above[j] *= coeff_above; + sigma_below[j] *= coeff_below; + } + + std::vector above_weights = get_weights(above_num_); + std::vector below_weights = get_weights(n - above_num_); + std::reverse(below_weights.begin(), below_weights.end()); // below_weights is ascending order + + // calculate the sum of weights to normalize + double above_sum = std::accumulate(above_weights.begin(), above_weights.end(), 0.0); + double below_sum = std::accumulate(below_weights.begin(), below_weights.end(), 0.0); + + // above includes prior + above_sum += PRIOR_WEIGHT; + + for (int64_t i = 0; i < n; i++) { + if (i < above_num_) { + const double log_p = log_gaussian_pdf(input, trials_[i].input, sigma_above); + const double w = above_weights[i] / above_sum; + const double log_w = std::log(w); + above_logs.push_back(log_p + log_w); + } else { + const double log_p = log_gaussian_pdf(input, trials_[i].input, sigma_below); + const double w = below_weights[i - above_num_] / below_sum; + const double log_w = std::log(w); + below_logs.push_back(log_p + log_w); + } + } + + // prior + if (PRIOR_WEIGHT > 0.0) { + const double log_p = log_gaussian_pdf(input, Input(input_dimension_, 0.0), base_stddev_); + const double log_w = std::log(PRIOR_WEIGHT / above_sum); + above_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 = 0.0; + for (const double log_v : log_vec) { + sum += 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); + const double r = above - below; + return r; +} + +double TreeStructuredParzenEstimator::log_gaussian_pdf( + const Input & input, const Input & mu, const Input & sigma) const +{ + 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 int64_t n = input.size(); + + double result = 0.0; + for (int64_t i = 0; i < n; i++) { + double diff = input[i] - mu[i]; + if (is_loop_variable_[i]) { + diff = normalize_loop_variable(diff); + } + result += log_gaussian_pdf_1d(diff, sigma[i]); + } + return result; +} + +std::vector TreeStructuredParzenEstimator::get_weights(const int64_t n) +{ + // See optuna + // https://github.com/optuna/optuna/blob/4bfab78e98bf786f6a2ce6e593a26e3f8403e08d/optuna/samplers/_tpe/sampler.py#L50-L58 + std::vector weights; + constexpr int64_t WEIGHT_ALPHA = 25; + if (n == 0) { + return weights; + } else if (n < WEIGHT_ALPHA) { + weights.resize(n, 1.0); + } else { + weights.resize(n); + const double unit = (1.0 - 1.0 / n) / (n - WEIGHT_ALPHA); + for (int64_t i = 0; i < n; i++) { + weights[i] = (i < WEIGHT_ALPHA ? 1.0 : 1.0 - unit * (i - WEIGHT_ALPHA)); + } + } + + return weights; +} + +double TreeStructuredParzenEstimator::normalize_loop_variable(const double value) +{ + // Normalize the loop variable to [-1, 1) + double result = value; + while (result >= MAX_VALUE) { + result -= VALUE_WIDTH; + } + while (result < MIN_VALUE) { + result += VALUE_WIDTH; + } + return result; +} diff --git a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp new file mode 100644 index 0000000000000..32eb66e70fb16 --- /dev/null +++ b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp @@ -0,0 +1,65 @@ +// 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 "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" + +#include + +TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) +{ + auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { + double value = 0.0; + const int64_t n = input.size(); + for (int64_t i = 0; i < n; i++) { + const double v = input[i] * 10; + value += v * v; + } + return value; + }; + + constexpr int64_t kOuterTrialsNum = 10; + constexpr int64_t kInnerTrialsNum = 100; + std::cout << std::fixed; + std::vector mean_scores; + for (const int64_t n_startup_trials : {kInnerTrialsNum, kInnerTrialsNum / 10}) { + const std::string method = ((n_startup_trials == kInnerTrialsNum) ? "Random" : "TPE"); + + std::vector scores; + for (int64_t i = 0; i < kOuterTrialsNum; i++) { + double best_score = std::numeric_limits::lowest(); + const std::vector is_loop_variable(6, false); + TreeStructuredParzenEstimator estimator( + TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, is_loop_variable); + for (int64_t trial = 0; trial < kInnerTrialsNum; 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 / scores.size(); + mean_scores.push_back(mean); + double sq_sum = 0.0; + for (const double score : scores) { + sq_sum += (score - mean) * (score - mean); + } + const double stddev = std::sqrt(sq_sum / scores.size()); + + std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; + } + ASSERT_LT(mean_scores[0], mean_scores[1]); +} diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index 8f6ccf969c9a4..f50eba9218d67 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -19,7 +19,6 @@ pcl_conversions rclcpp sensor_msgs - tf2 tf2_geometry_msgs tf2_ros tier4_localization_msgs diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 745edd0daca99..b94eaac7ee34d 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -4,7 +4,7 @@ project(map_loader) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(PCL REQUIRED COMPONENTS io) +find_package(PCL REQUIRED COMPONENTS common io filters) ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/pointcloud_map_loader_node.cpp diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index e663d19d516ac..5230d4bd03214 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -24,13 +24,8 @@ lanelet2_extension libpcl-all-dev pcl_conversions - pcl_ros rclcpp rclcpp_components - std_msgs - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils tier4_map_msgs visualization_msgs yaml-cpp diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index 03053533d3ead..b128c2cf15e15 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -16,7 +16,6 @@ component_interface_utils lanelet2_extension rclcpp - rclcpp_components tier4_map_msgs yaml-cpp diff --git a/map/util/lanelet2_map_preprocessor/CMakeLists.txt b/map/util/lanelet2_map_preprocessor/CMakeLists.txt index caa1d27b3e292..8a6b16c05011b 100644 --- a/map/util/lanelet2_map_preprocessor/CMakeLists.txt +++ b/map/util/lanelet2_map_preprocessor/CMakeLists.txt @@ -4,12 +4,18 @@ project(lanelet2_map_preprocessor) find_package(autoware_cmake REQUIRED) autoware_package() +find_package(PCL REQUIRED COMPONENTS common io kdtree) + include_directories( include SYSTEM ${PCL_INCLUDE_DIRS} ) +link_libraries( + ${PCL_LIBRARIES} +) + ament_auto_add_executable(fix_z_value_by_pcd src/fix_z_value_by_pcd.cpp) ament_auto_add_executable(transform_maps src/transform_maps.cpp) ament_auto_add_executable(merge_close_lines src/merge_close_lines.cpp) diff --git a/map/util/lanelet2_map_preprocessor/package.xml b/map/util/lanelet2_map_preprocessor/package.xml index 15f061369e649..79fce94f87a95 100644 --- a/map/util/lanelet2_map_preprocessor/package.xml +++ b/map/util/lanelet2_map_preprocessor/package.xml @@ -11,7 +11,7 @@ autoware_cmake lanelet2_extension - pcl_ros + libpcl-all-dev rclcpp ament_lint_auto diff --git a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp index 600da31df5868..ae6162542a1c3 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp @@ -51,6 +51,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::ConstLanelets road_lanelets_; + lanelet::ConstLanelets shoulder_lanelets_; std::string lanelet_frame_id_; tf2_ros::Buffer tf_buffer_; diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 3b78ae449e2da..f9b208cca02bc 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -62,6 +62,7 @@ void ObjectLaneletFilterNode::mapCallback( lanelet::utils::conversion::fromBinMsg(*map_msg, lanelet_map_ptr_); const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); + shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); } void ObjectLaneletFilterNode::objectCallback( @@ -87,7 +88,10 @@ void ObjectLaneletFilterNode::objectCallback( // calculate convex hull const auto convex_hull = getConvexHull(transformed_objects); // get intersected lanelets - lanelet::ConstLanelets intersected_lanelets = getIntersectedLanelets(convex_hull, road_lanelets_); + lanelet::ConstLanelets intersected_road_lanelets = + getIntersectedLanelets(convex_hull, road_lanelets_); + lanelet::ConstLanelets intersected_shoulder_lanelets = + getIntersectedLanelets(convex_hull, shoulder_lanelets_); int index = 0; for (const auto & object : transformed_objects.objects) { @@ -101,7 +105,9 @@ void ObjectLaneletFilterNode::objectCallback( polygon.outer().emplace_back(point_transformed.x, point_transformed.y); } polygon.outer().push_back(polygon.outer().front()); - if (isPolygonOverlapLanelets(polygon, intersected_lanelets)) { + if (isPolygonOverlapLanelets(polygon, intersected_road_lanelets)) { + output_object_msg.objects.emplace_back(input_msg->objects.at(index)); + } else if (isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets)) { output_object_msg.objects.emplace_back(input_msg->objects.at(index)); } } else { diff --git a/perception/euclidean_cluster/README.md b/perception/euclidean_cluster/README.md index 6c8995a44c56c..b57204c84cc3b 100644 --- a/perception/euclidean_cluster/README.md +++ b/perception/euclidean_cluster/README.md @@ -10,7 +10,7 @@ This package has two clustering methods: `euclidean_cluster` and `voxel_grid_bas ### euclidean_cluster -`pcl::EuclideanClusterExtraction` is applied to points. See [official document](https://pcl.readthedocs.io/en/latest/cluster_extraction.html) for details. +`pcl::EuclideanClusterExtraction` is applied to points. See [official document](https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html) for details. ### voxel_grid_based_euclidean_cluster diff --git a/perception/ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg b/perception/ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg new file mode 100644 index 0000000000000..985b99d82ba9b --- /dev/null +++ b/perception/ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg @@ -0,0 +1,225 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
+ detection_range_z_max +
+
+
+
+ + detection_range_z_max + + + + + + + + + + + +
+
+
+
+ global_slope +
+
+
+
+ + global_slope + + + + + + + + + + + + +
+
+
+
+ local_slope +
+
+
+
+ + local_slope + + + + + + + + + + + +
+
+
+
+
split_height_distance
+
+
+
+
+ + split_height_distance + + + + + + + + + +
+
+
+
+
+
grid_size_m
+
+
+
+
+
+ + grid_size_m + + + + + + + + + + + + +
+
+
+
+ z +
+
+
+
+ + z + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/ground_segmentation/docs/scan-ground-filter.md b/perception/ground_segmentation/docs/scan-ground-filter.md index 0c07ce600f625..bc44544fa298c 100644 --- a/perception/ground_segmentation/docs/scan-ground-filter.md +++ b/perception/ground_segmentation/docs/scan-ground-filter.md @@ -29,25 +29,26 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref #### Core Parameters -| Name | Type | Default Value | Description | -| --------------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------- | -| `input_frame` | string | "base_link" | frame id of input pointcloud | -| `output_frame` | string | "base_link" | frame id of output pointcloud | -| `global_slope_max_angle_deg` | double | 8.0 | The global angle to classify as the ground or object [deg] | -| `local_slope_max_angle_deg` | double | 10.0 | The local angle to classify as the ground or object [deg] | -| `radial_divider_angle_deg` | double | 1.0 | The angle which divide the whole pointcloud to sliced group [deg] | -| `split_points_distance_tolerance` | double | 0.2 | The xy-distance threshold to to distinguishing far and near [m] | -| `split_height_distance` | double | 0.2 | The height threshold to distinguishing far and near [m] | -| `use_virtual_ground_point` | bool | true | whether to use the ground center of front wheels as the virtual ground point. | -| `detection_range_z_max` | float | 2.5 | Maximum height of detection range [m], applied only for elevation_grid_mode | -| `center_pcl_shift` | float | 0.0 | The x-axis offset of addition LiDARs from vehicle center of mass [m],
recommended to use only for additional LiDARs in elevation_grid_mode | -| `non_ground_height_threshold` | float | 0.2 | Height threshold of non ground objects [m], applied only for elevation_grid_mode | -| `grid_mode_switch_radius` | float | 20.0 | The distance where grid division mode change from by distance to by vertical angle [m],
applied only for elevation_grid_mode | -| `grid_size_m` | float | 0.5 | The first grid size [m], applied only for elevation_grid_mode | -| `gnd_grid_buffer_size` | uint16 | 4 | Number of grids using to estimate local ground slope,
applied only for elevation_grid_mode | -| `low_priority_region_x` | float | -20.0 | The non-zero x threshold in back side from which small objects detection is low priority [m] | -| `elevation_grid_mode` | bool | true | Elevation grid scan mode option | -| `use_recheck_ground_cluster` | bool | true | Enable recheck ground cluster | +![scan_ground_parameter](./image/scan_ground_filter_parameters.drawio.svg) +| Name | Type | Default Value | Description | +| --------------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `input_frame` | string | "base_link" | frame id of input pointcloud | +| `output_frame` | string | "base_link" | frame id of output pointcloud | +| `global_slope_max_angle_deg` | double | 8.0 | The global angle to classify as the ground or object [deg].
A large threshold may reduce false positive of high slope road classification but it may lead to increase false negative of non-ground classification, particularly for small objects. | +| `local_slope_max_angle_deg` | double | 10.0 | The local angle to classify as the ground or object [deg] when comparing with adjacent point.
A small value enhance accuracy classification of object with inclined surface. This should be considered together with `split_points_distance_tolerance` value. | +| `radial_divider_angle_deg` | double | 1.0 | The angle which divide the whole pointcloud to sliced group [deg] | +| `split_points_distance_tolerance` | double | 0.2 | The xy-distance threshold to distinguish far and near [m] | +| `split_height_distance` | double | 0.2 | The height threshold to distinguish ground and non-ground pointcloud when comparing with adjacent points [m].
A small threshold improves classification of non-ground point, especially for high elevation resolution pointcloud lidar. However, it might cause false positive for small step-like road surface or misaligned multiple lidar configuration. | +| `use_virtual_ground_point` | bool | true | whether to use the ground center of front wheels as the virtual ground point. | +| `detection_range_z_max` | float | 2.5 | Maximum height of detection range [m], applied only for elevation_grid_mode | +| `center_pcl_shift` | float | 0.0 | The x-axis offset of addition LiDARs from vehicle center of mass [m],
recommended to use only for additional LiDARs in elevation_grid_mode | +| `non_ground_height_threshold` | float | 0.2 | Height threshold of non ground objects [m] as `split_height_distance` and applied only for elevation_grid_mode | +| `grid_mode_switch_radius` | float | 20.0 | The distance where grid division mode change from by distance to by vertical angle [m],
applied only for elevation_grid_mode | +| `grid_size_m` | float | 0.5 | The first grid size [m], applied only for elevation_grid_mode.
A large value enhances the prediction stability for ground surface. suitable for rough surface or multiple lidar configuration. | +| `gnd_grid_buffer_size` | uint16 | 4 | Number of grids using to estimate local ground slope,
applied only for elevation_grid_mode | +| `low_priority_region_x` | float | -20.0 | The non-zero x threshold in back side from which small objects detection is low priority [m] | +| `elevation_grid_mode` | bool | true | Elevation grid scan mode option | +| `use_recheck_ground_cluster` | bool | true | Enable recheck ground cluster | ## Assumptions / Known limits diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 7631b47772862..6d3d7f5e035a9 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -109,6 +109,21 @@ For the additional information, here we show how we calculate lateral velocity. Currently, we use the upper method with a low-pass filter to calculate lateral velocity. +### Path generation + +Path generation is generated on the frenet frame. The path is generated by the following steps: + +1. Get the frenet frame of the reference path. +2. Generate the frenet frame of the current position of the object and the finite position of the object. +3. Optimize the path in each longitudinal and lateral coordinate of the frenet frame. (Use the quintic polynomial to fit start and end conditions.) +4. Convert the path to the global coordinate. + +See paper [2] for more details. + +#### Tuning lateral path shape + +`lateral_control_time_horizon` parameter supports the tuning of the lateral path shape. This parameter is used to calculate the time to reach the reference path. The smaller the value, the more the path will be generated to reach the reference path quickly. (Mostly the center of the lane.) + ### Path prediction for crosswalk users This module treats **Pedestrians** and **Bicycles** as objects using the crosswalk, and outputs prediction path based on map and estimated object's velocity, assuming the object has intention to cross the crosswalk, if the objects satisfies at least one of the following conditions: @@ -155,6 +170,7 @@ If the target object is inside the road or crosswalk, this module outputs one or | ---------------------------------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------------------- | | `enable_delay_compensation` | [-] | bool | flag to enable the time delay compensation for the position of the object | | `prediction_time_horizon` | [s] | double | predict time duration for predicted path | +| `lateral_control_time_horizon` | [s] | double | time duration for predicted path will reach the reference path (mostly center of the lane) | | `prediction_sampling_delta_time` | [s] | double | sampling time for points in predicted path | | `min_velocity_for_map_based_prediction` | [m/s] | double | apply map-based prediction to the objects with higher velocity than this value | | `min_crosswalk_user_velocity` | [m/s] | double | minimum velocity used when crosswalk user's velocity is calculated | diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index 31fae9c811092..260ae45da2e05 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -2,6 +2,7 @@ ros__parameters: enable_delay_compensation: true prediction_time_horizon: 10.0 #[s] + lateral_control_time_horizon: 5.0 #[s] prediction_sampling_delta_time: 0.5 #[s] min_velocity_for_map_based_prediction: 1.39 #[m/s] min_crosswalk_user_velocity: 1.39 #[m/s] diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 3d01ab96e9b62..01f39057aef36 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -135,6 +135,7 @@ class MapBasedPredictionNode : public rclcpp::Node // Parameters bool enable_delay_compensation_; double prediction_time_horizon_; + double lateral_control_time_horizon_; double prediction_time_horizon_rate_for_validate_lane_length_; double prediction_sampling_time_interval_; double min_velocity_for_map_based_prediction_; @@ -165,6 +166,14 @@ class MapBasedPredictionNode : public rclcpp::Node void mapCallback(const HADMapBin::ConstSharedPtr msg); void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects); + bool doesPathCrossAnyFence(const PredictedPath & predicted_path); + bool doesPathCrossFence( + const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line); + lanelet::BasicLineString2d convertToFenceLine(const lanelet::ConstLineString3d & fence); + bool isIntersecting( + const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, + const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4); + PredictedObjectKinematics convertToPredictedKinematics( const TrackedObjectKinematics & tracked_object); diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 7bb1542557d2c..4da4f62be2ede 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -81,8 +81,8 @@ class PathGenerator { public: PathGenerator( - const double time_horizon, const double sampling_time_interval, - const double min_crosswalk_user_velocity); + const double time_horizon, const double lateral_time_horizon, + const double sampling_time_interval, const double min_crosswalk_user_velocity); PredictedPath generatePathForNonVehicleObject(const TrackedObject & object); @@ -102,6 +102,7 @@ class PathGenerator private: // Parameters double time_horizon_; + double lateral_time_horizon_; double sampling_time_interval_; double min_crosswalk_user_velocity_; diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 58356f4f96ed6..08fc06330b1d8 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -369,17 +370,13 @@ bool withinLanelet( boost::geometry::correct(polygon); bool with_in_polygon = boost::geometry::within(p_object, polygon); - if (!use_yaw_information) { - return with_in_polygon; - } else { - // use yaw angle to compare - const double abs_yaw_diff = calcAbsYawDiffBetweenLaneletAndObject(object, lanelet); - if (abs_yaw_diff < yaw_threshold) { - return with_in_polygon; - } else { - return false; - } - } + if (!use_yaw_information) return with_in_polygon; + + // use yaw angle to compare + const double abs_yaw_diff = calcAbsYawDiffBetweenLaneletAndObject(object, lanelet); + if (abs_yaw_diff < yaw_threshold) return with_in_polygon; + + return false; } bool withinRoadLanelet( @@ -594,53 +591,53 @@ ObjectClassification::_label_type changeLabelForPrediction( const lanelet::LaneletMapPtr & lanelet_map_ptr_) { // for car like vehicle do not change labels - if ( - label == ObjectClassification::CAR || label == ObjectClassification::BUS || - label == ObjectClassification::TRUCK || label == ObjectClassification::TRAILER || - label == ObjectClassification::UNKNOWN) { - return label; - } else if ( // for bicycle and motorcycle - label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE) { - // if object is within road lanelet and satisfies yaw constraints - const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); - const float high_speed_threshold = 25.0 / 18.0 * 5.0; // High speed bicycle 25 km/h - // calc abs speed from x and y velocity - const double abs_speed = std::hypot( - object.kinematics.twist_with_covariance.twist.linear.x, - object.kinematics.twist_with_covariance.twist.linear.y); - const bool high_speed_object = abs_speed > high_speed_threshold; - - // if the object is within lanelet, do the same estimation with vehicle - if (within_road_lanelet) { - return ObjectClassification::MOTORCYCLE; - } else if (high_speed_object) { + switch (label) { + case ObjectClassification::CAR: + case ObjectClassification::BUS: + case ObjectClassification::TRUCK: + case ObjectClassification::TRAILER: + case ObjectClassification::UNKNOWN: + return label; + + case ObjectClassification::MOTORCYCLE: + case ObjectClassification::BICYCLE: { // if object is within road lanelet and satisfies yaw + // constraints + const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); + const float high_speed_threshold = + tier4_autoware_utils::kmph2mps(25.0); // High speed bicycle 25 km/h + // calc abs speed from x and y velocity + const double abs_speed = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const bool high_speed_object = abs_speed > high_speed_threshold; + + // if the object is within lanelet, do the same estimation with vehicle + if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; // high speed object outside road lanelet will move like unknown object // return ObjectClassification::UNKNOWN; // temporary disabled - return label; - } else { + if (high_speed_object) return label; // Do nothing for now return ObjectClassification::BICYCLE; } - } else if (label == ObjectClassification::PEDESTRIAN) { - const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); - const float max_velocity_for_human_mps = - 25.0 / 18.0 * 5.0; // Max human being motion speed is 25km/h - const double abs_speed = std::hypot( - object.kinematics.twist_with_covariance.twist.linear.x, - object.kinematics.twist_with_covariance.twist.linear.y); - const bool high_speed_object = abs_speed > max_velocity_for_human_mps; - // fast, human-like object: like segway - if (within_road_lanelet && high_speed_object) { - return label; // currently do nothing + + case ObjectClassification::PEDESTRIAN: { + const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); + const float max_velocity_for_human_mps = + tier4_autoware_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h + const double abs_speed = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const bool high_speed_object = abs_speed > max_velocity_for_human_mps; + // fast, human-like object: like segway + if (within_road_lanelet && high_speed_object) return label; // currently do nothing // return ObjectClassification::MOTORCYCLE; - } else if (high_speed_object) { - return label; // currently do nothing + if (high_speed_object) return label; // currently do nothing // fast human outside road lanelet will move like unknown object // return ObjectClassification::UNKNOWN; - } else { return label; } - } else { - return label; + + default: + return label; } } @@ -697,6 +694,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ { enable_delay_compensation_ = declare_parameter("enable_delay_compensation"); prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); + lateral_control_time_horizon_ = + declare_parameter("lateral_control_time_horizon"); // [s] for lateral control point prediction_sampling_time_interval_ = declare_parameter("prediction_sampling_delta_time"); min_velocity_for_map_based_prediction_ = declare_parameter("min_velocity_for_map_based_prediction"); @@ -743,7 +742,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ declare_parameter("prediction_time_horizon_rate_for_validate_shoulder_lane_length"); path_generator_ = std::make_shared( - prediction_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_); + prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, + min_crosswalk_user_velocity_); sub_objects_ = this->create_subscription( "~/input/objects", 1, @@ -850,102 +850,99 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt const auto & label_ = transformed_object.classification.front().label; const auto label = changeLabelForPrediction(label_, object, lanelet_map_ptr_); - // For crosswalk user - if (label == ObjectClassification::PEDESTRIAN || label == ObjectClassification::BICYCLE) { - const auto predicted_object = getPredictedObjectAsCrosswalkUser(transformed_object); - output.objects.push_back(predicted_object); - // For road user - } else if ( - label == ObjectClassification::CAR || label == ObjectClassification::BUS || - label == ObjectClassification::TRAILER || label == ObjectClassification::MOTORCYCLE || - label == ObjectClassification::TRUCK) { - // Update object yaw and velocity - updateObjectData(transformed_object); - - // Get Closest Lanelet - const auto current_lanelets = getCurrentLanelets(transformed_object); - - // Update Objects History - updateObjectsHistory(output.header, transformed_object, current_lanelets); - - // For off lane obstacles - if (current_lanelets.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForOffLaneVehicle(transformed_object); - predicted_path.confidence = 1.0; - if (predicted_path.path.empty()) { - continue; - } - - auto predicted_object = convertToPredictedObject(transformed_object); - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); - continue; + switch (label) { + case ObjectClassification::PEDESTRIAN: + case ObjectClassification::BICYCLE: { + const auto predicted_object_crosswalk = + getPredictedObjectAsCrosswalkUser(transformed_object); + output.objects.push_back(predicted_object_crosswalk); + break; } - - // For too-slow vehicle - const double abs_obj_speed = std::hypot( - transformed_object.kinematics.twist_with_covariance.twist.linear.x, - transformed_object.kinematics.twist_with_covariance.twist.linear.y); - if (std::fabs(abs_obj_speed) < min_velocity_for_map_based_prediction_) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); - predicted_path.confidence = 1.0; - if (predicted_path.path.empty()) { - continue; + case ObjectClassification::CAR: + case ObjectClassification::BUS: + case ObjectClassification::TRAILER: + case ObjectClassification::MOTORCYCLE: + case ObjectClassification::TRUCK: { + // Update object yaw and velocity + updateObjectData(transformed_object); + + // Get Closest Lanelet + const auto current_lanelets = getCurrentLanelets(transformed_object); + + // Update Objects History + updateObjectsHistory(output.header, transformed_object, current_lanelets); + + // For off lane obstacles + if (current_lanelets.empty()) { + PredictedPath predicted_path = + path_generator_->generatePathForOffLaneVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) break; + + auto predicted_object_vehicle = convertToPredictedObject(transformed_object); + predicted_object_vehicle.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_object_vehicle); + break; } - auto predicted_object = convertToPredictedObject(transformed_object); - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); - continue; - } - - // Get Predicted Reference Path for Each Maneuver and current lanelets - // return: - const auto ref_paths = - getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); - - // If predicted reference path is empty, assume this object is out of the lane - if (ref_paths.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); - predicted_path.confidence = 1.0; - if (predicted_path.path.empty()) { - continue; + // For too-slow vehicle + const double abs_obj_speed = std::hypot( + transformed_object.kinematics.twist_with_covariance.twist.linear.x, + transformed_object.kinematics.twist_with_covariance.twist.linear.y); + if (std::fabs(abs_obj_speed) < min_velocity_for_map_based_prediction_) { + PredictedPath predicted_path = + path_generator_->generatePathForLowSpeedVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) break; + + auto predicted_slow_object = convertToPredictedObject(transformed_object); + predicted_slow_object.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_slow_object); + break; } - auto predicted_object = convertToPredictedObject(transformed_object); - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); - continue; - } + // Get Predicted Reference Path for Each Maneuver and current lanelets + // return: + const auto ref_paths = + getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); + + // If predicted reference path is empty, assume this object is out of the lane + if (ref_paths.empty()) { + PredictedPath predicted_path = + path_generator_->generatePathForLowSpeedVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) break; + + auto predicted_object_out_of_lane = convertToPredictedObject(transformed_object); + predicted_object_out_of_lane.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_object_out_of_lane); + break; + } - // Get Debug Marker for On Lane Vehicles - const auto max_prob_path = std::max_element( - ref_paths.begin(), ref_paths.end(), - [](const PredictedRefPath & a, const PredictedRefPath & b) { - return a.probability < b.probability; - }); - const auto debug_marker = - getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); - debug_markers.markers.push_back(debug_marker); - - // Generate Predicted Path - std::vector predicted_paths; - for (const auto & ref_path : ref_paths) { - PredictedPath predicted_path = - path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); - if (predicted_path.path.empty()) { - continue; + // Get Debug Marker for On Lane Vehicles + const auto max_prob_path = std::max_element( + ref_paths.begin(), ref_paths.end(), + [](const PredictedRefPath & a, const PredictedRefPath & b) { + return a.probability < b.probability; + }); + const auto debug_marker = + getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); + debug_markers.markers.push_back(debug_marker); + + // Generate Predicted Path + std::vector predicted_paths; + for (const auto & ref_path : ref_paths) { + PredictedPath predicted_path = + path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); + if (predicted_path.path.empty()) { + continue; + } + predicted_path.confidence = ref_path.probability; + predicted_paths.push_back(predicted_path); } - predicted_path.confidence = ref_path.probability; - predicted_paths.push_back(predicted_path); - } + // Normalize Path Confidence and output the predicted object - // Normalize Path Confidence and output the predicted object - { float sum_confidence = 0.0; for (const auto & predicted_path : predicted_paths) { sum_confidence += predicted_path.confidence; @@ -953,25 +950,25 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt const float min_sum_confidence_value = 1e-3; sum_confidence = std::max(sum_confidence, min_sum_confidence_value); + auto predicted_object = convertToPredictedObject(transformed_object); + for (auto & predicted_path : predicted_paths) { predicted_path.confidence = predicted_path.confidence / sum_confidence; - } - - auto predicted_object = convertToPredictedObject(transformed_object); - for (const auto & predicted_path : predicted_paths) { predicted_object.kinematics.predicted_paths.push_back(predicted_path); } output.objects.push_back(predicted_object); + break; } - // For unknown object - } else { - auto predicted_object = convertToPredictedObject(transformed_object); - PredictedPath predicted_path = - path_generator_->generatePathForNonVehicleObject(transformed_object); - predicted_path.confidence = 1.0; + default: { + auto predicted_unknown_object = convertToPredictedObject(transformed_object); + PredictedPath predicted_path = + path_generator_->generatePathForNonVehicleObject(transformed_object); + predicted_path.confidence = 1.0; - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); + predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_unknown_object); + break; + } } } @@ -982,6 +979,46 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt pub_calculation_time_->publish(calculation_time_msg); } +bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) +{ + const lanelet::ConstLineStrings3d & all_fences = + lanelet::utils::query::getAllFences(lanelet_map_ptr_); + for (const auto & fence_line : all_fences) { + if (doesPathCrossFence(predicted_path, fence_line)) { + return true; + } + } + return false; +} + +bool MapBasedPredictionNode::doesPathCrossFence( + const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line) +{ + // check whether the predicted path cross with fence + for (size_t i = 0; i < predicted_path.path.size(); ++i) { + for (size_t j = 0; j < fence_line.size() - 1; ++j) { + if (isIntersecting( + predicted_path.path[i].position, predicted_path.path[i + 1].position, fence_line[j], + fence_line[j + 1])) { + return true; + } + } + } + return false; +} + +bool MapBasedPredictionNode::isIntersecting( + const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, + const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4) +{ + const auto p1 = tier4_autoware_utils::createPoint(point1.x, point1.y, 0.0); + const auto p2 = tier4_autoware_utils::createPoint(point2.x, point2.y, 0.0); + const auto p3 = tier4_autoware_utils::createPoint(point3.x(), point3.y(), 0.0); + const auto p4 = tier4_autoware_utils::createPoint(point4.x(), point4.y(), 0.0); + const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + return intersection.has_value(); +} + PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const TrackedObject & object) { @@ -1001,6 +1038,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } } + // If the object is in the crosswalk, generate path to the crosswalk edge if (crossing_crosswalk) { const auto edge_points = getCrosswalkEdgePoints(crossing_crosswalk.get()); @@ -1024,6 +1062,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( predicted_object.kinematics.predicted_paths.push_back(predicted_path); } + // If the object is not crossing the crosswalk, in the road lanelets, try to find the closest + // crosswalk and generate path to the crosswalk edge } else if (withinRoadLanelet(object, lanelet_map_ptr_)) { lanelet::ConstLanelet closest_crosswalk{}; const auto & obj_pose = object.kinematics.pose_with_covariance.pose; @@ -1054,6 +1094,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } } + // If the object is not crossing the crosswalk, not in the road lanelets, try to find the edge + // points for all crosswalks and generate path to the crosswalk edge } else { for (const auto & crosswalk : crosswalks_) { const auto edge_points = getCrosswalkEdgePoints(crosswalk); @@ -1086,6 +1128,10 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( if (predicted_path.path.empty()) { continue; } + // If the predicted path to the crosswalk is crossing the fence, don't use it + if (doesPathCrossAnyFence(predicted_path)) { + continue; + } predicted_object.kinematics.predicted_paths.push_back(predicted_path); } @@ -1114,26 +1160,28 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) object_pose, object_twist.linear.x * 0.1, object_twist.linear.y * 0.1, 0.0); // assumption: the object vx is much larger than vy - if (object.kinematics.twist_with_covariance.twist.linear.x < 0.0) { - if ( - object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { + if (object.kinematics.twist_with_covariance.twist.linear.x >= 0.0) return; + + switch (object.kinematics.orientation_availability) { + case autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN: { const auto original_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); // flip the angle object.kinematics.pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(tier4_autoware_utils::pi + original_yaw); - } else { + break; + } + default: { const auto updated_object_yaw = tier4_autoware_utils::calcAzimuthAngle(object_pose.position, future_object_pose.position); object.kinematics.pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(updated_object_yaw); + break; } - - object.kinematics.twist_with_covariance.twist.linear.x *= -1.0; - object.kinematics.twist_with_covariance.twist.linear.y *= -1.0; } + object.kinematics.twist_with_covariance.twist.linear.x *= -1.0; + object.kinematics.twist_with_covariance.twist.linear.y *= -1.0; return; } @@ -1864,16 +1912,10 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( constexpr float LC_PROB = 1.0; // probability for left lane change constexpr float RC_PROB = 1.0; // probability for right lane change lane_follow_probability = 0.0; - if (!left_paths.empty() && right_paths.empty()) { - left_lane_change_probability = LC_PROB; - right_lane_change_probability = 0.0; - } else if (left_paths.empty() && !right_paths.empty()) { - left_lane_change_probability = 0.0; - right_lane_change_probability = RC_PROB; - } else { - left_lane_change_probability = LC_PROB; - right_lane_change_probability = RC_PROB; - } + + // If the given lane is empty, the probability goes to 0 + left_lane_change_probability = left_paths.empty() ? 0.0 : LC_PROB; + right_lane_change_probability = right_paths.empty() ? 0.0 : RC_PROB; } const float MIN_PROBABILITY = 1e-3; diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 547132410badf..5cb7e186b7cc4 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -23,9 +23,10 @@ namespace map_based_prediction { PathGenerator::PathGenerator( - const double time_horizon, const double sampling_time_interval, + const double time_horizon, const double lateral_time_horizon, const double sampling_time_interval, const double min_crosswalk_user_velocity) : time_horizon_(time_horizon), + lateral_time_horizon_(lateral_time_horizon), sampling_time_interval_(sampling_time_interval), min_crosswalk_user_velocity_(min_crosswalk_user_velocity) { @@ -213,18 +214,25 @@ FrenetPath PathGenerator::generateFrenetPath( { FrenetPath path; const double duration = time_horizon_; + const double lateral_duration = lateral_time_horizon_; // Compute Lateral and Longitudinal Coefficients to generate the trajectory - const Eigen::Vector3d lat_coeff = calcLatCoefficients(current_point, target_point, duration); + const Eigen::Vector3d lat_coeff = + calcLatCoefficients(current_point, target_point, lateral_duration); const Eigen::Vector2d lon_coeff = calcLonCoefficients(current_point, target_point, duration); path.reserve(static_cast(duration / sampling_time_interval_)); for (double t = 0.0; t <= duration; t += sampling_time_interval_) { - const double d_next = current_point.d + current_point.d_vel * t + 0 * 2 * std::pow(t, 2) + - lat_coeff(0) * std::pow(t, 3) + lat_coeff(1) * std::pow(t, 4) + - lat_coeff(2) * std::pow(t, 5); - const double s_next = current_point.s + current_point.s_vel * t + 2 * 0 * std::pow(t, 2) + - lon_coeff(0) * std::pow(t, 3) + lon_coeff(1) * std::pow(t, 4); + const double current_acc = + 0.0; // Currently we assume the object is traveling at a constant speed + const double d_next_ = current_point.d + current_point.d_vel * t + + current_acc * 2.0 * std::pow(t, 2) + lat_coeff(0) * std::pow(t, 3) + + lat_coeff(1) * std::pow(t, 4) + lat_coeff(2) * std::pow(t, 5); + // t > lateral_duration: 0.0, else d_next_ + const double d_next = t > lateral_duration ? 0.0 : d_next_; + const double s_next = current_point.s + current_point.s_vel * t + + 2.0 * current_acc * std::pow(t, 2) + lon_coeff(0) * std::pow(t, 3) + + lon_coeff(1) * std::pow(t, 4); if (s_next > max_length) { break; } diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt new file mode 100644 index 0000000000000..108749c5f91a7 --- /dev/null +++ b/perception/tracking_object_merger/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(tracking_object_merger VERSION 1.0.0) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wconversion) +endif() + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(nlohmann_json REQUIRED) # for debug + + +# find dependencies +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIR} +) + +ament_auto_add_library(mu_successive_shortest_path SHARED + src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +) + +ament_auto_add_library(decorative_tracker_merger_node SHARED + src/data_association/data_association.cpp + src/decorative_tracker_merger.cpp + src/utils/utils.cpp + src/utils/tracker_state.cpp +) + +target_link_libraries(decorative_tracker_merger_node + mu_successive_shortest_path + Eigen3::Eigen + nlohmann_json::nlohmann_json # for debug +) + +rclcpp_components_register_node(decorative_tracker_merger_node + PLUGIN "tracking_object_merger::DecorativeTrackerMergerNode" + EXECUTABLE decorative_tracker_merger +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/perception/tracking_object_merger/README.md b/perception/tracking_object_merger/README.md new file mode 100644 index 0000000000000..9964534372037 --- /dev/null +++ b/perception/tracking_object_merger/README.md @@ -0,0 +1,114 @@ +# Tracking Object Merger + +## Purpose + +This package try to merge two tracking objects from different sensor. + +## Inner-workings / Algorithms + +Merging tracking objects from different sensor is a combination of data association and state fusion algorithms. + +Detailed process depends on the merger policy. + +### decorative_tracker_merger + +In decorative_tracker_merger, we assume there are dominant tracking objects and sub tracking objects. +The name `decorative` means that sub tracking objects are used to complement the main objects. + +Usually the dominant tracking objects are from LiDAR and sub tracking objects are from Radar or Camera. + +Here show the processing pipeline. + +![decorative_tracker_merger](./image/decorative_tracker_merger.drawio.svg) + +#### time sync + +Sub object(Radar or Camera) often has higher frequency than dominant object(LiDAR). So we need to sync the time of sub object to dominant object. + +![time sync](image/time_sync.drawio.svg) + +#### data association + +In the data association, we use the following rules to determine whether two tracking objects are the same object. + +- gating + - `distance gate`: distance between two tracking objects + - `angle gate`: angle between two tracking objects + - `mahalanobis_distance_gate`: Mahalanobis distance between two tracking objects + - `min_iou_gate`: minimum IoU between two tracking objects + - `max_velocity_gate`: maximum velocity difference between two tracking objects +- score + - score used in matching is equivalent to the distance between two tracking objects + +#### tracklet update + +Sub tracking objects are merged into dominant tracking objects. + +Depends on the tracklet input sensor state, we update the tracklet state with different rules. + +| state\priority | 1st | 2nd | 3rd | +| -------------------------- | ------ | ----- | ------ | +| Kinematics except velocity | LiDAR | Radar | Camera | +| Forward velocity | Radar | LiDAR | Camera | +| Object classification | Camera | LiDAR | Radar | + +#### tracklet management + +We use the `existence_probability` to manage tracklet. + +- When we create a new tracklet, we set the `existence_probability` to $p_{sensor}$ value. +- In each update with specific sensor, we set the `existence_probability` to $p_{sensor}$ value. +- When tracklet does not have update with specific sensor, we reduce the `existence_probability` by `decay_rate` +- Object can be published if `existence_probability` is larger than `publish_probability_threshold` +- Object will be removed if `existence_probability` is smaller than `remove_probability_threshold` + +![tracklet_management](./image/tracklet_management.drawio.svg) + +These parameter can be set in `config/decorative_tracker_merger.param.yaml`. + +```yaml +tracker_state_parameter: + remove_probability_threshold: 0.3 + publish_probability_threshold: 0.6 + default_lidar_existence_probability: 0.7 + default_radar_existence_probability: 0.6 + default_camera_existence_probability: 0.6 + decay_rate: 0.1 + max_dt: 1.0 +``` + +#### input/parameters + +| topic name | message type | description | +| ------------------------------- | ----------------------------------------------- | ------------------------------------------------------------------------------------- | +| `~input/main_object` | `autoware_auto_perception_msgs::TrackedObjects` | Dominant tracking objects. Output will be published with this dominant object stamps. | +| `~input/sub_object` | `autoware_auto_perception_msgs::TrackedObjects` | Sub tracking objects. | +| `output/object` | `autoware_auto_perception_msgs::TrackedObjects` | Merged tracking objects. | +| `debug/interpolated_sub_object` | `autoware_auto_perception_msgs::TrackedObjects` | Interpolated sub tracking objects. | + +Default parameters are set in [config/decorative_tracker_merger.param.yaml](./config/decorative_tracker_merger.param.yaml). + +| parameter name | description | default value | +| ------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `base_link_frame_id` | base link frame id. This is used to transform the tracking object. | "base_link" | +| `time_sync_threshold` | time sync threshold. If the time difference between two tracking objects is smaller than this value, we consider these two tracking objects are the same object. | 0.05 | +| `sub_object_timeout_sec` | sub object timeout. If the sub object is not updated for this time, we consider this object is not exist. | 0.5 | +| `main_sensor_type` | main sensor type. This is used to determine the dominant tracking object. | "lidar" | +| `sub_sensor_type` | sub sensor type. This is used to determine the sub tracking object. | "radar" | +| `tracker_state_parameter` | tracker state parameter. This is used to manage the tracklet. | | + +- the detail of `tracker_state_parameter` is described in [tracklet management](#tracklet-management) + +#### tuning + +As explained in [tracklet management](#tracklet-management), this tracker merger tend to maintain the both input tracking objects. + +If there are many false positive tracking objects, + +- decrease `default__existence_probability` of that sensor +- increase `decay_rate` +- increase `publish_probability_threshold` to publish only reliable tracking objects + +### equivalent_tracker_merger + +This is future work. diff --git a/perception/tracking_object_merger/config/data_association_matrix.param.yaml b/perception/tracking_object_merger/config/data_association_matrix.param.yaml new file mode 100644 index 0000000000000..702809b3cede1 --- /dev/null +++ b/perception/tracking_object_merger/config/data_association_matrix.param.yaml @@ -0,0 +1,168 @@ +/**: + ros__parameters: + lidar-lidar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 2.0, 2.0, 2.0, #UNKNOWN + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN + + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # If value is negative, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] #PEDESTRIAN + + lidar-radar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 3.5, 3.5, 3.5, #UNKNOWN + 4.0, 5.5, 6.0, 6.0, 6.0, 1.0, 1.0, 1.0, #CAR + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #TRUCK + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #BUS + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #TRAILER + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.5] #PEDESTRIAN + + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # set all value to 0.0 to disable this constraint + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #UNKNOWN + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #CAR + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRUCK + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BUS + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRAILER + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #MOTORBIKE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BICYCLE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #PEDESTRIAN + + radar-radar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 3.5, 3.5, 3.5, #UNKNOWN + 4.0, 7.0, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #CAR + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #TRUCK + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #BUS + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #TRAILER + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.5] #PEDESTRIAN + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # set all value to 0.0 to disable this constraint + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #UNKNOWN + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #CAR + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRUCK + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BUS + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRAILER + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #MOTORBIKE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BICYCLE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #PEDESTRIAN diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml new file mode 100644 index 0000000000000..4a108be657a27 --- /dev/null +++ b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml @@ -0,0 +1,26 @@ +# Node parameters +/**: + ros__parameters: + base_link_frame_id: "base_link" + time_sync_threshold: 0.999 + sub_object_timeout_sec: 0.8 + publish_interpolated_sub_objects: true #for debug + + # choose the input sensor type for each object type + # "lidar", "radar", "camera" are available + main_sensor_type: "lidar" + sub_sensor_type: "radar" + + # tracker settings + tracker_state_parameter: + remove_probability_threshold: 0.3 + publish_probability_threshold: 0.5 + default_lidar_existence_probability: 0.7 + default_radar_existence_probability: 0.6 + default_camera_existence_probability: 0.5 + decay_rate: 0.1 + max_dt: 1.0 + + # logging settings + enable_logging: false + log_file_path: "/tmp/decorative_tracker_merger.log" diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml b/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml new file mode 100644 index 0000000000000..0b98e1b202957 --- /dev/null +++ b/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml @@ -0,0 +1,16 @@ +# Merger policy for decorative tracker merger node +# decorative tracker merger works by merging the sub-object trackers into a main object tracker result +# There are 3 merger policy: +# 1. "skip": skip the sub-object tracker result +# 2. "overwrite": overwrite the main object tracker result with sub-object tracker result +# 3. "fusion": merge the main object tracker result with sub-object tracker result by using covariance based fusion +/**: + ros__parameters: + kinematics_to_be_merged: "velocity" # currently only support "velocity" + + # choose the merger policy for each object type + # : "skip", "overwrite", "fusion" + kinematics_merge_policy: "overwrite" + classification_merge_policy: "skip" + existence_prob_merge_policy: "skip" + shape_merge_policy: "skip" diff --git a/perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg b/perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg new file mode 100644 index 0000000000000..3e35fa4d1c338 --- /dev/null +++ b/perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg @@ -0,0 +1,345 @@ + + + + + + + + + + + + +
+
+
+ main_object +
+
+
+ + main_object + + + + + + +
+
+
+ decorative_tracker_merger_node +
+
+
+ + decorative_tracker_merger_node + + + + + + + + +
+
+
+ sub_object +
+
+
+ + sub_object + + + + + + + + +
+
+
+ time-sync +
+ (delay compensation) +
+
+
+ + time-sync... + + + + + + + + +
+
+
+ buffer +
+
+
+ + buffer + + + + + + +
+
+
+ main obj +
+ timestamp +
+
+
+ + main obj... + + + + + + + + + + +
+
+
+ inner +
+ tracklet +
+
+
+ + inner... + + + + + + + +
+
+
+ not +
+ found +
+
+
+ + not... + + + + + + + +
+
+
+ found +
+
+
+ + found + + + + + + +
+
+
+ association +
+
+
+ + association + + + + + + + + +
+
+
+ update +
+ tracklet +
+
+
+ + update... + + + + + + + + +
+
+
+ add new +
+ tracklet +
+
+
+ + add new... + + + + + + + +
+
+
+ converted +
+ tracked object +
+
+
+ + converted... + + + + + + + + +
+
+
+ tracklet management +
+ (publisher) +
+
+
+ + tracklet management... + + + + + + +
+
+
+ merged object +
+
+
+ + merged object + + + + + + +
+
+
msg: TrackedObjects
+
+
+ + msg: TrackedObjects + + + + + + +
+
+
+ remove when low existence +
+ probability objects +
+
+
+ + remove when low existence... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/tracking_object_merger/image/time_sync.drawio.svg b/perception/tracking_object_merger/image/time_sync.drawio.svg new file mode 100644 index 0000000000000..dad2eb1a7fd27 --- /dev/null +++ b/perception/tracking_object_merger/image/time_sync.drawio.svg @@ -0,0 +1,223 @@ + + + + + + + + + + + +
+
+
time
+
+
+ + time + + + + + + + +
+
+
main object topic
+
+
+ + main object topic + + + + + + +
+
+
sub object topic
+
+
+ + sub object topic + + + + + + + + + + + + + + + + + + + + +
+
+
publish timing
+
+
+ + publish timing + + + + + + +
+
+
+ interpolated sub object +
+
+
+ + interpolated sub object + + + + + + + + +
+
+
raw main/sub object
+
+
+ + raw main/sub object + + + + + + + + + +
+
+
+ time sync threshold +
+
+
+ + time sync threshold + + + + + + +
+
+
+ time synchronize prediction pattern +
+
+
+ + time synchronize prediction pattern + + + + + + + + + +
+
+
+ 1. forward +
+
+
+ 2. backward +
+
+
+ 3. interpolation +
+
+
+
+ + 1. forward... + + + + + + + + + + + + + + + + + + +
+
+
legend
+
+
+ + legend + + + + + + +
+
+
+ disabled +
+ now +
+
+
+ + disabled... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/tracking_object_merger/image/tracklet_management.drawio.svg b/perception/tracking_object_merger/image/tracklet_management.drawio.svg new file mode 100644 index 0000000000000..4f2a00dfd4dfd --- /dev/null +++ b/perception/tracking_object_merger/image/tracklet_management.drawio.svg @@ -0,0 +1,216 @@ + + + + + + + + +
+
+
0.0
+
+
+ + 0.0 + + + + + + +
+
+
1.0
+
+
+ + 1.0 + + + + + + +
+
+
Existence Probability
+
+
+ + Existence Probability + + + + + + + +
+
+
remove threshold
+
+
+ + remove threshold + + + + + + +
+
+
can publish threshold
+
+
+ + can publish threshold + + + + + + + + + +
+
+
+ decay when +
+ not observed +
+
+
+ + decay when... + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + +
+
+
+ init or update probability +
+ from sensor +
+
+
+ + init or update probabilit... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp new file mode 100644 index 0000000000000..05fc9f6caccb5 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp @@ -0,0 +1,73 @@ +// 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. +// +// +// Author: v1.0 Yukihiro Saito +// + +#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ + +// #include // for debug json library + +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include "tracking_object_merger/data_association/solver/gnn_solver.hpp" +#include "tracking_object_merger/utils/tracker_state.hpp" + +#include +#include + +#include +#include + +#include + +class DataAssociation +{ +private: + Eigen::MatrixXi can_assign_matrix_; + Eigen::MatrixXd max_dist_matrix_; + Eigen::MatrixXd max_rad_matrix_; + Eigen::MatrixXd min_iou_matrix_; + Eigen::MatrixXd max_velocity_diff_matrix_; + const double score_threshold_; + std::unique_ptr gnn_solver_ptr_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_rad_vector, std::vector min_iou_vector, + std::vector max_velocity_diff_vector); + void assign( + const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment); + Eigen::MatrixXd calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const autoware_auto_perception_msgs::msg::TrackedObjects & objects1); + Eigen::MatrixXd calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const std::vector & trackers); + double calcScoreBetweenObjects( + const autoware_auto_perception_msgs::msg::TrackedObject & object0, + const autoware_auto_perception_msgs::msg::TrackedObject & object1) const; + virtual ~DataAssociation() {} +}; + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp new file mode 100644 index 0000000000000..31240848f1977 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp @@ -0,0 +1,22 @@ +// 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 TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ + +#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" +#include "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp" +#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp new file mode 100644 index 0000000000000..e915b5d2a9e7b --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp @@ -0,0 +1,35 @@ +// 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 TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ + +#include +#include + +namespace gnn_solver +{ +class GnnSolverInterface +{ +public: + GnnSolverInterface() = default; + virtual ~GnnSolverInterface() = default; + + virtual void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) = 0; +}; +} // namespace gnn_solver + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp new file mode 100644 index 0000000000000..5c0ebc04fdde3 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp @@ -0,0 +1,37 @@ +// 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 TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ + +#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" + +#include +#include + +namespace gnn_solver +{ +class MuSSP : public GnnSolverInterface +{ +public: + MuSSP() = default; + ~MuSSP() = default; + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) override; +}; +} // namespace gnn_solver + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp new file mode 100644 index 0000000000000..47a737cf58298 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp @@ -0,0 +1,37 @@ +// 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 TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ + +#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" + +#include +#include + +namespace gnn_solver +{ +class SSP : public GnnSolverInterface +{ +public: + SSP() = default; + ~SSP() = default; + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) override; +}; +} // namespace gnn_solver + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp new file mode 100644 index 0000000000000..0509fb2a07dc5 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp @@ -0,0 +1,134 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ +#define TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ + +#include "tracking_object_merger/data_association/data_association.hpp" +#include "tracking_object_merger/utils/tracker_state.hpp" +#include "tracking_object_merger/utils/utils.hpp" + +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace tracking_object_merger +{ + +class DecorativeTrackerMergerNode : public rclcpp::Node +{ +public: + explicit DecorativeTrackerMergerNode(const rclcpp::NodeOptions & node_options); + enum class PriorityMode : int { Object0 = 0, Object1 = 1, Confidence = 2 }; + +private: + void set3dDataAssociation( + const std::string & prefix, + std::unordered_map> & data_association_map); + + void mainObjectsCallback( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + void subObjectsCallback( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + + bool decorativeMerger( + const MEASUREMENT_STATE input_index, + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + autoware_auto_perception_msgs::msg::TrackedObjects predictFutureState( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg, + const std_msgs::msg::Header & header); + std::optional interpolateObjectState( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg1, + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg2, + const std_msgs::msg::Header & header); + TrackedObjects getTrackedObjects(const std_msgs::msg::Header & header); + TrackerState createNewTracker( + const MEASUREMENT_STATE input_index, rclcpp::Time current_time, + const autoware_auto_perception_msgs::msg::TrackedObject & input_object); + +private: + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + rclcpp::Publisher::SharedPtr + merged_object_pub_; + rclcpp::Subscription::SharedPtr + sub_main_objects_; + rclcpp::Subscription::SharedPtr + sub_sub_objects_; + // debug object publisher + rclcpp::Publisher::SharedPtr + debug_object_pub_; + bool publish_interpolated_sub_objects_; + + /* handle objects */ + std::unordered_map> + input_merger_map_; + MEASUREMENT_STATE main_sensor_type_; + MEASUREMENT_STATE sub_sensor_type_; + std::vector inner_tracker_objects_; + std::unordered_map> data_association_map_; + std::string target_frame_; + std::string base_link_frame_id_; + // buffer to save the sub objects + std::vector + sub_objects_buffer_; + double sub_object_timeout_sec_; + double time_sync_threshold_; + + // tracker default settings + TrackerStateParameter tracker_state_parameter_; + + // merge policy (currently not used) + struct + { + std::string kinematics_to_be_merged; + merger_utils::MergePolicy kinematics_merge_policy; + merger_utils::MergePolicy classification_merge_policy; + merger_utils::MergePolicy existence_prob_merge_policy; + merger_utils::MergePolicy shape_merge_policy; + } merger_policy_params_; + + std::map merger_policy_map_ = { + {"skip", merger_utils::MergePolicy::SKIP}, + {"overwrite", merger_utils::MergePolicy::OVERWRITE}, + {"fusion", merger_utils::MergePolicy::FUSION}}; + + // debug parameters + struct logging + { + bool enable = false; + std::string path; + } logging_; +}; + +} // namespace tracking_object_merger + +#endif // TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp new file mode 100644 index 0000000000000..6bedf7db8d727 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp @@ -0,0 +1,148 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ +#define TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +// Enum classes to distinguish input sources +enum class MEASUREMENT_STATE : int { + NONE = 0, // 0000 + LIDAR = 1, // 0001 + RADAR = 2, // 0010 + CAMERA = 4, // 0100 + LIDAR_RADAR = 3, // 0011 + LIDAR_CAMERA = 5, // 0101 + RADAR_CAMERA = 6, // 0110 + LIDAR_RADAR_CAMERA = 7, // 0111 +}; + +// Operator overloading for MEASUREMENT_STATE +// 1. operator| for MEASUREMENT_STATE +// e.g. MEASUREMENT_STATE::LIDAR | MEASUREMENT_STATE::RADAR == MEASUREMENT_STATE::LIDAR_RADAR +// 2. operator& for MEASUREMENT_STATE +// e.g. MEASUREMENT_STATE::LIDAR_RADAR & MEASUREMENT_STATE::LIDAR == true +// e.g. MEASUREMENT_STATE::LIDAR_RADAR & MEASUREMENT_STATE::CAMERA == false +inline MEASUREMENT_STATE operator|(MEASUREMENT_STATE lhs, MEASUREMENT_STATE rhs) +{ + return static_cast(static_cast(lhs) | static_cast(rhs)); +} +inline bool operator&(MEASUREMENT_STATE combined, MEASUREMENT_STATE target) +{ + return (static_cast(combined) & static_cast(target)) != 0; +} + +// Struct to handle tracker state public parameter +struct TrackerStateParameter +{ + double publish_probability_threshold = 0.5; + double remove_probability_threshold = 0.3; + double default_lidar_existence_probability = 0.8; + double default_radar_existence_probability = 0.7; + double default_camera_existence_probability = 0.6; + double decay_rate = 0.1; + double max_dt = 2.0; +}; + +// Class to handle tracker state +class TrackerState +{ +private: + /* data */ + TrackedObject tracked_object_; + rclcpp::Time last_update_time_; + // Eigen::MatrixXf state_matrix_; + // Eigen::MatrixXf covariance_matrix_; + + // timer handle + std::unordered_map last_updated_time_map_; + double max_dt_ = 2.0; + +public: + TrackerState( + const MEASUREMENT_STATE input, const rclcpp::Time & last_update_time, + const TrackedObject & tracked_object); + TrackerState( + const MEASUREMENT_STATE input_source, const rclcpp::Time & last_update_time, + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object, + const unique_identifier_msgs::msg::UUID & uuid); + + ~TrackerState(); + +public: + void setParameter(const TrackerStateParameter & parameter); + bool predict(const rclcpp::Time & time); + bool predict(const double dt, std::function func); + MEASUREMENT_STATE getCurrentMeasurementState(const rclcpp::Time & current_time) const; + bool updateState( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & tracked_object); + void updateWithLIDAR(const rclcpp::Time & current_time, const TrackedObject & tracked_object); + void updateWithRADAR(const rclcpp::Time & current_time, const TrackedObject & tracked_object); + void updateWithCAMERA(const rclcpp::Time & current_time, const TrackedObject & tracked_object); + void updateWithoutSensor(const rclcpp::Time & current_time); + bool update(const MEASUREMENT_STATE input, const TrackedObject & tracked_object); + bool updateWithFunction( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & tracked_object, + std::function update_func); + // const functions + bool hasUUID(const MEASUREMENT_STATE input, const unique_identifier_msgs::msg::UUID & uuid) const; + bool isValid() const; + bool canPublish() const; + TrackedObject getObject() const; + +public: + // handle uuid + unique_identifier_msgs::msg::UUID const_uuid_; + // each sensor input to uuid map + std::unordered_map> + input_uuid_map_; + MEASUREMENT_STATE measurement_state_; + + // following lifecycle control parameter is overwritten by TrackerStateParameter + std::unordered_map default_existence_probability_map_ = { + {MEASUREMENT_STATE::LIDAR, 0.8}, + {MEASUREMENT_STATE::RADAR, 0.7}, + {MEASUREMENT_STATE::CAMERA, 0.6}, + }; + double existence_probability_ = 0.0; + double publish_probability_threshold_ = 0.5; + double remove_probability_threshold_ = 0.3; + double decay_rate_ = 0.1; +}; + +TrackedObjects getTrackedObjectsFromTrackerStates( + std::vector & tracker_states, const rclcpp::Time & time); + +#endif // TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp new file mode 100644 index 0000000000000..bc6dfef9b80bf --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp @@ -0,0 +1,133 @@ +// 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 TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ +#define TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ + +// #include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +namespace utils +{ +enum MSG_COV_IDX { + X_X = 0, + X_Y = 1, + X_Z = 2, + X_ROLL = 3, + X_PITCH = 4, + X_YAW = 5, + Y_X = 6, + Y_Y = 7, + Y_Z = 8, + Y_ROLL = 9, + Y_PITCH = 10, + Y_YAW = 11, + Z_X = 12, + Z_Y = 13, + Z_Z = 14, + Z_ROLL = 15, + Z_PITCH = 16, + Z_YAW = 17, + ROLL_X = 18, + ROLL_Y = 19, + ROLL_Z = 20, + ROLL_ROLL = 21, + ROLL_PITCH = 22, + ROLL_YAW = 23, + PITCH_X = 24, + PITCH_Y = 25, + PITCH_Z = 26, + PITCH_ROLL = 27, + PITCH_PITCH = 28, + PITCH_YAW = 29, + YAW_X = 30, + YAW_Y = 31, + YAW_Z = 32, + YAW_ROLL = 33, + YAW_PITCH = 34, + YAW_YAW = 35 +}; + +// linear interpolation for tracked objects +TrackedObject linearInterpolationForTrackedObject( + const TrackedObject & obj1, const TrackedObject & obj2); + +// predict tracked objects +TrackedObject predictPastOrFutureTrackedObject(const TrackedObject & obj, const double dt); + +TrackedObjects predictPastOrFutureTrackedObjects( + const TrackedObjects & obj, const std_msgs::msg::Header & header); + +// predict tracked objects +TrackedObjects interpolateTrackedObjects( + const TrackedObjects & objects1, const TrackedObjects & objects2, std_msgs::msg::Header header); + +} // namespace utils + +namespace merger_utils +{ +// merge policy +enum MergePolicy : int { SKIP = 0, OVERWRITE = 1, FUSION = 2 }; + +// object kinematics velocity merger +autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); + +// object classification merger +TrackedObject objectClassificationMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); + +// probability merger +float probabilityMerger(const float main_prob, const float sub_prob, const MergePolicy policy); + +// shape merger +autoware_auto_perception_msgs::msg::Shape shapeMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); + +// update tracked object +void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj); + +void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj); + +void updateOnlyClassification(TrackedObject & main_obj, const TrackedObject & sub_obj); + +void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & sub_obj); + +} // namespace merger_utils + +#endif // TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml new file mode 100644 index 0000000000000..5affbe474e8ae --- /dev/null +++ b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/tracking_object_merger/package.xml b/perception/tracking_object_merger/package.xml new file mode 100644 index 0000000000000..d74a8449b20e6 --- /dev/null +++ b/perception/tracking_object_merger/package.xml @@ -0,0 +1,32 @@ + + + + tracking_object_merger + 0.0.0 + merge tracking object + Yukihiro Saito + Yoshi Ri + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + eigen + mussp + object_recognition_utils + rclcpp + rclcpp_components + tf2 + tf2_ros + tier4_autoware_utils + tier4_perception_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/tracking_object_merger/src/data_association/data_association.cpp b/perception/tracking_object_merger/src/data_association/data_association.cpp new file mode 100644 index 0000000000000..e7ab6077250f8 --- /dev/null +++ b/perception/tracking_object_merger/src/data_association/data_association.cpp @@ -0,0 +1,238 @@ +// 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 "tracking_object_merger/data_association/data_association.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tracking_object_merger/data_association/solver/gnn_solver.hpp" +#include "tracking_object_merger/utils/utils.hpp" + +#include +#include +#include +#include +#include +#include +namespace +{ +double getFormedYawAngle( + const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1, + const bool distinguish_front_or_back = true) +{ + const double yaw0 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat0)); + const double yaw1 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat1)); + const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; + const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; + // Fixed yaw0 to be in the range of +-90 or 180 degrees of yaw1 + double fixed_yaw0 = yaw0; + while (angle_range <= yaw1 - fixed_yaw0) { + fixed_yaw0 = fixed_yaw0 + angle_step; + } + while (angle_range <= fixed_yaw0 - yaw1) { + fixed_yaw0 = fixed_yaw0 - angle_step; + } + return std::fabs(fixed_yaw0 - yaw1); +} +} // namespace + +DataAssociation::DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_rad_vector, std::vector min_iou_vector, + std::vector max_velocity_diff_vector) +: score_threshold_(0.01) +{ + { + const int assign_label_num = static_cast(std::sqrt(can_assign_vector.size())); + Eigen::Map can_assign_matrix_tmp( + can_assign_vector.data(), assign_label_num, assign_label_num); + can_assign_matrix_ = can_assign_matrix_tmp.transpose(); + } + { + const int max_dist_label_num = static_cast(std::sqrt(max_dist_vector.size())); + Eigen::Map max_dist_matrix_tmp( + max_dist_vector.data(), max_dist_label_num, max_dist_label_num); + max_dist_matrix_ = max_dist_matrix_tmp.transpose(); + } + { + const int max_rad_label_num = static_cast(std::sqrt(max_rad_vector.size())); + Eigen::Map max_rad_matrix_tmp( + max_rad_vector.data(), max_rad_label_num, max_rad_label_num); + max_rad_matrix_ = max_rad_matrix_tmp.transpose(); + } + { + const int min_iou_label_num = static_cast(std::sqrt(min_iou_vector.size())); + Eigen::Map min_iou_matrix_tmp( + min_iou_vector.data(), min_iou_label_num, min_iou_label_num); + min_iou_matrix_ = min_iou_matrix_tmp.transpose(); + } + { + const int max_velocity_diff_label_num = + static_cast(std::sqrt(max_velocity_diff_vector.size())); + Eigen::Map max_velocity_diff_matrix_tmp( + max_velocity_diff_vector.data(), max_velocity_diff_label_num, max_velocity_diff_label_num); + max_velocity_diff_matrix_ = max_velocity_diff_matrix_tmp.transpose(); + } + + gnn_solver_ptr_ = std::make_unique(); +} + +void DataAssociation::assign( + const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment) +{ + std::vector> score(src.rows()); + for (int row = 0; row < src.rows(); ++row) { + score.at(row).resize(src.cols()); + for (int col = 0; col < src.cols(); ++col) { + score.at(row).at(col) = src(row, col); + } + } + // Solve + gnn_solver_ptr_->maximizeLinearAssignment(score, &direct_assignment, &reverse_assignment); + + for (auto itr = direct_assignment.begin(); itr != direct_assignment.end();) { + if (src(itr->first, itr->second) < score_threshold_) { + itr = direct_assignment.erase(itr); + continue; + } else { + ++itr; + } + } + for (auto itr = reverse_assignment.begin(); itr != reverse_assignment.end();) { + if (src(itr->second, itr->first) < score_threshold_) { + itr = reverse_assignment.erase(itr); + continue; + } else { + ++itr; + } + } +} + +/** + * @brief calc score matrix between two tracked objects + * This is used to associate input measurements + * + * @param objects0 : measurements + * @param objects1 : base objects(tracker objects) + * @return Eigen::MatrixXd + */ +Eigen::MatrixXd DataAssociation::calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const autoware_auto_perception_msgs::msg::TrackedObjects & objects1) +{ + Eigen::MatrixXd score_matrix = + Eigen::MatrixXd::Zero(objects1.objects.size(), objects0.objects.size()); + for (size_t objects1_idx = 0; objects1_idx < objects1.objects.size(); ++objects1_idx) { + const auto & object1 = objects1.objects.at(objects1_idx); + for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { + const auto & object0 = objects0.objects.at(objects0_idx); + const double score = calcScoreBetweenObjects(object0, object1); + + score_matrix(objects1_idx, objects0_idx) = score; + } + } + return score_matrix; +} + +/** + * @brief calc score matrix between two tracked object and Tracker State + * + * @param objects0 : measurements + * @param objects1 : tracker inner objects + * @return Eigen::MatrixXd + */ +Eigen::MatrixXd DataAssociation::calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const std::vector & trackers) +{ + Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), objects0.objects.size()); + for (size_t trackers_idx = 0; trackers_idx < trackers.size(); ++trackers_idx) { + const auto & object1 = trackers.at(trackers_idx).getObject(); + + for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { + const auto & object0 = objects0.objects.at(objects0_idx); + const double score = calcScoreBetweenObjects(object0, object1); + + score_matrix(trackers_idx, objects0_idx) = score; + } + } + return score_matrix; +} + +double DataAssociation::calcScoreBetweenObjects( + const autoware_auto_perception_msgs::msg::TrackedObject & object0, + const autoware_auto_perception_msgs::msg::TrackedObject & object1) const +{ + const std::uint8_t object1_label = + object_recognition_utils::getHighestProbLabel(object1.classification); + const std::uint8_t object0_label = + object_recognition_utils::getHighestProbLabel(object0.classification); + + std::vector tracker_pose = { + object1.kinematics.pose_with_covariance.pose.position.x, + object1.kinematics.pose_with_covariance.pose.position.y}; + std::vector measurement_pose = { + object0.kinematics.pose_with_covariance.pose.position.x, + object0.kinematics.pose_with_covariance.pose.position.y}; + + double score = 0.0; + if (can_assign_matrix_(object1_label, object0_label)) { + const double max_dist = max_dist_matrix_(object1_label, object0_label); + const double dist = tier4_autoware_utils::calcDistance2d( + object0.kinematics.pose_with_covariance.pose.position, + object1.kinematics.pose_with_covariance.pose.position); + + bool passed_gate = true; + // dist gate + if (passed_gate) { + if (max_dist < dist) passed_gate = false; + } + // angle gate + if (passed_gate) { + const double max_rad = max_rad_matrix_(object1_label, object0_label); + const double angle = getFormedYawAngle( + object0.kinematics.pose_with_covariance.pose.orientation, + object1.kinematics.pose_with_covariance.pose.orientation, false); + if (std::fabs(max_rad) < M_PI && std::fabs(max_rad) < std::fabs(angle)) passed_gate = false; + } + // 2d iou gate + if (passed_gate) { + const double min_iou = min_iou_matrix_(object1_label, object0_label); + const double min_union_iou_area = 1e-2; + const double iou = object_recognition_utils::get2dIoU(object0, object1, min_union_iou_area); + if (iou < min_iou) passed_gate = false; + } + // max velocity diff gate + if (passed_gate) { + const double max_velocity_diff = max_velocity_diff_matrix_(object1_label, object0_label); + // calc absolute velocity diff (only thinking about speed) + const auto speed0 = std::hypot( + object0.kinematics.twist_with_covariance.twist.linear.x, + object0.kinematics.twist_with_covariance.twist.linear.y); + const auto speed1 = std::hypot( + object1.kinematics.twist_with_covariance.twist.linear.x, + object1.kinematics.twist_with_covariance.twist.linear.y); + const double velocity_diff = std::fabs(speed0 - speed1); + if (max_velocity_diff < velocity_diff) passed_gate = false; + } + + // all gate is passed + if (passed_gate) { + score = (max_dist - std::min(dist, max_dist)) / max_dist; + if (score < score_threshold_) score = 0.0; + } + } + return score; +} diff --git a/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp new file mode 100644 index 0000000000000..fcc79c0a3cbd7 --- /dev/null +++ b/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp @@ -0,0 +1,41 @@ +// 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 "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace gnn_solver +{ +void MuSSP::maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) +{ + // Terminate if the graph is empty + if (cost.size() == 0 || cost.at(0).size() == 0) { + return; + } + + // Solve DA by muSSP + solve_muSSP(cost, direct_assignment, reverse_assignment); +} +} // namespace gnn_solver diff --git a/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp new file mode 100644 index 0000000000000..133f0d377373f --- /dev/null +++ b/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp @@ -0,0 +1,370 @@ +// 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 "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace gnn_solver +{ +struct ResidualEdge +{ + // Destination node + const int dst; + int capacity; + const double cost; + int flow; + // Access to the reverse edge by adjacency_list.at(dst).at(reverse) + const int reverse; + + // ResidualEdge() + // : dst(0), capacity(0), cost(0), flow(0), reverse(0) {} + + ResidualEdge(int dst, int capacity, double cost, int flow, int reverse) + : dst(dst), capacity(capacity), cost(cost), flow(flow), reverse(reverse) + { + } +}; + +void SSP::maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) +{ + // NOTE: Need to set as default arguments + bool sparse_cost = true; + // bool sparse_cost = false; + + // Hyperparameters + // double MAX_COST = 6; + const double MAX_COST = 10; + const double INF_DIST = 10000000; + const double EPS = 1e-5; + + // When there is no agents or no tasks, terminate + if (cost.size() == 0 || cost.at(0).size() == 0) { + return; + } + + // Construct a bipartite graph from the cost matrix + int n_agents = cost.size(); + int n_tasks = cost.at(0).size(); + + int n_dummies; + if (sparse_cost) { + n_dummies = n_agents; + } else { + n_dummies = 0; + } + + int source = 0; + int sink = n_agents + n_tasks + 1; + int n_nodes = n_agents + n_tasks + n_dummies + 2; + + // // Print cost matrix + // std::cout << std::endl; + // for (int agent = 0; agent < n_agents; agent++) + // { + // for (int task = 0; task < n_tasks; task++) + // { + // std::cout << cost.at(agent).at(task) << " "; + // } + // std::cout << std::endl; + // } + + // std::chrono::system_clock::time_point start_time, end_time; + // start_time = std::chrono::system_clock::now(); + + // Adjacency list of residual graph (index: nodes) + // - 0: source node + // - {1, ..., n_agents}: agent nodes + // - {n_agents+1, ..., n_agents+n_tasks}: task nodes + // - n_agents+n_tasks+1: sink node + // - {n_agents+n_tasks+2, ..., n_agents+n_tasks+1+n_agents}: + // dummy node (when sparse_cost is true) + std::vector> adjacency_list(n_nodes); + + // Reserve memory + for (int v = 0; v < n_nodes; ++v) { + if (v == source) { + // Source + adjacency_list.at(v).reserve(n_agents); + } else if (v <= n_agents) { + // Agents + adjacency_list.at(v).reserve(n_tasks + 1 + 1); + } else if (v <= n_agents + n_tasks) { + // Tasks + adjacency_list.at(v).reserve(n_agents + 1); + } else if (v == sink) { + // Sink + adjacency_list.at(v).reserve(n_tasks + n_dummies); + } else { + // Dummies + adjacency_list.at(v).reserve(2); + } + } + + // Add edges form source + for (int agent = 0; agent < n_agents; ++agent) { + // From source to agent + adjacency_list.at(source).emplace_back(agent + 1, 1, 0, 0, adjacency_list.at(agent + 1).size()); + // From agent to source + adjacency_list.at(agent + 1).emplace_back( + source, 0, 0, 0, adjacency_list.at(source).size() - 1); + } + + // Add edges from agents + for (int agent = 0; agent < n_agents; ++agent) { + for (int task = 0; task < n_tasks; ++task) { + if (!sparse_cost || cost.at(agent).at(task) > EPS) { + // From agent to task + adjacency_list.at(agent + 1).emplace_back( + task + n_agents + 1, 1, MAX_COST - cost.at(agent).at(task), 0, + adjacency_list.at(task + n_agents + 1).size()); + + // From task to agent + adjacency_list.at(task + n_agents + 1) + .emplace_back( + agent + 1, 0, cost.at(agent).at(task) - MAX_COST, 0, + adjacency_list.at(agent + 1).size() - 1); + } + } + } + + // Add edges form tasks + for (int task = 0; task < n_tasks; ++task) { + // From task to sink + adjacency_list.at(task + n_agents + 1) + .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size()); + + // From sink to task + adjacency_list.at(sink).emplace_back( + task + n_agents + 1, 0, 0, 0, adjacency_list.at(task + n_agents + 1).size() - 1); + } + + // Add edges from dummy + if (sparse_cost) { + for (int agent = 0; agent < n_agents; ++agent) { + // From agent to dummy + adjacency_list.at(agent + 1).emplace_back( + agent + n_agents + n_tasks + 2, 1, MAX_COST, 0, + adjacency_list.at(agent + n_agents + n_tasks + 2).size()); + + // From dummy to agent + adjacency_list.at(agent + n_agents + n_tasks + 2) + .emplace_back(agent + 1, 0, -MAX_COST, 0, adjacency_list.at(agent + 1).size() - 1); + + // From dummy to sink + adjacency_list.at(agent + n_agents + n_tasks + 2) + .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size()); + + // From sink to dummy + adjacency_list.at(sink).emplace_back( + agent + n_agents + n_tasks + 2, 0, 0, 0, + adjacency_list.at(agent + n_agents + n_tasks + 2).size() - 1); + } + } + + // Maximum flow value + const int max_flow = std::min(n_agents, n_tasks); + + // Feasible potentials + std::vector potentials(n_nodes, 0); + + // Shortest path lengths + std::vector distances(n_nodes, INF_DIST); + + // Whether previously visited the node or not + std::vector is_visited(n_nodes, false); + + // Parent node () + std::vector> prev_values(n_nodes); + + for (int i = 0; i < max_flow; ++i) { + // Initialize priority queue () + std::priority_queue< + std::pair, std::vector>, + std::greater>> + p_queue; + + // Reset all trajectory states + if (i > 0) { + std::fill(distances.begin(), distances.end(), INF_DIST); + std::fill(is_visited.begin(), is_visited.end(), false); + } + + // Start trajectory from the source node + p_queue.push(std::make_pair(0, source)); + distances.at(source) = 0; + + while (!p_queue.empty()) { + // Get the next element + std::pair cur_elem = p_queue.top(); + // std::cout << "[pop]: (" << cur_elem.first << ", " << cur_elem.second << ")" << std::endl; + p_queue.pop(); + + double cur_node_dist = cur_elem.first; + int cur_node = cur_elem.second; + + // If already visited node, skip and continue + if (is_visited.at(cur_node)) { + continue; + } + assert(cur_node_dist == distances.at(cur_node)); + + // Mark as visited + is_visited.at(cur_node) = true; + // Update potential + potentials.at(cur_node) += cur_node_dist; + + // When reached to the sink node, terminate. + if (cur_node == sink) { + break; + } + + // Loop over the incident nodes(/edges) + for (auto it_incident_edge = adjacency_list.at(cur_node).cbegin(); + it_incident_edge != adjacency_list.at(cur_node).cend(); it_incident_edge++) { + // If the node is not visited and have capacity to increase flow, visit. + if (!is_visited.at(it_incident_edge->dst) && it_incident_edge->capacity > 0) { + // Calculate reduced cost + double reduced_cost = + it_incident_edge->cost + potentials.at(cur_node) - potentials.at(it_incident_edge->dst); + assert(reduced_cost >= 0); + if (distances.at(it_incident_edge->dst) > reduced_cost) { + distances.at(it_incident_edge->dst) = reduced_cost; + prev_values.at(it_incident_edge->dst) = + std::make_pair(cur_node, it_incident_edge - adjacency_list.at(cur_node).cbegin()); + // std::cout << "[push]: (" << reduced_cost << ", " << next_v << ")" << std::endl; + p_queue.push(std::make_pair(reduced_cost, it_incident_edge->dst)); + } + } + } + } + + // Shortest path length to sink is greater than MAX_COST, + // which means no non-dummy routes left, terminate + if (potentials.at(sink) >= MAX_COST) { + break; + } + + // Update potentials of unvisited nodes + for (int v = 0; v < n_nodes; ++v) { + if (!is_visited.at(v)) { + potentials.at(v) += distances.at(sink); + } + } + // //Print potentials + // for (int v = 0; v < n_nodes; ++v) + // { + // std::cout << potentials.at(v) << ", "; + // } + // std::cout << std::endl; + + // Increase/decrease flow and capacity along the shortest path from the source to the sink + int v = sink; + int prev_v; + while (v != source) { + ResidualEdge & e_forward = + adjacency_list.at(prev_values.at(v).first).at(prev_values.at(v).second); + assert(e_forward.dst == v); + ResidualEdge & e_backward = adjacency_list.at(v).at(e_forward.reverse); + prev_v = e_backward.dst; + + if (e_backward.flow == 0) { + // Increase flow + // State A + assert(e_forward.capacity == 1); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 0); + assert(e_backward.flow == 0); + + e_forward.capacity -= 1; + e_forward.flow += 1; + e_backward.capacity += 1; + + // State B + assert(e_forward.capacity == 0); + assert(e_forward.flow == 1); + assert(e_backward.capacity == 1); + assert(e_backward.flow == 0); + } else { + // Decrease flow + // State B + assert(e_forward.capacity == 1); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 0); + assert(e_backward.flow == 1); + + e_forward.capacity -= 1; + e_backward.capacity += 1; + e_backward.flow -= 1; + + // State A + assert(e_forward.capacity == 0); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 1); + assert(e_backward.flow == 0); + } + + v = prev_v; + } + +#ifndef NDEBUG + // Check if the potentials are feasible potentials + for (int v = 0; v < n_nodes; ++v) { + for (auto it_incident_edge = adjacency_list.at(v).cbegin(); + it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) { + if (it_incident_edge->capacity > 0) { + double reduced_cost = + it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst); + assert(reduced_cost >= 0); + } + } + } +#endif + } + + // Output + for (int agent = 0; agent < n_agents; ++agent) { + for (auto it_incident_edge = adjacency_list.at(agent + 1).cbegin(); + it_incident_edge != adjacency_list.at(agent + 1).cend(); ++it_incident_edge) { + int task = it_incident_edge->dst - (n_agents + 1); + + // If the flow value is 1 and task is not dummy, assign the task to the agent. + if (it_incident_edge->flow == 1 && 0 <= task && task < n_tasks) { + (*direct_assignment)[agent] = task; + (*reverse_assignment)[task] = agent; + break; + } + } + } + +#ifndef NDEBUG + // Check if the result is valid assignment + for (int agent = 0; agent < n_agents; ++agent) { + if (direct_assignment->find(agent) != direct_assignment->cend()) { + int task = (*direct_assignment).at(agent); + assert(direct_assignment->at(agent) == task); + assert(reverse_assignment->at(task) == agent); + } + } +#endif +} +} // namespace gnn_solver diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp new file mode 100644 index 0000000000000..47a333691eabf --- /dev/null +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -0,0 +1,403 @@ +// 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 "tracking_object_merger/decorative_tracker_merger.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" +#include "tracking_object_merger/utils/utils.hpp" + +#include + +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +namespace tracking_object_merger +{ + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +// get unix time from header +double getUnixTime(const std_msgs::msg::Header & header) +{ + return header.stamp.sec + header.stamp.nanosec * 1e-9; +} + +// calc association score matrix +Eigen::MatrixXd calcScoreMatrixForAssociation( + const MEASUREMENT_STATE measurement_state, + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const std::vector & trackers, + const std::unordered_map> & data_association_map + // const bool debug_log, const std::string & file_name // do not logging for now +) +{ + // get current time + const rclcpp::Time current_time = rclcpp::Time(objects0.header.stamp); + + // calc score matrix + Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), objects0.objects.size()); + for (size_t trackers_idx = 0; trackers_idx < trackers.size(); ++trackers_idx) { + const auto & tracker_obj = trackers.at(trackers_idx); + const auto & object1 = tracker_obj.getObject(); + const auto & tracker_state = tracker_obj.getCurrentMeasurementState(current_time); + + for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { + const auto & object0 = objects0.objects.at(objects0_idx); + // switch calc score function by input and trackers measurement state + // we assume that lidar and radar are exclusive + double score; + const auto input_has_lidar = measurement_state & MEASUREMENT_STATE::LIDAR; + const auto tracker_has_lidar = tracker_state & MEASUREMENT_STATE::LIDAR; + if (input_has_lidar && tracker_has_lidar) { + score = data_association_map.at("lidar-lidar")->calcScoreBetweenObjects(object0, object1); + } else if (!input_has_lidar && !tracker_has_lidar) { + score = data_association_map.at("radar-radar")->calcScoreBetweenObjects(object0, object1); + } else { + score = data_association_map.at("lidar-radar")->calcScoreBetweenObjects(object0, object1); + } + score_matrix(trackers_idx, objects0_idx) = score; + } + } + return score_matrix; +} + +DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("decorative_object_merger_node", node_options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_) +{ + // Subscriber + sub_main_objects_ = create_subscription( + "input/main_object", rclcpp::QoS{1}, + std::bind(&DecorativeTrackerMergerNode::mainObjectsCallback, this, std::placeholders::_1)); + sub_sub_objects_ = create_subscription( + "input/sub_object", rclcpp::QoS{1}, + std::bind(&DecorativeTrackerMergerNode::subObjectsCallback, this, std::placeholders::_1)); + + // merged object publisher + merged_object_pub_ = create_publisher("output/object", rclcpp::QoS{1}); + // debug object publisher + debug_object_pub_ = + create_publisher("debug/interpolated_sub_object", rclcpp::QoS{1}); + + // logging + logging_.enable = declare_parameter("enable_logging"); + logging_.path = declare_parameter("logging_file_path"); + + // Parameters + publish_interpolated_sub_objects_ = declare_parameter("publish_interpolated_sub_objects"); + base_link_frame_id_ = declare_parameter("base_link_frame_id"); + time_sync_threshold_ = declare_parameter("time_sync_threshold"); + sub_object_timeout_sec_ = declare_parameter("sub_object_timeout_sec"); + // default setting parameter for tracker + tracker_state_parameter_.remove_probability_threshold = + declare_parameter("tracker_state_parameter.remove_probability_threshold"); + tracker_state_parameter_.publish_probability_threshold = + declare_parameter("tracker_state_parameter.publish_probability_threshold"); + tracker_state_parameter_.default_lidar_existence_probability = + declare_parameter("tracker_state_parameter.default_lidar_existence_probability"); + tracker_state_parameter_.default_radar_existence_probability = + declare_parameter("tracker_state_parameter.default_radar_existence_probability"); + tracker_state_parameter_.default_camera_existence_probability = + declare_parameter("tracker_state_parameter.default_camera_existence_probability"); + tracker_state_parameter_.decay_rate = + declare_parameter("tracker_state_parameter.decay_rate"); + tracker_state_parameter_.max_dt = declare_parameter("tracker_state_parameter.max_dt"); + + const std::string main_sensor_type = declare_parameter("main_sensor_type"); + const std::string sub_sensor_type = declare_parameter("sub_sensor_type"); + // str to MEASUREMENT_STATE + auto str2measurement_state = [](const std::string & str) { + if (str == "lidar") { + return MEASUREMENT_STATE::LIDAR; + } else if (str == "radar") { + return MEASUREMENT_STATE::RADAR; + } else { + throw std::runtime_error("invalid sensor type"); + } + }; + main_sensor_type_ = str2measurement_state(main_sensor_type); + sub_sensor_type_ = str2measurement_state(sub_sensor_type); + + /* init association **/ + // lidar-lidar association matrix + set3dDataAssociation("lidar-lidar", data_association_map_); + // lidar-radar association matrix + set3dDataAssociation("lidar-radar", data_association_map_); + // radar-radar association matrix + set3dDataAssociation("radar-radar", data_association_map_); +} + +void DecorativeTrackerMergerNode::set3dDataAssociation( + const std::string & prefix, + std::unordered_map> & data_association_map) +{ + const auto tmp = this->declare_parameter>(prefix + ".can_assign_matrix"); + const std::vector can_assign_matrix(tmp.begin(), tmp.end()); + const auto max_dist_matrix = + this->declare_parameter>(prefix + ".max_dist_matrix"); + const auto max_rad_matrix = + this->declare_parameter>(prefix + ".max_rad_matrix"); + const auto min_iou_matrix = + this->declare_parameter>(prefix + ".min_iou_matrix"); + const auto max_velocity_diff_matrix = + this->declare_parameter>(prefix + ".max_velocity_diff_matrix"); + + data_association_map[prefix] = std::make_unique( + can_assign_matrix, max_dist_matrix, max_rad_matrix, min_iou_matrix, max_velocity_diff_matrix); +} + +/** + * @brief callback function for main objects + * + * @param main_objects + * @note if there are no sub objects, publish main objects as it is + * else, merge main objects and sub objects + */ +void DecorativeTrackerMergerNode::mainObjectsCallback( + const TrackedObjects::ConstSharedPtr & main_objects) +{ + // try to merge sub object + if (!sub_objects_buffer_.empty()) { + // get interpolated sub objects + // get newest sub objects which timestamp is earlier to main objects + TrackedObjects::ConstSharedPtr closest_time_sub_objects; + TrackedObjects::ConstSharedPtr closest_time_sub_objects_later; + for (const auto & sub_object : sub_objects_buffer_) { + if (getUnixTime(sub_object->header) < getUnixTime(main_objects->header)) { + closest_time_sub_objects = sub_object; + } else { + closest_time_sub_objects_later = sub_object; + break; + } + } + // get delay compensated sub objects + const auto interpolated_sub_objects = interpolateObjectState( + closest_time_sub_objects, closest_time_sub_objects_later, main_objects->header); + if (interpolated_sub_objects.has_value()) { + // Merge sub objects + const auto interp_sub_objs = interpolated_sub_objects.value(); + debug_object_pub_->publish(interp_sub_objs); + this->decorativeMerger( + sub_sensor_type_, std::make_shared(interpolated_sub_objects.value())); + } else { + RCLCPP_DEBUG(this->get_logger(), "interpolated_sub_objects is null"); + } + } + + // try to merge main object + this->decorativeMerger(main_sensor_type_, main_objects); + + merged_object_pub_->publish(getTrackedObjects(main_objects->header)); +} + +/** + * @brief callback function for sub objects + * + * @param msg + * @note push back sub objects to buffer and remove old sub objects + */ +void DecorativeTrackerMergerNode::subObjectsCallback(const TrackedObjects::ConstSharedPtr & msg) +{ + sub_objects_buffer_.push_back(msg); + // remove old sub objects + // const auto now = get_clock()->now(); + const auto now = rclcpp::Time(msg->header.stamp); + const auto remove_itr = std::remove_if( + sub_objects_buffer_.begin(), sub_objects_buffer_.end(), [now, this](const auto & sub_object) { + return (now - rclcpp::Time(sub_object->header.stamp)).seconds() > sub_object_timeout_sec_; + }); + sub_objects_buffer_.erase(remove_itr, sub_objects_buffer_.end()); +} + +/** + * @brief merge objects into inner_tracker_objects_ + * + * @param main_objects + * @return TrackedObjects + * + * @note 1. interpolate sub objects to sync main objects + * 2. do association + * 3. merge objects + */ +bool DecorativeTrackerMergerNode::decorativeMerger( + const MEASUREMENT_STATE input_sensor, const TrackedObjects::ConstSharedPtr & input_objects_msg) +{ + // get current time + const auto current_time = rclcpp::Time(input_objects_msg->header.stamp); + if (input_objects_msg->objects.empty()) { + return false; + } + if (inner_tracker_objects_.empty()) { + for (const auto & object : input_objects_msg->objects) { + inner_tracker_objects_.push_back(createNewTracker(input_sensor, current_time, object)); + } + } + + // do prediction for inner objects + for (auto & object : inner_tracker_objects_) { + object.predict(current_time); + } + + // TODO(yoshiri): pre-association + + // associate inner objects and input objects + /* global nearest neighbor */ + std::unordered_map direct_assignment, reverse_assignment; + const auto & objects1 = input_objects_msg->objects; + Eigen::MatrixXd score_matrix = calcScoreMatrixForAssociation( + input_sensor, *input_objects_msg, inner_tracker_objects_, data_association_map_); + data_association_map_.at("lidar-lidar") + ->assign(score_matrix, direct_assignment, reverse_assignment); + + // look for tracker + for (int tracker_idx = 0; tracker_idx < static_cast(inner_tracker_objects_.size()); + ++tracker_idx) { + auto & object0_state = inner_tracker_objects_.at(tracker_idx); + if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found and merge + const auto & object1 = objects1.at(direct_assignment.at(tracker_idx)); + // merge object1 into object0_state + object0_state.updateState(input_sensor, current_time, object1); + } else { // not found + // decrease existence probability + object0_state.updateWithoutSensor(current_time); + } + } + // look for new object + for (int object1_idx = 0; object1_idx < static_cast(objects1.size()); ++object1_idx) { + const auto & object1 = objects1.at(object1_idx); + if (reverse_assignment.find(object1_idx) != reverse_assignment.end()) { // found + } else { // not found + inner_tracker_objects_.push_back(createNewTracker(input_sensor, current_time, object1)); + } + } + return true; +} + +/** + * @brief interpolate sub objects to sync main objects + * + * @param former_msg + * @param latter_msg + * @param output_header + * @return std::optional + * + * @note 1. if both msg is nullptr, return null optional + * 2. if former_msg is nullptr, return latter_msg + * 3. if latter_msg is nullptr, return former_msg + * 4. if both msg is not nullptr, do the interpolation + */ +std::optional DecorativeTrackerMergerNode::interpolateObjectState( + const TrackedObjects::ConstSharedPtr & former_msg, + const TrackedObjects::ConstSharedPtr & latter_msg, const std_msgs::msg::Header & output_header) +{ + // Assumption: output_header must be newer than former_msg and older than latter_msg + // There three possible patterns + // 1. both msg is nullptr + // 2. former_msg is nullptr + // 3. latter_msg is nullptr + // 4. both msg is not nullptr + + // 1. both msg is nullptr + if (former_msg == nullptr && latter_msg == nullptr) { + // return null optional + RCLCPP_DEBUG(this->get_logger(), "interpolation err: both msg is nullptr"); + return std::nullopt; + } + + // 2. former_msg is nullptr + if (former_msg == nullptr) { + // std::cout << "interpolation: 2. former_msg is nullptr" << std::endl; + // depends on header stamp difference + if ( + (rclcpp::Time(latter_msg->header.stamp) - rclcpp::Time(output_header.stamp)).seconds() > + time_sync_threshold_) { + // do nothing + RCLCPP_DEBUG( + this->get_logger(), "interpolation err: latter msg is too different from output msg"); + return std::nullopt; + } else { // else, return latter_msg + return *latter_msg; + } + + // 3. latter_msg is nullptr + } else if (latter_msg == nullptr) { + // std::cout << "interpolation: 3. latter_msg is nullptr" << std::endl; + // depends on header stamp difference + const auto dt = + (rclcpp::Time(output_header.stamp) - rclcpp::Time(former_msg->header.stamp)).seconds(); + if (dt > time_sync_threshold_) { + // do nothing + RCLCPP_DEBUG(this->get_logger(), "interpolation err: former msg is too old"); + return std::nullopt; + } else { + // else, return former_msg + return utils::predictPastOrFutureTrackedObjects(*former_msg, output_header); + } + + // 4. both msg is not nullptr + } else { + // do the interpolation + // std::cout << "interpolation: 4. both msg is not nullptr" << std::endl; + TrackedObjects interpolated_msg = + utils::interpolateTrackedObjects(*former_msg, *latter_msg, output_header); + return interpolated_msg; + } +} + +// get merged objects +TrackedObjects DecorativeTrackerMergerNode::getTrackedObjects(const std_msgs::msg::Header & header) +{ + // get main objects + rclcpp::Time current_time = rclcpp::Time(header.stamp); + return getTrackedObjectsFromTrackerStates(inner_tracker_objects_, current_time); +} + +// create new tracker +TrackerState DecorativeTrackerMergerNode::createNewTracker( + const MEASUREMENT_STATE input_index, rclcpp::Time current_time, + const autoware_auto_perception_msgs::msg::TrackedObject & input_object) +{ + // check if object id is not included in innner_tracker_objects_ + for (const auto & object : inner_tracker_objects_) { + if (object.const_uuid_ == input_object.object_id) { + // create new uuid + unique_identifier_msgs::msg::UUID uuid; + // Generate random number + std::mt19937 gen(std::random_device{}()); + std::independent_bits_engine bit_eng(gen); + std::generate(uuid.uuid.begin(), uuid.uuid.end(), bit_eng); + auto new_tracker = TrackerState(input_index, current_time, input_object, uuid); + new_tracker.setParameter(tracker_state_parameter_); + return new_tracker; + } + } + // if not found, just create new tracker + auto new_tracker = TrackerState(input_index, current_time, input_object); + new_tracker.setParameter(tracker_state_parameter_); + return new_tracker; +} + +} // namespace tracking_object_merger + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(tracking_object_merger::DecorativeTrackerMergerNode) diff --git a/perception/tracking_object_merger/src/utils/tracker_state.cpp b/perception/tracking_object_merger/src/utils/tracker_state.cpp new file mode 100644 index 0000000000000..f0dc657145a24 --- /dev/null +++ b/perception/tracking_object_merger/src/utils/tracker_state.cpp @@ -0,0 +1,321 @@ +// 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 "tracking_object_merger/utils/tracker_state.hpp" + +#include "tracking_object_merger/utils/utils.hpp" + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +/** + * @brief Construct a new Tracker State:: Tracker State object + * + * @param input_source : input source distinguished + * @param tracked_object : input source tracked object + * @param last_update_time : last update time + */ +TrackerState::TrackerState( + const MEASUREMENT_STATE input_source, const rclcpp::Time & last_update_time, + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object) +: tracked_object_(tracked_object), + last_update_time_(last_update_time), + const_uuid_(tracked_object.object_id), + measurement_state_(input_source) +{ + input_uuid_map_[input_source] = tracked_object_.object_id; + last_updated_time_map_[input_source] = last_update_time; + existence_probability_ = default_existence_probability_map_[input_source]; +} + +TrackerState::TrackerState( + const MEASUREMENT_STATE input_source, const rclcpp::Time & last_update_time, + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object, + const unique_identifier_msgs::msg::UUID & uuid) +: tracked_object_(tracked_object), + last_update_time_(last_update_time), + const_uuid_(uuid), + measurement_state_(input_source) +{ + input_uuid_map_[input_source] = tracked_object_.object_id; + last_updated_time_map_[input_source] = last_update_time; + existence_probability_ = default_existence_probability_map_[input_source]; +} + +void TrackerState::setParameter(const TrackerStateParameter & parameter) +{ + max_dt_ = parameter.max_dt; + publish_probability_threshold_ = parameter.publish_probability_threshold; + remove_probability_threshold_ = parameter.remove_probability_threshold; + default_existence_probability_map_.at(MEASUREMENT_STATE::LIDAR) = + parameter.default_lidar_existence_probability; + default_existence_probability_map_.at(MEASUREMENT_STATE::RADAR) = + parameter.default_radar_existence_probability; + default_existence_probability_map_.at(MEASUREMENT_STATE::CAMERA) = + parameter.default_camera_existence_probability; + decay_rate_ = parameter.decay_rate; +} + +/** + * @brief Predict state to current time + * + * @param current_time + * @return true + * @return false + */ +bool TrackerState::predict(const rclcpp::Time & current_time) +{ + // get dt and give warning if dt is too large + double dt = (current_time - last_update_time_).seconds(); + if (std::abs(dt) > max_dt_) { + std::cerr << "[tracking_object_merger] dt is too large: " << dt << std::endl; + return false; + } + + // do prediction + last_update_time_ = current_time; + return this->predict(dt, utils::predictPastOrFutureTrackedObject); +} + +/** + * @brief Predict state to current time with given update function + * + * @param dt : time to predict + * @param func : update function (e.g. PredictPastOrFutureTrackedObject) + * @return true: prediction succeeded + * @return false: prediction failed + */ +bool TrackerState::predict( + const double dt, std::function func) +{ + const auto predicted_object = func(tracked_object_, dt); + tracked_object_ = predicted_object; + return true; +} + +// get current measurement state +MEASUREMENT_STATE TrackerState::getCurrentMeasurementState(const rclcpp::Time & current_time) const +{ + MEASUREMENT_STATE measurement_state = MEASUREMENT_STATE::NONE; + // check LIDAR + if (last_updated_time_map_.find(MEASUREMENT_STATE::LIDAR) != last_updated_time_map_.end()) { + if ((current_time - last_updated_time_map_.at(MEASUREMENT_STATE::LIDAR)).seconds() < max_dt_) { + measurement_state = measurement_state | MEASUREMENT_STATE::LIDAR; + } + } + // check RADAR + if (last_updated_time_map_.find(MEASUREMENT_STATE::RADAR) != last_updated_time_map_.end()) { + if ((current_time - last_updated_time_map_.at(MEASUREMENT_STATE::RADAR)).seconds() < max_dt_) { + measurement_state = measurement_state | MEASUREMENT_STATE::RADAR; + } + } + // check CAMERA + if (last_updated_time_map_.find(MEASUREMENT_STATE::CAMERA) != last_updated_time_map_.end()) { + if ((current_time - last_updated_time_map_.at(MEASUREMENT_STATE::CAMERA)).seconds() < max_dt_) { + measurement_state = measurement_state | MEASUREMENT_STATE::CAMERA; + } + } + return measurement_state; +} + +bool TrackerState::updateState( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & tracked_object) +{ + // calc dt + double dt = (current_time - last_update_time_).seconds(); + if (dt < 0.0) { + std::cerr << "[tracking_object_merger] dt is negative: " << dt << std::endl; + return false; + } + + // predict + if (dt > 0.0) { + this->predict(dt, utils::predictPastOrFutureTrackedObject); + } + + // get current measurement state + measurement_state_ = getCurrentMeasurementState(current_time); + + // update with input + if (input & MEASUREMENT_STATE::LIDAR) { + updateWithLIDAR(current_time, tracked_object); + } + if (input & MEASUREMENT_STATE::RADAR) { + updateWithRADAR(current_time, tracked_object); + } + if (input & MEASUREMENT_STATE::CAMERA) { + updateWithCAMERA(current_time, tracked_object); + } + return true; +} + +void TrackerState::updateWithCAMERA( + const rclcpp::Time & current_time, const TrackedObject & tracked_object) +{ + // update tracked object + updateWithFunction( + MEASUREMENT_STATE::CAMERA, current_time, tracked_object, + merger_utils::updateOnlyClassification); +} + +void TrackerState::updateWithLIDAR( + const rclcpp::Time & current_time, const TrackedObject & tracked_object) +{ + // if radar is available, do not update velocity + if (measurement_state_ & MEASUREMENT_STATE::RADAR) { + // update tracked object + updateWithFunction( + MEASUREMENT_STATE::LIDAR, current_time, tracked_object, merger_utils::updateExceptVelocity); + } else { + // else just update tracked object + updateWithFunction( + MEASUREMENT_STATE::LIDAR, current_time, tracked_object, + merger_utils::updateWholeTrackedObject); + } +} + +void TrackerState::updateWithRADAR( + const rclcpp::Time & current_time, const TrackedObject & tracked_object) +{ + // if lidar is available, update only velocity + if (measurement_state_ & MEASUREMENT_STATE::LIDAR) { + // update tracked object + updateWithFunction( + MEASUREMENT_STATE::RADAR, current_time, tracked_object, + merger_utils::updateOnlyObjectVelocity); + } else { + // else just update tracked object + updateWithFunction( + MEASUREMENT_STATE::RADAR, current_time, tracked_object, + merger_utils::updateWholeTrackedObject); + } +} + +bool TrackerState::updateWithFunction( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & input_tracked_object, + std::function update_func) +{ + // put input uuid and last update time + if (current_time > last_update_time_) { + const auto predict_successful = predict(current_time); + if (!predict_successful) { + return false; + } + } + + // update with measurement state + last_updated_time_map_[input] = current_time; + input_uuid_map_[input] = input_tracked_object.object_id; + measurement_state_ = measurement_state_ | input; + existence_probability_ = + std::max(existence_probability_, default_existence_probability_map_[input]); + + // update tracked object + update_func(tracked_object_, input_tracked_object); + tracked_object_.object_id = const_uuid_; // overwrite uuid to stay same + return true; +} + +void TrackerState::updateWithoutSensor(const rclcpp::Time & current_time) +{ + // calc dt + double dt = (current_time - last_update_time_).seconds(); + if (dt < 0) { + std::cerr << "[tracking_object_merger] dt is negative: " << dt << std::endl; + return; + } + + // predict + if (dt > 0.0) { + const auto predict_successful = this->predict(dt, utils::predictPastOrFutureTrackedObject); + if (!predict_successful) { + existence_probability_ = 0.0; // remove object + } + } + + // reduce probability + existence_probability_ -= decay_rate_; + if (existence_probability_ < 0.0) { + existence_probability_ = 0.0; + } +} + +TrackedObject TrackerState::getObject() const +{ + TrackedObject tracked_object = tracked_object_; + tracked_object.object_id = const_uuid_; + tracked_object.existence_probability = + static_cast(existence_probability_); // double -> float + return tracked_object; +} + +bool TrackerState::hasUUID( + const MEASUREMENT_STATE input, const unique_identifier_msgs::msg::UUID & uuid) const +{ + if (input_uuid_map_.find(input) == input_uuid_map_.end()) { + return false; + } + return input_uuid_map_.at(input) == uuid; +} + +bool TrackerState::isValid() const +{ + // check if tracker state is valid + if (existence_probability_ < remove_probability_threshold_) { + return false; + } + return true; +} + +bool TrackerState::canPublish() const +{ + // check if tracker state is valid + if (existence_probability_ < publish_probability_threshold_) { + return false; + } + return true; +} + +TrackerState::~TrackerState() +{ + // destructor +} + +TrackedObjects getTrackedObjectsFromTrackerStates( + std::vector & tracker_states, const rclcpp::Time & current_time) +{ + TrackedObjects tracked_objects; + + // sanitize and get tracked objects + for (auto it = tracker_states.begin(); it != tracker_states.end();) { + // check if tracker state is valid + if (it->isValid()) { + if (it->canPublish()) { + // if valid, get tracked object + tracked_objects.objects.push_back(it->getObject()); + } + ++it; + } else { + // if not valid, delete tracker state + it = tracker_states.erase(it); + } + } + + // update header + tracked_objects.header.stamp = current_time; + tracked_objects.header.frame_id = "map"; // TODO(yoshiri): get frame_id from input + return tracked_objects; +} diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp new file mode 100644 index 0000000000000..5987bdc2d1bef --- /dev/null +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -0,0 +1,403 @@ +// 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 "tracking_object_merger/utils/utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +namespace utils +{ + +/** + * @brief linear interpolation for tracked object + * + * @param obj1 : obj1 and obj2 must have same shape type + * @param obj2 : obj1 and obj2 must have same shape type + * @param weight : 0 <= weight <= 1, weight = 0: obj1, weight = 1: obj2 + * @return TrackedObject : interpolated object + */ +TrackedObject linearInterpolationForTrackedObject( + const TrackedObject & obj1, const TrackedObject & obj2, const double weight) +{ + // interpolate obj1 and obj2 with weight: obj1 * (1 - weight) + obj2 * weight + // weight = 0: obj1, weight = 1: obj2 + + // weight check (0 <= weight <= 1) + if (weight < 0 || weight > 1) { + std::cerr << "weight must be 0 <= weight <= 1 in linearInterpolationForTrackedObject function." + << std::endl; + return obj1; + } + + // output object + TrackedObject output; + // copy input object at first + output = obj1; + + // get current twist and pose + const auto twist1 = obj1.kinematics.twist_with_covariance.twist; + const auto pose1 = obj1.kinematics.pose_with_covariance.pose; + const auto twist2 = obj2.kinematics.twist_with_covariance.twist; + const auto pose2 = obj2.kinematics.pose_with_covariance.pose; + + // interpolate twist + auto & output_twist = output.kinematics.twist_with_covariance.twist; + output_twist.linear.x = twist1.linear.x * (1 - weight) + twist2.linear.x * weight; + output_twist.linear.y = twist1.linear.y * (1 - weight) + twist2.linear.y * weight; + output_twist.linear.z = twist1.linear.z * (1 - weight) + twist2.linear.z * weight; + output_twist.angular.x = twist1.angular.x * (1 - weight) + twist2.angular.x * weight; + output_twist.angular.y = twist1.angular.y * (1 - weight) + twist2.angular.y * weight; + output_twist.angular.z = twist1.angular.z * (1 - weight) + twist2.angular.z * weight; + + // interpolate pose + auto & output_pose = output.kinematics.pose_with_covariance.pose; + output_pose.position.x = pose1.position.x * (1 - weight) + pose2.position.x * weight; + output_pose.position.y = pose1.position.y * (1 - weight) + pose2.position.y * weight; + output_pose.position.z = pose1.position.z * (1 - weight) + pose2.position.z * weight; + // interpolate orientation with slerp + // https://en.wikipedia.org/wiki/Slerp + const auto q1 = tf2::Quaternion( + pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w); + const auto q2 = tf2::Quaternion( + pose2.orientation.x, pose2.orientation.y, pose2.orientation.z, pose2.orientation.w); + const auto q = q1.slerp(q2, weight); + output_pose.orientation.x = q.x(); + output_pose.orientation.y = q.y(); + output_pose.orientation.z = q.z(); + output_pose.orientation.w = q.w(); + + // interpolate shape(mostly for bounding box) + const auto shape1 = obj1.shape; + const auto shape2 = obj2.shape; + if (shape1.type != shape2.type) { + // if shape type is different, return obj1 + } else { + if (shape1.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + auto & output_shape = output.shape; + output_shape.dimensions.x = shape1.dimensions.x * (1 - weight) + shape2.dimensions.x * weight; + output_shape.dimensions.y = shape1.dimensions.y * (1 - weight) + shape2.dimensions.y * weight; + output_shape.dimensions.z = shape1.dimensions.z * (1 - weight) + shape2.dimensions.z * weight; + } else if (shape1.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // (TODO) implement + } else if (shape1.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + // (TODO) implement + } else { + // when type is unknown, print warning and do nothing + std::cerr << "unknown shape type in linearInterpolationForTrackedObject function." + << std::endl; + return output; + } + } + + return output; +} + +/** + * @brief predict past or future tracked object + * + * @param obj + * @param dt + * @return TrackedObject + */ +TrackedObject predictPastOrFutureTrackedObject(const TrackedObject & obj, const double dt) +{ + // output object + TrackedObject output; + // copy input object at first + output = obj; + if (dt == 0) { + return output; + } + + // get current twist and pose + const auto twist = obj.kinematics.twist_with_covariance.twist; + const auto pose = obj.kinematics.pose_with_covariance.pose; + + // get yaw + const auto yaw = tf2::getYaw(pose.orientation); + const auto vx = twist.linear.x; + const auto vy = twist.linear.y; + + // linear prediction equation: + // x = x0 + vx * cos(yaw) * dt - vy * sin(yaw) * dt + // y = y0 + vx * sin(yaw) * dt + vy * cos(yaw) * dt + auto & output_pose = output.kinematics.pose_with_covariance.pose; + output_pose.position.x += vx * std::cos(yaw) * dt - vy * std::sin(yaw) * dt; + output_pose.position.y += vx * std::sin(yaw) * dt + vy * std::cos(yaw) * dt; + + // (TODO) covariance prediction + + return output; +} + +/** + * @brief predict past or future tracked objects + * + * @param obj + * @param header + * @return TrackedObjects + */ +TrackedObjects predictPastOrFutureTrackedObjects( + const TrackedObjects & obj, const std_msgs::msg::Header & header) +{ + // for each object, predict past or future + TrackedObjects output_objects; + output_objects.header = obj.header; + output_objects.header.stamp = header.stamp; + + const auto dt = (rclcpp::Time(header.stamp) - rclcpp::Time(obj.header.stamp)).seconds(); + for (const auto & obj : obj.objects) { + output_objects.objects.push_back(predictPastOrFutureTrackedObject(obj, dt)); + } + return output_objects; +} + +/** + * @brief interpolate tracked objects + * + * @param objects1 + * @param objects2 + * @param header : header of output object + * @return TrackedObjects + */ +TrackedObjects interpolateTrackedObjects( + const TrackedObjects & objects1, const TrackedObjects & objects2, std_msgs::msg::Header header) +{ + // Assumption: time of output header is between objects1 and objects2 + // time of objects1 < header < objects2 + + // define output objects + TrackedObjects output_objects; + output_objects.header = header; + + // calc weight + const auto dt = + (rclcpp::Time(objects1.header.stamp) - rclcpp::Time(objects2.header.stamp)).seconds(); + const auto dt1 = (rclcpp::Time(objects1.header.stamp) - rclcpp::Time(header.stamp)).seconds(); + const auto weight = dt1 / dt; + + // check if object number is not zero + if (objects1.objects.size() == 0 && objects2.objects.size() == 0) { + // if objects1 and objects2 are empty, return empty objects + return output_objects; + } else if (objects1.objects.size() == 0) { + // if objects1 is empty return objects2 + for (const auto & obj2 : objects2.objects) { + output_objects.objects.push_back(predictPastOrFutureTrackedObject(obj2, dt1 - dt)); + } + return output_objects; + } else if (objects2.objects.size() == 0) { + // if objects2 is empty return objects1 + for (const auto & obj1 : objects1.objects) { + output_objects.objects.push_back(predictPastOrFutureTrackedObject(obj1, dt1)); + } + } else { + // if both objects1 and objects2 are not empty do nothing + } + + // map to handle selected objects + std::unordered_map selected_objects1; + // iterate with int + for (std::size_t i = 0; i < objects1.objects.size(); i++) { + selected_objects1[i] = false; + } + // search for objects with int iterator + for (std::size_t i = 0; i < objects1.objects.size(); i++) { + for (std::size_t j = 0; j < objects2.objects.size(); j++) { + if (objects1.objects[i].object_id == objects2.objects[j].object_id) { + // if id is same, interpolate + const auto interp_objects = + linearInterpolationForTrackedObject(objects1.objects[i], objects2.objects[j], weight); + output_objects.objects.push_back(interp_objects); + selected_objects1[i] = true; + break; + } + } + if (selected_objects1[i] == false) { + // if obj1 is not selected, predict future + const auto pred_objects = predictPastOrFutureTrackedObject(objects1.objects[i], dt1); + output_objects.objects.push_back(pred_objects); + } + } + + return output_objects; +} + +} // namespace utils + +namespace merger_utils +{ + +double mean(const double a, const double b) +{ + return (a + b) / 2.0; +} + +// object kinematics merger +// currently only support velocity fusion +autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy) +{ + autoware_auto_perception_msgs::msg::TrackedObjectKinematics output_kinematics; + // copy main object at first + output_kinematics = main_obj.kinematics; + if (policy == MergePolicy::SKIP) { + return output_kinematics; + } else if (policy == MergePolicy::OVERWRITE) { + output_kinematics.twist_with_covariance.twist.linear.x = + sub_obj.kinematics.twist_with_covariance.twist.linear.x; + return output_kinematics; + } else if (policy == MergePolicy::FUSION) { + const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vx = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + // inverse weight + const auto main_vx_cov = main_obj.kinematics.twist_with_covariance.covariance[0]; + const auto sub_vx_cov = sub_obj.kinematics.twist_with_covariance.covariance[0]; + double main_vx_weight, sub_vx_weight; + if (main_vx_cov == 0.0) { + main_vx_weight = 1.0 * 1e6; + } else { + main_vx_weight = 1.0 / main_vx_cov; + } + if (sub_vx_cov == 0.0) { + sub_vx_weight = 1.0 * 1e6; + } else { + sub_vx_weight = 1.0 / sub_vx_cov; + } + // merge with covariance + output_kinematics.twist_with_covariance.twist.linear.x = + (main_vx * main_vx_weight + sub_vx * sub_vx_weight) / (main_vx_weight + sub_vx_weight); + output_kinematics.twist_with_covariance.covariance[0] = 1.0 / (main_vx_weight + sub_vx_weight); + return output_kinematics; + } else { + std::cerr << "unknown merge policy in objectKinematicsMerger function." << std::endl; + return output_kinematics; + } + return output_kinematics; +} + +// object classification merger +TrackedObject objectClassificationMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy) +{ + if (policy == MergePolicy::SKIP) { + return main_obj; + } else if (policy == MergePolicy::OVERWRITE) { + return sub_obj; + } else if (policy == MergePolicy::FUSION) { + // merge two std::vector into one + TrackedObject dummy_obj; + dummy_obj.classification = main_obj.classification; + for (const auto & sub_class : sub_obj.classification) { + dummy_obj.classification.push_back(sub_class); + } + return dummy_obj; + } else { + std::cerr << "unknown merge policy in objectClassificationMerger function." << std::endl; + return main_obj; + } +} + +// probability merger +float probabilityMerger(const float main_prob, const float sub_prob, const MergePolicy policy) +{ + if (policy == MergePolicy::SKIP) { + return main_prob; + } else if (policy == MergePolicy::OVERWRITE) { + return sub_prob; + } else if (policy == MergePolicy::FUSION) { + return static_cast(mean(main_prob, sub_prob)); + } else { + std::cerr << "unknown merge policy in probabilityMerger function." << std::endl; + return main_prob; + } +} + +// shape merger +autoware_auto_perception_msgs::msg::Shape shapeMerger( + const autoware_auto_perception_msgs::msg::Shape & main_obj_shape, + const autoware_auto_perception_msgs::msg::Shape & sub_obj_shape, const MergePolicy policy) +{ + autoware_auto_perception_msgs::msg::Shape output_shape; + // copy main object at first + output_shape = main_obj_shape; + + if (main_obj_shape.type != sub_obj_shape.type) { + // if shape type is different, return main object + return output_shape; + } + + if (policy == MergePolicy::SKIP) { + return output_shape; + } else if (policy == MergePolicy::OVERWRITE) { + return sub_obj_shape; + } else if (policy == MergePolicy::FUSION) { + // write down fusion method for each shape type + if (main_obj_shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + // if shape type is bounding box, merge bounding box + output_shape.dimensions.x = mean(main_obj_shape.dimensions.x, sub_obj_shape.dimensions.x); + output_shape.dimensions.y = mean(main_obj_shape.dimensions.y, sub_obj_shape.dimensions.y); + output_shape.dimensions.z = mean(main_obj_shape.dimensions.z, sub_obj_shape.dimensions.z); + return output_shape; + } else if (main_obj_shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // if shape type is cylinder, merge cylinder + // (TODO) implement + return output_shape; + } else if (main_obj_shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + // if shape type is polygon, merge polygon + // (TODO) + return output_shape; + } else { + // when type is unknown, print warning and do nothing + std::cerr << "unknown shape type in shapeMerger function." << std::endl; + return output_shape; + } + } else { + std::cerr << "unknown merge policy in shapeMerger function." << std::endl; + return output_shape; + } +} + +void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + const auto vx_temp = main_obj.kinematics.twist_with_covariance.twist.linear.x; + main_obj = sub_obj; + main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp; +} + +void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + main_obj.kinematics = objectKinematicsVXMerger(main_obj, sub_obj, MergePolicy::OVERWRITE); +} + +void updateOnlyClassification(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + main_obj = objectClassificationMerger(main_obj, sub_obj, MergePolicy::OVERWRITE); +} + +void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + main_obj = sub_obj; +} + +} // namespace merger_utils diff --git a/perception/traffic_light_fine_detector/README.md b/perception/traffic_light_fine_detector/README.md index dcc89c76387c6..9e69c22fdc17b 100644 --- a/perception/traffic_light_fine_detector/README.md +++ b/perception/traffic_light_fine_detector/README.md @@ -16,7 +16,8 @@ The model was fine-tuned on around 17,000 TIER IV internal images of Japanese tr ### Trained Onnx model -- +You can download the ONNX file using these instructions. +Please visit [autoware-documentation](https://github.com/autowarefoundation/autoware-documentation/blob/main/docs/models/index.md) for more information. ## Inner-workings / Algorithms diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index f62e418371401..ee6e50f5a9553 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -50,7 +50,6 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/drivable_area_expansion/drivable_area_expansion.cpp src/utils/drivable_area_expansion/map_utils.cpp src/utils/drivable_area_expansion/footprints.cpp - src/utils/drivable_area_expansion/expansion.cpp src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp src/marker_utils/utils.cpp diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index df2dd806fe2c7..25e42d20d144a 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -118,8 +118,6 @@ object_ignore_section_crosswalk_in_front_distance: 30.0 # [m] object_ignore_section_crosswalk_behind_distance: 30.0 # [m] # detection range - object_check_forward_distance: 150.0 # [m] - object_check_backward_distance: 2.0 # [m] object_check_goal_distance: 20.0 # [m] # filtering parking objects threshold_distance_object_is_on_center: 1.0 # [m] @@ -128,6 +126,13 @@ # lost object compensation object_last_seen_threshold: 2.0 + # detection area generation parameters + detection_area: + static: true # [-] + min_forward_distance: 50.0 # [m] + max_forward_distance: 150.0 # [m] + backward_distance: 10.0 # [m] + # For safety check safety_check: # safety check configuration @@ -169,6 +174,7 @@ hard_road_shoulder_margin: 0.3 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 + max_deviation_from_lane: 0.5 # [m] # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 95c60612bfdb7..d333cbfd778cb 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: verbose: false + max_iteration_num: 100 planning_hz: 10.0 backward_path_length: 5.0 forward_path_length: 300.0 diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml index 160ebdc180020..c0b6f259c7726 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -5,15 +5,19 @@ drivable_area_left_bound_offset: 0.0 drivable_area_types_to_skip: [road_border] - # Dynamic expansion by projecting the ego footprint along the path + # Dynamic expansion by using the path curvature dynamic_expansion: - enabled: false + enabled: true + print_runtime: false + max_expansion_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) + smoothing: + curvature_average_window: 3 # window size used for smoothing the curvatures using a moving window average + max_bound_rate: 1.0 # [m/m] maximum rate of change of the bound lateral distance over its arc length + extra_arc_length: 2.0 # [m] extra arc length where an expansion distance is initially applied ego: - extra_footprint_offset: - front: 0.5 # [m] extra length to add to the front of the ego footprint - rear: 0.5 # [m] extra length to add to the rear of the ego footprint - left: 0.5 # [m] extra length to add to the left of the ego footprint - right: 0.5 # [m] extra length to add to the rear of the ego footprint + extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase + extra_front_overhang: 0.5 # [m] extra length to add to the front overhang + extra_width: 1.0 # [m] extra length to add to the width dynamic_objects: avoid: true # if true, the drivable area is not expanded in the predicted path of dynamic objects extra_footprint_offset: @@ -24,16 +28,8 @@ path_preprocessing: max_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used) - expansion: - method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. - # 'lanelet': add lanelets overlapped by the ego footprints - # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area - max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) - extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint + reuse_max_deviation: 0.5 # [m] if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused. avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area - road_border distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid - compensate: - enable: true # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction - extra_distance: 3.0 # [m] extra distance to add to the compensation diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index e7f3b51bd2d26..d3c0a22e867f9 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -45,6 +45,14 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 2.5 longitudinal_velocity_delta_time: 0.6 + stuck: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 # lane expansion for object filtering lane_expansion: diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md index 95a9697a1d471..131202038c270 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md @@ -62,7 +62,7 @@ PullOutPath --o PullOutPlannerBase | Name | Unit | Type | Description | Default value | | :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------ | | th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | | th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | | th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | | th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | @@ -82,8 +82,108 @@ PullOutPath --o PullOutPlannerBase ## Safety check with dynamic obstacles +### **Basic concept of safety check against dynamic obstacles** + +This is based on the concept of RSS. For the logic used, refer to the link below. +See [safety check feature explanation](../docs/behavior_path_planner_safety_check.md) + +### **Collision check performed range** + +A collision check with dynamic objects is primarily performed between the shift start point and end point. The range for safety check varies depending on the type of path generated, so it will be explained for each pattern. + +#### **Shift pull out** + +For the "shift pull out", safety verification starts at the beginning of the shift and ends at the shift's conclusion. + +#### **Geometric pull out** + +Since there's a stop at the midpoint during the shift, this becomes the endpoint for safety verification. After stopping, safety verification resumes. + +#### **Backward pull out start point search** + +During backward movement, no safety check is performed. Safety check begins at the point where the backward movement ends. + +![collision_check_range](../image/start_planner/collision_check_range.drawio.svg) + +### **Ego vehicle's velocity planning** + +WIP + +### **Safety check in free space area** + WIP +## Parameters for safety check + +### Stop Condition Parameters + +Parameters under `stop_condition` define the criteria for stopping conditions. + +| Name | Unit | Type | Description | Default value | +| :---------------------------- | :------ | :----- | :-------------------------------------- | :------------ | +| maximum_deceleration_for_stop | [m/s^2] | double | Maximum deceleration allowed for a stop | 1.0 | +| maximum_jerk_for_stop | [m/s^3] | double | Maximum jerk allowed for a stop | 1.0 | + +### Ego Predicted Path Parameters + +Parameters under `path_safety_check.ego_predicted_path` specify the ego vehicle's predicted path characteristics. + +| Name | Unit | Type | Description | Default value | +| :---------------------------- | :------ | :----- | :--------------------------------------------------- | :------------ | +| min_velocity | [m/s] | double | Minimum velocity of the ego vehicle's predicted path | 0.0 | +| acceleration | [m/s^2] | double | Acceleration for the ego vehicle's predicted path | 1.0 | +| max_velocity | [m/s] | double | Maximum velocity of the ego vehicle's predicted path | 1.0 | +| time_horizon_for_front_object | [s] | double | Time horizon for predicting front objects | 10.0 | +| time_horizon_for_rear_object | [s] | double | Time horizon for predicting rear objects | 10.0 | +| time_resolution | [s] | double | Time resolution for the ego vehicle's predicted path | 0.5 | +| delay_until_departure | [s] | double | Delay until the ego vehicle departs | 1.0 | + +### Target Object Filtering Parameters + +Parameters under `target_filtering` are related to filtering target objects for safety check. + +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :---- | :----- | :------------------------------------------------- | :------------ | +| safety_check_time_horizon | [s] | double | Time horizon for safety check | 5.0 | +| safety_check_time_resolution | [s] | double | Time resolution for safety check | 1.0 | +| object_check_forward_distance | [m] | double | Forward distance for object detection | 10.0 | +| object_check_backward_distance | [m] | double | Backward distance for object detection | 100.0 | +| ignore_object_velocity_threshold | [m/s] | double | Velocity threshold below which objects are ignored | 1.0 | +| object_types_to_check.check_car | - | bool | Flag to check cars | true | +| object_types_to_check.check_truck | - | bool | Flag to check trucks | true | +| object_types_to_check.check_bus | - | bool | Flag to check buses | true | +| object_types_to_check.check_trailer | - | bool | Flag to check trailers | true | +| object_types_to_check.check_bicycle | - | bool | Flag to check bicycles | true | +| object_types_to_check.check_motorcycle | - | bool | Flag to check motorcycles | true | +| object_types_to_check.check_pedestrian | - | bool | Flag to check pedestrians | true | +| object_types_to_check.check_unknown | - | bool | Flag to check unknown object types | false | +| object_lane_configuration.check_current_lane | - | bool | Flag to check the current lane | true | +| object_lane_configuration.check_right_side_lane | - | bool | Flag to check the right side lane | true | +| object_lane_configuration.check_left_side_lane | - | bool | Flag to check the left side lane | true | +| object_lane_configuration.check_shoulder_lane | - | bool | Flag to check the shoulder lane | true | +| object_lane_configuration.check_other_lane | - | bool | Flag to check other lanes | false | +| include_opposite_lane | - | bool | Flag to include the opposite lane in check | false | +| invert_opposite_lane | - | bool | Flag to invert the opposite lane check | false | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| use_all_predicted_path | - | bool | Flag to use all predicted paths | true | +| use_predicted_path_outside_lanelet | - | bool | Flag to use predicted paths outside of lanelets | false | + +### Safety Check Parameters + +Parameters under `safety_check_params` define the configuration for safety check. + +| Name | Unit | Type | Description | Default value | +| :--------------------------------------------- | :--- | :----- | :------------------------------------------ | :------------ | +| enable_safety_check | - | bool | Flag to enable safety check | true | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | +| rss_params.rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | +| rss_params.rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | +| rss_params.lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | +| rss_params.longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | +| rss_params.longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | +| hysteresis_factor_expand_rate | - | double | Rate to expand/shrink the hysteresis factor | 1.0 | + ## **Path Generation** There are two path generation methods. @@ -113,9 +213,7 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral | maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | | minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 | | minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 | -| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | - -| 0.07 | +| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | 0.07 | ### **geometric pull out** @@ -128,14 +226,14 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474 #### parameters for geometric pull out -| Name | Unit | Type | Description | Default value | -| :-------------------------- | :---- | :----- | :--------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | -| divide_pull_out_path | [-] | bool | flag whether to divide arc paths.　The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | -| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | -| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | -| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | -| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | +| Name | Unit | Type | Description | Default value | +| :-------------------------- | :---- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | +| divide_pull_out_path | [-] | bool | flag whether to divide arc paths. The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | +| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | +| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | +| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | +| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | ## **backward pull out start point search** diff --git a/planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg b/planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg new file mode 100644 index 0000000000000..064f3602d6b6a --- /dev/null +++ b/planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg @@ -0,0 +1,119 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
safety check range
+
+ + +
+
no-safety check range
+
+ + +
+
safety check start point with vehicle frame
+
+ + +
+
safety check end point with vehicle frame
+
+ + diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index e8ad6c7e2f987..420a5a8aa6ee5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -17,13 +17,7 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/planner_manager.hpp" -#include "behavior_path_planner/scene_module/avoidance/manager.hpp" -#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" -#include "behavior_path_planner/scene_module/goal_planner/manager.hpp" -#include "behavior_path_planner/scene_module/lane_change/manager.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/scene_module/side_shift/manager.hpp" -#include "behavior_path_planner/scene_module/start_planner/manager.hpp" #include "behavior_path_planner/steering_factor_interface.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" @@ -66,7 +60,6 @@ using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::PoseWithUuidStamped; -using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using rcl_interfaces::msg::SetParametersResult; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 0a7b59d52ff5a..9280a81e643ca 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -141,16 +141,15 @@ struct PlannerData OccupancyGrid::ConstSharedPtr costmap{}; LateralOffset::ConstSharedPtr lateral_offset{}; OperationModeState::ConstSharedPtr operation_mode{}; - PathWithLaneId::SharedPtr reference_path{std::make_shared()}; PathWithLaneId::SharedPtr prev_output_path{std::make_shared()}; std::optional prev_modified_goal{}; std::optional prev_route_id{}; - lanelet::ConstLanelets current_lanes{}; std::shared_ptr route_handler{std::make_shared()}; BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; - mutable std::optional drivable_area_expansion_prev_crop_pose; + mutable std::vector drivable_area_expansion_prev_path_poses{}; + mutable std::vector drivable_area_expansion_prev_curvatures{}; mutable TurnSignalDecider turn_signal_decider; TurnIndicatorsCommand getTurnSignal( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index e3a3784c5e928..a4e6fc18ab050 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -18,7 +18,6 @@ #include #include -#include #include #include @@ -77,6 +76,7 @@ struct LateralAccelerationMap struct BehaviorPathPlannerParameters { bool verbose; + size_t max_iteration_num{100}; ModuleConfigParameters config_avoidance; ModuleConfigParameters config_avoidance_by_lc; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 72c32039da627..1d1ad56342f4d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -18,20 +18,17 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/utils/lane_following/module_data.hpp" #include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include -#include "tier4_debug_msgs/msg/float64_stamped.hpp" #include #include #include #include -#include #include #include #include @@ -97,7 +94,7 @@ struct SceneModuleStatus class PlannerManager { public: - PlannerManager(rclcpp::Node & node, const bool verbose); + PlannerManager(rclcpp::Node & node, const size_t max_iteration_num, const bool verbose); /** * @brief run all candidate and approved modules. @@ -272,6 +269,8 @@ class PlannerManager const auto result = module_ptr->run(); module_ptr->unlockRTCCommand(); + module_ptr->postProcess(); + module_ptr->updateCurrentState(); module_ptr->publishRTCStatus(); @@ -444,6 +443,8 @@ class PlannerManager mutable std::shared_ptr debug_msg_ptr_; + size_t max_iteration_num_{100}; + bool verbose_{false}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index d362a5e758a3e..cbf40e59535be 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -334,11 +334,10 @@ class AvoidanceModule : public SceneModuleInterface /* * @brief generate candidate shift lines. * @param one-shot shift lines. - * @param path shifter. * @param debug data. */ AvoidLineArray generateCandidateShiftLine( - const AvoidLineArray & shift_lines, const PathShifter & path_shifter, DebugData & debug) const; + const AvoidLineArray & shift_lines, DebugData & debug) const; /** * @brief clean up raw shift lines. @@ -478,13 +477,6 @@ class AvoidanceModule : public SceneModuleInterface */ TurnSignalInfo calcTurnSignalInfo(const ShiftedPath & path) const; - // TODO(murooka) judge when and which way to extend drivable area. current implementation is keep - // extending during avoidance module - // TODO(murooka) freespace during turning in intersection where there is no neighbor lanes - // NOTE: Assume that there is no situation where there is an object in the middle lane of more - // than two lanes since which way to avoid is not obvious - void generateExpandDrivableLanes(BehaviorModuleOutput & output) const; - /** * @brief fill debug markers. */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index ab8e3cfd70dff..6c522c79a8774 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -18,8 +18,10 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include +#include #include +#include #include #include #include @@ -35,6 +37,9 @@ namespace behavior_path_planner { +using autoware_auto_perception_msgs::msg::PredictedPath; +using tier4_autoware_utils::Polygon2d; + struct MinMaxValue { double min_value{0.0}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index a82cff16756f8..21372ed6e2c2f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include @@ -65,6 +66,7 @@ using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; +using tier4_autoware_utils::Polygon2d; enum class PathType { NONE = 0, @@ -74,28 +76,114 @@ enum class PathType { FREESPACE, }; -struct PullOverStatus +#define DEFINE_SETTER_GETTER(TYPE, NAME) \ +public: \ + void set_##NAME(const TYPE & value) \ + { \ + const std::lock_guard lock(mutex_); \ + NAME##_ = value; \ + } \ + \ + TYPE get_##NAME() const \ + { \ + const std::lock_guard lock(mutex_); \ + return NAME##_; \ + } + +class PullOverStatus { - std::shared_ptr pull_over_path{}; - std::shared_ptr lane_parking_pull_over_path{}; - size_t current_path_idx{0}; - bool require_increment_{true}; // if false, keep current path idx. - std::shared_ptr prev_stop_path{nullptr}; - std::shared_ptr prev_stop_path_after_approval{nullptr}; - // stop path after approval, stop path is not updated until safety is confirmed - lanelet::ConstLanelets current_lanes{}; // TODO(someone): explain - lanelet::ConstLanelets pull_over_lanes{}; // TODO(someone): explain - std::vector lanes{}; // current + pull_over - bool has_decided_path{false}; // if true, the path has is decided and safe against static objects - bool is_safe_static_objects{false}; // current path is safe against *static* objects - bool is_safe_dynamic_objects{false}; // current path is safe against *dynamic* objects - bool prev_is_safe{false}; - bool prev_is_safe_dynamic_objects{false}; - bool has_decided_velocity{false}; - bool has_requested_approval{false}; - bool is_ready{false}; +public: + explicit PullOverStatus(std::recursive_mutex & mutex) : mutex_(mutex) {} + + // Reset all data members to their initial states + void reset() + { + std::lock_guard lock(mutex_); + pull_over_path_ = nullptr; + lane_parking_pull_over_path_ = nullptr; + current_path_idx_ = 0; + require_increment_ = true; + prev_stop_path_ = nullptr; + prev_stop_path_after_approval_ = nullptr; + current_lanes_.clear(); + pull_over_lanes_.clear(); + lanes_.clear(); + has_decided_path_ = false; + is_safe_static_objects_ = false; + is_safe_dynamic_objects_ = false; + prev_is_safe_ = false; + prev_is_safe_dynamic_objects_ = false; + has_decided_velocity_ = false; + has_requested_approval_ = false; + is_ready_ = false; + } + + DEFINE_SETTER_GETTER(std::shared_ptr, pull_over_path) + DEFINE_SETTER_GETTER(std::shared_ptr, lane_parking_pull_over_path) + DEFINE_SETTER_GETTER(size_t, current_path_idx) + DEFINE_SETTER_GETTER(bool, require_increment) + DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path) + DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path_after_approval) + DEFINE_SETTER_GETTER(lanelet::ConstLanelets, current_lanes) + DEFINE_SETTER_GETTER(lanelet::ConstLanelets, pull_over_lanes) + DEFINE_SETTER_GETTER(std::vector, lanes) + DEFINE_SETTER_GETTER(bool, has_decided_path) + DEFINE_SETTER_GETTER(bool, is_safe_static_objects) + DEFINE_SETTER_GETTER(bool, is_safe_dynamic_objects) + DEFINE_SETTER_GETTER(bool, prev_is_safe) + DEFINE_SETTER_GETTER(bool, prev_is_safe_dynamic_objects) + DEFINE_SETTER_GETTER(bool, has_decided_velocity) + DEFINE_SETTER_GETTER(bool, has_requested_approval) + DEFINE_SETTER_GETTER(bool, is_ready) + DEFINE_SETTER_GETTER(std::shared_ptr, last_approved_time) + DEFINE_SETTER_GETTER(std::shared_ptr, last_increment_time) + DEFINE_SETTER_GETTER(std::shared_ptr, last_path_update_time) + DEFINE_SETTER_GETTER(std::shared_ptr, last_approved_pose) + DEFINE_SETTER_GETTER(std::optional, modified_goal_pose) + DEFINE_SETTER_GETTER(Pose, refined_goal_pose) + DEFINE_SETTER_GETTER(GoalCandidates, goal_candidates) + DEFINE_SETTER_GETTER(std::vector, pull_over_path_candidates) + DEFINE_SETTER_GETTER(std::optional, closest_start_pose) + +private: + std::shared_ptr pull_over_path_{nullptr}; + std::shared_ptr lane_parking_pull_over_path_{nullptr}; + size_t current_path_idx_{0}; + bool require_increment_{true}; + std::shared_ptr prev_stop_path_{nullptr}; + std::shared_ptr prev_stop_path_after_approval_{nullptr}; + lanelet::ConstLanelets current_lanes_{}; + lanelet::ConstLanelets pull_over_lanes_{}; + std::vector lanes_{}; + bool has_decided_path_{false}; + bool is_safe_static_objects_{false}; + bool is_safe_dynamic_objects_{false}; + bool prev_is_safe_{false}; + bool prev_is_safe_dynamic_objects_{false}; + bool has_decided_velocity_{false}; + bool has_requested_approval_{false}; + bool is_ready_{false}; + + // save last time and pose + std::shared_ptr last_approved_time_; + std::shared_ptr last_increment_time_; + std::shared_ptr last_path_update_time_; + std::shared_ptr last_approved_pose_; + + // goal modification + std::optional modified_goal_pose_; + Pose refined_goal_pose_{}; + GoalCandidates goal_candidates_{}; + + // pull over path + std::vector pull_over_path_candidates_; + std::optional closest_start_pose_; + + std::recursive_mutex & mutex_; }; +#undef DEFINE_SETTER_GETTER + struct FreespacePlannerDebugData { bool is_planning{false}; @@ -129,33 +217,28 @@ class GoalPlannerModule : public SceneModuleInterface } } - // TODO(someone): remove this, and use base class function - [[deprecated]] BehaviorModuleOutput run() override; - bool isExecutionRequested() const override; - bool isExecutionReady() const override; - // TODO(someone): remove this, and use base class function - [[deprecated]] ModuleStatus updateState() override; + CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; - void processOnEntry() override; + bool isExecutionRequested() const override; + bool isExecutionReady() const override; void processOnExit() override; + void updateData() override; void setParameters(const std::shared_ptr & parameters); void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override { } - // not used, but need to override - CandidateOutput planCandidate() const override { return CandidateOutput{}; }; - private: + // The start_planner activates when it receives a new route, + // so there is no need to terminate the goal planner. + // If terminating it, it may switch to lane following and could generate an inappropriate path. bool canTransitSuccessState() override { return false; } bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return false; } - - PullOverStatus status_; + bool canTransitIdleToRunningState() override { return true; } mutable StartGoalPlannerData goal_planner_data_; @@ -174,29 +257,19 @@ class GoalPlannerModule : public SceneModuleInterface // goal searcher std::shared_ptr goal_searcher_; - std::optional modified_goal_pose_; - Pose refined_goal_pose_{}; - GoalCandidates goal_candidates_{}; // collision detector // need to be shared_ptr to be used in planner and goal searcher std::shared_ptr occupancy_grid_map_; - // pull over path - std::vector pull_over_path_candidates_; - std::optional closest_start_pose_; - // check stopped and stuck state std::deque odometry_buffer_stopped_; std::deque odometry_buffer_stuck_; tier4_autoware_utils::LinearRing2d vehicle_footprint_; - // save last time and pose - std::unique_ptr last_approved_time_; - std::unique_ptr last_increment_time_; - std::unique_ptr last_path_update_time_; - std::unique_ptr last_approved_pose_; + std::recursive_mutex mutex_; + PullOverStatus status_; // approximate distance from the start point to the end point of pull_over. // this is used as an assumed value to decelerate, etc., before generating the actual path. @@ -210,7 +283,6 @@ class GoalPlannerModule : public SceneModuleInterface // pre-generate lane parking paths in a separate thread rclcpp::TimerBase::SharedPtr lane_parking_timer_; rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_; - std::mutex mutex_; // generate freespace parking paths in a separate thread rclcpp::TimerBase::SharedPtr freespace_parking_timer_; @@ -245,7 +317,6 @@ class GoalPlannerModule : public SceneModuleInterface bool hasFinishedCurrentPath(); bool isOnModifiedGoal() const; double calcModuleRequestLength() const; - void resetStatus(); bool needPathUpdate(const double path_update_duration) const; bool isStuck(); bool hasDecidedPath() const; @@ -268,8 +339,8 @@ class GoalPlannerModule : public SceneModuleInterface BehaviorModuleOutput planWithGoalModification(); BehaviorModuleOutput planWaitingApprovalWithGoalModification(); void selectSafePullOverPath(); - void sortPullOverPathCandidatesByGoalPriority( - std::vector & pull_over_path_candidates, + std::vector sortPullOverPathCandidatesByGoalPriority( + const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; // deal with pull over partial paths diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index a3469dee2909b..472564a061f04 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -230,7 +230,8 @@ class LaneChangeBase virtual bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, Direction direction, - LaneChangePaths * candidate_paths, const bool check_safety) const = 0; + LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, + const bool is_stuck, const bool check_safety) const = 0; virtual TurnSignalInfo calcTurnSignalInfo() = 0; @@ -270,6 +271,9 @@ class LaneChangeBase mutable LaneChangeTargetObjects debug_filtered_objects_{}; mutable double object_debug_lifetime_{0.0}; mutable StopWatch stop_watch_; + + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change"); + mutable rclcpp::Clock clock_{RCL_ROS_TIME}; }; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index f1e4483c8bf51..54883489f2fe6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -138,6 +138,7 @@ class NormalLaneChange : public LaneChangeBase bool getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, Direction direction, LaneChangePaths * candidate_paths, + const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const override; TurnSignalInfo calcTurnSignalInfo() override; @@ -146,20 +147,33 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const utils::path_safety_checker::RSSparams & rss_params, + const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck, CollisionCheckDebugMap & debug_data) const; LaneChangeTargetObjectIndices filterObject( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const; + //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. + //! @param obstacle_check_distance Distance to check ahead for any objects that might be + //! obstructing ego path. It makes sense to use values like the maximum lane change distance. + bool isVehicleStuck( + const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const; + + bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + + double calcMaximumLaneChangeLength( + const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const; + + std::pair calcCurrentMinMaxAcceleration() const; + void setStopPose(const Pose & stop_pose); void updateStopTime(); double getStopTime() const { return stop_time_; } - rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); double stop_time_{0.0}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 077f7a3a763d5..7f24683e5680c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -131,6 +132,12 @@ class SceneModuleInterface */ virtual void updateData() {} + /** + * @brief After executing run(), update the module-specific status and/or data used for internal + * processing that are not defined in ModuleStatus. + */ + virtual void postProcess() {} + /** * @brief Execute module. Once this function is executed, * it will continue to run as long as it is in the RUNNING state. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp index 8afe0576ec1aa..3a334cf67edbf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp @@ -15,15 +15,13 @@ #ifndef BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_ #define BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_ -#include "rclcpp/rclcpp.hpp" +#include -#include "autoware_adapi_v1_msgs/msg/steering_factor_array.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "unique_identifier_msgs/msg/uuid.hpp" +#include +#include #include #include -#include namespace steering_factor_interface { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 6173ecb15a5c2..74464be0fa5ea 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ -#include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" @@ -145,9 +145,9 @@ struct AvoidanceParameters double object_ignore_section_crosswalk_behind_distance{0.0}; // distance to avoid object detection - double object_check_forward_distance{0.0}; - - // continue to detect backward vehicles as avoidance targets until they are this distance away + bool use_static_detection_area{true}; + double object_check_min_forward_distance{0.0}; + double object_check_max_forward_distance{0.0}; double object_check_backward_distance{0.0}; // if the distance between object and goal position is less than this parameter, the module ignore @@ -232,6 +232,9 @@ struct AvoidanceParameters // Even if the obstacle is very large, it will not avoid more than this length for left direction double max_left_shift_length{0.0}; + // Validate vehicle departure from driving lane. + double max_deviation_from_lane{0.0}; + // To prevent large acceleration while avoidance. double max_lateral_acceleration{0.0}; @@ -490,8 +493,16 @@ struct AvoidancePlanningData // safe shift point AvoidLineArray safe_shift_line{}; + std::vector drivable_lanes{}; + + lanelet::BasicLineString3d right_bound{}; + + lanelet::BasicLineString3d left_bound{}; + bool safe{false}; + bool valid{false}; + bool comfortable{false}; bool avoid_required{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp index 77fe2e29c4978..11388dd6bb74a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp @@ -173,6 +173,22 @@ class AvoidanceHelper : std::max(shift_length, getRightShiftBound()); } + double getForwardDetectionRange() const + { + if (parameters_->use_static_detection_area) { + return parameters_->object_check_max_forward_distance; + } + + const auto max_shift_length = std::max( + std::abs(parameters_->max_right_shift_length), std::abs(parameters_->max_left_shift_length)); + const auto dynamic_distance = PathShifter::calcLongitudinalDistFromJerk( + max_shift_length, getLateralMinJerkLimit(), getEgoSpeed()); + + return std::clamp( + 1.5 * dynamic_distance, parameters_->object_check_min_forward_distance, + parameters_->object_check_max_forward_distance); + } + void alignShiftLinesOrder(AvoidLineArray & lines, const bool align_shift_length = true) const { if (lines.empty()) { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index c0bd621cc86b4..0237fb458ea0b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -18,11 +18,8 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include #include -#include #include #include @@ -80,6 +77,9 @@ void setStartData( AvoidLine & al, const double start_shift_length, const geometry_msgs::msg::Pose & start, const size_t start_idx, const double start_dist); +Polygon2d createEnvelopePolygon( + const Polygon2d & object_polygon, const Pose & closest_pose, const double envelope_buffer); + Polygon2d createEnvelopePolygon( const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer); @@ -164,7 +164,7 @@ std::vector getSafetyCheckTargetObjects( std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, - DebugData & debug); + const double object_check_forward_distance, const bool is_running, DebugData & debug); DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index 8293e0a1d10a9..3f6f107cdce51 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -24,30 +24,75 @@ #include #include +#include namespace drivable_area_expansion { -/// @brief Expand the drivable area based on the projected ego footprint along the path +/// @brief Expand the drivable area based on the path curvature and the vehicle dimensions /// @param[inout] path path whose drivable area will be expanded -/// @param[inout] planner_data planning data (params, dynamic objects, route handler, ...) -/// @param[in] path_lanes lanelets of the path -void expandDrivableArea( +/// @param[inout] planner_data planning data (params, dynamic objects, vehicle info, ...) +void expand_drivable_area( PathWithLaneId & path, - const std::shared_ptr planner_data, - const lanelet::ConstLanelets & path_lanes); - -/// @brief Create a polygon combining the drivable area of a path and some expansion polygons -/// @param[in] path path and its drivable area -/// @param[in] expansion_polygons polygons to add to the drivable area -/// @return expanded drivable area polygon -polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multi_polygon_t & expansion_polygons); - -/// @brief Update the drivable area of the given path with the given polygon -/// @details this function splits the polygon into a left and right bound and sets it in the path -/// @param[in] path path whose drivable area will be expanded -/// @param[in] expanded_drivable_area polygon of the new drivable area -void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area); + const std::shared_ptr planner_data); + +/// @brief prepare path poses and try to reuse their previously calculated curvatures +/// @details poses are reused if they do not deviate too much from the current path +/// @param [in] path input path +/// @param [inout] prev_poses previous poses to reuse +/// @param [inout] prev_curvatures previous curvatures to reuse +/// @param [in] ego_point current ego point +/// @param [in] params parameters for reuse criteria and resampling interval +void update_path_poses_and_previous_curvatures( + const PathWithLaneId & path, std::vector & prev_poses, + std::vector & prev_curvatures, const Point & ego_point, + const DrivableAreaExpansionParameters & params); + +/// @brief calculate the minimum lane width based on the path curvature and the vehicle dimensions +/// @cite Lim, H., Kim, C., and Jo, A., "Model Predictive Control-Based Lateral Control of +/// Autonomous Large-Size Bus on Road with Large Curvature," SAE Technical Paper 2021-01-0099, 2021, +/// https://doi.org/10.4271/2021-01-0099 +/// @param [in] curvature curvature +/// @param [in] params parameters containing the vehicle dimensions +/// @return minimum lane width +double calculate_minimum_lane_width( + const double curvature_radius, const DrivableAreaExpansionParameters & params); + +/// @brief smooth the bound by applying a limit on its rate of change +/// @details rate of change is the lateral distance from the path over the arc length along the path +/// @param [inout] bound_distances bound distances (lateral distance from the path) +/// @param [in] bound_points bound points +/// @param [in] max_rate [m/m] maximum rate of lateral deviation over arc length +void apply_bound_change_rate_limit( + std::vector & distances, const std::vector & bound, const double max_rate); + +/// @brief calculate the maximum distance by which a bound can be expanded +/// @param [in] path_poses input path +/// @param [in] bound bound points +/// @param [in] uncrossable_segments segments that limit the bound expansion, indexed in a Rtree +/// @param [in] uncrossable_polygons polygons that limit the bound expansion +/// @param [in] params parameters with the buffer distance to keep with lines, +/// and the static maximum expansion distance +std::vector calculate_maximum_distance( + const std::vector & path_poses, const std::vector bound, + const SegmentRtree & uncrossable_lines, const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params); + +/// @brief expand a bound by the given lateral distances away from the path +/// @param [inout] bound bound points to expand +/// @param [in] path_poses input path +/// @param [in] distances new lateral distances of the bound points away from the path +void expand_bound( + std::vector & bound, const std::vector & path_poses, + const std::vector & distances); + +/// @brief calculate smoothed curvatures +/// @details smoothing is done using a moving average +/// @param [in] poses input poses used to calculate the curvatures +/// @param [in] smoothing_window_size size of the window used for the moving average +/// @return smoothed curvatures of the input poses +std::vector calculate_smoothed_curvatures( + const std::vector & poses, const size_t smoothing_window_size); + } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp deleted file mode 100644 index 70cc8a8bc5925..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp +++ /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. - -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ - -#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" - -#include - -#include - -#include -#include - -namespace drivable_area_expansion -{ -/// @brief Calculate the distance limit required for the polygon to not cross the limit lines -/// @details Calculate the minimum distance from base_ls to an intersection of limit_lines and -/// expansion_polygon -/// @param[in] base_ls base linestring from which the distance is calculated -/// @param[in] expansion_polygon polygon to consider -/// @param[in] limit_lines lines we do not want to cross -/// @return distance limit -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_linestring_t & limit_lines); - -/// @brief Calculate the distance limit required for the polygon to not cross the limit polygons. -/// @details Calculate the minimum distance from base_ls to an intersection of limit_polygons and -/// expansion_polygon -/// @param[in] base_ls base linestring from which the distance is calculated -/// @param[in] expansion_polygon polygon to consider -/// @param[in] limit_polygons polygons we do not want to cross -/// @return distance limit -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_polygon_t & limit_polygons); - -/// @brief Create a polygon from a base line with a given expansion distance -/// @param[in] base_ls base linestring from which the polygon is created -/// @param[in] dist desired expansion distance from the base line -/// @param[in] is_left_side desired side of the expansion from the base line -/// @return expansion polygon -polygon_t createExpansionPolygon( - const linestring_t & base_ls, const double dist, const bool is_left_side); - -/// @brief Create polygons for the area where the drivable area should be expanded -/// @param[in] path path and its drivable area -/// @param[in] path_footprints polygons of the ego footprint projected along the path -/// @param[in] predicted_paths polygons of the dynamic objects' predicted paths -/// @param[in] uncrossable_lines lines that should not be crossed by the expanded drivable area -/// @param[in] params expansion parameters -/// @return expansion polygons -multi_polygon_t createExpansionPolygons( - const PathWithLaneId & path, const multi_polygon_t & path_footprints, - const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, - const DrivableAreaExpansionParameters & params); - -/// @brief Create polygons for the area where the drivable area should be expanded -/// @param[in] path_lanes lanelets of the current path -/// @param[in] route_handler route handler -/// @param[in] path_footprints polygons of the ego footprint projected along the path -/// @param[in] predicted_paths polygons of the dynamic objects' predicted paths -/// @param[in] params expansion parameters -/// @return expansion polygons -multi_polygon_t createExpansionLaneletPolygons( - const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, - const DrivableAreaExpansionParameters & params); -} // namespace drivable_area_expansion - -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp index 8fc0157710dc8..9591ac0582e04 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp @@ -20,7 +20,6 @@ #include -#include "autoware_auto_planning_msgs/msg/detail/path_point__struct.hpp" #include #include #include @@ -31,11 +30,6 @@ #include #include -#include -#include -#include -#include - namespace drivable_area_expansion { /// @brief translate a polygon by some (x,y) vector @@ -43,27 +37,20 @@ namespace drivable_area_expansion /// @param[in] x translation distance on the x axis /// @param[in] y translation distance on the y axis /// @return translated polygon -polygon_t translatePolygon(const polygon_t & polygon, const double x, const double y); +Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const double y); /// @brief create the footprint of a pose and its base footprint /// @param[in] pose the origin pose of the footprint /// @param[in] base_footprint the base axis-aligned footprint /// @return footprint polygon -polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t base_footprint); +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint); /// @brief create footprints of the predicted paths of an object /// @param [in] objects objects from which to create polygons /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects /// @return footprint polygons of the object's predicted paths -multi_polygon_t createObjectFootprints( +MultiPolygon2d create_object_footprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); - -/// @brief create the footprint polygon from a path -/// @param[in] path the path for which to create a footprint -/// @param[in] params expansion parameters defining how to create the footprint -/// @return footprint polygons of the path -multi_polygon_t createPathFootprints( - const std::vector & path, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp index 4524bd2be2299..8c6bdb8a6943b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include @@ -24,18 +25,20 @@ namespace drivable_area_expansion { -/// @brief Extract uncrossable linestrings from the lanelet map +/// @brief Extract uncrossable segments from the lanelet map that are in range of ego /// @param[in] lanelet_map lanelet map -/// @param[in] uncrossable_types types that cannot be crossed -/// @return the uncrossable linestrings -multi_linestring_t extractUncrossableLines( - const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types); +/// @param[in] ego_point point of the current ego position +/// @param[in] params parameters with linestring types that cannot be crossed and maximum range +/// @return the uncrossable segments stored in a rtree +SegmentRtree extract_uncrossable_segments( + const lanelet::LaneletMap & lanelet_map, const Point & ego_point, + const DrivableAreaExpansionParameters & params); /// @brief Determine if the given linestring has one of the given types /// @param[in] ls linestring to check /// @param[in] types type strings to check /// @return true if the linestring has one of the given types -bool hasTypes(const lanelet::ConstLineString3d & ls, const std::vector & types); +bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index 81eab9560b736..2f9604223a248 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -15,8 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" - #include #include @@ -32,12 +30,9 @@ struct DrivableAreaExpansionParameters static constexpr auto DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM = "drivable_area_left_bound_offset"; static constexpr auto DRIVABLE_AREA_TYPES_TO_SKIP_PARAM = "drivable_area_types_to_skip"; static constexpr auto ENABLED_PARAM = "dynamic_expansion.enabled"; - static constexpr auto EGO_EXTRA_OFFSET_FRONT = - "dynamic_expansion.ego.extra_footprint_offset.front"; - static constexpr auto EGO_EXTRA_OFFSET_REAR = "dynamic_expansion.ego.extra_footprint_offset.rear"; - static constexpr auto EGO_EXTRA_OFFSET_LEFT = "dynamic_expansion.ego.extra_footprint_offset.left"; - static constexpr auto EGO_EXTRA_OFFSET_RIGHT = - "dynamic_expansion.ego.extra_footprint_offset.right"; + static constexpr auto EGO_EXTRA_FRONT_OVERHANG = "dynamic_expansion.ego.extra_front_overhang"; + static constexpr auto EGO_EXTRA_WHEELBASE = "dynamic_expansion.ego.extra_wheelbase"; + static constexpr auto EGO_EXTRA_WIDTH = "dynamic_expansion.ego.extra_width"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_FRONT = "dynamic_expansion.dynamic_objects.extra_footprint_offset.front"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_REAR = @@ -46,34 +41,36 @@ struct DrivableAreaExpansionParameters "dynamic_expansion.dynamic_objects.extra_footprint_offset.left"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_RIGHT = "dynamic_expansion.dynamic_objects.extra_footprint_offset.right"; - static constexpr auto EXPANSION_METHOD_PARAM = "dynamic_expansion.expansion.method"; - static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.expansion.max_distance"; + static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.max_expansion_distance"; static constexpr auto RESAMPLE_INTERVAL_PARAM = "dynamic_expansion.path_preprocessing.resample_interval"; static constexpr auto MAX_PATH_ARC_LENGTH_PARAM = "dynamic_expansion.path_preprocessing.max_arc_length"; - static constexpr auto EXTRA_ARC_LENGTH_PARAM = "dynamic_expansion.expansion.extra_arc_length"; + static constexpr auto MAX_REUSE_DEVIATION_PARAM = + "dynamic_expansion.path_preprocessing.reuse_max_deviation"; static constexpr auto AVOID_DYN_OBJECTS_PARAM = "dynamic_expansion.dynamic_objects.avoid"; static constexpr auto AVOID_LINESTRING_TYPES_PARAM = "dynamic_expansion.avoid_linestring.types"; static constexpr auto AVOID_LINESTRING_DIST_PARAM = "dynamic_expansion.avoid_linestring.distance"; - static constexpr auto COMPENSATE_PARAM = "dynamic_expansion.avoid_linestring.compensate.enable"; - static constexpr auto EXTRA_COMPENSATE_PARAM = - "dynamic_expansion.avoid_linestring.compensate.extra_distance"; + static constexpr auto SMOOTHING_CURVATURE_WINDOW_PARAM = + "dynamic_expansion.smoothing.curvature_average_window"; + static constexpr auto SMOOTHING_MAX_BOUND_RATE_PARAM = + "dynamic_expansion.smoothing.max_bound_rate"; + static constexpr auto SMOOTHING_EXTRA_ARC_LENGTH_PARAM = + "dynamic_expansion.smoothing.extra_arc_length"; + static constexpr auto PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime"; - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip; + // static expansion + double drivable_area_right_bound_offset{}; + double drivable_area_left_bound_offset{}; + std::vector drivable_area_types_to_skip{}; + // dynamic expansion bool enabled = false; - std::string expansion_method{}; double avoid_linestring_dist{}; - double ego_left_offset{}; - double ego_right_offset{}; - double ego_rear_offset{}; - double ego_front_offset{}; - double ego_extra_left_offset{}; - double ego_extra_right_offset{}; - double ego_extra_rear_offset{}; - double ego_extra_front_offset{}; + double extra_front_overhang{}; + double extra_wheelbase{}; + double extra_width{}; + int curvature_average_window{}; + double max_bound_rate{}; double dynamic_objects_extra_left_offset{}; double dynamic_objects_extra_right_offset{}; double dynamic_objects_extra_rear_offset{}; @@ -82,10 +79,11 @@ struct DrivableAreaExpansionParameters double max_path_arc_length{}; double resample_interval{}; double extra_arc_length{}; + double max_reuse_deviation{}; bool avoid_dynamic_objects{}; + bool print_runtime{}; std::vector avoid_linestring_types{}; - bool compensate_uncrossable_lines = false; - double compensate_extra_dist{}; + vehicle_info_util::VehicleInfo vehicle_info; DrivableAreaExpansionParameters() = default; explicit DrivableAreaExpansionParameters(rclcpp::Node & node) { init(node); } @@ -100,12 +98,15 @@ struct DrivableAreaExpansionParameters node.declare_parameter>(DRIVABLE_AREA_TYPES_TO_SKIP_PARAM); enabled = node.declare_parameter(ENABLED_PARAM); max_expansion_distance = node.declare_parameter(MAX_EXP_DIST_PARAM); + extra_front_overhang = node.declare_parameter(EGO_EXTRA_FRONT_OVERHANG); + extra_wheelbase = node.declare_parameter(EGO_EXTRA_WHEELBASE); + extra_width = node.declare_parameter(EGO_EXTRA_WIDTH); + curvature_average_window = node.declare_parameter(SMOOTHING_CURVATURE_WINDOW_PARAM); + max_bound_rate = node.declare_parameter(SMOOTHING_MAX_BOUND_RATE_PARAM); + extra_arc_length = node.declare_parameter(SMOOTHING_EXTRA_ARC_LENGTH_PARAM); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); resample_interval = node.declare_parameter(RESAMPLE_INTERVAL_PARAM); - ego_extra_front_offset = node.declare_parameter(EGO_EXTRA_OFFSET_FRONT); - ego_extra_rear_offset = node.declare_parameter(EGO_EXTRA_OFFSET_REAR); - ego_extra_left_offset = node.declare_parameter(EGO_EXTRA_OFFSET_LEFT); - ego_extra_right_offset = node.declare_parameter(EGO_EXTRA_OFFSET_RIGHT); + max_reuse_deviation = node.declare_parameter(MAX_REUSE_DEVIATION_PARAM); dynamic_objects_extra_front_offset = node.declare_parameter(DYN_OBJECTS_EXTRA_OFFSET_FRONT); dynamic_objects_extra_rear_offset = @@ -118,16 +119,9 @@ struct DrivableAreaExpansionParameters node.declare_parameter>(AVOID_LINESTRING_TYPES_PARAM); avoid_dynamic_objects = node.declare_parameter(AVOID_DYN_OBJECTS_PARAM); avoid_linestring_dist = node.declare_parameter(AVOID_LINESTRING_DIST_PARAM); - extra_arc_length = node.declare_parameter(EXTRA_ARC_LENGTH_PARAM); - compensate_uncrossable_lines = node.declare_parameter(COMPENSATE_PARAM); - compensate_extra_dist = node.declare_parameter(EXTRA_COMPENSATE_PARAM); - expansion_method = node.declare_parameter(EXPANSION_METHOD_PARAM); + print_runtime = node.declare_parameter(PRINT_RUNTIME_PARAM); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - ego_left_offset = vehicle_info.max_lateral_offset_m; - ego_right_offset = vehicle_info.min_lateral_offset_m; - ego_rear_offset = vehicle_info.min_longitudinal_offset_m; - ego_front_offset = vehicle_info.max_longitudinal_offset_m; + vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); } }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp index 3e2b177f59167..3f5327acbca68 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp @@ -21,7 +21,6 @@ #include -#include #include namespace drivable_area_expansion @@ -33,10 +32,10 @@ namespace drivable_area_expansion /// @param p2 second segment point /// @return projected point and corresponding distance inline PointDistance point_to_segment_projection( - const point_t & p, const point_t & p1, const point_t & p2) + const Point2d & p, const Point2d & p1, const Point2d & p2) { - const point_t p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; - const point_t p_vec = {p.x() - p1.x(), p.y() - p1.y()}; + const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; + const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()}; const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x(); const auto dist_sign = cross < 0.0 ? -1.0 : 1.0; @@ -48,7 +47,7 @@ inline PointDistance point_to_segment_projection( if (c2 <= c1) return {p2, boost::geometry::distance(p, p2) * dist_sign}; const auto projection = p1 + (p2_vec * c1 / c2); - const auto projection_point = point_t{projection.x(), projection.y()}; + const auto projection_point = Point2d{projection.x(), projection.y()}; return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; } @@ -59,10 +58,10 @@ inline PointDistance point_to_segment_projection( /// @param p2 second line point /// @return projected point and corresponding distance inline PointDistance point_to_line_projection( - const point_t & p, const point_t & p1, const point_t & p2) + const Point2d & p, const Point2d & p1, const Point2d & p2) { - const point_t p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; - const point_t p_vec = {p.x() - p1.x(), p.y() - p1.y()}; + const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; + const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()}; const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x(); const auto dist_sign = cross < 0.0 ? -1.0 : 1.0; @@ -70,7 +69,7 @@ inline PointDistance point_to_line_projection( const auto c1 = boost::geometry::dot_product(p_vec, p2_vec); const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec); const auto projection = p1 + (p2_vec * c1 / c2); - const auto projection_point = point_t{projection.x(), projection.y()}; + const auto projection_point = Point2d{projection.x(), projection.y()}; return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; } @@ -78,7 +77,7 @@ inline PointDistance point_to_line_projection( /// @param p point to project /// @param ls linestring /// @return projected point, corresponding distance, and arc length along the linestring -inline Projection point_to_linestring_projection(const point_t & p, const linestring_t & ls) +inline Projection point_to_linestring_projection(const Point2d & p, const LineString2d & ls) { Projection closest; closest.distance = std::numeric_limits::max(); @@ -100,14 +99,14 @@ inline Projection point_to_linestring_projection(const point_t & p, const linest /// @param p2 second vector point /// @param dist distance /// @return point p such that (p1,p) is orthogonal to (p1,p2) at the given distance -inline point_t normal_at_distance(const point_t & p1, const point_t & p2, const double dist) +inline Point2d normal_at_distance(const Point2d & p1, const Point2d & p2, const double dist) { auto base = p1; auto normal_vector = p2; boost::geometry::subtract_point(normal_vector, base); boost::geometry::detail::vec_normalize(normal_vector); boost::geometry::multiply_value(normal_vector, dist); - return point_t{base.x() - normal_vector.y(), base.y() + normal_vector.x()}; + return Point2d{base.x() - normal_vector.y(), base.y() + normal_vector.x()}; } /// @brief interpolate between two points @@ -115,7 +114,7 @@ inline point_t normal_at_distance(const point_t & p1, const point_t & p2, const /// @param b second point /// @param ratio interpolation ratio such that 0 yields a, and 1 yields b /// @return point interpolated between a and b as per the given ratio -inline point_t lerp_point(const point_t & a, const point_t & b, const double ratio) +inline Point2d lerp_point(const Point2d & a, const Point2d & b, const double ratio) { return {interpolation::lerp(a.x(), b.x(), ratio), interpolation::lerp(a.y(), b.y(), ratio)}; } @@ -125,10 +124,10 @@ inline point_t lerp_point(const point_t & a, const point_t & b, const double rat /// @param arc_length arc length along the reference linestring of the resulting point /// @param distance distance from the reference linestring of the resulting point /// @return point at the distance and arc length relative to the reference linestring -inline segment_t linestring_to_point_projection( - const linestring_t & ls, const double arc_length, const double distance) +inline Segment2d linestring_to_point_projection( + const LineString2d & ls, const double arc_length, const double distance) { - if (ls.empty()) return segment_t{}; + if (ls.empty()) return Segment2d{}; if (ls.size() == 1lu) return {ls.front(), ls.front()}; auto ls_iterator = ls.begin(); auto prev_arc_length = 0.0; @@ -156,10 +155,10 @@ inline segment_t linestring_to_point_projection( /// @param from_arc_length arc length of the first point of the sub linestring /// @param to_arc_length arc length of the last point of the sub linestring /// @return sub linestring -inline linestring_t sub_linestring( - const linestring_t & ls, const double from_arc_length, const double to_arc_length) +inline LineString2d sub_linestring( + const LineString2d & ls, const double from_arc_length, const double to_arc_length) { - linestring_t sub_ls; + LineString2d sub_ls; if (from_arc_length >= to_arc_length || ls.empty()) throw(std::runtime_error("sub_linestring: bad inputs")); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index e300a347e47a8..8da1521db6c28 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -22,6 +22,8 @@ #include #include +#include + namespace drivable_area_expansion { using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -29,26 +31,30 @@ using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; + +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::MultiLineString2d; +using tier4_autoware_utils::MultiPoint2d; +using tier4_autoware_utils::MultiPolygon2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using tier4_autoware_utils::Segment2d; -using point_t = tier4_autoware_utils::Point2d; -using multi_point_t = tier4_autoware_utils::MultiPoint2d; -using polygon_t = tier4_autoware_utils::Polygon2d; -using ring_t = tier4_autoware_utils::LinearRing2d; -using multi_polygon_t = tier4_autoware_utils::MultiPolygon2d; -using segment_t = tier4_autoware_utils::Segment2d; -using linestring_t = tier4_autoware_utils::LineString2d; -using multi_linestring_t = tier4_autoware_utils::MultiLineString2d; +typedef boost::geometry::index::rtree> SegmentRtree; struct PointDistance { - point_t point; + Point2d point; double distance; }; struct Projection { - point_t projected_point; + Point2d projected_point; double distance; double arc_length; }; +enum Side { LEFT, RIGHT }; + } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index c2d36fdd6e0d7..f4ab10db61b9c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -30,9 +30,7 @@ #include -#include #include -#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp index 40c339e507683..9cede67fff6e5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp @@ -20,7 +20,6 @@ #include #include -#include namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp index 3bc0e960fe468..f587051442dcd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp @@ -16,13 +16,11 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/parameters.hpp" #include #include #include -#include using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index 9d957ae9767c9..2ed58b9678e70 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -18,14 +18,11 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" - #include #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using tier4_autoware_utils::LinearRing2d; using BasicPolygons2d = std::vector; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp index 7364ea91339e4..ab319111f6da6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp @@ -16,12 +16,10 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include -#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index 20788e53309ec..54bba6aee2fc2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 1dc6cc411a31f..062a84bcd5aef 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -63,7 +63,7 @@ MarkerArray createPosesMarkerArray( MarkerArray createTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createGoalCandidatesMarkerArray( - GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); + const GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); MarkerArray createNumObjectsToAvoidTextsMarkerArray( const GoalCandidates & goal_candidates, std::string && ns, const std_msgs::msg::ColorRGBA & color); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 24a507d1f8695..c664ae3e15aef 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -16,17 +16,9 @@ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "lanelet2_core/geometry/Lanelet.h" - -#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" #include -#include -#include -#include -#include #include namespace behavior_path_planner @@ -85,6 +77,7 @@ struct LaneChangeParameters bool allow_loose_check_for_cancel{true}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; + utils::path_safety_checker::RSSparams rss_params_for_stuck{}; // abort LaneChangeCancelParameters cancel{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index b2592bc1b899b..ac712fa2f0fa5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -15,12 +15,10 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ -#include "behavior_path_planner/marker_utils/lane_change/debug.hpp" #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -32,10 +30,7 @@ #include -#include #include -#include -#include #include namespace behavior_path_planner::utils::lane_change @@ -52,6 +47,7 @@ using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using path_safety_checker::CollisionCheckDebugMap; using route_handler::Direction; using tier4_autoware_utils::Polygon2d; @@ -170,7 +166,8 @@ bool isParkedObject( bool passParkedObject( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters); + const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, + CollisionCheckDebugMap & object_debug); boost::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp index 93e4542568745..98d52b79e2edf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp @@ -15,9 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ -#include -#include - namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index 15b15bac40b51..67588ed0a67b6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -27,7 +27,6 @@ #include #include -#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 889f016e7182f..dc113b0b0be18 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -182,18 +182,19 @@ struct SafetyCheckParams struct CollisionCheckDebug { - std::string unsafe_reason; ///< Reason indicating unsafe situation. - Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. - Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. - Pose current_obj_pose{}; ///< Detected object's current pose. - Twist object_twist{}; ///< Detected object's velocity and rotation. - Pose expected_obj_pose{}; ///< Predicted future pose of object. - double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. - double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. - double extended_polygon_lon_offset{0.0}; ///< Longitudinal offset for extended polygon. - double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon. - bool is_front{false}; ///< True if object is in front of ego vehicle. - bool is_safe{false}; ///< True if situation is deemed safe. + std::string unsafe_reason; ///< Reason indicating unsafe situation. + Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. + Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. + Pose current_obj_pose{}; ///< Detected object's current pose. + Twist object_twist{}; ///< Detected object's velocity and rotation. + Pose expected_obj_pose{}; ///< Predicted future pose of object. + double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. + double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. + double forward_lon_offset{0.0}; ///< Forward longitudinal offset for extended polygon. + double backward_lon_offset{0.0}; ///< Backward longitudinal offset for extended polygon. + double lat_offset{0.0}; ///< Lateral offset for extended polygon. + bool is_front{false}; ///< True if object is in front of ego vehicle. + bool is_safe{false}; ///< True if situation is deemed safe. std::vector ego_predicted_path; ///< ego vehicle's predicted path. std::vector obj_predicted_path; ///< object's predicted path. Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 4983060aa1c0a..8409116236169 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -35,7 +34,6 @@ #include #endif -#include #include namespace behavior_path_planner::utils::path_safety_checker @@ -52,8 +50,6 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using vehicle_info_util::VehicleInfo; -namespace bg = boost::geometry; - bool isTargetObjectOncoming( const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose); @@ -66,10 +62,11 @@ bool isTargetObjectFront( Polygon2d createExtendedPolygon( const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double lon_length, const double lat_margin, CollisionCheckDebug & debug); + const double lon_length, const double lat_margin, const double is_stopped_obj, + CollisionCheckDebug & debug); Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, - CollisionCheckDebug & debug); + const double is_stopped_obj, CollisionCheckDebug & debug); PredictedPath convertToPredictedPath( const std::vector & path, const double time_resolution); @@ -80,7 +77,7 @@ double calcRssDistance( double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, - const BehaviorPathPlannerParameters & params); + const RSSparams & rss_params); boost::optional calcInterpolatedPoseWithVelocity( const std::vector & path, const double relative_time); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp index 4b1115a049ab0..91ed95b14b905 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp @@ -15,8 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ -#include "behavior_path_planner/parameters.hpp" - #include #include @@ -24,7 +22,6 @@ #include -#include #include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp index 65d79a2a4977b..6cdfd84d79075 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp @@ -20,9 +20,6 @@ #include #include -#include -#include - namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp index 55b8bdf777692..7a973a3bd699f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp @@ -15,7 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ -#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp index 2b2de183b2dac..d78df16f89a80 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -24,7 +24,6 @@ #include #include -#include namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index cb662efd767bf..4c9271d57127d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PLANNER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" @@ -27,14 +26,13 @@ #include #include -#include +namespace behavior_path_planner +{ using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; -namespace behavior_path_planner -{ enum class PlannerType { NONE = 0, SHIFT = 1, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index f2dba684de991..e7f51b2e86d5b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -18,7 +18,6 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include @@ -32,7 +31,6 @@ #include #include -#include namespace behavior_path_planner::start_planner_utils { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 3e03a09d3adf8..f418c9c7300f7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -16,14 +16,8 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_following/module_data.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -51,8 +45,6 @@ #include #include #include -#include -#include #include namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 755817aa22ae7..0777822650dfb 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -15,8 +15,13 @@ #include "behavior_path_planner/behavior_path_planner_node.hpp" #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/scene_module/avoidance/manager.hpp" +#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" +#include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include "behavior_path_planner/scene_module/lane_change/interface.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" +#include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "behavior_path_planner/scene_module/side_shift/manager.hpp" +#include "behavior_path_planner/scene_module/start_planner/manager.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include @@ -26,7 +31,6 @@ #include #include -#include #include namespace @@ -132,7 +136,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const std::lock_guard lock(mutex_manager_); // for planner_manager_ const auto & p = planner_data_->parameters; - planner_manager_ = std::make_shared(*this, p.verbose); + planner_manager_ = std::make_shared(*this, p.max_iteration_num, p.verbose); const auto register_and_create_publisher = [&](const auto & manager, const bool create_publishers) { @@ -272,6 +276,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() BehaviorPathPlannerParameters p{}; p.verbose = declare_parameter("verbose"); + p.max_iteration_num = declare_parameter("max_iteration_num"); const auto get_scene_module_manager_param = [&](std::string && ns) { ModuleConfigParameters config; @@ -1003,9 +1008,6 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updateParam( parameters, DrivableAreaExpansionParameters::AVOID_DYN_OBJECTS_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_dynamic_objects); - updateParam( - parameters, DrivableAreaExpansionParameters::EXPANSION_METHOD_PARAM, - planner_data_->drivable_area_expansion_parameters.expansion_method); updateParam( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_TYPES_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_types); @@ -1013,17 +1015,14 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_DIST_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_dist); updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_FRONT, - planner_data_->drivable_area_expansion_parameters.ego_extra_front_offset); - updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_REAR, - planner_data_->drivable_area_expansion_parameters.ego_extra_rear_offset); + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_FRONT_OVERHANG, + planner_data_->drivable_area_expansion_parameters.extra_front_overhang); updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_LEFT, - planner_data_->drivable_area_expansion_parameters.ego_extra_left_offset); + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WHEELBASE, + planner_data_->drivable_area_expansion_parameters.extra_wheelbase); updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_RIGHT, - planner_data_->drivable_area_expansion_parameters.ego_extra_right_offset); + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WIDTH, + planner_data_->drivable_area_expansion_parameters.extra_width); updateParam( parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_FRONT, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_front_offset); @@ -1046,14 +1045,20 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( parameters, DrivableAreaExpansionParameters::RESAMPLE_INTERVAL_PARAM, planner_data_->drivable_area_expansion_parameters.resample_interval); updateParam( - parameters, DrivableAreaExpansionParameters::EXTRA_ARC_LENGTH_PARAM, - planner_data_->drivable_area_expansion_parameters.extra_arc_length); + parameters, DrivableAreaExpansionParameters::MAX_REUSE_DEVIATION_PARAM, + planner_data_->drivable_area_expansion_parameters.max_reuse_deviation); updateParam( - parameters, DrivableAreaExpansionParameters::COMPENSATE_PARAM, - planner_data_->drivable_area_expansion_parameters.compensate_uncrossable_lines); + parameters, DrivableAreaExpansionParameters::SMOOTHING_CURVATURE_WINDOW_PARAM, + planner_data_->drivable_area_expansion_parameters.curvature_average_window); + updateParam( + parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM, + planner_data_->drivable_area_expansion_parameters.max_bound_rate); + updateParam( + parameters, DrivableAreaExpansionParameters::SMOOTHING_EXTRA_ARC_LENGTH_PARAM, + planner_data_->drivable_area_expansion_parameters.extra_arc_length); updateParam( - parameters, DrivableAreaExpansionParameters::EXTRA_COMPENSATE_PARAM, - planner_data_->drivable_area_expansion_parameters.compensate_extra_dist); + parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM, + planner_data_->drivable_area_expansion_parameters.print_runtime); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp index 14a7d5be6a58a..ce9cedc816a19 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -14,9 +14,7 @@ #include "behavior_path_planner/marker_utils/avoidance/debug.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include @@ -29,7 +27,6 @@ namespace marker_utils::avoidance_marker { -using behavior_path_planner::AvoidLine; using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index a1b77b1802999..a7604124ebe43 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -25,8 +25,6 @@ #include #include -#include -#include #include #include diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index a2883403e1ccd..031926f2d9d1d 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -17,7 +17,6 @@ #include "behavior_path_planner/marker_utils/colors.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -641,8 +640,9 @@ MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, st << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal << "\nEgo to obj: " << info.inter_vehicle_distance << "\nExtended polygon: " << (info.is_front ? "ego" : "object") - << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset - << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset + << "\nExtended polygon lateral offset: " << info.lat_offset + << "\nExtended polygon forward longitudinal offset: " << info.forward_lon_offset + << "\nExtended polygon backward longitudinal offset: " << info.backward_lon_offset << "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego") << "\nSafe: " << (info.is_safe ? "Yes" : "No"); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index d0e55ffa574fd..79e6055d26a81 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -14,7 +14,6 @@ #include "behavior_path_planner/planner_manager.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" #include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -29,9 +28,11 @@ namespace behavior_path_planner { -PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose) +PlannerManager::PlannerManager( + rclcpp::Node & node, const size_t max_iteration_num, const bool verbose) : logger_(node.get_logger().get_child("planner_manager")), clock_(*node.get_clock()), + max_iteration_num_{max_iteration_num}, verbose_{verbose} { processing_time_.emplace("total_time", 0.0); @@ -82,7 +83,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da return output; } - while (rclcpp::ok()) { + for (size_t itr_num = 1;; ++itr_num) { /** * STEP1: get approved modules' output */ @@ -128,8 +129,17 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da addApprovedModule(highest_priority_module); clearCandidateModules(); debug_info_.emplace_back(highest_priority_module, Action::ADD, "To Approval"); + + if (itr_num >= max_iteration_num_) { + RCLCPP_WARN_THROTTLE( + logger_, clock_, 1000, "Reach iteration limit (max: %ld). Output current result.", + max_iteration_num_); + processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + return candidate_modules_output; + } } - return BehaviorModuleOutput{}; + + return BehaviorModuleOutput{}; // something wrong. }(); std::for_each( diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index a8dd0b794245e..84064a1c38bcb 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -52,17 +52,12 @@ namespace behavior_path_planner { -using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; -using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcInterpolatedPose; using tier4_autoware_utils::calcLateralDeviation; -using tier4_autoware_utils::calcLongitudinalDeviation; -using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::getPoint; using tier4_autoware_utils::getPose; using tier4_autoware_utils::toHexString; @@ -143,6 +138,14 @@ AvoidLineArray toArray(const AvoidOutlines & outlines) } return ret; } + +lanelet::BasicLineString3d toLineString3d(const std::vector & bound) +{ + lanelet::BasicLineString3d ret{}; + std::for_each( + bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); }); + return ret; +} } // namespace AvoidanceModule::AvoidanceModule( @@ -171,7 +174,10 @@ bool AvoidanceModule::isExecutionRequested() const return false; } - return !avoid_data_.target_objects.empty(); + return std::any_of( + avoid_data_.target_objects.begin(), avoid_data_.target_objects.end(), [](const auto & o) { + return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + }); } bool AvoidanceModule::isExecutionReady() const @@ -245,6 +251,23 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( *getPreviousModuleOutput().reference_path, planner_data_); + // expand drivable lanes + std::for_each( + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + data.drivable_lanes.push_back( + utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_)); + }); + + // calc drivable bound + const auto shorten_lanes = + utils::cutOverlappedLanes(*getPreviousModuleOutput().path, data.drivable_lanes); + data.left_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, + parameters_->use_intersection_areas, true)); + data.right_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, + parameters_->use_intersection_areas, false)); + // reference path if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); @@ -289,12 +312,17 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::filterTargetObjects; using utils::avoidance::getTargetLanelets; + // Add margin in order to prevent avoidance request chattering only when the module is running. + const auto is_running = getCurrentStatus() == ModuleStatus::RUNNING || + getCurrentStatus() == ModuleStatus::WAITING_APPROVAL; + // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. + const auto sparse_resample_path = utils::resamplePathWithSpline( + helper_.getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output); const auto [object_within_target_lane, object_outside_target_lane] = utils::avoidance::separateObjectsByPath( - utils::resamplePathWithSpline( - helper_.getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output), - planner_data_, data, parameters_, debug); + sparse_resample_path, planner_data_, data, parameters_, helper_.getForwardDetectionRange(), + is_running, debug); for (const auto & object : object_outside_target_lane.objects) { ObjectData other_object; @@ -440,13 +468,21 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * STEP3: Create candidate shift lines. * Merge rough shift lines, and extract new shift lines. */ - data.new_shift_line = generateCandidateShiftLine(data.raw_shift_line, path_shifter, debug); + const auto processed_shift_lines = generateCandidateShiftLine(data.raw_shift_line, debug); + + /** + * Step4: Validate new shift lines. + * Output new shift lines only when the avoidance path which is generated from them doesn't have + * huge offset from ego. + */ + data.valid = isValidShiftLine(processed_shift_lines, path_shifter); + data.new_shift_line = data.valid ? processed_shift_lines : AvoidLineArray{}; const auto found_new_sl = data.new_shift_line.size() > 0; const auto registered = path_shifter.getShiftLines().size() > 0; data.found_avoidance_path = found_new_sl || registered; /** - * STEP4: Set new shift lines. + * STEP5: Set new shift lines. * If there are new shift points, these shift points are registered in path_shifter in order to * generate candidate avoidance path. */ @@ -455,7 +491,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de } /** - * STEP5: Generate avoidance path. + * STEP6: Generate avoidance path. */ ShiftedPath spline_shift_path = utils::avoidance::toShiftedPath(data.reference_path); const auto success_spline_path_generation = @@ -465,7 +501,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de : utils::avoidance::toShiftedPath(data.reference_path); /** - * STEP6: Check avoidance path safety. + * STEP7: Check avoidance path safety. * For each target objects and the objects in adjacent lanes, * check that there is a certain amount of margin in the lateral and longitudinal direction. */ @@ -476,6 +512,17 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de void AvoidanceModule::fillEgoStatus( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { + /** + * TODO(someone): prevent meaningless stop point insertion in other way. + * If the candidate shift line is invalid, manage all objects as unavoidable. + */ + if (!data.valid) { + std::for_each(data.target_objects.begin(), data.target_objects.end(), [](auto & o) { + o.is_avoidable = false; + o.reason = "InvalidShiftLine"; + }); + } + /** * Find the nearest object that should be avoid. When the ego follows reference path, * if the both of following two conditions are satisfied, the module surely avoid the object. @@ -762,7 +809,7 @@ AvoidLineArray AvoidanceModule::applyPreProcess( } AvoidLineArray AvoidanceModule::generateCandidateShiftLine( - const AvoidLineArray & shift_lines, const PathShifter & path_shifter, DebugData & debug) const + const AvoidLineArray & shift_lines, DebugData & debug) const { AvoidLineArray processed_shift_lines = shift_lines; @@ -782,15 +829,7 @@ AvoidLineArray AvoidanceModule::generateCandidateShiftLine( * Step3: Extract new shift lines. * Compare processed shift lines and registered shift lines in order to find new shift lines. */ - processed_shift_lines = findNewShiftLine(processed_shift_lines, debug); - - /** - * Step4: Validate new shift lines. - * Output new shift lines only when the avoidance path which is generated from them doesn't have - * huge offset from ego. - */ - return isValidShiftLine(processed_shift_lines, path_shifter) ? processed_shift_lines - : AvoidLineArray{}; + return findNewShiftLine(processed_shift_lines, debug); } void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) @@ -1914,34 +1953,6 @@ bool AvoidanceModule::isSafePath( return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count; } -void AvoidanceModule::generateExpandDrivableLanes(BehaviorModuleOutput & output) const -{ - std::vector drivable_lanes; - for (const auto & lanelet : avoid_data_.current_lanelets) { - drivable_lanes.push_back( - utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_)); - } - - { // for new architecture - DrivableAreaInfo current_drivable_area_info; - // generate drivable lanes - current_drivable_area_info.drivable_lanes = drivable_lanes; - // generate obstacle polygons - current_drivable_area_info.obstacles = - utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); - // expand hatched road markings - current_drivable_area_info.enable_expanding_hatched_road_markings = - parameters_->use_hatched_road_markings; - // expand intersection areas - current_drivable_area_info.enable_expanding_intersection_areas = - parameters_->use_intersection_areas; - - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - } -} - PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & original_path) const { const auto previous_path = helper_.getPreviousReferencePath(); @@ -2075,8 +2086,26 @@ BehaviorModuleOutput AvoidanceModule::plan() utils::clipPathLength(*output.path, ego_idx, planner_data_->parameters); // Drivable area generation. - generateExpandDrivableLanes(output); - setDrivableLanes(output.drivable_area_info.drivable_lanes); + { + DrivableAreaInfo current_drivable_area_info; + // generate drivable lanes + current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes; + // generate obstacle polygons + current_drivable_area_info.obstacles = + utils::avoidance::generateObstaclePolygonsForDrivableArea( + avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + // expand hatched road markings + current_drivable_area_info.enable_expanding_hatched_road_markings = + parameters_->use_hatched_road_markings; + // expand intersection areas + current_drivable_area_info.enable_expanding_intersection_areas = + parameters_->use_intersection_areas; + + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + + setDrivableLanes(output.drivable_area_info.drivable_lanes); + } return output; } @@ -2331,7 +2360,7 @@ bool AvoidanceModule::isValidShiftLine( const AvoidLineArray & shift_lines, const PathShifter & shifter) const { if (shift_lines.empty()) { - return false; + return true; } auto shifter_for_validate = shifter; @@ -2357,6 +2386,32 @@ bool AvoidanceModule::isValidShiftLine( } } + // check if the vehicle is in road. (yaw angle is not considered) + { + const auto minimum_distance = 0.5 * planner_data_->parameters.vehicle_width + + parameters_->hard_road_shoulder_margin - + parameters_->max_deviation_from_lane; + + const size_t start_idx = shift_lines.front().start_idx; + const size_t end_idx = shift_lines.back().end_idx; + + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto p = getPoint(shifter_for_validate.getReferencePath().points.at(i)); + lanelet::BasicPoint2d basic_point{p.x, p.y}; + + const auto shift_length = proposed_shift_path.shift_length.at(i); + const auto bound = shift_length > 0.0 ? avoid_data_.left_bound : avoid_data_.right_bound; + const auto THRESHOLD = minimum_distance + std::abs(shift_length); + + if (boost::geometry::distance(basic_point, lanelet::utils::to2D(bound)) < THRESHOLD) { + RCLCPP_DEBUG_THROTTLE( + getLogger(), *clock_, 1000, + "following latest new shift line may cause deviation from drivable area."); + return false; + } + } + } + return true; // valid shift line. } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 8c65323e5a123..763cf17890721 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -114,10 +114,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( *node, ns + "object_ignore_section_crosswalk_in_front_distance"); p.object_ignore_section_crosswalk_behind_distance = getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); - p.object_check_forward_distance = - getOrDeclareParameter(*node, ns + "object_check_forward_distance"); - p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "object_check_backward_distance"); p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -130,6 +126,17 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } + { + std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); + } + // safety check general params { std::string ns = "avoidance.safety_check."; @@ -201,6 +208,8 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "lateral_avoid_check_threshold"); p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); + p.max_deviation_from_lane = + getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); } // avoidance maneuver (longitudinal) diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 24c074bcb629b..2c5e78a267d5a 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -14,8 +14,8 @@ #include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "object_recognition_utils/predicted_path_utils.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include @@ -30,9 +30,9 @@ #include #include #include -#include #include #include + namespace behavior_path_planner { namespace @@ -106,21 +106,17 @@ std::pair projectObstacleVelocityToTrajectory( const std::vector & path_points, const PredictedObject & object) { const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; - const double obj_vel_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); - - const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); - const double obj_yaw = tf2::getYaw(obj_pose.orientation); - const double obj_vel_yaw = - obj_yaw + std::atan2( - object.kinematics.initial_twist_with_covariance.twist.linear.y, - object.kinematics.initial_twist_with_covariance.twist.linear.x); + const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_vel_yaw - path_yaw); - return std::make_pair(obj_vel_norm * std::cos(diff_yaw), obj_vel_norm * std::sin(diff_yaw)); + const Eigen::Rotation2Dd R_ego_to_obstacle(obj_yaw - path_yaw); + const Eigen::Vector2d obstacle_velocity( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + const Eigen::Vector2d projected_velocity = R_ego_to_obstacle * obstacle_velocity; + + return std::make_pair(projected_velocity[0], projected_velocity[1]); } double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 9c2544cf60cec..95ac47494d016 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -17,11 +17,11 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -38,7 +38,6 @@ #include using behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance; -using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; @@ -57,7 +56,8 @@ GoalPlannerModule::GoalPlannerModule( const std::unordered_map > & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, + status_{mutex_} { LaneDepartureChecker lane_departure_checker{}; lane_departure_checker.setVehicleInfo(vehicle_info_); @@ -113,16 +113,14 @@ GoalPlannerModule::GoalPlannerModule( freespace_parking_timer_cb_group_); } - resetStatus(); -} + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); + } -void GoalPlannerModule::resetStatus() -{ - PullOverStatus initial_status{}; - status_ = initial_status; - pull_over_path_candidates_.clear(); - closest_start_pose_.reset(); - goal_candidates_.clear(); + status_.reset(); } // This function is needed for waiting for planner_data_ @@ -139,17 +137,15 @@ void GoalPlannerModule::updateOccupancyGrid() void GoalPlannerModule::onTimer() { // already generated pull over candidate paths - if (!pull_over_path_candidates_.empty()) { + if (!status_.get_pull_over_path_candidates().empty()) { return; } // goals are not yet available. - if (goal_candidates_.empty()) { + if (status_.get_goal_candidates().empty()) { return; } - mutex_.lock(); - const auto goal_candidates = goal_candidates_; - mutex_.unlock(); + const auto goal_candidates = status_.get_goal_candidates(); // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( @@ -177,7 +173,6 @@ void GoalPlannerModule::onTimer() } } }; - // plan candidate paths and set them to the member variable if (parameters_->path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { @@ -199,10 +194,11 @@ void GoalPlannerModule::onTimer() } // set member variables - mutex_.lock(); - pull_over_path_candidates_ = path_candidates; - closest_start_pose_ = closest_start_pose; - mutex_.unlock(); + { + const std::lock_guard lock(mutex_); + status_.set_pull_over_path_candidates(path_candidates); + status_.set_closest_start_pose(closest_start_pose); + } } void GoalPlannerModule::onFreespaceParkingTimer() @@ -218,6 +214,10 @@ void GoalPlannerModule::onFreespaceParkingTimer() return; } + if (isOnModifiedGoal()) { + return; + } + const bool is_new_costmap = (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; constexpr double path_update_duration = 1.0; @@ -226,16 +226,18 @@ void GoalPlannerModule::onFreespaceParkingTimer() } } -BehaviorModuleOutput GoalPlannerModule::run() +void GoalPlannerModule::updateData() { - current_state_ = ModuleStatus::RUNNING; - updateOccupancyGrid(); - - if (!isActivated()) { - return planWaitingApproval(); + // Initialize Occupancy Grid Map + // This operation requires waiting for `planner_data_`, hence it is executed here instead of in + // the constructor. Ideally, this operation should only need to be performed once. + if ( + parameters_->use_occupancy_grid_for_goal_search || + parameters_->use_occupancy_grid_for_path_collision_check) { + initializeOccupancyGridMap(); } - return plan(); + updateOccupancyGrid(); } void GoalPlannerModule::initializeOccupancyGridMap() @@ -262,22 +264,6 @@ void GoalPlannerModule::initializeSafetyCheckParameters() objects_filtering_params_, parameters_); } -void GoalPlannerModule::processOnEntry() -{ - // Initialize occupancy grid map - if ( - parameters_->use_occupancy_grid_for_goal_search || - parameters_->use_occupancy_grid_for_path_collision_check) { - initializeOccupancyGridMap(); - } - // Initialize safety checker - if (parameters_->safety_check_params.enable_safety_check) { - initializeSafetyCheckParameters(); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - goal_planner_data_.collision_check); - } -} - void GoalPlannerModule::processOnExit() { resetPathCandidate(); @@ -444,28 +430,25 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const return refined_goal_pose; } -ModuleStatus GoalPlannerModule::updateState() -{ - // start_planner module will be run when setting new goal, so not need finishing pull_over module. - // Finishing it causes wrong lane_following path generation. - return current_state_; -} - bool GoalPlannerModule::planFreespacePath() { - mutex_.lock(); goal_searcher_->setPlannerData(planner_data_); - goal_searcher_->update(goal_candidates_); - const auto goal_candidates = goal_candidates_; - debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); - debug_data_.freespace_planner.is_planning = true; - mutex_.unlock(); + auto goal_candidates = status_.get_goal_candidates(); + goal_searcher_->update(goal_candidates); + status_.set_goal_candidates(goal_candidates); + + { + const std::lock_guard lock(mutex_); + debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); + debug_data_.freespace_planner.is_planning = true; + } for (size_t i = 0; i < goal_candidates.size(); i++) { const auto goal_candidate = goal_candidates.at(i); - mutex_.lock(); - debug_data_.freespace_planner.current_goal_idx = i; - mutex_.unlock(); + { + const std::lock_guard lock(mutex_); + debug_data_.freespace_planner.current_goal_idx = i; + } if (!goal_candidate.is_safe) { continue; @@ -476,17 +459,21 @@ bool GoalPlannerModule::planFreespacePath() if (!freespace_path) { continue; } - mutex_.lock(); - status_.pull_over_path = std::make_shared(*freespace_path); - status_.current_path_idx = 0; - status_.is_safe_static_objects = true; - modified_goal_pose_ = goal_candidate; - last_path_update_time_ = std::make_unique(clock_->now()); - debug_data_.freespace_planner.is_planning = false; - mutex_.unlock(); + + { + const std::lock_guard lock(mutex_); + status_.set_pull_over_path(std::make_shared(*freespace_path)); + status_.set_current_path_idx(0); + status_.set_is_safe_static_objects(true); + status_.set_modified_goal_pose(goal_candidate); + status_.set_last_path_update_time(std::make_shared(clock_->now())); + debug_data_.freespace_planner.is_planning = false; + } + return true; } + const std::lock_guard lock(mutex_); debug_data_.freespace_planner.is_planning = false; return false; } @@ -498,11 +485,11 @@ void GoalPlannerModule::returnToLaneParking() return; } - if (!status_.lane_parking_pull_over_path) { + if (!status_.get_lane_parking_pull_over_path()) { return; } - const PathWithLaneId path = status_.lane_parking_pull_over_path->getFullPath(); + const PathWithLaneId path = status_.get_lane_parking_pull_over_path()->getFullPath(); if (checkCollision(path)) { return; } @@ -515,13 +502,14 @@ void GoalPlannerModule::returnToLaneParking() return; } - mutex_.lock(); - status_.is_safe_static_objects = true; - status_.has_decided_path = false; - status_.pull_over_path = status_.lane_parking_pull_over_path; - status_.current_path_idx = 0; - last_path_update_time_ = std::make_unique(clock_->now()); - mutex_.unlock(); + { + const std::lock_guard lock(mutex_); + status_.set_is_safe_static_objects(true); + status_.set_has_decided_path(false); + status_.set_pull_over_path(status_.get_lane_parking_pull_over_path()); + status_.set_current_path_idx(0); + status_.set_last_path_update_time(std::make_shared(clock_->now())); + } RCLCPP_INFO(getLogger(), "return to lane parking"); } @@ -533,22 +521,24 @@ void GoalPlannerModule::generateGoalCandidates() // with old architecture, module instance is not cleared when new route is received // so need to reset status here. // todo: move this check out of this function after old architecture is removed - if (!goal_candidates_.empty()) { + if (!status_.get_goal_candidates().empty()) { return; } // calculate goal candidates const Pose goal_pose = route_handler->getOriginalGoalPose(); - refined_goal_pose_ = calcRefinedGoal(goal_pose); + status_.set_refined_goal_pose(calcRefinedGoal(goal_pose)); if (goal_planner_utils::isAllowedGoalModification(route_handler)) { goal_searcher_->setPlannerData(planner_data_); - goal_searcher_->setReferenceGoal(refined_goal_pose_); - goal_candidates_ = goal_searcher_->search(); + goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose()); + status_.set_goal_candidates(goal_searcher_->search()); } else { GoalCandidate goal_candidate{}; goal_candidate.goal_pose = goal_pose; goal_candidate.distance_from_original_goal = 0.0; - goal_candidates_.push_back(goal_candidate); + GoalCandidates goal_candidates{}; + goal_candidates.push_back(goal_candidate); + status_.set_goal_candidates(goal_candidates); } } @@ -569,10 +559,12 @@ BehaviorModuleOutput GoalPlannerModule::plan() } } -void GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( - std::vector & pull_over_path_candidates, +std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( + const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const { + auto sorted_pull_over_path_candidates = pull_over_path_candidates; + // Create a map of goal_id to its index in goal_candidates std::map goal_id_to_index; for (size_t i = 0; i < goal_candidates.size(); ++i) { @@ -581,7 +573,7 @@ void GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( // Sort pull_over_path_candidates based on the order in goal_candidates std::stable_sort( - pull_over_path_candidates.begin(), pull_over_path_candidates.end(), + sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), [&goal_id_to_index](const auto & a, const auto & b) { return goal_id_to_index[a.goal_id] < goal_id_to_index[b.goal_id]; }); @@ -589,7 +581,7 @@ void GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( // Sort pull_over_path_candidates based on the order in efficient_path_order if (parameters_->path_priority == "efficient_path") { std::stable_sort( - pull_over_path_candidates.begin(), pull_over_path_candidates.end(), + sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), [this](const auto & a, const auto & b) { const auto & order = parameters_->efficient_path_order; const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type)); @@ -597,20 +589,27 @@ void GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( return a_pos < b_pos; }); } + + return sorted_pull_over_path_candidates; } void GoalPlannerModule::selectSafePullOverPath() { // select safe lane pull over path from candidates - mutex_.lock(); - goal_searcher_->setPlannerData(planner_data_); - goal_searcher_->update(goal_candidates_); - sortPullOverPathCandidatesByGoalPriority(pull_over_path_candidates_, goal_candidates_); - const auto pull_over_path_candidates = pull_over_path_candidates_; - const auto goal_candidates = goal_candidates_; - mutex_.unlock(); + std::vector pull_over_path_candidates{}; + GoalCandidates goal_candidates{}; + { + const std::lock_guard lock(mutex_); + goal_searcher_->setPlannerData(planner_data_); + goal_candidates = status_.get_goal_candidates(); + goal_searcher_->update(goal_candidates); + status_.set_goal_candidates(goal_candidates); + status_.set_pull_over_path_candidates(sortPullOverPathCandidatesByGoalPriority( + status_.get_pull_over_path_candidates(), status_.get_goal_candidates())); + pull_over_path_candidates = status_.get_pull_over_path_candidates(); + status_.set_is_safe_static_objects(false); + } - status_.is_safe_static_objects = false; for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe const auto goal_candidate_it = std::find_if( @@ -629,71 +628,69 @@ void GoalPlannerModule::selectSafePullOverPath() continue; } - status_.is_safe_static_objects = true; - mutex_.lock(); - status_.pull_over_path = std::make_shared(pull_over_path); - status_.current_path_idx = 0; - status_.lane_parking_pull_over_path = status_.pull_over_path; - modified_goal_pose_ = *goal_candidate_it; - last_path_update_time_ = std::make_unique(clock_->now()); - mutex_.unlock(); + // found safe pull over path + { + const std::lock_guard lock(mutex_); + status_.set_is_safe_static_objects(true); + status_.set_pull_over_path(std::make_shared(pull_over_path)); + status_.set_current_path_idx(0); + status_.set_lane_parking_pull_over_path(status_.get_pull_over_path()); + status_.set_modified_goal_pose(*goal_candidate_it); + status_.set_last_path_update_time(std::make_shared(clock_->now())); + } break; } + if (!status_.get_is_safe_static_objects()) { + return; + } + // decelerate before the search area start - if (status_.is_safe_static_objects) { - const auto search_start_offset_pose = calcLongitudinalOffsetPose( - status_.pull_over_path->getFullPath().points, refined_goal_pose_.position, - -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - - approximate_pull_over_distance_); - auto & first_path = status_.pull_over_path->partial_paths.front(); - - if (search_start_offset_pose) { - decelerateBeforeSearchStart(*search_start_offset_pose, first_path); - } else { - // if already passed the search start offset pose, set pull_over_velocity to first_path. - const auto min_decel_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, - parameters_->pull_over_velocity); - for (auto & p : first_path.points) { - const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); - if (min_decel_distance && distance_from_ego < *min_decel_distance) { - continue; - } - p.point.longitudinal_velocity_mps = std::min( - p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); + const auto search_start_offset_pose = calcLongitudinalOffsetPose( + status_.get_pull_over_path()->getFullPath().points, status_.get_refined_goal_pose().position, + -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - + approximate_pull_over_distance_); + auto & first_path = status_.get_pull_over_path()->partial_paths.front(); + if (search_start_offset_pose) { + decelerateBeforeSearchStart(*search_start_offset_pose, first_path); + } else { + // if already passed the search start offset pose, set pull_over_velocity to first_path. + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + parameters_->pull_over_velocity); + for (auto & p : first_path.points) { + const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); + if (min_decel_distance && distance_from_ego < *min_decel_distance) { + continue; } + p.point.longitudinal_velocity_mps = std::min( + p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); } } - - // generate drivable area for each partial path - for (auto & path : status_.pull_over_path->partial_paths) { - const size_t ego_idx = planner_data_->findEgoIndex(path.points); - utils::clipPathLength(path, ego_idx, planner_data_->parameters); - } } void GoalPlannerModule::setLanes() { - status_.current_lanes = utils::getExtendedCurrentLanes( + const std::lock_guard lock(mutex_); + status_.set_current_lanes(utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, - /*forward_only_in_route*/ false); - status_.pull_over_lanes = goal_planner_utils::getPullOverLanes( + /*forward_only_in_route*/ false)); + status_.set_pull_over_lanes(goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); - status_.lanes = - utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_over_lanes); + parameters_->forward_goal_search_length)); + status_.set_lanes(utils::generateDrivableLanesWithShoulderLanes( + status_.get_current_lanes(), status_.get_pull_over_lanes())); } void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { - if (!status_.is_safe_static_objects) { + if (!status_.get_is_safe_static_objects()) { // situation : not safe against static objects use stop_path setStopPath(output); } else if ( parameters_->safety_check_params.enable_safety_check && !isSafePath() && - status_.has_decided_path && isActivated()) { + status_.get_has_decided_path() && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints @@ -718,36 +715,37 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) setModifiedGoal(output); // set hazard and turn signal - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { setTurnSignalInfo(output); } // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. - status_.prev_is_safe = status_.is_safe_static_objects; - status_.prev_is_safe_dynamic_objects = - parameters_->safety_check_params.enable_safety_check ? isSafePath() : true; + const std::lock_guard lock(mutex_); + status_.set_prev_is_safe(status_.get_is_safe_static_objects()); + status_.set_prev_is_safe_dynamic_objects( + parameters_->safety_check_params.enable_safety_check ? isSafePath() : true); } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) { - if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { + if (status_.get_prev_is_safe() || !status_.get_prev_stop_path()) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); - status_.prev_stop_path = output.path; + const std::lock_guard lock(mutex_); + status_.set_prev_stop_path(output.path); // set stop path as pull over path - mutex_.lock(); - PullOverPath pull_over_path{}; - status_.pull_over_path = std::make_shared(pull_over_path); - status_.current_path_idx = 0; - status_.pull_over_path->partial_paths.push_back(*output.path); - last_path_update_time_ = std::make_unique(clock_->now()); - mutex_.unlock(); + auto stop_pull_over_path = std::make_shared(); + stop_pull_over_path->partial_paths.push_back(*output.path); + status_.set_pull_over_path(stop_pull_over_path); + status_.set_current_path_idx(0); + status_.set_last_path_update_time(std::make_shared(clock_->now())); + RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); } else { // not_safe -> not_safe: use previous stop path - output.path = status_.prev_stop_path; + output.path = status_.get_prev_stop_path(); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } @@ -757,7 +755,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) { // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path - if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) { + if (status_.get_prev_is_safe_dynamic_objects() || !status_.get_prev_stop_path_after_approval()) { auto current_path = getCurrentPath(); const auto stop_path = behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( @@ -765,7 +763,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output parameters_->maximum_jerk_for_stop); if (stop_path) { output.path = std::make_shared(*stop_path); - status_.prev_stop_path_after_approval = output.path; + status_.set_prev_stop_path_after_approval(output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); } else { output.path = std::make_shared(getCurrentPath()); @@ -773,11 +771,10 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output getLogger(), *clock_, 5000, "Collision detected, no feasible stop path found, cannot stop."); } - std::lock_guard lock(mutex_); - last_path_update_time_ = std::make_unique(clock_->now()); + status_.set_last_path_update_time(std::make_shared(clock_->now())); } else { // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path - output.path = status_.prev_stop_path_after_approval; + output.path = status_.get_prev_stop_path_after_approval(); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); } output.reference_path = getPreviousModuleOutput().reference_path; @@ -785,13 +782,13 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { - if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, status_.lanes, planner_data_->drivable_area_expansion_parameters); + *output.path, status_.get_lanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -803,10 +800,10 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (status_.is_safe_static_objects) { + if (status_.get_is_safe_static_objects()) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); - modified_goal.pose = modified_goal_pose_->goal_pose; + modified_goal.pose = status_.get_modified_goal_pose()->goal_pose; modified_goal.header = route_handler->getRouteHeader(); output.modified_goal = modified_goal; } else { @@ -846,12 +843,12 @@ void GoalPlannerModule::updateSteeringFactor( bool GoalPlannerModule::hasDecidedPath() const { // once decided, keep the decision - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { return true; } // if path is not safe, not decided - if (!status_.is_safe_static_objects) { + if (!status_.get_is_safe_static_objects()) { return false; } @@ -863,10 +860,10 @@ bool GoalPlannerModule::hasDecidedPath() const return false; } const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - getCurrentPath().points, status_.pull_over_path->start_pose.position); + getCurrentPath().points, status_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( getCurrentPath().points, current_pose.position, *ego_segment_idx, - status_.pull_over_path->start_pose.position, start_pose_segment_idx); + status_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); return dist_to_parking_start_pose < parameters_->decide_path_distance; } @@ -875,15 +872,21 @@ void GoalPlannerModule::decideVelocity() const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; // decide velocity to guarantee turn signal lighting time - if (!status_.has_decided_velocity) { - auto & first_path = status_.pull_over_path->partial_paths.front(); + if (!status_.get_has_decided_velocity()) { + auto & first_path = status_.get_pull_over_path()->partial_paths.front(); const auto vel = static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); for (auto & p : first_path.points) { p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, vel); } } - status_.has_decided_velocity = true; + status_.set_has_decided_velocity(true); +} + +CandidateOutput GoalPlannerModule::planCandidate() const +{ + return CandidateOutput( + status_.get_pull_over_path() ? status_.get_pull_over_path()->getFullPath() : PathWithLaneId()); } BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() @@ -897,21 +900,26 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() setLanes(); // Check if it needs to decide path - status_.has_decided_path = hasDecidedPath(); + status_.set_has_decided_path(hasDecidedPath()); // Use decided path - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { if (isActivated() && isWaitingApproval()) { - last_approved_time_ = std::make_unique(clock_->now()); - last_approved_pose_ = std::make_unique(planner_data_->self_odometry->pose.pose); + { + const std::lock_guard lock(mutex_); + status_.set_last_approved_time(std::make_shared(clock_->now())); + status_.set_last_approved_pose( + std::make_shared(planner_data_->self_odometry->pose.pose)); + } clearWaitingApproval(); decideVelocity(); } transitionToNextPathIfFinishingCurrentPath(); - } else if (!pull_over_path_candidates_.empty() && needPathUpdate(path_update_duration)) { + } else if ( + !status_.get_pull_over_path_candidates().empty() && needPathUpdate(path_update_duration)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates - // and set it to status_.pull_over_path + // and set it to status_.get_pull_over_path() selectSafePullOverPath(); } // else: stop path is generated and set by setOutput() @@ -919,21 +927,21 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() // set output and status BehaviorModuleOutput output{}; setOutput(output); - path_candidate_ = std::make_shared(status_.pull_over_path->getFullPath()); + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; // return to lane parking if it is possible - if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { returnToLaneParking(); } const auto distance_to_path_change = calcDistanceToPathChange(); - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } // TODO(tkhmy) add handle status TRYING updateSteeringFactor( - {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, + {status_.get_pull_over_path()->start_pose, status_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::TURNING); // For debug @@ -943,7 +951,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() printParkingPositionError(); } - setStopReason(StopReason::GOAL_PLANNER, status_.pull_over_path->getFullPath()); + setStopReason(StopReason::GOAL_PLANNER, status_.get_pull_over_path()->getFullPath()); return output; } @@ -972,20 +980,18 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = status_.is_safe_static_objects - ? std::make_shared(status_.pull_over_path->getFullPath()) - : out.path; + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; const auto distance_to_path_change = calcDistanceToPathChange(); // generate drivable area info for new architecture - if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; out.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *out.path, status_.lanes, planner_data_->drivable_area_expansion_parameters); + *out.path, status_.get_lanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -993,21 +999,21 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); } - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } updateSteeringFactor( - {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, + {status_.get_pull_over_path()->start_pose, status_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::APPROACHING); - setStopReason(StopReason::GOAL_PLANNER, status_.pull_over_path->getFullPath()); + setStopReason(StopReason::GOAL_PLANNER, status_.get_pull_over_path()->getFullPath()); return out; } std::pair GoalPlannerModule::calcDistanceToPathChange() const { - const auto & full_path = status_.pull_over_path->getFullPath(); + const auto & full_path = status_.get_pull_over_path()->getFullPath(); const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), @@ -1017,15 +1023,15 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const } const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - full_path.points, status_.pull_over_path->start_pose.position); + full_path.points, status_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - status_.pull_over_path->start_pose.position, start_pose_segment_idx); + status_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); const size_t goal_pose_segment_idx = motion_utils::findNearestSegmentIndex( - full_path.points, modified_goal_pose_->goal_pose.position); + full_path.points, status_.get_modified_goal_pose()->goal_pose.position); const double dist_to_parking_finish_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - modified_goal_pose_->goal_pose.position, goal_pose_segment_idx); + status_.get_modified_goal_pose()->goal_pose.position, goal_pose_segment_idx); return {dist_to_parking_start_pose, dist_to_parking_finish_pose}; } @@ -1043,17 +1049,17 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; const double pull_over_velocity = parameters_->pull_over_velocity; - if (status_.current_lanes.empty()) { + if (status_.get_current_lanes().empty()) { return PathWithLaneId{}; } // generate reference path const auto s_current = - lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length; + lanelet::utils::getArcCoordinates(status_.get_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; auto reference_path = - route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); + route_handler->getCenterLinePath(status_.get_current_lanes(), s_start, s_end, true); // if not approved stop road lane. // stop point priority is @@ -1064,17 +1070,20 @@ PathWithLaneId GoalPlannerModule::generateStopPath() // difference between the outer and inner sides) // 4. feasible stop const auto search_start_offset_pose = calcLongitudinalOffsetPose( - reference_path.points, refined_goal_pose_.position, + reference_path.points, status_.get_refined_goal_pose().position, -parameters_->backward_goal_search_length - common_parameters.base_link2front - approximate_pull_over_distance_); - if (!status_.is_safe_static_objects && !closest_start_pose_ && !search_start_offset_pose) { + if ( + !status_.get_is_safe_static_objects() && !status_.get_closest_start_pose() && + !search_start_offset_pose) { return generateFeasibleStopPath(); } const Pose stop_pose = - status_.is_safe_static_objects - ? status_.pull_over_path->start_pose - : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_offset_pose); + status_.get_is_safe_static_objects() + ? status_.get_pull_over_path()->start_pose + : (status_.get_closest_start_pose() ? status_.get_closest_start_pose().value() + : *search_start_offset_pose); // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); @@ -1120,10 +1129,11 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() // generate stop reference path const auto s_current = - lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length; + lanelet::utils::getArcCoordinates(status_.get_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; - auto stop_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); + auto stop_path = + route_handler->getCenterLinePath(status_.get_current_lanes(), s_start, s_end, true); // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( @@ -1144,15 +1154,16 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { - if (isActivated() && last_approved_time_ != nullptr) { + if (isActivated() && status_.get_last_approved_time()) { // if using arc_path and finishing current_path, get next path // enough time for turn signal - const bool has_passed_enough_time = (clock_->now() - *last_approved_time_).seconds() > - planner_data_->parameters.turn_signal_search_time; + const bool has_passed_enough_time = + (clock_->now() - *status_.get_last_approved_time()).seconds() > + planner_data_->parameters.turn_signal_search_time; - if (hasFinishedCurrentPath() && has_passed_enough_time && status_.require_increment_) { + if (hasFinishedCurrentPath() && has_passed_enough_time && status_.get_require_increment()) { if (incrementPathIndex()) { - last_increment_time_ = std::make_unique(clock_->now()); + status_.set_last_increment_time(std::make_shared(clock_->now())); } } } @@ -1160,23 +1171,23 @@ void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() bool GoalPlannerModule::incrementPathIndex() { - if (status_.current_path_idx == status_.pull_over_path->partial_paths.size() - 1) { + if (status_.get_current_path_idx() == status_.get_pull_over_path()->partial_paths.size() - 1) { return false; } - status_.current_path_idx = status_.current_path_idx + 1; + status_.set_current_path_idx(status_.get_current_path_idx() + 1); return true; } PathWithLaneId GoalPlannerModule::getCurrentPath() const { - if (status_.pull_over_path == nullptr) { + if (status_.get_pull_over_path() == nullptr) { return PathWithLaneId{}; } - if (status_.pull_over_path->partial_paths.size() <= status_.current_path_idx) { + if (status_.get_pull_over_path()->partial_paths.size() <= status_.get_current_path_idx()) { return PathWithLaneId{}; } - return status_.pull_over_path->partial_paths.at(status_.current_path_idx); + return status_.get_pull_over_path()->partial_paths.at(status_.get_current_path_idx()); } bool GoalPlannerModule::isStopped( @@ -1205,23 +1216,28 @@ bool GoalPlannerModule::isStopped( bool GoalPlannerModule::isStopped() { + const std::lock_guard lock(mutex_); return isStopped(odometry_buffer_stopped_, parameters_->th_stopped_time); } bool GoalPlannerModule::isStuck() { + if (isOnModifiedGoal()) { + return false; + } + constexpr double stuck_time = 5.0; if (!isStopped(odometry_buffer_stuck_, stuck_time)) { return false; } // not found safe path - if (!status_.is_safe_static_objects) { + if (!status_.get_is_safe_static_objects()) { return true; } // any path has never been found - if (!status_.pull_over_path) { + if (!status_.get_pull_over_path()) { return false; } @@ -1240,12 +1256,12 @@ bool GoalPlannerModule::hasFinishedCurrentPath() bool GoalPlannerModule::isOnModifiedGoal() const { - if (!modified_goal_pose_) { + if (!status_.get_modified_goal_pose()) { return false; } const Pose current_pose = planner_data_->self_odometry->pose.pose; - return calcDistance2d(current_pose, modified_goal_pose_->goal_pose) < + return calcDistance2d(current_pose, status_.get_modified_goal_pose()->goal_pose) < parameters_->th_arrived_distance; } @@ -1254,9 +1270,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const TurnSignalInfo turn_signal{}; // output const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & start_pose = status_.pull_over_path->start_pose; - const auto & end_pose = status_.pull_over_path->end_pose; - const auto full_path = status_.pull_over_path->getFullPath(); + const auto & start_pose = status_.get_pull_over_path()->start_pose; + const auto & end_pose = status_.get_pull_over_path()->end_pose; + const auto full_path = status_.get_pull_over_path()->getFullPath(); // calc TurnIndicatorsCommand { @@ -1279,7 +1295,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const // ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds // before starting pull_over turn_signal.desired_start_point = - last_approved_pose_ && status_.has_decided_path ? *last_approved_pose_ : current_pose; + status_.get_last_approved_pose() && status_.get_has_decided_path() + ? *status_.get_last_approved_pose() + : current_pose; turn_signal.desired_end_point = end_pose; turn_signal.required_start_point = start_pose; turn_signal.required_end_point = end_pose; @@ -1336,7 +1354,7 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c // so need enough distance to restart. // distance to restart should be less than decide_path_distance. // otherwise, the goal would change immediately after departure. - const bool is_separated_path = status_.pull_over_path->partial_paths.size() > 1; + const bool is_separated_path = status_.get_pull_over_path()->partial_paths.size() > 1; const double distance_to_start = calcSignedArcLength( pull_over_path.getFullPath().points, current_pose.position, pull_over_path.start_pose.position); const double distance_to_restart = parameters_->decide_path_distance / 2; @@ -1367,10 +1385,10 @@ void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) { constexpr double keep_stop_time = 2.0; constexpr double keep_current_idx_buffer_time = 2.0; - if (last_increment_time_) { - const auto time_diff = (clock_->now() - *last_increment_time_).seconds(); + if (status_.get_last_increment_time()) { + const auto time_diff = (clock_->now() - *status_.get_last_increment_time()).seconds(); if (time_diff < keep_stop_time) { - status_.require_increment_ = false; + status_.set_require_increment(false); for (auto & p : path.points) { p.point.longitudinal_velocity_mps = 0.0; } @@ -1378,7 +1396,7 @@ void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) // require increment only when the time passed is enough // to prevent increment before driving // when the end of the current path is close to the current pose - status_.require_increment_ = true; + status_.set_require_increment(true); } } } @@ -1544,7 +1562,7 @@ bool GoalPlannerModule::isCrossingPossible( bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) const { - const lanelet::ConstLanelets lanes = utils::transformToLanelets(status_.lanes); + const lanelet::ConstLanelets lanes = utils::transformToLanelets(status_.get_lanes()); const Pose & start_pose = pull_over_path.start_pose; const Pose & end_pose = pull_over_path.end_pose; @@ -1579,11 +1597,12 @@ bool GoalPlannerModule::isSafePath() const const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); const std::pair terminal_velocity_and_accel = utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( - status_.pull_over_path->pairs_terminal_velocity_and_accel, status_.current_path_idx); + status_.get_pull_over_path()->pairs_terminal_velocity_and_accel, + status_.get_current_path_idx()); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); - RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); + RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.get_current_path_idx()); utils::start_goal_planner_common::updatePathProperty( ego_predicted_path_params_, terminal_velocity_and_accel); // TODO(Sugahara): shoule judge is_object_front properly @@ -1604,7 +1623,7 @@ bool GoalPlannerModule::isSafePath() const pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); const double hysteresis_factor = - status_.prev_is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + status_.get_prev_is_safe_dynamic_objects() ? 1.0 : parameters_->hysteresis_factor_expand_rate; utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); @@ -1666,29 +1685,31 @@ void GoalPlannerModule::setDebugData() }; if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas - const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow - : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green - const double z = refined_goal_pose_.position.z; + const auto color = status_.get_has_decided_path() + ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow + : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green + const double z = status_.get_refined_goal_pose().position.z; add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z)); // Visualize goal candidates - add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates_, color)); + const auto goal_candidates = status_.get_goal_candidates(); + add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color)); } // Visualize path and related pose - if (status_.is_safe_static_objects) { + if (status_.get_is_safe_static_objects()) { add(createPoseMarkerArray( - status_.pull_over_path->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); + status_.get_pull_over_path()->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - status_.pull_over_path->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); - add( - createPathMarkerArray(status_.pull_over_path->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + status_.get_pull_over_path()->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + add(createPathMarkerArray( + status_.get_pull_over_path()->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray(getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); // visualize each partial path - for (size_t i = 0; i < status_.pull_over_path->partial_paths.size(); ++i) { - const auto & partial_path = status_.pull_over_path->partial_paths.at(i); + for (size_t i = 0; i < status_.get_pull_over_path()->partial_paths.size(); ++i) { + const auto & partial_path = status_.get_pull_over_path()->partial_paths.at(i); add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } @@ -1734,16 +1755,17 @@ void GoalPlannerModule::setDebugData() // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.get_is_safe_static_objects() + ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = modified_goal_pose_ ? modified_goal_pose_->goal_pose - : planner_data_->self_odometry->pose.pose; - marker.text = magic_enum::enum_name(status_.pull_over_path->type); - marker.text += " " + std::to_string(status_.current_path_idx) + "/" + - std::to_string(status_.pull_over_path->partial_paths.size() - 1); + marker.pose = status_.get_modified_goal_pose() ? status_.get_modified_goal_pose()->goal_pose + : planner_data_->self_odometry->pose.pose; + marker.text = magic_enum::enum_name(status_.get_pull_over_path()->type); + marker.text += " " + std::to_string(status_.get_current_path_idx()) + "/" + + std::to_string(status_.get_pull_over_path()->partial_paths.size() - 1); if (isStuck()) { marker.text += " stuck"; } else if (isStopped()) { @@ -1761,7 +1783,7 @@ void GoalPlannerModule::setDebugData() } // Visualize debug poses - const auto & debug_poses = status_.pull_over_path->debug_poses; + const auto & debug_poses = status_.get_pull_over_path()->debug_poses; for (size_t i = 0; i < debug_poses.size(); ++i) { add(createPoseMarkerArray( debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); @@ -1773,7 +1795,8 @@ void GoalPlannerModule::printParkingPositionError() const const auto current_pose = planner_data_->self_odometry->pose.pose; const double real_shoulder_to_map_shoulder = 0.0; - const Pose goal_to_ego = inverseTransformPose(current_pose, modified_goal_pose_->goal_pose); + const Pose goal_to_ego = + inverseTransformPose(current_pose, status_.get_modified_goal_pose()->goal_pose); const double dx = goal_to_ego.position.x; const double dy = goal_to_ego.position.y; const double distance_from_real_shoulder = @@ -1782,7 +1805,7 @@ void GoalPlannerModule::printParkingPositionError() const getLogger(), "current pose to goal, dx:%f dy:%f dyaw:%f from_real_shoulder:%f", dx, dy, tier4_autoware_utils::rad2deg( tf2::getYaw(current_pose.orientation) - - tf2::getYaw(modified_goal_pose_->goal_pose.orientation)), + tf2::getYaw(status_.get_modified_goal_pose()->goal_pose.orientation)), distance_from_real_shoulder); } @@ -1808,10 +1831,10 @@ bool GoalPlannerModule::needPathUpdate(const double path_update_duration) const bool GoalPlannerModule::hasEnoughTimePassedSincePathUpdate(const double duration) const { - if (!last_path_update_time_) { + if (!status_.get_last_path_update_time()) { return true; } - return (clock_->now() - *last_path_update_time_).seconds() > duration; + return (clock_->now() - *status_.get_last_path_update_time()).seconds() > duration; } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 1f305e3988828..daba02e1cee89 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -17,7 +17,6 @@ #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp index a74a51b7e2cdb..3a304a9b5bb53 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp @@ -14,18 +14,9 @@ #include "behavior_path_planner/scene_module/lane_change/external_request.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" - #include -#include -#include #include -#include -#include -#include namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 51875952d40fb..22c8ac022ad1a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -19,12 +19,10 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include #include -#include #include #include #include @@ -147,7 +145,7 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::RUNNING; } - if (module_type_->isEgoOnPreparePhase()) { + if (module_type_->isEgoOnPreparePhase() && module_type_->isAbleToReturnCurrentLane()) { RCLCPP_WARN_STREAM_THROTTLE( getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, "Lane change path is unsafe. Cancel lane change."); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index e33a6c4b05ee3..78e05c940a814 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -21,7 +21,6 @@ #include #include -#include #include namespace behavior_path_planner @@ -128,6 +127,21 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter( *node, parameter("safety_check.cancel.lateral_distance_max_threshold")); + p.rss_params_for_stuck.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.stuck.longitudinal_distance_min_threshold")); + p.rss_params_for_stuck.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.stuck.longitudinal_velocity_delta_time")); + p.rss_params_for_stuck.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.stuck.expected_front_deceleration")); + p.rss_params_for_stuck.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.stuck.expected_rear_deceleration")); + p.rss_params_for_stuck.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.stuck.rear_vehicle_reaction_time")); + p.rss_params_for_stuck.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.stuck.rear_vehicle_safety_time_margin")); + p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.stuck.lateral_distance_max_threshold")); + // target object { std::string ns = "lane_change.target_object."; @@ -314,10 +328,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( *node, ns + "object_ignore_section_crosswalk_in_front_distance"); p.object_ignore_section_crosswalk_behind_distance = getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); - p.object_check_forward_distance = - getOrDeclareParameter(*node, ns + "object_check_forward_distance"); - p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "object_check_backward_distance"); p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -330,6 +340,17 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } + { + std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); + } + // safety check { std::string ns = "avoidance.safety_check."; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 71ba8c51ec6ad..de120592e7505 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -15,9 +15,9 @@ #include "behavior_path_planner/scene_module/lane_change/normal.hpp" #include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -37,6 +37,8 @@ namespace behavior_path_planner { +using motion_utils::calcSignedArcLength; + NormalLaneChange::NormalLaneChange( const std::shared_ptr & parameters, LaneChangeModuleType type, Direction direction) @@ -81,12 +83,22 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) // find candidate paths LaneChangePaths valid_paths{}; - const auto found_safe_path = - getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths); + const bool is_stuck = isVehicleStuck(current_lanes); + bool found_safe_path = getLaneChangePaths( + current_lanes, target_lanes, direction_, &valid_paths, lane_change_parameters_->rss_params, + is_stuck); + // if no safe path is found and ego is stuck, try to find a path with a small margin + if (!found_safe_path && is_stuck) { + found_safe_path = getLaneChangePaths( + current_lanes, target_lanes, direction_, &valid_paths, + lane_change_parameters_->rss_params_for_stuck, is_stuck); + } + debug_valid_path_ = valid_paths; if (valid_paths.empty()) { safe_path.info.current_lanes = current_lanes; + safe_path.info.target_lanes = target_lanes; return {false, false}; } @@ -139,7 +151,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() if (isStopState()) { const auto current_velocity = getEgoVelocity(); - const auto current_dist = motion_utils::calcSignedArcLength( + const auto current_dist = calcSignedArcLength( output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); @@ -191,18 +203,105 @@ void NormalLaneChange::insertStopPoint( const double lane_change_buffer = utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); + const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { + return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); + }; + // If lanelets.back() is in goal route section, get distance to goal. // Otherwise, get distance to end of lane. double distance_to_terminal = 0.0; if (route_handler->isInGoalRouteSection(lanelets.back())) { const auto goal = route_handler->getGoalPose(); - distance_to_terminal = utils::getSignedDistance(path.points.front().point.pose, goal, lanelets); + distance_to_terminal = getDistanceAlongLanelet(goal); } else { distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); } const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; - const double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); + double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + + // calculate minimum distance from path front to the stationary object on the ego lane. + const auto distance_to_ego_lane_obj = [&]() -> double { + double distance_to_obj = distance_to_terminal; + const double distance_to_ego = getDistanceAlongLanelet(getEgoPose()); + + for (const auto & object : target_objects.current_lane) { + // check if stationary + const auto obj_v = std::abs(object.initial_twist.twist.linear.x); + if (obj_v > lane_change_parameters_->stop_velocity_threshold) { + continue; + } + + // calculate distance from path front to the stationary object polygon on the ego lane. + const auto polygon = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + for (const auto & polygon_p : polygon) { + const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d()); + const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp); + + // ignore if the point is around the ego path + if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { + continue; + } + + const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); + + // ignore backward object + if (current_distance_to_obj < distance_to_ego) { + continue; + } + distance_to_obj = std::min(distance_to_obj, current_distance_to_obj); + } + } + return distance_to_obj; + }(); + + // Need to stop before blocking obstacle + if (distance_to_ego_lane_obj < distance_to_terminal) { + // consider rss distance when the LC need to avoid obstacles + const auto rss_dist = calcRssDistance( + 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + lane_change_parameters_->rss_params); + + const auto stopping_distance_for_obj = distance_to_ego_lane_obj - lane_change_buffer - + stop_point_buffer - rss_dist - + getCommonParam().base_link2front; + + // If the target lane in the lane change section is blocked by a stationary obstacle, there + // is no reason for stopping with a lane change margin. Instead, stop right behind the + // obstacle. + // ---------------------------------------------------------- + // [obj]> + // ---------------------------------------------------------- + // [ego]> | <--- lane change margin ---> [obj]> + // ---------------------------------------------------------- + const bool has_blocking_target_lane_obj = std::any_of( + target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) { + const auto v = std::abs(o.initial_twist.twist.linear.x); + if (v > lane_change_parameters_->stop_velocity_threshold) { + return false; + } + + // target_objects includes objects out of target lanes, so filter them out + if (!boost::geometry::intersects( + tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + lanelet::utils::combineLaneletsShape(status_.target_lanes) + .polygon2d() + .basicPolygon())) { + return false; + } + + const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); + return stopping_distance_for_obj < distance_to_target_lane_obj && + distance_to_target_lane_obj < distance_to_ego_lane_obj; + }); + + if (!has_blocking_target_lane_obj) { + stopping_distance = stopping_distance_for_obj; + } + } + if (stopping_distance > 0.0) { const auto stop_point = utils::insertStopPoint(stopping_distance, path); setStopPose(stop_point.point.pose); @@ -220,8 +319,7 @@ std::optional NormalLaneChange::extendPath() const auto path = status_.lane_change_path.path; const auto lc_start_point = status_.lane_change_path.info.lane_changing_start.position; - const auto dist = - motion_utils::calcSignedArcLength(path.points, lc_start_point, getEgoPosition()); + const auto dist = calcSignedArcLength(path.points, lc_start_point, getEgoPosition()); if (dist < 0.0) { return std::nullopt; @@ -412,6 +510,12 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } + if (!utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters, + lane_change_parameters_->cancel.overhang_tolerance)) { + return false; + } + const auto nearest_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( status_.lane_change_path.path.points, getEgoPose(), planner_data_->parameters.ego_nearest_dist_threshold, @@ -423,7 +527,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { - dist += motion_utils::calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); + dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( @@ -439,7 +543,7 @@ bool NormalLaneChange::isEgoOnPreparePhase() const { const auto & start_position = status_.lane_change_path.info.shift_line.start.position; const auto & path_points = status_.lane_change_path.path.points; - return motion_utils::calcSignedArcLength(path_points, start_position, getEgoPosition()) < 0.0; + return calcSignedArcLength(path_points, start_position, getEgoPosition()) < 0.0; } bool NormalLaneChange::isAbleToStopSafely() const @@ -459,7 +563,7 @@ bool NormalLaneChange::isAbleToStopSafely() const double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { - dist += motion_utils::calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); + dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > stop_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( @@ -475,7 +579,7 @@ bool NormalLaneChange::hasFinishedAbort() const return true; } - const auto distance_to_finish = motion_utils::calcSignedArcLength( + const auto distance_to_finish = calcSignedArcLength( abort_path_->path.points, getEgoPosition(), abort_path_->info.shift_line.end.position); if (distance_to_finish < 0.0) { @@ -508,6 +612,37 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane, get_opposite_direction)); } +std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() const +{ + const auto & p = getCommonParam(); + + const auto vehicle_min_acc = std::max(p.min_acc, lane_change_parameters_->min_longitudinal_acc); + const auto vehicle_max_acc = std::min(p.max_acc, lane_change_parameters_->max_longitudinal_acc); + + const auto ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_module_path_.points, getEgoPose(), p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold); + const auto max_path_velocity = + prev_module_path_.points.at(ego_seg_idx).point.longitudinal_velocity_mps; + + // calculate minimum and maximum acceleration + const auto min_acc = + utils::lane_change::calcMinimumAcceleration(getEgoVelocity(), vehicle_min_acc, p); + const auto max_acc = utils::lane_change::calcMaximumAcceleration( + getEgoVelocity(), max_path_velocity, vehicle_max_acc, p); + + return {min_acc, max_acc}; +} + +double NormalLaneChange::calcMaximumLaneChangeLength( + const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const +{ + const auto shift_intervals = + getRouteHandler()->getLateralIntervalsToPreferredLane(current_terminal_lanelet); + return utils::lane_change::calcMaximumLaneChangeLength( + getEgoVelocity(), getCommonParam(), shift_intervals, max_acc); +} + std::vector NormalLaneChange::sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { @@ -515,42 +650,37 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( return {}; } - const auto & common_parameters = planner_data_->parameters; const auto & route_handler = *getRouteHandler(); const auto current_pose = getEgoPose(); - const auto current_velocity = getEgoVelocity(); - const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; - const auto vehicle_min_acc = - std::max(common_parameters.min_acc, lane_change_parameters_->min_longitudinal_acc); - const auto vehicle_max_acc = - std::min(common_parameters.max_acc, lane_change_parameters_->max_longitudinal_acc); - const double nearest_dist_threshold = common_parameters.ego_nearest_dist_threshold; - const double nearest_yaw_threshold = common_parameters.ego_nearest_yaw_threshold; - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_module_path_.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double & max_path_velocity = - prev_module_path_.points.at(current_seg_idx).point.longitudinal_velocity_mps; - - // calculate minimum and maximum acceleration - const auto min_acc = utils::lane_change::calcMinimumAcceleration( - current_velocity, vehicle_min_acc, common_parameters); - const auto max_acc = utils::lane_change::calcMaximumAcceleration( - current_velocity, max_path_velocity, vehicle_max_acc, common_parameters); + const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); // if max acc is not positive, then we do the normal sampling if (max_acc <= 0.0) { + RCLCPP_DEBUG( + logger_, "Available max acc <= 0. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); return utils::lane_change::getAccelerationValues( min_acc, max_acc, longitudinal_acc_sampling_num); } // calculate maximum lane change length - const double max_lane_change_length = utils::lane_change::calcMaximumLaneChangeLength( - current_velocity, common_parameters, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), max_acc); + const double max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + RCLCPP_DEBUG( + logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, + max_acc); + return utils::lane_change::getAccelerationValues( + min_acc, max_acc, longitudinal_acc_sampling_num); + } + + // If the ego is in stuck, sampling all possible accelerations to find avoiding path. + if (isVehicleStuck(current_lanes)) { + auto clock = rclcpp::Clock(RCL_ROS_TIME); + RCLCPP_INFO_THROTTLE( + logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, + max_acc); return utils::lane_change::getAccelerationValues( min_acc, max_acc, longitudinal_acc_sampling_num); } @@ -560,12 +690,17 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( if (route_handler.isInGoalRouteSection(target_lanes.back())) { const auto goal_pose = route_handler.getGoalPose(); if (max_lane_change_length < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { + RCLCPP_DEBUG( + logger_, "Distance to goal has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; } } else if (max_lane_change_length < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { + RCLCPP_DEBUG( + logger_, "Distance to end of lane has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; } + RCLCPP_DEBUG(logger_, "Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); } @@ -613,7 +748,9 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; - const auto & objects = *planner_data_->dynamic_object; + auto objects = *planner_data_->dynamic_object; + utils::path_safety_checker::filterObjectsByClass( + objects, lane_change_parameters_->object_types_to_check); // get backward lanes const auto backward_length = lane_change_parameters_->backward_lane_length; @@ -621,7 +758,8 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( route_handler, target_lanes, current_pose, backward_length); // filter objects to get target object index - const auto target_obj_index = filterObject(current_lanes, target_lanes, target_backward_lanes); + const auto target_obj_index = + filterObject(objects, current_lanes, target_lanes, target_backward_lanes); LaneChangeTargetObjects target_objects; target_objects.current_lane.reserve(target_obj_index.current_lane.size()); @@ -653,13 +791,13 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( } LaneChangeTargetObjectIndices NormalLaneChange::filterObject( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const { const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; - const auto & objects = *planner_data_->dynamic_object; // Guard if (objects.objects.empty()) { @@ -699,15 +837,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( target_backward_polygons.push_back(lane_polygon); } - auto filtered_objects = objects; - - utils::path_safety_checker::filterObjectsByClass( - filtered_objects, lane_change_parameters_->object_types_to_check); - LaneChangeTargetObjectIndices filtered_obj_indices; - for (size_t i = 0; i < filtered_objects.objects.size(); ++i) { - const auto & object = filtered_objects.objects.at(i); - const auto & obj_velocity_norm = std::hypot( + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & object = objects.objects.at(i); + const auto obj_velocity_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y); const auto extended_object = @@ -718,10 +851,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // calc distance from the current ego position double max_dist_ego_to_obj = std::numeric_limits::lowest(); double min_dist_ego_to_obj = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & polygon_p : obj_polygon_outer) { const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const double dist_ego_to_obj = - motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p); + const double dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); } @@ -908,10 +1041,13 @@ bool NormalLaneChange::hasEnoughLengthToIntersection( bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, LaneChangePaths * candidate_paths, const bool check_safety) const + Direction direction, LaneChangePaths * candidate_paths, + const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, + const bool check_safety) const { object_debug_.clear(); if (current_lanes.empty() || target_lanes.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; } const auto & route_handler = *getRouteHandler(); @@ -947,6 +1083,7 @@ bool NormalLaneChange::getLaneChangePaths( const auto target_neighbor_preferred_lane_poly_2d = utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); if (target_neighbor_preferred_lane_poly_2d.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; } @@ -958,8 +1095,18 @@ bool NormalLaneChange::getLaneChangePaths( candidate_paths->reserve( longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size()); + RCLCPP_DEBUG( + logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", + prepare_durations.size(), longitudinal_acc_sampling_values.size()); + for (const auto & prepare_duration : prepare_durations) { for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { + const auto debug_print = [&](const auto & s) { + RCLCPP_DEBUG_STREAM( + logger_, " - " << s << " : prep_time = " << prepare_duration + << ", lon_acc = " << sampled_longitudinal_acc); + }; + // get path on original lanes const auto prepare_velocity = std::max( current_velocity + sampled_longitudinal_acc * prepare_duration, @@ -977,7 +1124,7 @@ bool NormalLaneChange::getLaneChangePaths( auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); if (prepare_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "prepare segment is empty!!"); + debug_print("prepare segment is empty...? Unexpected."); continue; } @@ -988,8 +1135,7 @@ bool NormalLaneChange::getLaneChangePaths( // Check if the lane changing start point is not on the lanes next to target lanes, if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_DEBUG( - logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!"); + debug_print("lane change start getEgoPose() is behind target lanelet!"); break; } @@ -1004,11 +1150,21 @@ bool NormalLaneChange::getLaneChangePaths( common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity); const auto lateral_acc_resolution = std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; - constexpr double lateral_acc_epsilon = 0.01; - for (double lateral_acc = min_lateral_acc; - lateral_acc < max_lateral_acc + lateral_acc_epsilon; - lateral_acc += lateral_acc_resolution) { + std::vector sample_lat_acc; + constexpr double eps = 0.01; + for (double a = min_lateral_acc; a < max_lateral_acc + eps; a += lateral_acc_resolution) { + sample_lat_acc.push_back(a); + } + RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); + + for (const auto & lateral_acc : sample_lat_acc) { + const auto debug_print = [&](const auto & s) { + RCLCPP_DEBUG_STREAM( + logger_, " - " << s << " : prep_time = " << prepare_duration << ", lon_acc = " + << sampled_longitudinal_acc << ", lat_acc = " << lateral_acc); + }; + const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc); const double longitudinal_acc_on_lane_changing = @@ -1024,7 +1180,7 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment, current_velocity, terminal_lane_changing_velocity); if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); + debug_print("Reject: length of lane changing path is longer than length to goal!!"); continue; } @@ -1042,7 +1198,7 @@ bool NormalLaneChange::getLaneChangePaths( s_start + lane_changing_length + finish_judge_buffer + backward_buffer + next_lane_change_buffer > s_goal) { - RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); + debug_print("Reject: length of lane changing path is longer than length to goal!!"); continue; } } @@ -1052,7 +1208,7 @@ bool NormalLaneChange::getLaneChangePaths( initial_lane_changing_velocity, next_lane_change_buffer); if (target_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "target segment is empty!! something wrong..."); + debug_print("Reject: target segment is empty!! something wrong..."); continue; } @@ -1069,7 +1225,9 @@ bool NormalLaneChange::getLaneChangePaths( boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); if (!is_valid_start_point) { - // lane changing points are not inside of the target preferred lanes or its neighbors + debug_print( + "Reject: lane changing points are not inside of the target preferred lanes or its " + "neighbors"); continue; } @@ -1081,7 +1239,7 @@ bool NormalLaneChange::getLaneChangePaths( next_lane_change_buffer); if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG(logger_, "target_lane_reference_path is empty!!"); + debug_print("Reject: target_lane_reference_path is empty!!"); continue; } @@ -1106,58 +1264,68 @@ bool NormalLaneChange::getLaneChangePaths( sorted_lane_ids); if (!candidate_path) { - RCLCPP_DEBUG(logger_, "no candidate path!!"); + debug_print("Reject: failed to generate candidate path!!"); continue; } if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { - RCLCPP_DEBUG(logger_, "invalid candidate path!!"); + debug_print("Reject: invalid candidate path!!"); continue; } if ( lane_change_parameters_->regulate_on_crosswalk && !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) { - RCLCPP_DEBUG(logger_, "Including crosswalk!!"); if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + debug_print("Reject: including crosswalk!!"); continue; } - RCLCPP_WARN_STREAM( - logger_, "Stop time is over threshold. Allow lane change in crosswalk."); + RCLCPP_INFO_THROTTLE( + logger_, clock_, 1000, "Stop time is over threshold. Allow lane change in crosswalk."); } if ( lane_change_parameters_->regulate_on_intersection && !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) { - RCLCPP_DEBUG(logger_, "Including intersection!!"); if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + debug_print("Reject: including intersection!!"); continue; } RCLCPP_WARN_STREAM( logger_, "Stop time is over threshold. Allow lane change in intersection."); } - if (utils::lane_change::passParkedObject( - route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_)) { + candidate_paths->push_back(*candidate_path); + if ( + !is_stuck && + utils::lane_change::passParkedObject( + route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, + is_goal_in_route, *lane_change_parameters_, object_debug_)) { + debug_print( + "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " + "lane change."); return false; } - candidate_paths->push_back(*candidate_path); if (!check_safety) { + debug_print("ACCEPT!!!: it is valid (and safety check is skipped)."); return false; } const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_); + *candidate_path, target_objects, rss_params, is_stuck, object_debug_); if (is_safe) { + debug_print("ACCEPT!!!: it is valid and safe!"); return true; } + + debug_print("Reject: sampled path is not safe."); } } } + RCLCPP_DEBUG(logger_, "No safety path found."); return false; } @@ -1171,8 +1339,9 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const debug_filtered_objects_ = target_objects; CollisionCheckDebugMap debug_data; + const bool is_stuck = isVehicleStuck(current_lanes); const auto safety_status = isLaneChangePathSafe( - path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); + path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data); { // only for debug purpose object_debug_.clear(); @@ -1429,7 +1598,7 @@ bool NormalLaneChange::getAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const utils::path_safety_checker::RSSparams & rss_params, + const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck, CollisionCheckDebugMap & debug_data) const { PathSafetyStatus path_safety_status; @@ -1452,8 +1621,9 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); auto collision_check_objects = target_objects.target_lane; + const auto current_lanes = getCurrentLanes(); - if (lane_change_parameters_->check_objects_on_current_lanes) { + if (lane_change_parameters_->check_objects_on_current_lanes || is_stuck) { collision_check_objects.insert( collision_check_objects.end(), target_objects.current_lane.begin(), target_objects.current_lane.end()); @@ -1474,12 +1644,14 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( auto current_debug_data = marker_utils::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); + auto is_safe = true; for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, current_debug_data.second); if (collided_polygons.empty()) { + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); continue; } @@ -1489,25 +1661,109 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); if (!collision_in_current_lanes && !collision_in_target_lanes) { + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); continue; } + is_safe = false; path_safety_status.is_safe = false; - marker_utils::updateCollisionCheckDebugMap( - debug_data, current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); const auto & obj_pose = obj.initial_pose.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); path_safety_status.is_object_coming_from_rear |= !utils::path_safety_checker::isTargetObjectFront( path, current_pose, common_parameters.vehicle_info, obj_polygon); } - marker_utils::updateCollisionCheckDebugMap( - debug_data, current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); } return path_safety_status; } +// Check if the ego vehicle is in stuck by a stationary obstacle or by the terminal of current lanes +bool NormalLaneChange::isVehicleStuck( + const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const +{ + // Ego is still moving, not in stuck + if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { + RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); + return false; + } + + // Ego is just stopped, not sure it is in stuck yet. + if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); + return false; + } + + // Check if any stationary object exist in obstacle_check_distance + using lanelet::utils::getArcCoordinates; + const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; + + for (const auto & object : debug_filtered_objects_.current_lane) { + const auto & p = object.initial_pose.pose; // TODO(Horibe): consider footprint point + + // Note: it needs chattering prevention. + if (std::abs(object.initial_twist.twist.linear.x) > 0.3) { // check if stationary + continue; + } + + const auto ego_to_obj_dist = getArcCoordinates(current_lanes, p).length - base_distance; + if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { + RCLCPP_DEBUG(logger_, "Stationary object is in front of ego."); + return true; // Stationary object is in front of ego. + } + } + + // Check if Ego is in terminal of current lanes + const auto & route_handler = getRouteHandler(); + const double distance_to_terminal = + route_handler->isInGoalRouteSection(current_lanes.back()) + ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) + : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); + const auto shift_intervals = + route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); + const double lane_change_buffer = + utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); + const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; + const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; + if (distance_to_terminal < terminal_judge_buffer) { + return true; + } + + // No stationary objects found in obstacle_check_distance and Ego is not in terminal of current + RCLCPP_DEBUG( + logger_, + "No stationary objects found in obstacle_check_distance and Ego is not in " + "terminal of current lanes"); + return false; +} + +bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const +{ + if (current_lanes.empty()) { + return false; // can not check + } + + const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); + const auto max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); + const auto rss_dist = calcRssDistance( + 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + lane_change_parameters_->rss_params); + + // It is difficult to define the detection range. If it is too short, the stuck will not be + // determined, even though you are stuck by an obstacle. If it is too long, + // the ego will be judged to be stuck by a distant vehicle, even though the ego is only + // stopped at a traffic light. Essentially, the calculation should be based on the information of + // the stop reason, but this is outside the scope of one module. I keep it as a TODO. + constexpr double DETECTION_DISTANCE_MARGIN = 10.0; + const auto detection_distance = max_lane_change_length + rss_dist + + getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; + RCLCPP_DEBUG(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc); + + return isVehicleStuck(current_lanes, detection_distance); +} + void NormalLaneChange::setStopPose(const Pose & stop_pose) { lane_change_stop_pose_ = stop_pose; diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 4e9680cee4d59..2232c6d750d55 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -32,7 +32,6 @@ using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::getPoint; SideShiftModule::SideShiftModule( diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index c828a747856d8..62f3dbfe191ac 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -20,7 +20,6 @@ #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -39,7 +38,6 @@ using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::inverseTransformPoint; namespace behavior_path_planner { @@ -626,6 +624,9 @@ lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId std::vector lane_ids; for (const auto & p : path.points) { for (const auto & id : p.lane_ids) { + if (id == lanelet::InvalId) { + continue; + } if (route_handler->isShoulderLanelet(lanelet_layer.get(id))) { continue; } @@ -638,9 +639,7 @@ lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId lanelet::ConstLanelets path_lanes; path_lanes.reserve(lane_ids.size()); for (const auto & id : lane_ids) { - if (id != lanelet::InvalId) { - path_lanes.push_back(lanelet_layer.get(id)); - } + path_lanes.push_back(lanelet_layer.get(id)); } return path_lanes; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 0e7eb87feed9b..a9f40dad86ae4 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -18,7 +18,6 @@ #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -29,9 +28,12 @@ #include +#include #include #include #include +#include +#include #include #include @@ -472,15 +474,14 @@ void setStartData( } Polygon2d createEnvelopePolygon( - const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer) + const Polygon2d & object_polygon, const Pose & closest_pose, const double envelope_buffer) { namespace bg = boost::geometry; + using tier4_autoware_utils::expandPolygon; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using Box = bg::model::box; - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); - const auto toPolygon2d = [](const geometry_msgs::msg::Polygon & polygon) { Polygon2d ret{}; @@ -515,11 +516,17 @@ Polygon2d createEnvelopePolygon( tf2::doTransform( toMsg(envelope_poly, closest_pose.position.z), envelope_ros_polygon, geometry_tf); - const auto expanded_polygon = - tier4_autoware_utils::expandPolygon(toPolygon2d(envelope_ros_polygon), envelope_buffer); + const auto expanded_polygon = expandPolygon(toPolygon2d(envelope_ros_polygon), envelope_buffer); return expanded_polygon; } +Polygon2d createEnvelopePolygon( + const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer) +{ + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + return createEnvelopePolygon(object_polygon, closest_pose, envelope_buffer); +} + std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width) @@ -655,8 +662,6 @@ void fillObjectEnvelopePolygon( ObjectData & object_data, const ObjectDataArray & registered_objects, const Pose & closest_pose, const std::shared_ptr & parameters) { - using boost::geometry::within; - const auto object_type = utils::getHighestProbLabel(object_data.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); @@ -674,15 +679,26 @@ void fillObjectEnvelopePolygon( return; } - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + const auto envelope_poly = + createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); + + if (boost::geometry::within(envelope_poly, same_id_obj->envelope_poly)) { + object_data.envelope_poly = same_id_obj->envelope_poly; + return; + } - if (!within(object_polygon, same_id_obj->envelope_poly)) { + std::vector unions; + boost::geometry::union_(envelope_poly, same_id_obj->envelope_poly, unions); + + if (unions.empty()) { object_data.envelope_poly = createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); return; } - object_data.envelope_poly = same_id_obj->envelope_poly; + boost::geometry::correct(unions.front()); + + object_data.envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0); } void fillObjectMovingTime( @@ -974,7 +990,7 @@ void filterTargetObjects( data.other_objects.push_back(o); continue; } - if (o.longitudinal > parameters->object_check_forward_distance) { + if (o.longitudinal > parameters->object_check_max_forward_distance) { o.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; data.other_objects.push_back(o); continue; @@ -1462,7 +1478,7 @@ lanelet::ConstLanelets getAdjacentLane( const std::shared_ptr & parameters, const bool is_right_shift) { const auto & rh = planner_data->route_handler; - const auto & forward_distance = parameters->object_check_forward_distance; + const auto & forward_distance = parameters->object_check_max_forward_distance; const auto & backward_distance = parameters->safety_check_backward_distance; const auto & vehicle_pose = planner_data->self_odometry->pose.pose; @@ -1602,7 +1618,7 @@ std::vector getSafetyCheckTargetObjects( std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, - DebugData & debug) + const double object_check_forward_distance, const bool is_running, DebugData & debug) { PredictedObjects target_objects; PredictedObjects other_objects; @@ -1625,7 +1641,7 @@ std::pair separateObjectsByPath( const auto & p_ego_back = path.points.at(i + 1).point.pose; const auto distance_from_ego = calcSignedArcLength(path.points, ego_idx, i); - if (distance_from_ego > parameters->object_check_forward_distance) { + if (distance_from_ego > object_check_forward_distance) { break; } @@ -1639,6 +1655,25 @@ std::pair separateObjectsByPath( } } + // expand detection area width only when the module is running. + if (is_running) { + constexpr int PER_CIRCLE = 36; + constexpr double MARGIN = 1.0; // [m] + boost::geometry::strategy::buffer::distance_symmetric distance_strategy(MARGIN); + boost::geometry::strategy::buffer::join_round join_strategy(PER_CIRCLE); + boost::geometry::strategy::buffer::end_round end_strategy(PER_CIRCLE); + boost::geometry::strategy::buffer::point_circle circle_strategy(PER_CIRCLE); + boost::geometry::strategy::buffer::side_straight side_strategy; + boost::geometry::model::multi_polygon result; + // Create the buffer of a multi polygon + boost::geometry::buffer( + attention_area, result, distance_strategy, side_strategy, join_strategy, end_strategy, + circle_strategy); + if (!result.empty()) { + attention_area = result.front(); + } + } + debug.detection_area = toMsg(attention_area, data.reference_pose.position.z); const auto objects = planner_data->dynamic_object->objects; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 60fb2ba2ff2e8..3008f98331c92 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -14,10 +14,10 @@ #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/footprints.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include @@ -25,304 +25,311 @@ #include #include #include +#include #include +#include + namespace drivable_area_expansion { -std::vector crop_and_resample( - const std::vector & points, - const std::shared_ptr planner_data, - const double resample_interval) +namespace +{ +Point2d convert_point(const Point & p) +{ + return Point2d{p.x, p.y}; +} +} // namespace + +void reuse_previous_poses( + const PathWithLaneId & path, std::vector & prev_poses, + std::vector & prev_curvatures, const Point & ego_point, + const DrivableAreaExpansionParameters & params) { - auto lon_offset = 0.0; - auto crop_pose = *planner_data->drivable_area_expansion_prev_crop_pose; - // reuse or update the previous crop point - if (planner_data->drivable_area_expansion_prev_crop_pose) { - const auto lon_offset = motion_utils::calcSignedArcLength( - points, points.front().point.pose.position, crop_pose.position); - if (lon_offset < 0.0) { - planner_data->drivable_area_expansion_prev_crop_pose.reset(); - } else { - const auto is_behind_ego = - motion_utils::calcSignedArcLength( - points, crop_pose.position, planner_data->self_odometry->pose.pose.position) > 0.0; - const auto is_too_far = motion_utils::calcLateralOffset(points, crop_pose.position) > 0.1; - if (!is_behind_ego || is_too_far) - planner_data->drivable_area_expansion_prev_crop_pose.reset(); + std::vector cropped_poses; + std::vector cropped_curvatures; + const auto ego_is_behind = prev_poses.size() > 1 && motion_utils::calcLongitudinalOffsetToSegment( + prev_poses, 0, ego_point) < 0.0; + const auto ego_is_far = !prev_poses.empty() && + tier4_autoware_utils::calcDistance2d(ego_point, prev_poses.front()) < 0.0; + if (!ego_is_behind && !ego_is_far && prev_poses.size() > 1) { + const auto first_idx = + motion_utils::findNearestSegmentIndex(prev_poses, path.points.front().point.pose); + const auto deviation = + motion_utils::calcLateralOffset(prev_poses, path.points.front().point.pose.position); + if (first_idx && deviation < params.max_reuse_deviation) { + LineString2d path_ls; + for (const auto & p : path.points) path_ls.push_back(convert_point(p.point.pose.position)); + for (auto idx = *first_idx; idx < prev_poses.size(); ++idx) { + double lateral_offset = std::numeric_limits::max(); + for (auto segment_idx = 0LU; segment_idx + 1 < path_ls.size(); ++segment_idx) { + const auto projection = point_to_line_projection( + convert_point(prev_poses[idx].position), path_ls[segment_idx], + path_ls[segment_idx + 1]); + lateral_offset = std::min(projection.distance, lateral_offset); + } + if (lateral_offset > params.max_reuse_deviation) break; + cropped_poses.push_back(prev_poses[idx]); + cropped_curvatures.push_back(prev_curvatures[idx]); + } } } - if (!planner_data->drivable_area_expansion_prev_crop_pose) { - crop_pose = planner_data->drivable_area_expansion_prev_crop_pose.value_or( - motion_utils::calcInterpolatedPose(points, resample_interval - lon_offset)); - } - // crop - const auto crop_seg_idx = motion_utils::findNearestSegmentIndex(points, crop_pose.position); - const auto cropped_points = motion_utils::cropPoints( - points, crop_pose.position, crop_seg_idx, - planner_data->drivable_area_expansion_parameters.max_path_arc_length, 0.0); - planner_data->drivable_area_expansion_prev_crop_pose = crop_pose; - // resample - PathWithLaneId cropped_path; - if (tier4_autoware_utils::calcDistance2d(crop_pose, cropped_points.front()) > 1e-3) { - PathPointWithLaneId crop_path_point; - crop_path_point.point.pose = crop_pose; - cropped_path.points.push_back(crop_path_point); + if (cropped_poses.empty()) { + const auto resampled_path_points = + motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; + const auto cropped_path = + params.max_path_arc_length <= 0.0 + ? resampled_path_points + : motion_utils::cropForwardPoints( + resampled_path_points, resampled_path_points.front().point.pose.position, 0, + params.max_path_arc_length); + for (const auto & p : cropped_path) cropped_poses.push_back(p.point.pose); + } else { + const auto initial_arc_length = motion_utils::calcArcLength(cropped_poses); + const auto max_path_arc_length = motion_utils::calcArcLength(path.points); + const auto first_arc_length = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, cropped_poses.back().position); + for (auto arc_length = first_arc_length + params.resample_interval; + (params.max_path_arc_length <= 0.0 || + initial_arc_length + (arc_length - first_arc_length) <= params.max_path_arc_length) && + arc_length <= max_path_arc_length; + arc_length += params.resample_interval) + cropped_poses.push_back(motion_utils::calcInterpolatedPose(path.points, arc_length)); } - cropped_path.points.insert( - cropped_path.points.end(), cropped_points.begin(), cropped_points.end()); - const auto resampled_path = - motion_utils::resamplePath(cropped_path, resample_interval, true, true, false); - - return resampled_path.points; + prev_poses = motion_utils::removeOverlapPoints(cropped_poses); + prev_curvatures = cropped_curvatures; } -void expandDrivableArea( - PathWithLaneId & path, - const std::shared_ptr planner_data, - const lanelet::ConstLanelets & path_lanes) +double calculate_minimum_lane_width( + const double curvature_radius, const DrivableAreaExpansionParameters & params) { - const auto & params = planner_data->drivable_area_expansion_parameters; - const auto & dynamic_objects = *planner_data->dynamic_object; - const auto & route_handler = *planner_data->route_handler; - const auto uncrossable_lines = - extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types); - multi_linestring_t uncrossable_lines_in_range; - const auto & p = path.points.front().point.pose.position; - for (const auto & line : uncrossable_lines) - if (boost::geometry::distance(line, point_t{p.x, p.y}) < params.max_path_arc_length) - uncrossable_lines_in_range.push_back(line); - const auto points = crop_and_resample(path.points, planner_data, params.resample_interval); - const auto path_footprints = createPathFootprints(points, params); - const auto predicted_paths = createObjectFootprints(dynamic_objects, params); - const auto expansion_polygons = - params.expansion_method == "lanelet" - ? createExpansionLaneletPolygons( - path_lanes, route_handler, path_footprints, predicted_paths, params) - : createExpansionPolygons( - path, path_footprints, predicted_paths, uncrossable_lines_in_range, params); - const auto expanded_drivable_area = createExpandedDrivableAreaPolygon(path, expansion_polygons); - updateDrivableAreaBounds(path, expanded_drivable_area); + const auto k = curvature_radius; + const auto a = params.vehicle_info.front_overhang_m + params.extra_front_overhang; + const auto w = params.vehicle_info.vehicle_width_m + params.extra_width; + const auto l = params.vehicle_info.wheel_base_m + params.extra_wheelbase; + return (a * a + 2.0 * a * l + 2.0 * k * w + l * l + w * w) / (2.0 * k + w); } -point_t convert_point(const Point & p) +std::vector calculate_minimum_expansions( + const std::vector & path_poses, const std::vector bound, + const std::vector curvatures, const Side side, + const DrivableAreaExpansionParameters & params) { - return point_t{p.x, p.y}; + std::vector minimum_expansions(bound.size()); + size_t lb_idx = 0; + for (auto path_idx = 0UL; path_idx < path_poses.size(); ++path_idx) { + const auto & path_pose = path_poses[path_idx]; + if (curvatures[path_idx] == 0.0) continue; + const auto curvature_radius = 1 / curvatures[path_idx]; + const auto min_lane_width = calculate_minimum_lane_width(curvature_radius, params); + const auto side_distance = min_lane_width / 2.0 * (side == LEFT ? 1.0 : -1.0); + const auto offset_point = + tier4_autoware_utils::calcOffsetPose(path_pose, 0.0, side_distance, 0.0).position; + for (auto bound_idx = lb_idx; bound_idx + 1 < bound.size(); ++bound_idx) { + const auto & prev_p = bound[bound_idx]; + const auto & next_p = bound[bound_idx + 1]; + const auto intersection_point = + tier4_autoware_utils::intersect(offset_point, path_pose.position, prev_p, next_p); + if (intersection_point) { + lb_idx = bound_idx; + const auto dist = tier4_autoware_utils::calcDistance2d(*intersection_point, offset_point); + minimum_expansions[bound_idx] = std::max(minimum_expansions[bound_idx], dist); + minimum_expansions[bound_idx + 1] = std::max(minimum_expansions[bound_idx + 1], dist); + // apply the expansion to all bound points within the extra arc length + auto arc_length = + tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]); + for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { + arc_length += + tier4_autoware_utils::calcDistance2d(bound[up_bound_idx - 1], bound[up_bound_idx]); + if (arc_length > params.extra_arc_length) break; + minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist); + } + arc_length = tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx]); + for (auto down_offset_idx = 1LU; down_offset_idx < bound_idx; ++down_offset_idx) { + const auto idx = bound_idx - down_offset_idx; + arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]); + if (arc_length > params.extra_arc_length) break; + minimum_expansions[idx] = std::max(minimum_expansions[idx], dist); + } + break; + } + } + } + return minimum_expansions; } -Point convert_point(const point_t & p) +void apply_bound_change_rate_limit( + std::vector & distances, const std::vector & bound, const double max_rate) { - return Point().set__x(p.x()).set__y(p.y()); + if (distances.empty()) return; + const auto apply_max_vel = [&](auto & exp, const auto from, const auto to) { + if (exp[from] > exp[to]) { + const auto arc_length = tier4_autoware_utils::calcDistance2d(bound[from], bound[to]); + const auto smoothed_dist = exp[from] - arc_length * max_rate; + exp[to] = std::max(exp[to], smoothed_dist); + } + }; + for (auto idx = 0LU; idx + 1 < distances.size(); ++idx) apply_max_vel(distances, idx, idx + 1); + for (auto idx = distances.size() - 1; idx > 0; --idx) apply_max_vel(distances, idx, idx - 1); } -polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multi_polygon_t & expansion_polygons) +std::vector calculate_maximum_distance( + const std::vector & path_poses, const std::vector bound, + const SegmentRtree & uncrossable_segments, const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params) { - polygon_t original_da_poly; - original_da_poly.outer().reserve(path.left_bound.size() + path.right_bound.size() + 1); - for (const auto & p : path.left_bound) original_da_poly.outer().push_back(convert_point(p)); - for (auto it = path.right_bound.rbegin(); it != path.right_bound.rend(); ++it) - original_da_poly.outer().push_back(convert_point(*it)); - original_da_poly.outer().push_back(original_da_poly.outer().front()); - - multi_polygon_t unions; - auto expanded_da_poly = original_da_poly; - for (const auto & p : expansion_polygons) { - unions.clear(); - boost::geometry::union_(expanded_da_poly, p, unions); - if (unions.size() == 1) // union of overlapping polygons should produce a single polygon - expanded_da_poly = unions[0]; + // TODO(Maxime): improve performances (dont use bg::distance ? use rtree ?) + std::vector maximum_distances(bound.size(), std::numeric_limits::max()); + LineString2d path_ls; + LineString2d bound_ls; + for (const auto & p : bound) bound_ls.push_back(convert_point(p)); + for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); + for (auto i = 0UL; i + 1 < bound_ls.size(); ++i) { + const Segment2d segment_ls = {bound_ls[i], bound_ls[i + 1]}; + std::vector query_result; + boost::geometry::index::query( + uncrossable_segments, boost::geometry::index::nearest(segment_ls, 1), + std::back_inserter(query_result)); + if (!query_result.empty()) { + const auto bound_to_line_dist = boost::geometry::distance(segment_ls, query_result.front()); + const auto dist_limit = std::max(0.0, bound_to_line_dist - params.avoid_linestring_dist); + maximum_distances[i] = std::min(maximum_distances[i], dist_limit); + maximum_distances[i + 1] = std::min(maximum_distances[i + 1], dist_limit); + } + for (const auto & uncrossable_poly : uncrossable_polygons) { + const auto bound_to_poly_dist = boost::geometry::distance(segment_ls, uncrossable_poly); + maximum_distances[i] = std::min(maximum_distances[i], bound_to_poly_dist); + maximum_distances[i + 1] = std::min(maximum_distances[i + 1], bound_to_poly_dist); + } } - return expanded_da_poly; + if (params.max_expansion_distance > 0.0) + for (auto & d : maximum_distances) d = std::min(params.max_expansion_distance, d); + return maximum_distances; } -void copy_z_over_arc_length( - const std::vector & from, std::vector & to) +void expand_bound( + std::vector & bound, const std::vector & path_poses, + const std::vector & expansions) { - if (from.empty() || to.empty()) return; - to.front().z = from.front().z; - if (from.size() < 2 || to.size() < 2) return; - to.back().z = from.back().z; - auto i_from = 1lu; - auto s_from = tier4_autoware_utils::calcDistance2d(from[0], from[1]); - auto s_to = 0.0; - auto s_from_prev = 0.0; - for (auto i_to = 1lu; i_to + 1 < to.size(); ++i_to) { - s_to += tier4_autoware_utils::calcDistance2d(to[i_to - 1], to[i_to]); - for (; s_from < s_to && i_from + 1 < from.size(); ++i_from) { - s_from_prev = s_from; - s_from += tier4_autoware_utils::calcDistance2d(from[i_from], from[i_from + 1]); + LineString2d path_ls; + for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); + for (auto idx = 0LU; idx < bound.size(); ++idx) { + if (expansions[idx] > 0.0) { + const auto bound_p = convert_point(bound[idx]); + const auto projection = point_to_linestring_projection(bound_p, path_ls); + const auto expansion_ratio = + (expansions[idx] + std::abs(projection.distance)) / std::abs(projection.distance); + const auto & path_p = projection.projected_point; + const auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio); + bound[idx].x = expanded_p.x(); + bound[idx].y = expanded_p.y(); } - if (s_from - s_from_prev != 0.0) { - const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); - to[i_to].z = interpolation::lerp(from[i_from - 1].z, from[i_from].z, ratio); - } else { - to[i_to].z = to[i_to - 1].z; + } + + // remove any self intersection by skipping the points inside of the loop + std::vector no_loop_bound = {bound.front()}; + for (auto idx = 1LU; idx < bound.size(); ++idx) { + bool is_intersecting = false; + for (auto succ_idx = idx + 1; succ_idx < bound.size(); ++succ_idx) { + const auto intersection = tier4_autoware_utils::intersect( + bound[idx - 1], bound[idx], bound[succ_idx - 1], bound[succ_idx]); + if ( + intersection && + tier4_autoware_utils::calcDistance2d(*intersection, bound[idx - 1]) < 1e-3 && + tier4_autoware_utils::calcDistance2d(*intersection, bound[idx]) < 1e-3) { + idx = succ_idx; + is_intersecting = true; + } } + if (!is_intersecting) no_loop_bound.push_back(bound[idx]); } + bound = no_loop_bound; } -void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area) +std::vector calculate_smoothed_curvatures( + const std::vector & poses, const size_t smoothing_window_size) { - const auto original_left_bound = path.left_bound; - const auto original_right_bound = path.right_bound; - const auto is_left_of_path = [&](const point_t & p) { - return motion_utils::calcLateralOffset(path.points, convert_point(p)) > 0.0; - }; - // prepare delimiting lines: start and end of the original expanded drivable area - const auto start_segment = - segment_t{convert_point(path.left_bound.front()), convert_point(path.right_bound.front())}; - const auto end_segment = - segment_t{convert_point(path.left_bound.back()), convert_point(path.right_bound.back())}; - point_t start_segment_center; - boost::geometry::centroid(start_segment, start_segment_center); - const auto path_start_segment = - segment_t{start_segment_center, convert_point(path.points[1].point.pose.position)}; - point_t end_segment_center; - boost::geometry::centroid(end_segment, end_segment_center); - const auto path_end_segment = - segment_t{convert_point(path.points.back().point.pose.position), end_segment_center}; - const auto segment_to_line_intersection = - [](const auto p1, const auto p2, const auto q1, const auto q2) -> std::optional { - const auto line = Eigen::Hyperplane::Through(q1, q2); - const auto segment = Eigen::Hyperplane::Through(p1, p2); - const auto intersection = line.intersection(segment); - std::optional result; - const auto is_on_segment = - (p1.x() <= p2.x() ? intersection.x() >= p1.x() && intersection.x() <= p2.x() - : intersection.x() <= p1.x() && intersection.x() >= p2.x()) && - (p1.y() <= p2.y() ? intersection.y() >= p1.y() && intersection.y() <= p2.y() - : intersection.y() <= p1.y() && intersection.y() >= p2.y()); - if (is_on_segment) result = point_t{intersection.x(), intersection.y()}; - return result; - }; - // find intersection between the expanded drivable area and the delimiting lines - const auto & da = expanded_drivable_area.outer(); - struct Intersection - { - point_t intersection_point; - ring_t::const_iterator segment_it; - double distance = std::numeric_limits::max(); - explicit Intersection(ring_t::const_iterator it) : segment_it(it) {} - void update(const point_t & p, const ring_t::const_iterator & it, const double dist) - { - intersection_point = p; - segment_it = it; - distance = dist; - } - }; - Intersection start_left(da.end()); - Intersection end_left(da.end()); - Intersection start_right(da.end()); - Intersection end_right(da.end()); - for (auto it = da.begin(); it != da.end(); ++it) { - if (boost::geometry::distance(*it, start_segment.first) < 1e-3) - start_left.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, start_segment.second) < 1e-3) - start_right.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, end_segment.first) < 1e-3) - end_left.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, end_segment.second) < 1e-3) - end_right.update(*it, it, 0.0); - const auto inter_start = - std::next(it) == da.end() - ? segment_to_line_intersection(*it, da.front(), start_segment.first, start_segment.second) - : segment_to_line_intersection( - *it, *std::next(it), start_segment.first, start_segment.second); - if (inter_start) { - const auto dist = boost::geometry::distance(*inter_start, path_start_segment); - const auto is_left = is_left_of_path(*inter_start); - if (is_left && dist < start_left.distance) - start_left.update(*inter_start, it, dist); - else if (!is_left && dist < start_right.distance) - start_right.update(*inter_start, it, dist); - } - const auto inter_end = - std::next(it) == da.end() - ? segment_to_line_intersection(*it, da.front(), end_segment.first, end_segment.second) - : segment_to_line_intersection(*it, *std::next(it), end_segment.first, end_segment.second); - if (inter_end) { - const auto dist = boost::geometry::distance(*inter_end, path_end_segment); - const auto is_left = is_left_of_path(*inter_end); - if (is_left && dist < end_left.distance) - end_left.update(*inter_end, it, dist); - else if (!is_left && dist < end_right.distance) - end_right.update(*inter_end, it, dist); - } - } - if (start_left.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, start_segment.first) < - boost::geometry::distance(b, start_segment.first); - }); - start_left.update(*closest_it, closest_it, 0.0); - } - if (start_right.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, start_segment.second) < - boost::geometry::distance(b, start_segment.second); - }); - start_right.update(*closest_it, closest_it, 0.0); - } - if (end_left.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, end_segment.first) < - boost::geometry::distance(b, end_segment.first); - }); - end_left.update(*closest_it, closest_it, 0.0); - } - if (end_right.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, end_segment.second) < - boost::geometry::distance(b, end_segment.second); - }); - end_right.update(*closest_it, closest_it, 0.0); + const auto curvatures = motion_utils::calcCurvature(poses); + std::vector smoothed_curvatures(curvatures.size()); + for (auto i = 0UL; i < curvatures.size(); ++i) { + auto sum = 0.0; + const auto from_idx = (i >= smoothing_window_size ? i - smoothing_window_size : 0); + const auto to_idx = std::min(i + smoothing_window_size, curvatures.size() - 1); + for (auto j = from_idx; j <= to_idx; ++j) sum += std::abs(curvatures[j]); + smoothed_curvatures[i] = sum / static_cast(to_idx - from_idx + 1); } + return smoothed_curvatures; +} - // extract the expanded left and right bound from the expanded drivable area - path.left_bound.clear(); - path.right_bound.clear(); - path.left_bound.push_back(convert_point(start_left.intersection_point)); - path.right_bound.push_back(convert_point(start_right.intersection_point)); - if (!boost::geometry::equals(start_right.intersection_point, *start_right.segment_it)) - path.right_bound.push_back(convert_point(*start_right.segment_it)); - if (start_left.segment_it < end_left.segment_it) { - for (auto it = std::next(start_left.segment_it); it <= end_left.segment_it; ++it) - path.left_bound.push_back(convert_point(*it)); - } else { - for (auto it = std::next(start_left.segment_it); it < da.end(); ++it) - path.left_bound.push_back(convert_point(*it)); - for (auto it = da.begin(); it <= end_left.segment_it; ++it) - path.left_bound.push_back(convert_point(*it)); - } - if (!boost::geometry::equals(end_left.intersection_point, *end_left.segment_it)) - path.left_bound.push_back(convert_point(end_left.intersection_point)); - if (start_right.segment_it < end_right.segment_it) { - for (auto it = std::prev(start_right.segment_it); it >= da.begin(); --it) - path.right_bound.push_back(convert_point(*it)); - for (auto it = std::prev(da.end()); it > end_right.segment_it; --it) - path.right_bound.push_back(convert_point(*it)); - } else { - for (auto it = std::prev(start_right.segment_it); it > end_right.segment_it; --it) - path.right_bound.push_back(convert_point(*it)); - } - if (!boost::geometry::equals(end_right.intersection_point, *std::next(end_right.segment_it))) - path.right_bound.push_back(convert_point(end_right.intersection_point)); - // remove possible duplicated points - const auto point_cmp = [](const auto & p1, const auto & p2) { - return p1.x == p2.x && p1.y == p2.y; - }; - path.left_bound.erase( - std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp), path.left_bound.end()); - path.right_bound.erase( - std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp), - path.right_bound.end()); - copy_z_over_arc_length(original_left_bound, path.left_bound); - copy_z_over_arc_length(original_right_bound, path.right_bound); +void expand_drivable_area( + PathWithLaneId & path, + const std::shared_ptr planner_data) +{ + // skip if no bounds or not enough points to calculate path curvature + if (path.points.size() < 3 || path.left_bound.empty() || path.right_bound.empty()) return; + tier4_autoware_utils::StopWatch stop_watch; + stop_watch.tic("overall"); + stop_watch.tic("preprocessing"); + // crop first/last non deviating path_poses + const auto & params = planner_data->drivable_area_expansion_parameters; + const auto & route_handler = *planner_data->route_handler; + const auto uncrossable_segments = extract_uncrossable_segments( + *route_handler.getLaneletMapPtr(), planner_data->self_odometry->pose.pose.position, params); + const auto uncrossable_polygons = create_object_footprints(*planner_data->dynamic_object, params); + const auto preprocessing_ms = stop_watch.toc("preprocessing"); + + stop_watch.tic("crop"); + std::vector path_poses = planner_data->drivable_area_expansion_prev_path_poses; + std::vector curvatures = planner_data->drivable_area_expansion_prev_curvatures; + + reuse_previous_poses( + path, path_poses, curvatures, planner_data->self_odometry->pose.pose.position, params); + const auto crop_ms = stop_watch.toc("crop"); + + stop_watch.tic("curvatures_expansion"); + // Only add curvatures for the new points. Curvatures of reused path points are not updated. + const auto new_curvatures = + calculate_smoothed_curvatures(path_poses, params.curvature_average_window); + const auto first_new_point_idx = curvatures.size(); + curvatures.insert( + curvatures.end(), new_curvatures.begin() + first_new_point_idx, new_curvatures.end()); + auto left_expansions = + calculate_minimum_expansions(path_poses, path.left_bound, curvatures, LEFT, params); + auto right_expansions = + calculate_minimum_expansions(path_poses, path.right_bound, curvatures, RIGHT, params); + const auto curvature_expansion_ms = stop_watch.toc("curvatures_expansion"); + + stop_watch.tic("max_dist"); + const auto max_left_expansions = calculate_maximum_distance( + path_poses, path.left_bound, uncrossable_segments, uncrossable_polygons, params); + const auto max_right_expansions = calculate_maximum_distance( + path_poses, path.right_bound, uncrossable_segments, uncrossable_polygons, params); + for (auto i = 0LU; i < left_expansions.size(); ++i) + left_expansions[i] = std::min(left_expansions[i], max_left_expansions[i]); + for (auto i = 0LU; i < right_expansions.size(); ++i) + right_expansions[i] = std::min(right_expansions[i], max_right_expansions[i]); + const auto max_dist_ms = stop_watch.toc("max_dist"); + + stop_watch.tic("smooth"); + apply_bound_change_rate_limit(left_expansions, path.left_bound, params.max_bound_rate); + apply_bound_change_rate_limit(right_expansions, path.right_bound, params.max_bound_rate); + const auto smooth_ms = stop_watch.toc("smooth"); + + // TODO(Maxime): limit the distances based on the total width (left + right < min_lane_width) + stop_watch.tic("expand"); + expand_bound(path.left_bound, path_poses, left_expansions); + expand_bound(path.right_bound, path_poses, right_expansions); + const auto expand_ms = stop_watch.toc("expand"); + + const auto total_ms = stop_watch.toc("overall"); + if (params.print_runtime) + std::printf( + "Total runtime(ms): %2.2f\n\tPreprocessing: %2.2f\n\tCrop: %2.2f\n\tCurvature expansion: " + "%2.2f\n\tMaximum expansion: %2.2f\n\tSmoothing: %2.2f\n\tExpansion: %2.2f\n\n", + total_ms, preprocessing_ms, crop_ms, curvature_expansion_ms, max_dist_ms, smooth_ms, + expand_ms); + planner_data->drivable_area_expansion_prev_path_poses = path_poses; + planner_data->drivable_area_expansion_prev_curvatures = curvatures; } } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp deleted file mode 100644 index 828fdc2f17a51..0000000000000 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ /dev/null @@ -1,237 +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 "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" - -#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" - -namespace drivable_area_expansion -{ - -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_linestring_t & limit_lines) -{ - auto dist_limit = std::numeric_limits::max(); - for (const auto & line : limit_lines) { - multi_point_t intersections; - boost::geometry::intersection(expansion_polygon, limit_lines, intersections); - for (const auto & p : intersections) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - for (const auto & p : line) - if (boost::geometry::within(p, expansion_polygon)) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - } - return dist_limit; -} - -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_polygon_t & limit_polygons) -{ - auto dist_limit = std::numeric_limits::max(); - for (const auto & polygon : limit_polygons) { - multi_point_t intersections; - boost::geometry::intersection(expansion_polygon, polygon, intersections); - for (const auto & p : intersections) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - } - return dist_limit; -} - -polygon_t createExpansionPolygon( - const linestring_t & base_ls, const double dist, const bool is_left_side) -{ - namespace strategy = boost::geometry::strategy::buffer; - multi_polygon_t polygons; - // set a non 0 value for the buffer as it sometimes causes no polygon to be returned by bg:buffer - constexpr auto zero = 0.1; - const auto left_dist = is_left_side ? dist : zero; - const auto right_dist = !is_left_side ? dist : zero; - const auto distance_strategy = strategy::distance_asymmetric(left_dist, right_dist); - boost::geometry::buffer( - base_ls, polygons, distance_strategy, strategy::side_straight(), strategy::join_miter(), - strategy::end_flat(), strategy::point_square()); - return polygons.front(); -} - -std::array calculate_arc_length_range_and_distance( - const linestring_t & path_ls, const polygon_t & footprint, const linestring_t & bound, - const bool is_left, const double path_length) -{ - multi_point_t intersections; - double expansion_dist = 0.0; - double from_arc_length = std::numeric_limits::max(); - double to_arc_length = std::numeric_limits::min(); - boost::geometry::intersection(footprint, bound, intersections); - if (!intersections.empty()) { - for (const auto & intersection : intersections) { - const auto projection = point_to_linestring_projection(intersection, path_ls); - if (projection.arc_length <= 0.0 || projection.arc_length >= path_length) continue; - from_arc_length = std::min(from_arc_length, projection.arc_length); - to_arc_length = std::max(to_arc_length, projection.arc_length); - } - for (const auto & p : footprint.outer()) { - const auto projection = point_to_linestring_projection(p, path_ls); - if (projection.arc_length <= 0.0 || projection.arc_length >= path_length - 1e-3) continue; - if (is_left == (projection.distance > 0) && std::abs(projection.distance) > expansion_dist) { - expansion_dist = std::abs(projection.distance); - from_arc_length = std::min(from_arc_length, projection.arc_length); - to_arc_length = std::max(to_arc_length, projection.arc_length); - } - } - } - return std::array({from_arc_length, to_arc_length, expansion_dist}); -} - -polygon_t create_compensation_polygon( - const linestring_t & base_ls, const double compensation_dist, const bool is_left, - const multi_linestring_t uncrossable_lines, const multi_polygon_t & predicted_paths) -{ - polygon_t compensation_polygon = createExpansionPolygon(base_ls, compensation_dist, !is_left); - double dist_limit = std::min( - compensation_dist, calculateDistanceLimit(base_ls, compensation_polygon, uncrossable_lines)); - if (!predicted_paths.empty()) - dist_limit = - std::min(dist_limit, calculateDistanceLimit(base_ls, compensation_polygon, predicted_paths)); - if (dist_limit < compensation_dist) - compensation_polygon = createExpansionPolygon(base_ls, dist_limit, !is_left); - return compensation_polygon; -} - -multi_polygon_t createExpansionPolygons( - const PathWithLaneId & path, const multi_polygon_t & path_footprints, - const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, - const DrivableAreaExpansionParameters & params) -{ - linestring_t path_ls; - linestring_t left_ls; - linestring_t right_ls; - for (const auto & p : path.points) - path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); - for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y); - for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y); - // extend the path linestring to the beginning and end of the drivable area - if (!right_ls.empty() && !left_ls.empty() && path_ls.size() > 2) { - const auto left_proj_begin = point_to_line_projection(left_ls.front(), path_ls[0], path_ls[1]); - const auto right_proj_begin = - point_to_line_projection(right_ls.front(), path_ls[0], path_ls[1]); - const auto left_ls_proj_begin = point_to_linestring_projection(left_proj_begin.point, path_ls); - const auto right_ls_proj_begin = - point_to_linestring_projection(right_proj_begin.point, path_ls); - if (left_ls_proj_begin.arc_length < right_ls_proj_begin.arc_length) - path_ls.insert(path_ls.begin(), left_proj_begin.point); - else - path_ls.insert(path_ls.begin(), right_proj_begin.point); - const auto left_proj_end = - point_to_line_projection(left_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); - const auto right_proj_end = - point_to_line_projection(right_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); - const auto left_ls_proj_end = point_to_linestring_projection(left_proj_end.point, path_ls); - const auto right_ls_proj_end = point_to_linestring_projection(right_proj_end.point, path_ls); - if (left_ls_proj_end.arc_length > right_ls_proj_end.arc_length) - path_ls.push_back(left_proj_end.point); - else - path_ls.push_back(right_proj_end.point); - } - const auto path_length = static_cast(boost::geometry::length(path_ls)); - - multi_polygon_t expansion_polygons; - for (const auto & footprint : path_footprints) { - bool is_left = true; - for (const auto & bound : {left_ls, right_ls}) { - auto [from_arc_length, to_arc_length, footprint_dist] = - calculate_arc_length_range_and_distance(path_ls, footprint, bound, is_left, path_length); - if (footprint_dist > 0.0) { - from_arc_length -= params.extra_arc_length; - to_arc_length += params.extra_arc_length; - from_arc_length = std::max(0.0, from_arc_length); - to_arc_length = std::min(path_length, to_arc_length); - const auto base_ls = sub_linestring(path_ls, from_arc_length, to_arc_length); - const auto expansion_dist = params.max_expansion_distance != 0.0 - ? std::min(params.max_expansion_distance, footprint_dist) - : footprint_dist; - auto expansion_polygon = createExpansionPolygon(base_ls, expansion_dist, is_left); - auto limited_dist = expansion_dist; - const auto uncrossable_dist_limit = std::max( - 0.0, calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines) - - params.avoid_linestring_dist); - if (uncrossable_dist_limit < limited_dist) { - limited_dist = uncrossable_dist_limit; - if (params.compensate_uncrossable_lines) { - const auto compensation_dist = - footprint_dist - limited_dist + params.compensate_extra_dist; - expansion_polygons.push_back(create_compensation_polygon( - base_ls, compensation_dist, is_left, uncrossable_lines, predicted_paths)); - } - } - limited_dist = std::min( - limited_dist, calculateDistanceLimit(base_ls, expansion_polygon, predicted_paths)); - if (limited_dist < expansion_dist) - expansion_polygon = createExpansionPolygon(base_ls, limited_dist, is_left); - expansion_polygons.push_back(expansion_polygon); - } - is_left = false; - } - } - return expansion_polygons; -} - -multi_polygon_t createExpansionLaneletPolygons( - const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, - const DrivableAreaExpansionParameters & params) -{ - multi_polygon_t expansion_polygons; - lanelet::ConstLanelets candidates; - const auto already_added = [&](const auto & ll) { - return std::find_if(candidates.begin(), candidates.end(), [&](const auto & l) { - return ll.id() == l.id(); - }) != candidates.end(); - }; - const auto add_if_valid = [&](const auto & ll, const auto is_left) { - const auto bound_to_check = is_left ? ll.rightBound() : ll.leftBound(); - if (std::find_if(path_lanes.begin(), path_lanes.end(), [&](const auto & l) { - return ll.id() == l.id(); - }) == path_lanes.end()) - if (!already_added(ll) && !hasTypes(bound_to_check, params.avoid_linestring_types)) - candidates.push_back(ll); - }; - for (const auto & current_ll : path_lanes) { - for (const auto & left_ll : - route_handler.getLaneletsFromPoint(current_ll.leftBound3d().front())) - add_if_valid(left_ll, true); - for (const auto & left_ll : route_handler.getLaneletsFromPoint(current_ll.leftBound3d().back())) - add_if_valid(left_ll, true); - for (const auto & right_ll : - route_handler.getLaneletsFromPoint(current_ll.rightBound3d().front())) - add_if_valid(right_ll, false); - for (const auto & right_ll : - route_handler.getLaneletsFromPoint(current_ll.rightBound3d().back())) - add_if_valid(right_ll, false); - } - for (const auto & candidate : candidates) { - polygon_t candidate_poly; - for (const auto & p : candidate.polygon2d()) candidate_poly.outer().emplace_back(p.x(), p.y()); - boost::geometry::correct(candidate_poly); - if ( - !boost::geometry::overlaps(candidate_poly, predicted_paths) && - boost::geometry::overlaps(path_footprints, candidate_poly)) - expansion_polygons.push_back(candidate_poly); - } - return expansion_polygons; -} - -} // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp index 4a45dce6a0bcc..2e6d4929fdf02 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp @@ -28,63 +28,39 @@ namespace drivable_area_expansion { -polygon_t translatePolygon(const polygon_t & polygon, const double x, const double y) +Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const double y) { - polygon_t translated_polygon; + Polygon2d translated_polygon; const boost::geometry::strategy::transform::translate_transformer translation(x, y); boost::geometry::transform(polygon, translated_polygon, translation); return translated_polygon; } -polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t base_footprint) +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint) { const auto angle = tf2::getYaw(pose.orientation); - return translatePolygon(rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); + return translate_polygon( + tier4_autoware_utils::rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); } -multi_polygon_t createObjectFootprints( +MultiPolygon2d create_object_footprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params) { - multi_polygon_t footprints; + MultiPolygon2d footprints; if (params.avoid_dynamic_objects) { for (const auto & object : objects.objects) { const auto front = object.shape.dimensions.x / 2 + params.dynamic_objects_extra_front_offset; const auto rear = -object.shape.dimensions.x / 2 - params.dynamic_objects_extra_rear_offset; const auto left = object.shape.dimensions.y / 2 + params.dynamic_objects_extra_left_offset; const auto right = -object.shape.dimensions.y / 2 - params.dynamic_objects_extra_right_offset; - polygon_t base_footprint; + Polygon2d base_footprint; base_footprint.outer() = { - point_t{front, left}, point_t{front, right}, point_t{rear, right}, point_t{rear, left}, - point_t{front, left}}; + Point2d{front, left}, Point2d{front, right}, Point2d{rear, right}, Point2d{rear, left}, + Point2d{front, left}}; for (const auto & path : object.kinematics.predicted_paths) for (const auto & pose : path.path) - footprints.push_back(createFootprint(pose, base_footprint)); - } - } - return footprints; -} - -multi_polygon_t createPathFootprints( - const std::vector & points, const DrivableAreaExpansionParameters & params) -{ - const auto left = params.ego_left_offset + params.ego_extra_left_offset; - const auto right = params.ego_right_offset - params.ego_extra_right_offset; - const auto rear = params.ego_rear_offset - params.ego_extra_rear_offset; - const auto front = params.ego_front_offset + params.ego_extra_front_offset; - polygon_t base_footprint; - base_footprint.outer() = { - point_t{front, left}, point_t{front, right}, point_t{rear, right}, point_t{rear, left}, - point_t{front, left}}; - multi_polygon_t footprints; - // skip the last footprint as its orientation is usually wrong - footprints.reserve(points.size() - 1); - double arc_length = 0.0; - for (auto it = points.begin(); std::next(it) != points.end(); ++it) { - footprints.push_back(createFootprint(it->point.pose, base_footprint)); - if (params.max_path_arc_length > 0.0) { - arc_length += tier4_autoware_utils::calcDistance2d(it->point.pose, std::next(it)->point.pose); - if (arc_length > params.max_path_arc_length) break; + footprints.push_back(create_footprint(pose, base_footprint)); } } return footprints; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp index ded67c251f7ae..6ed14138c62e4 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp @@ -15,33 +15,38 @@ #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" -#include "lanelet2_core/primitives/LineString.h" - #include #include +#include #include namespace drivable_area_expansion { -multi_linestring_t extractUncrossableLines( - const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types) +SegmentRtree extract_uncrossable_segments( + const lanelet::LaneletMap & lanelet_map, const Point & ego_point, + const DrivableAreaExpansionParameters & params) { - multi_linestring_t lines; - linestring_t line; + SegmentRtree uncrossable_segments_in_range; + LineString2d line; + const auto ego_p = Point2d{ego_point.x, ego_point.y}; for (const auto & ls : lanelet_map.lineStringLayer) { - if (hasTypes(ls, uncrossable_types)) { + if (has_types(ls, params.avoid_linestring_types)) { line.clear(); - for (const auto & p : ls) line.push_back(point_t{p.x(), p.y()}); - lines.push_back(line); + for (const auto & p : ls) line.push_back(Point2d{p.x(), p.y()}); + for (auto segment_idx = 0LU; segment_idx + 1 < line.size(); ++segment_idx) { + Segment2d segment = {line[segment_idx], line[segment_idx + 1]}; + if (boost::geometry::distance(segment, ego_p) < params.max_path_arc_length) { + uncrossable_segments_in_range.insert(segment); + } + } } } - return lines; + return uncrossable_segments_in_range; } -bool hasTypes(const lanelet::ConstLineString3d & ls, const std::vector & types) +bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types) { constexpr auto no_type = ""; const auto type = ls.attributeOr(lanelet::AttributeName::Type, no_type); diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 3d315e7213f96..d5bf67f754ab7 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -17,7 +17,6 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -37,8 +36,6 @@ #include -#include -#include #include #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp index 7efdbdf1552d5..4c9832c374c63 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp @@ -21,7 +21,6 @@ #include #include -#include namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index a80f7892b0bef..017ecb218bea3 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -15,8 +15,6 @@ #include "behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 12044980ebd81..1e6dc1776359b 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -20,7 +20,6 @@ #include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/utility/utilities.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -37,7 +36,6 @@ using lane_departure_checker::LaneDepartureChecker; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::inverseTransformPose; // Sort with smaller longitudinal distances taking precedence over smaller lateral distances. struct SortByLongitudinalDistance diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index e1bdaf977dad8..60cc2d19c5f35 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -16,8 +16,6 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index ff26c7a3654c3..1cd985d566f73 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -14,9 +14,6 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" - #include #include #include @@ -29,7 +26,6 @@ #include #include -#include #include #include #include @@ -171,7 +167,7 @@ MarkerArray createNumObjectsToAvoidTextsMarkerArray( } MarkerArray createGoalCandidatesMarkerArray( - GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color) + const GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color) { GoalCandidates safe_goal_candidates{}; std::copy_if( diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 038cf9488345f..d5867c40824d2 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -14,13 +14,14 @@ #include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -39,7 +40,6 @@ #include #include -#include #include #include @@ -301,6 +301,15 @@ std::optional constructCandidatePath( "failed to generate shifted path."); } + // TODO(Zulfaqar Azmi): have to think of a more feasible solution for points being remove by path + // shifter. + if (shifted_path.path.points.size() < shift_line.end_idx + 1) { + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner").get_child("utils").get_child(__func__), + "path points are removed by PathShifter."); + return std::nullopt; + } + const auto & prepare_length = lane_change_length.prepare; const auto & lane_changing_length = lane_change_length.lane_changing; @@ -930,7 +939,8 @@ bool isParkedObject( bool passParkedObject( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters) + const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, + CollisionCheckDebugMap & object_debug) { const auto & object_check_min_road_shoulder_width = lane_change_parameters.object_check_min_road_shoulder_width; @@ -953,6 +963,7 @@ bool passParkedObject( } const auto & leading_obj = objects.at(*leading_obj_idx); + auto debug = marker_utils::createObjectDebug(leading_obj); const auto leading_obj_poly = tier4_autoware_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { @@ -976,6 +987,8 @@ bool passParkedObject( // If there are still enough length after the target object, we delay the lane change if (min_dist_to_end_of_current_lane > minimum_lane_change_length) { + debug.second.unsafe_reason = "delay lane change"; + marker_utils::updateCollisionCheckDebugMap(object_debug, debug, false); return true; } diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index dfc9f76b25e33..a7c604985de3a 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "object_recognition_utils/predicted_path_utils.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index b44a3f841035d..8ebc144a34584 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -14,11 +14,8 @@ #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" #include "interpolation/linear_interpolation.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -27,6 +24,9 @@ namespace behavior_path_planner::utils::path_safety_checker { + +namespace bg = boost::geometry; + void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { Point2d point; @@ -51,7 +51,8 @@ bool isTargetObjectFront( tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); // check all edges in the polygon - for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & obj_edge : obj_polygon_outer) { const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); if (tier4_autoware_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { return true; @@ -70,7 +71,8 @@ bool isTargetObjectFront( tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0).position; // check all edges in the polygon - for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & obj_edge : obj_polygon_outer) { const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); if (motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { return true; @@ -82,28 +84,33 @@ bool isTargetObjectFront( Polygon2d createExtendedPolygon( const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double lon_length, const double lat_margin, CollisionCheckDebug & debug) + const double lon_length, const double lat_margin, const double is_stopped_obj, + CollisionCheckDebug & debug) { const double & base_to_front = vehicle_info.max_longitudinal_offset_m; const double & width = vehicle_info.vehicle_width_m; const double & base_to_rear = vehicle_info.rear_overhang_m; - const double lon_offset = std::max(lon_length + base_to_front, base_to_front); - + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = base_to_front + (is_stopped_obj ? lon_length / 2 : lon_length); + const double backward_lon_offset = + -base_to_rear - (is_stopped_obj ? lon_length / 2 : 0); // minus value const double lat_offset = width / 2.0 + lat_margin; { - debug.extended_polygon_lon_offset = lon_offset; - debug.extended_polygon_lat_offset = lat_offset; + debug.forward_lon_offset = forward_lon_offset; + debug.backward_lon_offset = backward_lon_offset; + debug.lat_offset = lat_offset; } - const auto p1 = tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, lat_offset, 0.0); + const auto p1 = + tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0); const auto p2 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, -lat_offset, 0.0); + tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0); const auto p3 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, -lat_offset, 0.0); + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); const auto p4 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, lat_offset, 0.0); + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -118,7 +125,7 @@ Polygon2d createExtendedPolygon( Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, - CollisionCheckDebug & debug) + const double is_stopped_obj, CollisionCheckDebug & debug) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, shape); if (obj_polygon.outer().empty()) { @@ -129,7 +136,8 @@ Polygon2d createExtendedPolygon( double min_x = std::numeric_limits::max(); double max_y = std::numeric_limits::lowest(); double min_y = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & polygon_p : obj_polygon_outer) { const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto transformed_p = tier4_autoware_utils::inverseTransformPoint(obj_p, obj_pose); @@ -139,19 +147,27 @@ Polygon2d createExtendedPolygon( min_y = std::min(transformed_p.y, min_y); } - const double lon_offset = max_x + lon_length; + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = max_x + (is_stopped_obj ? lon_length / 2 : lon_length); + const double backward_lon_offset = min_x - (is_stopped_obj ? lon_length / 2 : 0); // minus value + const double left_lat_offset = max_y + lat_margin; const double right_lat_offset = min_y - lat_margin; { - debug.extended_polygon_lon_offset = lon_offset; - debug.extended_polygon_lat_offset = (left_lat_offset + right_lat_offset) / 2; + debug.forward_lon_offset = forward_lon_offset; + debug.backward_lon_offset = backward_lon_offset; + debug.lat_offset = std::max(std::abs(left_lat_offset), std::abs(right_lat_offset)); } - const auto p1 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, left_lat_offset, 0.0); - const auto p2 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, right_lat_offset, 0.0); - const auto p3 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, right_lat_offset, 0.0); - const auto p4 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, left_lat_offset, 0.0); + const auto p1 = + tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, left_lat_offset, 0.0); + const auto p2 = + tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, right_lat_offset, 0.0); + const auto p3 = + tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, right_lat_offset, 0.0); + const auto p4 = + tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -308,17 +324,15 @@ std::vector getCollidedPolygons( const auto & ego_polygon = interpolated_data->poly; const auto & ego_velocity = interpolated_data->velocity; - { - debug.expected_ego_pose = ego_pose; - debug.expected_obj_pose = obj_pose; - debug.extended_ego_polygon = ego_polygon; - debug.extended_obj_polygon = obj_polygon; - } - // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { debug.unsafe_reason = "overlap_polygon"; collided_polygons.push_back(obj_polygon); + + debug.expected_ego_pose = ego_pose; + debug.expected_obj_pose = obj_pose; + debug.extended_ego_polygon = ego_polygon; + debug.extended_obj_polygon = obj_polygon; continue; } @@ -338,28 +352,29 @@ std::vector getCollidedPolygons( const auto & lon_offset = std::max(rss_dist, min_lon_length) * hysteresis_factor; const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor; - const auto & extended_ego_polygon = - is_object_front - ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) - : ego_polygon; + // TODO(watanabe) fix hard coding value + const bool is_stopped_object = object_velocity < 0.3; + const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon( + ego_pose, ego_vehicle_info, lon_offset, + lat_margin, is_stopped_object, debug) + : ego_polygon; const auto & extended_obj_polygon = is_object_front ? obj_polygon - : createExtendedPolygon(obj_pose, target_object.shape, lon_offset, lat_margin, debug); + : createExtendedPolygon( + obj_pose, target_object.shape, lon_offset, lat_margin, is_stopped_object, debug); + + // check overlap with extended polygon + if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { + debug.unsafe_reason = "overlap_extended_polygon"; + collided_polygons.push_back(obj_polygon); - { debug.rss_longitudinal = rss_dist; debug.inter_vehicle_distance = min_lon_length; debug.extended_ego_polygon = extended_ego_polygon; debug.extended_obj_polygon = extended_obj_polygon; debug.is_front = is_object_front; } - - // check overlap with extended polygon - if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { - debug.unsafe_reason = "overlap_extended_polygon"; - collided_polygons.push_back(obj_polygon); - } } return collided_polygons; diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index 21d4ff666e28a..df2d82b1072d4 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -15,13 +15,11 @@ #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" #include #include #include -#include #include #include #include diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index 509b31ad3da2b..3ac3c09ba0d97 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -15,7 +15,6 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include @@ -23,7 +22,6 @@ #include #include -// #include #include #include @@ -226,7 +224,7 @@ std::pair getPathTurnSignal( turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; const double max_time = std::numeric_limits::max(); const double max_distance = std::numeric_limits::max(); - if (path.shift_length.empty()) { + if (path.shift_length.size() < shift_line.end_idx + 1) { return std::make_pair(turn_signal, max_distance); } const auto base_link2front = common_parameter.base_link2front; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 03063b5a9e2fe..9b10dfc32457f 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -15,7 +15,9 @@ #include "behavior_path_planner/utils/utils.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -24,9 +26,6 @@ #include #include -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_path.hpp" - #include #include @@ -1539,7 +1538,7 @@ void generateDrivableArea( } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { - drivable_area_expansion::expandDrivableArea(path, planner_data, transformed_lanes); + drivable_area_expansion::expand_drivable_area(path, planner_data); } // make bound longitudinally monotonic @@ -1731,7 +1730,7 @@ std::vector getBoundWithIntersectionAreas( const auto shared_point_itr_last = std::find_if(expanded_bound.rbegin(), expanded_bound.rend(), [&](const auto & p) { return std::any_of( - intersection_bound.begin(), intersection_bound.end(), + intersection_bound.rbegin(), intersection_bound.rend(), [&](const auto & point) { return point.id() == p.id(); }); }); @@ -1757,6 +1756,13 @@ std::vector getBoundWithIntersectionAreas( continue; } + // TODO(Satoshi OTA): remove this guard. + if ( + std::distance(intersection_bound.begin(), trim_point_itr_last) < + std::distance(intersection_bound.begin(), trim_point_itr_init)) { + continue; + } + std::vector tmp_bound{}; tmp_bound.insert(tmp_bound.end(), expanded_bound.begin(), shared_point_itr_init); @@ -2005,6 +2011,11 @@ void makeBoundLongitudinallyMonotonic( std::vector ret = bound; auto itr = std::next(ret.begin()); while (std::next(itr) != ret.end()) { + if (itr == ret.begin()) { + itr++; + continue; + } + const auto p1 = *std::prev(itr); const auto p2 = *itr; const auto p3 = *std::next(itr); @@ -2018,11 +2029,20 @@ void makeBoundLongitudinallyMonotonic( const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2); constexpr double epsilon = 1e-3; + + // Remove overlapped point. + if (dist_1to2 < epsilon || dist_3to2 < epsilon) { + itr = std::prev(ret.erase(itr)); + continue; + } + + // If the angle between the points is sharper than 45 degrees, remove the middle point. if (std::cos(M_PI_4) < product / dist_1to2 / dist_3to2 + epsilon) { - itr = ret.erase(itr); - } else { - itr++; + itr = std::prev(ret.erase(itr)); + continue; } + + itr++; } return ret; diff --git a/planning/behavior_path_planner/test/input.hpp b/planning/behavior_path_planner/test/input.hpp index de167bc8b4bcb..ededb32f78be0 100644 --- a/planning/behavior_path_planner/test/input.hpp +++ b/planning/behavior_path_planner/test/input.hpp @@ -16,9 +16,11 @@ #define INPUT_HPP_ #endif // INPUT_HPP_ -#include "behavior_path_planner/utils/utils.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" +#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" #include @@ -26,6 +28,9 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; PathWithLaneId generateStraightSamplePathWithLaneId( float initial_pose_value, float pose_increment, size_t point_sample); diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index e0a16089b8d84..99cd5cac2b9d9 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -14,7 +14,6 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" @@ -22,9 +21,9 @@ #include #include -using drivable_area_expansion::linestring_t; -using drivable_area_expansion::point_t; -using drivable_area_expansion::segment_t; +using drivable_area_expansion::LineString2d; +using drivable_area_expansion::Point2d; +using drivable_area_expansion::Segment2d; constexpr auto eps = 1e-9; TEST(DrivableAreaExpansionProjection, PointToSegment) @@ -32,56 +31,56 @@ TEST(DrivableAreaExpansionProjection, PointToSegment) using drivable_area_expansion::point_to_segment_projection; { - point_t query(1.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(1.0, 1.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, 1.0, eps); EXPECT_NEAR(projection.point.x(), 1.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(-1.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(-1.0, 1.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, std::sqrt(2), eps); EXPECT_NEAR(projection.point.x(), 0.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(11.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(11.0, 1.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, std::sqrt(2), eps); EXPECT_NEAR(projection.point.x(), 10.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(5.0, -5.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(5.0, -5.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, -5.0, eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(5.0, -5.0); - segment_t segment(point_t(0.0, 0.0), point_t(0.0, -10.0)); + Point2d query(5.0, -5.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(0.0, -10.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, 5.0, eps); EXPECT_NEAR(projection.point.x(), 0.0, eps); EXPECT_NEAR(projection.point.y(), -5.0, eps); } { - point_t query(5.0, 5.0); - segment_t segment(point_t(2.5, 7.5), point_t(7.5, 2.5)); + Point2d query(5.0, 5.0); + Segment2d segment(Point2d(2.5, 7.5), Point2d(7.5, 2.5)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, 0.0, eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 5.0, eps); } { - point_t query(0.0, 0.0); - segment_t segment(point_t(2.5, 7.5), point_t(7.5, 2.5)); + Point2d query(0.0, 0.0); + Segment2d segment(Point2d(2.5, 7.5), Point2d(7.5, 2.5)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, -std::sqrt(50), eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); @@ -93,11 +92,11 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) { using drivable_area_expansion::point_to_linestring_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; { - point_t query(0.0, 0.0); + Point2d query(0.0, 0.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 0.0, eps); EXPECT_NEAR(projection.distance, 0.0, eps); @@ -105,7 +104,7 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) EXPECT_NEAR(projection.projected_point.y(), 0.0, eps); } { - point_t query(2.0, 1.0); + Point2d query(2.0, 1.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 2.0, eps); EXPECT_NEAR(projection.distance, 1.0, eps); @@ -113,7 +112,7 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) EXPECT_NEAR(projection.projected_point.y(), 0.0, eps); } { - point_t query(0.0, 5.0); + Point2d query(0.0, 5.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 30.0 + std::sqrt(2.5 * 2.5 * 2), eps); EXPECT_NEAR(projection.distance, -std::sqrt(2.5 * 2.5 * 2), eps); @@ -126,9 +125,9 @@ TEST(DrivableAreaExpansionProjection, LinestringToPoint) { using drivable_area_expansion::linestring_to_point_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; for (auto arc_length = 0.0; arc_length <= 10.0; arc_length += 1.0) { const auto projection = linestring_to_point_projection(ls, arc_length, 0.0); EXPECT_NEAR(projection.first.x(), arc_length, eps); @@ -152,58 +151,18 @@ TEST(DrivableAreaExpansionProjection, LinestringToPoint) } } -TEST(DrivableAreaExpansionProjection, SubLinestring) -{ - using drivable_area_expansion::sub_linestring; - - const linestring_t ls = { - point_t{0.0, 0.0}, point_t{1.0, 0.0}, point_t{2.0, 0.0}, point_t{3.0, 0.0}, - point_t{4.0, 0.0}, point_t{5.0, 0.0}, point_t{6.0, 0.0}, - }; - { - // arc lengths equal to the original range: same linestring is returned - const auto sub = sub_linestring(ls, 0.0, 6.0); - ASSERT_EQ(ls.size(), sub.size()); - for (auto i = 0lu; i < ls.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i], sub[i])); - } - { - // arc lengths equal to existing point: sub-linestring with same points - const auto sub = sub_linestring(ls, 1.0, 5.0); - ASSERT_EQ(ls.size() - 2lu, sub.size()); - for (auto i = 0lu; i < sub.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i + 1], sub[i])); - } - { - // arc lengths inside the original: sub-linestring with some interpolated points - const auto sub = sub_linestring(ls, 1.5, 2.5); - ASSERT_EQ(sub.size(), 3lu); - EXPECT_NEAR(sub[0].x(), 1.5, eps); - EXPECT_NEAR(sub[1].x(), 2.0, eps); - EXPECT_NEAR(sub[2].x(), 2.5, eps); - for (const auto & p : sub) EXPECT_NEAR(p.y(), 0.0, eps); - } - { - // arc length outside of the original range: first & last point are replaced by interpolations - const auto sub = sub_linestring(ls, -0.5, 8.5); - ASSERT_EQ(sub.size(), ls.size()); - EXPECT_NEAR(sub.front().x(), -0.5, eps); - for (auto i = 1lu; i + 1 < ls.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i], sub[i])); - EXPECT_NEAR(sub.back().x(), 8.5, eps); - for (const auto & p : sub) EXPECT_NEAR(p.y(), 0.0, eps); - } -} - TEST(DrivableAreaExpansionProjection, InverseProjection) { using drivable_area_expansion::linestring_to_point_projection; using drivable_area_expansion::point_to_linestring_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; for (auto x = 0.0; x < 10.0; x += 0.1) { for (auto y = 0.0; x < 10.0; x += 0.1) { - point_t p(x, y); + Point2d p(x, y); const auto projection = point_to_linestring_projection(p, ls); const auto inverse = linestring_to_point_projection(ls, projection.arc_length, projection.distance); @@ -213,7 +172,7 @@ TEST(DrivableAreaExpansionProjection, InverseProjection) } } -TEST(DrivableAreaExpansionProjection, expandDrivableArea) +TEST(DrivableAreaExpansionProjection, expand_drivable_area) { drivable_area_expansion::DrivableAreaExpansionParameters params; drivable_area_expansion::PredictedObjects dynamic_objects; @@ -256,122 +215,59 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.avoid_dynamic_objects = false; params.avoid_linestring_dist = 0.0; params.avoid_linestring_types = {}; - params.compensate_extra_dist = false; params.max_expansion_distance = 0.0; // means no limit params.max_path_arc_length = 0.0; // means no limit params.resample_interval = 1.0; - params.extra_arc_length = 1.0; - params.expansion_method = "polygon"; // 2m x 4m ego footprint - params.ego_front_offset = 1.0; - params.ego_rear_offset = -1.0; - params.ego_left_offset = 2.0; - params.ego_right_offset = -2.0; + params.vehicle_info.front_overhang_m = 0.0; + params.vehicle_info.wheel_base_m = 2.0; + params.vehicle_info.vehicle_width_m = 2.0; } behavior_path_planner::PlannerData planner_data; planner_data.drivable_area_expansion_parameters = params; - planner_data.reference_path = std::make_shared(path); planner_data.dynamic_object = std::make_shared(dynamic_objects); + planner_data.self_odometry = std::make_shared(); planner_data.route_handler = std::make_shared(route_handler); - // we expect the drivable area to be expanded by 1m on each side - drivable_area_expansion::expandDrivableArea( - path, std::make_shared(planner_data), path_lanes); + drivable_area_expansion::expand_drivable_area( + path, std::make_shared(planner_data)); // unchanged path points ASSERT_EQ(path.points.size(), 3ul); for (auto i = 0.0; i < path.points.size(); ++i) { EXPECT_NEAR(path.points[i].point.pose.position.x, i, eps); EXPECT_NEAR(path.points[i].point.pose.position.y, 0.0, eps); } - + // straight path: no expansion // expanded left bound - ASSERT_EQ(path.left_bound.size(), 4ul); + ASSERT_EQ(path.left_bound.size(), 3ul); EXPECT_NEAR(path.left_bound[0].x, 0.0, eps); EXPECT_NEAR(path.left_bound[0].y, 1.0, eps); - EXPECT_NEAR(path.left_bound[1].x, 0.0, eps); - EXPECT_NEAR(path.left_bound[1].y, 2.0, eps); + EXPECT_NEAR(path.left_bound[1].x, 1.0, eps); + EXPECT_NEAR(path.left_bound[1].y, 1.0, eps); EXPECT_NEAR(path.left_bound[2].x, 2.0, eps); - EXPECT_NEAR(path.left_bound[2].y, 2.0, eps); - EXPECT_NEAR(path.left_bound[3].x, 2.0, eps); - EXPECT_NEAR(path.left_bound[3].y, 1.0, eps); + EXPECT_NEAR(path.left_bound[2].y, 1.0, eps); // expanded right bound ASSERT_EQ(path.right_bound.size(), 3ul); EXPECT_NEAR(path.right_bound[0].x, 0.0, eps); - EXPECT_NEAR(path.right_bound[0].y, -2.0, eps); - EXPECT_NEAR(path.right_bound[1].x, 2.0, eps); - EXPECT_NEAR(path.right_bound[1].y, -2.0, eps); + EXPECT_NEAR(path.right_bound[0].y, -1.0, eps); + EXPECT_NEAR(path.right_bound[1].x, 1.0, eps); + EXPECT_NEAR(path.right_bound[1].y, -1.0, eps); EXPECT_NEAR(path.right_bound[2].x, 2.0, eps); EXPECT_NEAR(path.right_bound[2].y, -1.0, eps); -} - -TEST(DrivableAreaExpansion, calculateDistanceLimit) -{ - using drivable_area_expansion::calculateDistanceLimit; - using drivable_area_expansion::linestring_t; - using drivable_area_expansion::multi_linestring_t; - using drivable_area_expansion::polygon_t; - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multi_linestring_t uncrossable_lines = {}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); - EXPECT_NEAR(limit_distance, std::numeric_limits::max(), 1e-9); - } - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const linestring_t uncrossable_line = {{0.0, 2.0}, {10.0, 2.0}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_line}); - EXPECT_NEAR(limit_distance, 2.0, 1e-9); - } - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multi_linestring_t uncrossable_lines = { - {{0.0, 2.0}, {10.0, 2.0}}, {{0.0, 1.5}, {10.0, 1.0}}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); - EXPECT_NEAR(limit_distance, 1.0, 1e-9); - } -} - -TEST(DrivableAreaExpansion, calculateDistanceLimitEdgeCases) -{ - using drivable_area_expansion::calculateDistanceLimit; - using drivable_area_expansion::linestring_t; - using drivable_area_expansion::polygon_t; + // add some curvature + path.points[1].point.pose.position.y = 0.5; - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - { // intersection points further than the line point inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 5.0}, {6.0, 3.0}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 3.0, 1e-9); - } - { // intersection points further than the line point inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 5.0}, {5.0, 2.0}, {6.0, 4.5}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 2.0, 1e-9); - } - { // line completely inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 2.0}, {6.0, 3.0}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 2.0, 1e-9); - } - { // line completely inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 3.5}, {6.0, 3.0}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 3.0, 1e-9); - } + drivable_area_expansion::expand_drivable_area( + path, std::make_shared(planner_data)); + // expanded left bound + ASSERT_EQ(path.left_bound.size(), 3ul); + EXPECT_GT(path.left_bound[0].y, 1.0); + EXPECT_GT(path.left_bound[1].y, 1.0); + EXPECT_GT(path.left_bound[2].y, 1.0); + // expanded right bound + ASSERT_EQ(path.right_bound.size(), 3ul); + EXPECT_LT(path.right_bound[0].y, -1.0); + EXPECT_LT(path.right_bound[1].y, -1.0); + EXPECT_LT(path.right_bound[2].y, -1.0); } diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index 39e831bd5565b..a5fc19d1ecbbe 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -52,9 +52,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; - const auto polygon = - createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); + const auto polygon = createExtendedPolygon( + ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -79,9 +80,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; - const auto polygon = - createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); + const auto polygon = createExtendedPolygon( + ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -107,9 +109,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; - const auto polygon = - createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); + const auto polygon = createExtendedPolygon( + ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -155,9 +158,11 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; CollisionCheckDebug debug; - const auto polygon = createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, debug); + const auto polygon = + createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 66f326ed799af..3830aa9edddff 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -9,6 +9,7 @@ Tomoya Kimura Shumpei Wakabayashi Takayuki Murooka + Kyoichi Sugahara Apache License 2.0 diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 94e87d0174193..dd8dc95a8ad3c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -128,8 +128,9 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - const auto launch = [this, &path](const auto id, const auto & use_regulatory_element) { - if (isModuleRegistered(id)) { + const auto launch = [this, &path]( + const auto lane_id, const std::optional & reg_elem_id) { + if (isModuleRegistered(lane_id)) { return; } @@ -137,24 +138,28 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) const auto logger = logger_.get_child("crosswalk_module"); const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + // NOTE: module_id is always a lane id so that isModuleRegistered works correctly in the case + // where both regulatory element and non-regulatory element crosswalks exist. registerModule(std::make_shared( - node_, id, lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); - generateUUID(id); - updateRTCStatus(getUUID(id), true, std::numeric_limits::lowest(), path.header.stamp); + node_, lane_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_)); + generateUUID(lane_id); + updateRTCStatus( + getUUID(lane_id), true, std::numeric_limits::lowest(), path.header.stamp); }; const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); for (const auto & crosswalk : crosswalk_leg_elem_map) { - launch(crosswalk.first->id(), true); + // NOTE: The former id is a lane id, and the latter one is a regulatory element's id. + launch(crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id()); } const auto crosswalk_lanelets = getCrosswalksOnPath( planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); for (const auto & crosswalk : crosswalk_lanelets) { - launch(crosswalk.id(), false); + launch(crosswalk.id(), std::nullopt); } } @@ -172,7 +177,7 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); for (const auto & crosswalk : crosswalk_leg_elem_map) { - crosswalk_id_set.insert(crosswalk.first->id()); + crosswalk_id_set.insert(crosswalk.first->crosswalkLanelet().id()); } return [crosswalk_id_set](const std::shared_ptr & scene_module) { diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index fe96985743b94..56f5072a2ade9 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -176,20 +176,20 @@ tier4_debug_msgs::msg::StringStamped createStringStampedMessage( } // namespace CrosswalkModule::CrosswalkModule( - rclcpp::Node & node, const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, - const PlannerParam & planner_param, const bool use_regulatory_element, + rclcpp::Node & node, const int64_t module_id, const std::optional & reg_elem_id, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), module_id_(module_id), planner_param_(planner_param), - use_regulatory_element_(use_regulatory_element) + use_regulatory_element_(reg_elem_id) { velocity_factor_.init(VelocityFactor::CROSSWALK); passed_safety_slow_point_ = false; if (use_regulatory_element_) { const auto reg_elem_ptr = std::dynamic_pointer_cast( - lanelet_map_ptr->regulatoryElementLayer.get(module_id)); + lanelet_map_ptr->regulatoryElementLayer.get(*reg_elem_id)); stop_lines_ = reg_elem_ptr->stopLines(); crosswalk_ = reg_elem_ptr->crosswalkLanelet(); } else { diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 063ea4f83cd45..0768101179857 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -299,8 +299,8 @@ class CrosswalkModule : public SceneModuleInterface }; CrosswalkModule( - rclcpp::Node & node, const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, - const PlannerParam & planner_param, const bool use_regulatory_element, + rclcpp::Node & node, const int64_t module_id, const std::optional & reg_elem_id, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 30fefcaeee035..fde55dc7a6c55 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -12,7 +12,14 @@ use_intersection_area: false path_interpolation_ds: 0.1 # [m] consider_wrong_direction_vehicle: false + max_accel: -2.8 + max_jerk: -5.0 + delay_response_time: 0.5 stuck_vehicle: + turn_direction: + left: true + right: true + straight: true use_stuck_stopline: true # stopline generated before the first conflicting area stuck_vehicle_detect_dist: 5.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h @@ -20,6 +27,12 @@ # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area enable_private_area_stuck_disregard: false #In the intersections which labeled as "private area", the obstacle vehicles judged as "stuck" are neglected if this param is set as true. + yield_stuck: + turn_direction: + left: true + right: false + straight: false + distance_thr: 1.0 # [m] collision_detection: state_transit_margin_time: 1.0 @@ -40,7 +53,9 @@ yield_on_green_traffic_light: distance_to_assigned_lanelet_start: 5.0 duration: 2.0 - range: 30.0 # [m] + object_dist_to_stopline: 10.0 # [m] + ignore_on_amber_traffic_light: + object_expected_deceleration: 2.0 # [m/ss] occlusion: enable: false @@ -64,6 +79,7 @@ maximum_peeking_distance: 6.0 # [m] attention_lane_crop_curvature_threshold: 0.25 attention_lane_curvature_calculation_ds: 0.5 + static_occlusion_with_traffic_light_timeout: 3.5 enable_rtc: intersection: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 9ff638cb61876..f472c4bf51e31 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -188,14 +188,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array); } - if (debug_data_.intersection_area) { - appendMarkerArray( - debug::createPolygonMarkerArray( - debug_data_.intersection_area.value(), "intersection_area", lane_id_, now, 0.3, 0.0, 0.0, - 0.0, 1.0, 0.0), - &debug_marker_array); - } - if (debug_data_.stuck_vehicle_detect_area) { appendMarkerArray( debug::createPolygonMarkerArray( @@ -233,11 +225,21 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), + &debug_marker_array, now); + appendMarkerArray( debug::createObjectsMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.yield_stuck_targets, "stuck_targets", module_id_, now, 0.4, 0.99, 0.2), + &debug_marker_array, now); + appendMarkerArray( debug::createObjectsMarkerArray( debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99, @@ -299,6 +301,12 @@ motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() if (debug_data_.occlusion_stop_wall_pose) { wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection_occlusion"; + if (debug_data_.static_occlusion_with_traffic_light_timeout) { + std::stringstream timeout; + timeout << std::setprecision(2) + << debug_data_.static_occlusion_with_traffic_light_timeout.value(); + wall.text += "(" + timeout.str() + ")"; + } wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.occlusion_stop_wall_pose.value(); virtual_walls.push_back(wall); diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 4137090a5b6ae..a65e06eedcdf0 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -62,7 +62,17 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); ip.common.consider_wrong_direction_vehicle = getOrDeclareParameter(node, ns + ".common.consider_wrong_direction_vehicle"); - + ip.common.max_accel = getOrDeclareParameter(node, ns + ".common.max_accel"); + ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); + ip.common.delay_response_time = + getOrDeclareParameter(node, ns + ".common.delay_response_time"); + + ip.stuck_vehicle.turn_direction.left = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); + ip.stuck_vehicle.turn_direction.right = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.right"); + ip.stuck_vehicle.turn_direction.straight = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.straight"); ip.stuck_vehicle.use_stuck_stopline = getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); ip.stuck_vehicle.stuck_vehicle_detect_dist = @@ -79,6 +89,14 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); ip.stuck_vehicle.enable_private_area_stuck_disregard = getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard"); + ip.stuck_vehicle.yield_stuck_turn_direction.left = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.left"); + ip.stuck_vehicle.yield_stuck_turn_direction.right = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.right"); + ip.stuck_vehicle.yield_stuck_turn_direction.straight = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.straight"); + ip.stuck_vehicle.yield_stuck_distance_thr = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.distance_thr"); ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); @@ -115,8 +133,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ns + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start"); ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter( node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); - ip.collision_detection.yield_on_green_traffic_light.range = getOrDeclareParameter( - node, ns + ".collision_detection.yield_on_green_traffic_light.range"); + ip.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline = + getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.object_dist_to_stopline"); + ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration = + getOrDeclareParameter( + node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = @@ -154,6 +176,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); ip.occlusion.attention_lane_curvature_calculation_ds = getOrDeclareParameter(node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); + ip.occlusion.static_occlusion_with_traffic_light_timeout = getOrDeclareParameter( + node, ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 75bb4e861a60f..4def152567b32 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -43,6 +44,32 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +namespace +{ +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware_auto_perception_msgs::msg::Shape & shape) +{ + const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + + Polygon2d one_step_poly; + for (const auto & point : prev_poly.outer()) { + one_step_poly.outer().push_back(point); + } + for (const auto & point : next_poly.outer()) { + one_step_poly.outer().push_back(point); + } + + bg::correct(one_step_poly); + + Polygon2d convex_one_step_poly; + bg::convex_hull(one_step_poly, convex_one_step_poly); + + return convex_one_step_poly; +} +} // namespace + static bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) { @@ -102,6 +129,11 @@ IntersectionModule::IntersectionModule( planner_param_.occlusion.before_creep_stop_time); temporal_stop_before_attention_state_machine_.setState(StateMachine::State::STOP); } + { + static_occlusion_timeout_state_machine_.setMarginTime( + planner_param_.occlusion.static_occlusion_with_traffic_light_timeout); + static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); + } decision_state_pub_ = node_.create_publisher("~/debug/intersection/decision_state", 1); @@ -168,6 +200,21 @@ void prepareRTCByDecisionResult( return; } +template <> +void prepareRTCByDecisionResult( + const IntersectionModule::YieldStuckStop & result, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) +{ + RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "YieldStuckStop"); + const auto closest_idx = result.closest_idx; + const auto stop_line_idx = result.stuck_stop_line_idx; + *default_safety = false; + *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); + *occlusion_safety = true; + return; +} + template <> void prepareRTCByDecisionResult( const IntersectionModule::NonOccludedCollisionStop & result, @@ -392,6 +439,38 @@ void reactRTCApprovalByDecisionResult( return; } +template <> +void reactRTCApprovalByDecisionResult( + const bool rtc_default_approved, const bool rtc_occlusion_approved, + const IntersectionModule::YieldStuckStop & decision_result, + [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, + const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("reactRTCApprovalByDecisionResult"), + "YieldStuckStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, + rtc_occlusion_approved); + const auto closest_idx = decision_result.closest_idx; + if (!rtc_default_approved) { + // use default_rtc uuid for stuck vehicle detection + const auto stop_line_idx = decision_result.stuck_stop_line_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->collision_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + return; +} + template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, @@ -515,6 +594,8 @@ void reactRTCApprovalByDecisionResult( planning_utils::setVelocityFromIndex(occlusion_peeking_stop_line, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_peeking_stop_line, baselink2front, *path); + debug_data->static_occlusion_with_traffic_light_timeout = + decision_result.static_occlusion_timeout; { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(occlusion_peeking_stop_line).point.pose; @@ -574,6 +655,8 @@ void reactRTCApprovalByDecisionResult( planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + debug_data->static_occlusion_with_traffic_light_timeout = + decision_result.static_occlusion_timeout; { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; @@ -753,6 +836,9 @@ static std::string formatDecisionResult(const IntersectionModule::DecisionResult if (std::holds_alternative(decision_result)) { return "StuckStop"; } + if (std::holds_alternative(decision_result)) { + return "YieldStuckStop"; + } if (std::holds_alternative(decision_result)) { return "NonOccludedCollisionStop"; } @@ -877,7 +963,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); const bool is_prioritized = traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; - intersection_lanelets.update(is_prioritized, interpolated_path_info); + const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint); // this is abnormal const auto & conflicting_lanelets = intersection_lanelets.conflicting(); @@ -900,7 +987,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin, - peeking_offset, path); + planner_param_.common.max_accel, planner_param_.common.max_jerk, + planner_param_.common.delay_response_time, peeking_offset, path); if (!intersection_stop_lines_opt) { return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"}; } @@ -939,6 +1027,19 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } return state_machine.getState() == StateMachine::State::GO; }; + auto stoppedAtPosition = [&](const size_t pos, const double duration) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); + const bool over_stopline = (dist_stopline < -planner_param_.common.stop_overshoot_margin); + const bool is_stopped = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + return true; + } else if (is_stopped && approached_dist_stopline) { + return true; + } + return false; + }; // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation @@ -957,6 +1058,23 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } + // yield stuck vehicle detection is viable even if attention area is empty + // so this needs to be checked before attention area validation + const bool yield_stuck_detected = checkYieldStuckVehicle( + planner_data_, path_lanelets, intersection_lanelets.first_attention_area()); + if (yield_stuck_detected && stuck_stop_line_idx_opt) { + auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); + if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { + if ( + default_stop_line_idx_opt && + fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { + stuck_stop_line_idx = default_stop_line_idx_opt.value(); + } + } else { + return IntersectionModule::YieldStuckStop{closest_idx, stuck_stop_line_idx}; + } + } + // if attention area is empty, collision/occlusion detection is impossible if (!first_attention_area_opt) { return IntersectionModule::Indecisive{"attention area is empty"}; @@ -1008,7 +1126,6 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value(); const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); - const auto & attention_lanelets = intersection_lanelets.attention(); const auto & adjacent_lanelets = intersection_lanelets.adjacent(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); @@ -1017,16 +1134,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); // get intersection area - const auto intersection_area = planner_param_.common.use_intersection_area - ? util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr) - : std::nullopt; - if (intersection_area) { - const auto intersection_area_2d = intersection_area.value(); - debug_data_.intersection_area = toGeomPoly(intersection_area_2d); - } + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); - const auto target_objects = - filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); + auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); // If there are any vehicles on the attention area when ego entered the intersection on green // light, do pseudo collision detection because the vehicles are very slow and no collisions may @@ -1051,10 +1161,12 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (initial_green_light_observed_time_) { const auto now = clock_->now(); const bool exist_close_vehicles = std::any_of( - target_objects.objects.begin(), target_objects.objects.end(), [&](const auto & object) { - return tier4_autoware_utils::calcDistance3d( - object.kinematics.initial_pose_with_covariance.pose, current_pose) < - planner_param_.collision_detection.yield_on_green_traffic_light.range; + target_objects.all_attention_objects.begin(), target_objects.all_attention_objects.end(), + [&](const auto & object) { + return object.dist_to_stop_line.has_value() && + object.dist_to_stop_line.value() < + planner_param_.collision_detection.yield_on_green_traffic_light + .object_dist_to_stopline; }); if ( exist_close_vehicles && @@ -1073,7 +1185,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getDuration()); const bool has_collision = checkCollision( - *path, target_objects, path_lanelets, closest_idx, + *path, &target_objects, path_lanelets, closest_idx, std::min(occlusion_stop_line_idx, path->points.size() - 1), time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( @@ -1100,32 +1212,30 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); - std::vector blocking_attention_objects; - std::copy_if( - target_objects.objects.begin(), target_objects.objects.end(), - std::back_inserter(blocking_attention_objects), - [thresh = planner_param_.occlusion.ignore_parked_vehicle_speed_threshold](const auto & object) { - return std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh; - }); - debug_data_.blocking_attention_objects.objects = blocking_attention_objects; - const bool is_occlusion_cleared = + auto occlusion_status = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) - ? isOcclusionCleared( + ? getOcclusionStatus( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, occlusion_attention_divisions, - blocking_attention_objects, occlusion_dist_thr) - : true; + target_objects, current_pose, occlusion_dist_thr) + : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime( - is_occlusion_cleared ? StateMachine::State::GO : StateMachine::STOP, + occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, logger_.get_child("occlusion_stop"), *clock_); const bool is_occlusion_cleared_with_margin = (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); - // distinguish if ego detected occlusion or RTC detects occlusion const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); + if (ext_occlusion_requested) { + occlusion_status = OcclusionType::RTC_OCCLUDED; + } const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); + if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { + occlusion_status = prev_occlusion_status_; + } else { + prev_occlusion_status_ = occlusion_status; + } + // Safe if (!is_occlusion_state && !has_collision_with_margin) { return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; @@ -1136,11 +1246,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } // Occluded + // occlusion_status is assured to be not NOT_OCCLUDED const bool stopped_at_default_line = stoppedForDuration( default_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, before_creep_state_machine_); if (stopped_at_default_line) { - // in this case ego will temporarily stop before entering attention area + // if specified the parameter occlusion.temporal_stop_before_attention_area OR + // has_no_traffic_light_, ego will temporarily stop before entering attention area const bool temporal_stop_before_attention_required = (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) ? !stoppedForDuration( @@ -1157,20 +1269,51 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( temporal_stop_before_attention_required, closest_idx, first_attention_stop_line_idx, occlusion_stop_line_idx}; } + // following remaining block is "has_traffic_light_" + // if ego is stuck by static occlusion in the presence of traffic light, start timeout count + const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; + const bool is_stuck_by_static_occlusion = + stoppedAtPosition(occlusion_stop_line_idx, planner_param_.occlusion.before_creep_stop_time) && + is_static_occlusion; + if (has_collision_with_margin) { + // if collision is detected, timeout is reset + static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); + } else if (is_stuck_by_static_occlusion) { + static_occlusion_timeout_state_machine_.setStateWithMarginTime( + StateMachine::State::GO, logger_.get_child("static_occlusion"), *clock_); + } + const bool release_static_occlusion_stuck = + (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); + if (!has_collision_with_margin && release_static_occlusion_stuck) { + return IntersectionModule::Safe{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED + const double max_timeout = + planner_param_.occlusion.static_occlusion_with_traffic_light_timeout + + planner_param_.occlusion.stop_release_margin_time; + const std::optional static_occlusion_timeout = + is_stuck_by_static_occlusion + ? std::make_optional( + max_timeout - static_occlusion_timeout_state_machine_.getDuration() - + occlusion_stop_state_machine_.getDuration()) + : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); if (has_collision_with_margin) { return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin, temporal_stop_before_attention_required, closest_idx, collision_stop_line_idx, first_attention_stop_line_idx, - occlusion_stop_line_idx}; + occlusion_stop_line_idx, + static_occlusion_timeout}; } else { return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin, temporal_stop_before_attention_required, closest_idx, collision_stop_line_idx, first_attention_stop_line_idx, - occlusion_stop_line_idx}; + occlusion_stop_line_idx, + static_occlusion_timeout}; } } else { const auto occlusion_stop_line = @@ -1185,6 +1328,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( bool IntersectionModule::checkStuckVehicle( const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) { + const bool stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || + (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || + (turn_direction_ == "straight" && planner_param_.stuck_vehicle.turn_direction.straight); + }(); + if (!stuck_detection_direction) { + return false; + } + const auto & objects_ptr = planner_data->predicted_objects; // considering lane change in the intersection, these lanelets are generated from the path @@ -1197,18 +1349,48 @@ bool IntersectionModule::checkStuckVehicle( &debug_data_); } -autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects( - const lanelet::ConstLanelets & attention_area_lanelets, - const lanelet::ConstLanelets & adjacent_lanelets, - const std::optional & intersection_area) const +bool IntersectionModule::checkYieldStuckVehicle( + const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets, + const std::optional & first_attention_area) { - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getPolygonFromArcLength; + if (!first_attention_area) { + return false; + } + + const bool yield_stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && + planner_param_.stuck_vehicle.yield_stuck_turn_direction.left) || + (turn_direction_ == "right" && + planner_param_.stuck_vehicle.yield_stuck_turn_direction.right) || + (turn_direction_ == "straight" && + planner_param_.stuck_vehicle.yield_stuck_turn_direction.straight); + }(); + if (!yield_stuck_detection_direction) { + return false; + } + + const auto & objects_ptr = planner_data->predicted_objects; + + const auto & ego_lane = path_lanelets.ego_or_entry2exit; + const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + + return util::checkYieldStuckVehicleInIntersection( + objects_ptr, ego_poly, first_attention_area.value(), + planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, + planner_param_.stuck_vehicle.yield_stuck_distance_thr, &debug_data_); +} +util::TargetObjects IntersectionModule::generateTargetObjects( + const util::IntersectionLanelets & intersection_lanelets, + const std::optional & intersection_area) const +{ const auto & objects_ptr = planner_data_->predicted_objects; // extract target objects - autoware_auto_perception_msgs::msg::PredictedObjects target_objects; + util::TargetObjects target_objects; target_objects.header = objects_ptr->header; + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stop_lines(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); for (const auto & object : objects_ptr->objects) { // ignore non-vehicle type objects, such as pedestrian. if (!isTargetCollisionVehicleType(object)) { @@ -1217,49 +1399,73 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto is_in_adjacent_lanelets = util::checkAngleForTargetLanelets( - object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, - adjacent_lanelets, planner_param_.common.attention_area_angle_thr, + const auto belong_adjacent_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold); - if (is_in_adjacent_lanelets) { + planner_param_.common.attention_area_margin, false); + if (belong_adjacent_lanelet_id) { continue; } + const auto is_parked_vehicle = + std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; + auto & container = is_parked_vehicle ? target_objects.parked_attention_objects + : target_objects.attention_objects; if (intersection_area) { + const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); const auto intersection_area_2d = intersection_area.value(); - const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d); - if (is_in_intersection_area) { - target_objects.objects.push_back(object); - } else if (util::checkAngleForTargetLanelets( - object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, - attention_area_lanelets, planner_param_.common.attention_area_angle_thr, - planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { - target_objects.objects.push_back(object); + const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, attention_lanelets, planner_param_.common.attention_area_angle_thr, + planner_param_.common.consider_wrong_direction_vehicle, + planner_param_.common.attention_area_margin, is_parked_vehicle); + if (belong_attention_lanelet_id) { + const auto id = belong_attention_lanelet_id.value(); + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stop_line = attention_lanelet_stoplines.at(id); + container.push_back(target_object); + } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = std::nullopt; + target_object.stop_line = std::nullopt; + target_objects.intersection_area_objects.push_back(target_object); } - } else if (util::checkAngleForTargetLanelets( - object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, - attention_area_lanelets, planner_param_.common.attention_area_angle_thr, + } else if (const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, attention_lanelets, + planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { + planner_param_.common.attention_area_margin, is_parked_vehicle); + belong_attention_lanelet_id.has_value()) { // intersection_area is not available, use detection_area_with_margin as before - target_objects.objects.push_back(object); + const auto id = belong_attention_lanelet_id.value(); + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stop_line = attention_lanelet_stoplines.at(id); + container.push_back(target_object); } } + for (const auto & object : target_objects.attention_objects) { + target_objects.all_attention_objects.push_back(object); + } + for (const auto & object : target_objects.parked_attention_objects) { + target_objects.all_attention_objects.push_back(object); + } + for (auto & object : target_objects.all_attention_objects) { + object.calc_dist_to_stop_line(); + } return target_objects; } bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, - const util::PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stop_line_candidate_idx, const double time_delay, - const util::TrafficPrioritizedLevel & traffic_prioritized_level) + util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1273,17 +1479,16 @@ bool IntersectionModule::checkCollision( planner_param_.collision_detection.use_upstream_velocity, planner_param_.collision_detection.minimum_upstream_velocity); const double passing_time = time_distance_array.back().first; - auto target_objects = objects; - util::cutPredictPathWithDuration(&target_objects, clock_, passing_time); + util::cutPredictPathWithDuration(target_objects, clock_, passing_time); const auto & concat_lanelets = path_lanelets.all; const auto closest_arc_coords = getArcCoordinates( concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); const auto & ego_lane = path_lanelets.ego_or_entry2exit; debug_data_.ego_lane = ego_lane.polygon3d(); - const auto ego_poly = ego_lane.polygon2d().basicPolygon(); - // check collision between predicted_path and ego_area + + // change TTC margin based on ego traffic light color const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { if (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED) { return std::make_pair( @@ -1299,8 +1504,33 @@ bool IntersectionModule::checkCollision( planner_param_.collision_detection.not_prioritized.collision_start_margin_time, planner_param_.collision_detection.not_prioritized.collision_end_margin_time); }(); + const auto expectedToStopBeforeStopLine = [&](const util::TargetObject & target_object) { + if (!target_object.dist_to_stop_line) { + return false; + } + const double dist_to_stop_line = target_object.dist_to_stop_line.value(); + if (dist_to_stop_line < 0) { + return false; + } + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + return dist_to_stop_line > braking_distance; + }; + + // check collision between predicted_path and ego_area bool collision_detected = false; - for (const auto & object : target_objects.objects) { + for (const auto & target_object : target_objects->all_attention_objects) { + const auto & object = target_object.object; + // If the vehicle is expected to stop before their stopline, ignore + if ( + traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + expectedToStopBeforeStopLine(target_object)) { + debug_data_.amber_ignore_targets.objects.push_back(object); + continue; + } for (const auto & predicted_path : object.kinematics.predicted_paths) { if ( predicted_path.confidence < @@ -1312,14 +1542,14 @@ bool IntersectionModule::checkCollision( // collision point const auto first_itr = std::adjacent_find( predicted_path.path.cbegin(), predicted_path.path.cend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); }); if (first_itr == predicted_path.path.cend()) continue; const auto last_itr = std::adjacent_find( predicted_path.path.crbegin(), predicted_path.path.crend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); }); if (last_itr == predicted_path.path.crend()) continue; @@ -1391,15 +1621,14 @@ bool IntersectionModule::checkCollision( return collision_detected; } -bool IntersectionModule::isOcclusionCleared( +IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const std::vector & - blocking_attention_objects, + const std::vector & lane_divisions, + const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, const double occlusion_dist_thr) { const auto & path_ip = interpolated_path_info.path; @@ -1408,7 +1637,7 @@ bool IntersectionModule::isOcclusionCleared( const auto first_attention_area_idx = util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); if (!first_attention_area_idx) { - return false; + return OcclusionType::NOT_OCCLUDED; } const auto first_inside_attention_idx_ip_opt = @@ -1523,29 +1752,30 @@ bool IntersectionModule::isOcclusionCleared( // re-use attention_mask attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded + const auto & blocking_attention_objects = target_objects.parked_attention_objects; + for (const auto & blocking_attention_object : blocking_attention_objects) { + debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); + } std::vector> blocking_polygons; for (const auto & blocking_attention_object : blocking_attention_objects) { - const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object); + const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } for (const auto & blocking_polygon : blocking_polygons) { cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); } - for (const auto & lane_division : lane_divisions) { - const auto & divisions = lane_division.divisions; - for (const auto & division : divisions) { - bool blocking_vehicle_found = false; - for (const auto & point_it : division) { - const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); - if (!valid) continue; - if (blocking_vehicle_found) { - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - continue; - } - if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { - blocking_vehicle_found = true; - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - } + for (const auto & division : lane_divisions) { + bool blocking_vehicle_found = false; + for (const auto & point_it : division) { + const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); + if (!valid) continue; + if (blocking_vehicle_found) { + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + continue; + } + if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { + blocking_vehicle_found = true; + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; } } } @@ -1618,95 +1848,105 @@ bool IntersectionModule::isOcclusionCleared( } } - auto findNearestPointToProjection = - [](lanelet::ConstLineString2d division, const Point2d & projection, const double dist_thresh) { - double min_dist = std::numeric_limits::infinity(); - auto nearest = division.end(); - for (auto it = division.begin(); it != division.end(); it++) { - const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); - if (dist < min_dist) { - min_dist = dist; - nearest = it; - } - if (dist < dist_thresh) { - break; - } + auto findNearestPointToProjection = []( + const lanelet::ConstLineString3d & division, + const Point2d & projection, const double dist_thresh) { + double min_dist = std::numeric_limits::infinity(); + auto nearest = division.end(); + for (auto it = division.begin(); it != division.end(); it++) { + const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); + if (dist < min_dist) { + min_dist = dist; + nearest = it; } - return nearest; - }; + if (dist < dist_thresh) { + break; + } + } + return nearest; + }; struct NearestOcclusionPoint { - int lane_id; - int64 division_index; - double dist; + int64 division_index{0}; + int64 point_index{0}; + double dist{0.0}; geometry_msgs::msg::Point point; geometry_msgs::msg::Point projection; } nearest_occlusion_point; double min_dist = std::numeric_limits::infinity(); - for (const auto & lane_division : lane_divisions) { - const auto & divisions = lane_division.divisions; - const auto lane_id = lane_division.lane_id; - for (unsigned division_index = 0; division_index < divisions.size(); ++division_index) { - const auto & division = divisions.at(division_index); - LineString2d division_linestring; - auto division_point_it = division.begin(); - division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); - for (auto point_it = division.begin(); point_it != division.end(); point_it++) { - if ( - std::hypot( - point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < - 3.0 /* rough tick for computational cost */) { - continue; - } - division_linestring.emplace_back(point_it->x(), point_it->y()); - division_point_it = point_it; - } - - // find the intersection point of lane_line and path - std::vector intersection_points; - boost::geometry::intersection(division_linestring, path_linestring, intersection_points); - if (intersection_points.empty()) { + for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { + const auto & division = lane_divisions.at(division_index); + LineString2d division_linestring; + auto division_point_it = division.begin(); + division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); + for (auto point_it = division.begin(); point_it != division.end(); point_it++) { + if ( + std::hypot(point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < + 3.0 /* rough tick for computational cost */) { continue; } - const auto & projection_point = intersection_points.at(0); - const auto projection_it = - findNearestPointToProjection(division, projection_point, resolution); - if (projection_it == division.end()) { - continue; + division_linestring.emplace_back(point_it->x(), point_it->y()); + division_point_it = point_it; + } + + // find the intersection point of lane_line and path + std::vector intersection_points; + boost::geometry::intersection(division_linestring, path_linestring, intersection_points); + if (intersection_points.empty()) { + continue; + } + const auto & projection_point = intersection_points.at(0); + const auto projection_it = findNearestPointToProjection(division, projection_point, resolution); + if (projection_it == division.end()) { + continue; + } + double acc_dist = 0.0; + auto acc_dist_it = projection_it; + for (auto point_it = projection_it; point_it != division.end(); point_it++) { + const double dist = + std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); + acc_dist += dist; + acc_dist_it = point_it; + const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); + if (!valid) continue; + const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); + if (pixel == BLOCKED) { + break; } - double acc_dist = 0.0; - auto acc_dist_it = projection_it; - for (auto point_it = projection_it; point_it != division.end(); point_it++) { - const double dist = - std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); - acc_dist += dist; - acc_dist_it = point_it; - const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); - // TODO(Mamoru Sobue): add handling for blocking vehicles - if (!valid) continue; - const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); - if (pixel == BLOCKED) { - break; - } - if (pixel == OCCLUDED) { - if (acc_dist < min_dist) { - min_dist = acc_dist; - nearest_occlusion_point = { - lane_id, std::distance(division.begin(), point_it), acc_dist, - tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), - tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; - } + if (pixel == OCCLUDED) { + if (acc_dist < min_dist) { + min_dist = acc_dist; + nearest_occlusion_point = { + division_index, std::distance(division.begin(), point_it), acc_dist, + tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), + tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; } } } } if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { - return true; + return OcclusionType::NOT_OCCLUDED; } + debug_data_.nearest_occlusion_projection = std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); - return false; + LineString2d ego_occlusion_line; + ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); + ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + for (const auto & attention_object : target_objects.all_attention_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + if (bg::intersects(obj_poly, ego_occlusion_line)) { + return OcclusionType::DYNAMICALLY_OCCLUDED; + } + } + for (const auto & attention_object : target_objects.intersection_area_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + if (bg::intersects(obj_poly, ego_occlusion_line)) { + return OcclusionType::DYNAMICALLY_OCCLUDED; + } + } + return OcclusionType::STATICALLY_OCCLUDED; } /* diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index de7d97a50c133..764f5bd7fe058 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -58,9 +58,19 @@ class IntersectionModule : public SceneModuleInterface bool use_intersection_area; bool consider_wrong_direction_vehicle; double path_interpolation_ds; + double max_accel; + double max_jerk; + double delay_response_time; } common; struct StuckVehicle { + struct TurnDirection + { + bool left; + bool right; + bool straight; + }; + TurnDirection turn_direction; bool use_stuck_stopline; //! stopline generate before the intersection lanelet when is_stuck. double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check double stuck_vehicle_vel_thr; //! Threshold of the speed to be recognized as stopped @@ -71,6 +81,8 @@ class IntersectionModule : public SceneModuleInterface */ double timeout_private_area; bool enable_private_area_stuck_disregard; + double yield_stuck_distance_thr; + TurnDirection yield_stuck_turn_direction; } stuck_vehicle; struct CollisionDetection { @@ -100,8 +112,12 @@ class IntersectionModule : public SceneModuleInterface { double distance_to_assigned_lanelet_start; double duration; - double range; + double object_dist_to_stopline; } yield_on_green_traffic_light; + struct IgnoreOnAmberTrafficLight + { + double object_expected_deceleration; + } ignore_on_amber_traffic_light; } collision_detection; struct Occlusion { @@ -128,9 +144,17 @@ class IntersectionModule : public SceneModuleInterface } absence_traffic_light; double attention_lane_crop_curvature_threshold; double attention_lane_curvature_calculation_ds; + double static_occlusion_with_traffic_light_timeout; } occlusion; }; + enum OcclusionType { + NOT_OCCLUDED, + STATICALLY_OCCLUDED, + DYNAMICALLY_OCCLUDED, + RTC_OCCLUDED, // actual occlusion does not exist, only disapproved by RTC + }; + struct Indecisive { std::string error; @@ -141,6 +165,11 @@ class IntersectionModule : public SceneModuleInterface size_t stuck_stop_line_idx{0}; std::optional occlusion_stop_line_idx{std::nullopt}; }; + struct YieldStuckStop + { + size_t closest_idx{0}; + size_t stuck_stop_line_idx{0}; + }; struct NonOccludedCollisionStop { size_t closest_idx{0}; @@ -154,6 +183,7 @@ class IntersectionModule : public SceneModuleInterface size_t first_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; }; + // A state peeking to occlusion limit line in the presence of traffic light struct PeekingTowardOcclusion { // NOTE: if intersection_occlusion is disapproved externally through RTC, @@ -164,7 +194,12 @@ class IntersectionModule : public SceneModuleInterface size_t collision_stop_line_idx{0}; size_t first_attention_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; + // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) + // if valid, it contains the remaining time to release the static occlusion stuck and shows up + // intersection_occlusion(x.y) + std::optional static_occlusion_timeout{std::nullopt}; }; + // A state detecting both collision and occlusion in the presence of traffic light struct OccludedCollisionStop { bool is_actually_occlusion_cleared{false}; @@ -173,6 +208,9 @@ class IntersectionModule : public SceneModuleInterface size_t collision_stop_line_idx{0}; size_t first_attention_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; + // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) + // if valid, it contains the remaining time to release the static occlusion stuck + std::optional static_occlusion_timeout{std::nullopt}; }; struct OccludedAbsenceTrafficLight { @@ -200,6 +238,7 @@ class IntersectionModule : public SceneModuleInterface using DecisionResult = std::variant< Indecisive, // internal process error, or over the pass judge line StuckStop, // detected stuck vehicle + YieldStuckStop, // detected yield stuck vehicle NonOccludedCollisionStop, // detected collision while FOV is clear FirstWaitBeforeOcclusion, // stop for a while before peeking to occlusion PeekingTowardOcclusion, // peeking into occlusion while collision is not detected @@ -243,6 +282,7 @@ class IntersectionModule : public SceneModuleInterface bool is_go_out_{false}; bool is_permanent_go_{false}; DecisionResult prev_decision_result_{Indecisive{""}}; + OcclusionType prev_occlusion_status_; // Parameter PlannerParam planner_param_; @@ -251,11 +291,13 @@ class IntersectionModule : public SceneModuleInterface // for occlusion detection const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_{std::nullopt}; + std::optional> occlusion_attention_divisions_{ + std::nullopt}; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; StateMachine temporal_stop_before_attention_state_machine_; + StateMachine static_occlusion_timeout_state_machine_; // for pseudo-collision detection when ego just entered intersection on green light and upcoming // vehicles are very slow @@ -282,27 +324,29 @@ class IntersectionModule : public SceneModuleInterface const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets); - autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( - const lanelet::ConstLanelets & attention_area_lanelets, - const lanelet::ConstLanelets & adjacent_lanelets, + bool checkYieldStuckVehicle( + const std::shared_ptr & planner_data, + const util::PathLanelets & path_lanelets, + const std::optional & first_attention_area); + + util::TargetObjects generateTargetObjects( + const util::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const; bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, - const util::PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stop_line_candidate_idx, const double time_delay, - const util::TrafficPrioritizedLevel & traffic_prioritized_level); + util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level); - bool isOcclusionCleared( + OcclusionType getOcclusionStatus( const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const std::vector & - parked_attention_objects, + const std::vector & lane_divisions, + const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, const double occlusion_dist_thr); /* diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index bb54d829cc477..698242da67528 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -94,8 +94,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR planner_param_.attention_area_length, planner_param_.occlusion_attention_area_length, planner_param_.consider_wrong_direction_vehicle); } - intersection_lanelets_.value().update(false, interpolated_path_info); - const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area(); + auto & intersection_lanelets = intersection_lanelets_.value(); + const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + intersection_lanelets.update(false, interpolated_path_info, local_footprint); + const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area(); if (!first_conflicting_area) { return false; } diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 2f3fcc6f6fde0..42412bbccd424 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -220,12 +220,57 @@ static std::optional getStopLineIndexFromMap( return stop_idx_ip_opt.get(); } +static std::optional getFirstPointInsidePolygonByFootprint( + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + + const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); + for (auto i = lane_start; i <= lane_end; ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, area_2d)) { + return std::make_optional(i); + } + } + return std::nullopt; +} + +static std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + + for (size_t i = lane_start; i <= lane_end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (size_t j = 0; j < polygons.size(); ++j) { + const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); + const bool is_in_polygon = bg::intersects(area_2d, path_footprint); + if (is_in_polygon) { + return std::make_optional>(i, j); + } + } + } + return std::nullopt; +} + std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double peeking_offset, + const double stop_line_margin, const double max_accel, const double max_jerk, + const double delay_response_time, const double peeking_offset, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) { const auto & path_ip = interpolated_path_info.path; @@ -236,6 +281,7 @@ std::optional generateIntersectionStopLines( const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); + /* // find the index of the first point that intersects with detection_areas const auto first_inside_detection_idx_ip_opt = getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_detection_area); @@ -244,6 +290,16 @@ std::optional generateIntersectionStopLines( return std::nullopt; } const auto first_inside_detection_ip = first_inside_detection_idx_ip_opt.value(); + */ + // find the index of the first point whose vehicle footprint on it intersects with detection_area + const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); + std::optional first_footprint_inside_detection_ip_opt = + getFirstPointInsidePolygonByFootprint( + first_detection_area, interpolated_path_info, local_footprint); + if (!first_footprint_inside_detection_ip_opt) { + return std::nullopt; + } + const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); // (1) default stop line position on interpolated path bool default_stop_line_valid = true; @@ -254,8 +310,7 @@ std::optional generateIntersectionStopLines( stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; } if (stop_idx_ip_int < 0) { - stop_idx_ip_int = static_cast(first_inside_detection_ip) - stop_line_margin_idx_dist - - base2front_idx_dist; + stop_idx_ip_int = first_footprint_inside_detection_ip - stop_line_margin_idx_dist; } if (stop_idx_ip_int < 0) { default_stop_line_valid = false; @@ -272,8 +327,6 @@ std::optional generateIntersectionStopLines( const auto closest_idx_ip = closest_idx_ip_opt.value(); // (3) occlusion peeking stop line position on interpolated path - const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); - const auto area_2d = lanelet::utils::to2D(first_detection_area).basicPolygon(); int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); bool occlusion_peeking_line_valid = true; { @@ -281,20 +334,13 @@ std::optional generateIntersectionStopLines( const auto & base_pose0 = path_ip.points.at(default_stop_line_ip).point.pose; const auto path_footprint0 = tier4_autoware_utils::transformVector( local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); - if (bg::intersects(path_footprint0, area_2d)) { + if (bg::intersects( + path_footprint0, lanelet::utils::to2D(first_detection_area).basicPolygon())) { occlusion_peeking_line_valid = false; } } if (occlusion_peeking_line_valid) { - for (size_t i = default_stop_line_ip + 1; i <= std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, area_2d)) { - occlusion_peeking_line_ip_int = i; - break; - } - } + occlusion_peeking_line_ip_int = first_footprint_inside_detection_ip; } const auto first_attention_stop_line_ip = static_cast(occlusion_peeking_line_ip_int); const bool first_attention_stop_line_valid = occlusion_peeking_line_valid; @@ -305,13 +351,10 @@ std::optional generateIntersectionStopLines( // (4) pass judge line position on interpolated path const double velocity = planner_data->current_velocity->twist.linear.x; const double acceleration = planner_data->current_acceleration->accel.accel.linear.x; - const double max_stop_acceleration = planner_data->max_stop_acceleration_threshold; - const double max_stop_jerk = planner_data->max_stop_jerk_threshold; - const double delay_response_time = planner_data->delay_response_time; const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( - velocity, acceleration, max_stop_acceleration, max_stop_jerk, delay_response_time); - int pass_judge_ip_int = static_cast(first_inside_detection_ip) - base2front_idx_dist - - std::ceil(braking_dist / ds); + velocity, acceleration, max_accel, max_jerk, delay_response_time); + int pass_judge_ip_int = + static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); const auto pass_judge_line_ip = static_cast( std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); @@ -321,18 +364,18 @@ std::optional generateIntersectionStopLines( if (use_stuck_stopline) { // NOTE: when ego vehicle is approaching detection area and already passed // first_conflicting_area, this could be null. - const auto stuck_stop_line_idx_ip_opt = - getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_conflicting_area); + const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_area, interpolated_path_info, local_footprint); if (!stuck_stop_line_idx_ip_opt) { stuck_stop_line_valid = false; stuck_stop_line_ip_int = 0; } else { - stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); + stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value() - stop_line_margin_idx_dist; } } else { - stuck_stop_line_ip_int = std::get<0>(lane_interval_ip); + stuck_stop_line_ip_int = + std::get<0>(lane_interval_ip) - (stop_line_margin_idx_dist + base2front_idx_dist); } - stuck_stop_line_ip_int -= (stop_line_margin_idx_dist + base2front_idx_dist); if (stuck_stop_line_ip_int < 0) { stuck_stop_line_valid = false; } @@ -430,6 +473,9 @@ std::optional getFirstPointInsidePolygon( if (is_in_lanelet) { return std::make_optional(i); } + if (i == 0) { + break; + } } } return std::nullopt; @@ -472,6 +518,9 @@ getFirstPointInsidePolygons( if (is_in_lanelet) { break; } + if (i == 0) { + break; + } } } return std::nullopt; @@ -519,6 +568,110 @@ static std::vector getPolygon3dFromLanelets( return polys; } +static std::string getTurnDirection(lanelet::ConstLanelet lane) +{ + return lane.attributeOr("turn_direction", "else"); +} + +/** + * @param pair lanelets and the vector of original lanelets in topological order (not reversed as + *in generateDetectionLaneDivisions()) + **/ +static std::pair> +mergeLaneletsByTopologicalSort( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr) +{ + const int n_node = lanelets.size(); + std::vector> adjacency(n_node); + for (int dst = 0; dst < n_node; ++dst) { + adjacency[dst].resize(n_node); + for (int src = 0; src < n_node; ++src) { + adjacency[dst][src] = false; + } + } + std::set lanelet_ids; + std::unordered_map id2ind; + std::unordered_map ind2id; + std::unordered_map id2lanelet; + int ind = 0; + for (const auto & lanelet : lanelets) { + lanelet_ids.insert(lanelet.id()); + const auto id = lanelet.id(); + id2ind[id] = ind; + ind2id[ind] = id; + id2lanelet[id] = lanelet; + ind++; + } + // NOTE: this function aims to traverse the detection lanelet backward from ego side to farthest + // side, so if lane B follows lane A on the routing_graph, adj[B][A] = true + for (const auto & lanelet : lanelets) { + const auto & followings = routing_graph_ptr->following(lanelet); + const int dst = lanelet.id(); + for (const auto & following : followings) { + if (const int src = following.id(); lanelet_ids.find(src) != lanelet_ids.end()) { + adjacency[(id2ind[src])][(id2ind[dst])] = true; + } + } + } + // terminal node + std::map> branches; + auto has_no_previous = [&](const int node) { + for (int dst = 0; dst < n_node; dst++) { + if (adjacency[dst][node]) { + return false; + } + } + return true; + }; + for (int src = 0; src < n_node; src++) { + if (!has_no_previous(src)) { + continue; + } + // So `src` has no previous lanelets + branches[(ind2id[src])] = std::vector{}; + auto & branch = branches[(ind2id[src])]; + lanelet::Id node_iter = ind2id[src]; + std::set visited_ids; + while (true) { + const auto & destinations = adjacency[(id2ind[node_iter])]; + // NOTE: assuming detection lanelets have only one "previous"(on the routing_graph) lanelet + const auto next = std::find(destinations.begin(), destinations.end(), true); + if (next == destinations.end()) { + branch.push_back(node_iter); + break; + } + if (visited_ids.find(node_iter) != visited_ids.end()) { + // loop detected + break; + } + branch.push_back(node_iter); + visited_ids.insert(node_iter); + node_iter = ind2id[std::distance(destinations.begin(), next)]; + } + } + for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { + auto & branch = it->second; + std::reverse(branch.begin(), branch.end()); + } + lanelet::ConstLanelets merged; + std::vector originals; + for (const auto & [id, sub_ids] : branches) { + if (sub_ids.size() == 0) { + continue; + } + lanelet::ConstLanelets merge; + originals.push_back(lanelet::ConstLanelets({})); + auto & original = originals.back(); + for (const auto sub_id : sub_ids) { + merge.push_back(id2lanelet[sub_id]); + original.push_back(id2lanelet[sub_id]); + } + merged.push_back(lanelet::utils::combineLaneletsShape(merge)); + } + return {merged, originals}; +} + IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, @@ -656,14 +809,52 @@ IntersectionLanelets getObjectiveLanelets( } } } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; + for (const auto & ll : occlusion_detection_and_preceding_lanelets) { + const auto turn_direction = getTurnDirection(ll); + if (turn_direction == "left" || turn_direction == "right") { + continue; + } + occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); + } + + auto [attention_lanelets, original_attention_lanelet_sequences] = + mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); IntersectionLanelets result; - result.attention_ = std::move(detection_and_preceding_lanelets); + result.attention_ = std::move(attention_lanelets); + for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { + // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that + // back() exists. + std::optional stop_line{std::nullopt}; + for (auto it = original_attention_lanelet_seq.rbegin(); + it != original_attention_lanelet_seq.rend(); ++it) { + const auto traffic_lights = it->regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stop_line_opt = traffic_light->stopLine(); + if (!stop_line_opt) continue; + stop_line = stop_line_opt.get(); + break; + } + if (stop_line) break; + } + result.attention_stop_lines_.push_back(stop_line); + } result.attention_non_preceding_ = std::move(detection_lanelets); + // TODO(Mamoru Sobue): find stop lines for attention_non_preceding_ if needed + for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { + result.attention_non_preceding_stop_lines_.push_back(std::nullopt); + } result.conflicting_ = std::move(conflicting_ex_ego_lanelets); result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids); - result.occlusion_attention_ = std::move(occlusion_detection_and_preceding_lanelets); - // compoundPolygon3d + // NOTE: occlusion_attention is not inverted here + // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and + // then trim part of them based on curvature threshold + result.occlusion_attention_ = + std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); + + // NOTE: to properly update(), each element in conflicting_/conflicting_area_, + // attention_non_preceding_/attention_non_preceding_area_ need to be matched result.attention_area_ = getPolygon3dFromLanelets(result.attention_); result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); @@ -672,11 +863,6 @@ IntersectionLanelets getObjectiveLanelets( return result; } -static std::string getTurnDirection(lanelet::ConstLanelet lane) -{ - return lane.attributeOr("turn_direction", "else"); -} - bool isOverTargetIndex( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, const geometry_msgs::msg::Pose & current_pose, const int target_idx) @@ -842,21 +1028,17 @@ double getHighestCurvature(const lanelet::ConstLineString3d & centerline) return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); } -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets_all, const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, const double curvature_threshold, const double curvature_calculation_ds) { using lanelet::utils::getCenterlineWithOffset; - using lanelet::utils::to2D; // (0) remove left/right lanelet lanelet::ConstLanelets detection_lanelets; for (const auto & detection_lanelet : detection_lanelets_all) { - const auto turn_direction = getTurnDirection(detection_lanelet); - if (turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0) { - continue; - } + // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet const auto fine_centerline = lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); const double highest_curvature = getHighestCurvature(fine_centerline); @@ -867,111 +1049,34 @@ std::vector generateDetectionLaneDivisions( } // (1) tsort detection_lanelets - // generate adjacency matrix - // if lanelet1 => lanelet2; then adjacency[lanelet2][lanelet1] = true - const int n_node = detection_lanelets.size(); - std::vector> adjacency(n_node); - for (int dst = 0; dst < n_node; ++dst) { - adjacency[dst].resize(n_node); - for (int src = 0; src < n_node; ++src) { - adjacency[dst][src] = false; - } - } - std::set detection_lanelet_ids; - std::unordered_map id2ind, ind2id; - std::unordered_map id2lanelet; - int ind = 0; - for (const auto & detection_lanelet : detection_lanelets) { - detection_lanelet_ids.insert(detection_lanelet.id()); - const int id = detection_lanelet.id(); - id2ind[id] = ind; - ind2id[ind] = id; - id2lanelet[id] = detection_lanelet; - ind++; - } - for (const auto & detection_lanelet : detection_lanelets) { - const auto & followings = routing_graph_ptr->following(detection_lanelet); - const int dst = detection_lanelet.id(); - for (const auto & following : followings) { - if (const int src = following.id(); - detection_lanelet_ids.find(src) != detection_lanelet_ids.end()) { - adjacency[(id2ind[src])][(id2ind[dst])] = true; - } - } - } - // terminal node - std::map> branches; - auto has_no_previous = [&](const int node) { - for (int dst = 0; dst < n_node; dst++) { - if (adjacency[dst][node]) { - return false; - } - } - return true; - }; - for (int src = 0; src < n_node; src++) { - if (!has_no_previous(src)) { - continue; - } - branches[(ind2id[src])] = std::vector{}; - auto & branch = branches[(ind2id[src])]; - int node_iter = ind2id[src]; - while (true) { - const auto & destinations = adjacency[(id2ind[node_iter])]; - // NOTE: assuming detection lanelets have only one previous lanelet - const auto next = std::find(destinations.begin(), destinations.end(), true); - if (next == destinations.end()) { - branch.push_back(node_iter); - break; - } - branch.push_back(node_iter); - node_iter = ind2id[std::distance(destinations.begin(), next)]; - } - } - for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { - auto & branch = it->second; - std::reverse(branch.begin(), branch.end()); - } + const auto [merged_detection_lanelets, originals] = + mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); // (2) merge each branch to one lanelet // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here - std::unordered_map> merged_branches; - for (const auto & [src, branch] : branches) { - lanelet::Points3d lefts; - lanelet::Points3d rights; + std::vector> merged_lanelet_with_area; + for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { + const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); + const auto & original = originals.at(i); double area = 0; - for (const auto & lane_id : branch) { - const auto lane = id2lanelet[lane_id]; - for (const auto & left_point : lane.leftBound()) { - lefts.push_back(lanelet::Point3d(left_point)); - } - for (const auto & right_point : lane.rightBound()) { - rights.push_back(lanelet::Point3d(right_point)); - } - area += bg::area(lane.polygon2d().basicPolygon()); + for (const auto & partition : original) { + area += bg::area(partition.polygon2d().basicPolygon()); } - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, lefts).invert(); - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, rights).invert(); - lanelet::Lanelet merged = lanelet::Lanelet(lanelet::InvalId, left, right); - merged_branches[src] = std::make_pair(merged, area); + merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); } // (3) discretize each merged lanelet - std::vector detection_divisions; - for (const auto & [last_lane_id, branch] : merged_branches) { - DiscretizedLane detection_division; - detection_division.lane_id = last_lane_id; - const auto detection_lanelet = branch.first; - const double area = branch.second; - const double length = bg::length(detection_lanelet.centerline()); + std::vector detection_divisions; + for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { + const double length = bg::length(merged_lanelet.centerline()); const double width = area / length; - auto & divisions = detection_division.divisions; for (int i = 0; i < static_cast(width / resolution); ++i) { const double offset = resolution * i - width / 2; - divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, offset, resolution))); + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); } - divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, width / 2, resolution))); - detection_divisions.push_back(detection_division); + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); } return detection_divisions; } @@ -1057,6 +1162,45 @@ bool checkStuckVehicleInIntersection( return false; } +bool checkYieldStuckVehicleInIntersection( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, + const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, DebugData * debug_data) +{ + const auto first_attention_area_2d = lanelet::utils::to2D(first_attention_area); + Polygon2d first_attention_area_poly; + for (const auto & p : first_attention_area_2d) { + first_attention_area_poly.outer().emplace_back(p.x(), p.y()); + } + + for (const auto & object : objects_ptr->objects) { + if (!isTargetStuckVehicleType(object)) { + continue; // not target vehicle type + } + const auto obj_v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (obj_v_norm > stuck_vehicle_vel_thr) { + continue; // not stop vehicle + } + + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + + // check if the object is too close to the ego path + if (yield_stuck_distance_thr < bg::distance(ego_poly, obj_footprint)) { + continue; + } + + // check if the footprint is in the stuck detect area + const bool is_in_stuck_area = bg::within(obj_footprint, first_attention_area_poly); + if (is_in_stuck_area && debug_data) { + debug_data->yield_stuck_targets.objects.push_back(object); + return true; + } + } + return false; +} + Polygon2d generateStuckVehicleDetectAreaPolygon( const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist) { @@ -1096,13 +1240,13 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( return polygon; } -bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, - const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, - const bool consider_wrong_direction_vehicle, const double dist_margin, - const double parked_vehicle_speed_threshold) +std::optional checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, + const double dist_margin, const bool is_parked_vehicle) { - for (const auto & ll : target_lanelets) { + for (unsigned i = 0; i < target_lanelets.size(); ++i) { + const auto & ll = target_lanelets.at(i); if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { continue; } @@ -1111,39 +1255,38 @@ bool checkAngleForTargetLanelets( const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); if (consider_wrong_direction_vehicle) { if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { - return true; + return std::make_optional(i); } } else { if (std::fabs(angle_diff) < detection_area_angle_thr) { - return true; + return std::make_optional(i); } // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is // positive if ( - std::fabs(longitudinal_velocity) < parked_vehicle_speed_threshold && - (std::fabs(angle_diff) < detection_area_angle_thr || - (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { - return true; + is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return std::make_optional(i); } } } - return false; + return std::nullopt; } void cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, - const rclcpp::Clock::SharedPtr clock, const double time_thr) + util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, const double time_thr) { const rclcpp::Time current_time = clock->now(); - for (auto & object : objects_ptr->objects) { // each objects - for (auto & predicted_path : object.kinematics.predicted_paths) { // each predicted paths + for (auto & target_object : target_objects->all_attention_objects) { // each objects + for (auto & predicted_path : + target_object.object.kinematics.predicted_paths) { // each predicted paths const auto origin_path = predicted_path; predicted_path.path.clear(); for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points const auto & predicted_pose = origin_path.path.at(k); const auto predicted_time = - rclcpp::Time(objects_ptr->header.stamp) + + rclcpp::Time(target_objects->header.stamp) + rclcpp::Duration(origin_path.time_step) * static_cast(k); if ((predicted_time - current_time).seconds() < time_thr) { predicted_path.path.push_back(predicted_pose); @@ -1191,6 +1334,19 @@ TimeDistanceArray calcIntersectionPassingTime( dist_sum = 0.0; double passing_time = time_delay; time_distance_array.emplace_back(passing_time, dist_sum); + + // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so + // `last_intersection_stop_line_candidate_idx` makes no sense + const auto last_intersection_stop_line_candidate_point_orig = + path.points.at(last_intersection_stop_line_candidate_idx).point.pose; + const auto last_intersection_stop_line_candidate_nearest_ind_opt = motion_utils::findNearestIndex( + smoothed_reference_path.points, last_intersection_stop_line_candidate_point_orig, 3.0, M_PI_4); + if (!last_intersection_stop_line_candidate_nearest_ind_opt) { + return time_distance_array; + } + const auto last_intersection_stop_line_candidate_nearest_ind = + last_intersection_stop_line_candidate_nearest_ind_opt.value(); + for (size_t i = 1; i < smoothed_reference_path.points.size(); ++i) { const auto & p1 = smoothed_reference_path.points.at(i - 1); const auto & p2 = smoothed_reference_path.points.at(i); @@ -1202,7 +1358,7 @@ TimeDistanceArray calcIntersectionPassingTime( const double average_velocity = (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; const double minimum_ego_velocity_division = - (use_upstream_velocity && i > last_intersection_stop_line_candidate_idx) + (use_upstream_velocity && i > last_intersection_stop_line_candidate_nearest_ind) ? minimum_upstream_velocity /* to avoid null division */ : minimum_ego_velocity; const double passing_velocity = @@ -1243,22 +1399,25 @@ double calcDistanceUntilIntersectionLanelet( } void IntersectionLanelets::update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info) + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) { is_prioritized_ = is_prioritized; // find the first conflicting/detection area polygon intersecting the path - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - { - auto first = getFirstPointInsidePolygons(path, lane_interval, conflicting_area_); - if (first && !first_conflicting_area_) { - first_conflicting_area_ = first.value().second; + if (!first_conflicting_area_) { + auto first = + getFirstPointInsidePolygonsByFootprint(conflicting_area_, interpolated_path_info, footprint); + if (first) { + first_conflicting_lane_ = conflicting_.at(first.value().second); + first_conflicting_area_ = conflicting_area_.at(first.value().second); } } - { - auto first = getFirstPointInsidePolygons(path, lane_interval, attention_area_); - if (first && !first_attention_area_) { - first_attention_area_ = first.value().second; + if (!first_attention_area_) { + auto first = getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_area_, interpolated_path_info, footprint); + if (first) { + first_attention_lane_ = attention_non_preceding_.at(first.value().second); + first_attention_area_ = attention_non_preceding_area_.at(first.value().second); } } } @@ -1344,5 +1503,23 @@ std::optional generatePathLanelets( return path_lanelets; } +void TargetObject::calc_dist_to_stop_line() +{ + if (!attention_lanelet || !stop_line) { + return; + } + const auto attention_lanelet_val = attention_lanelet.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); + const auto stop_line_val = stop_line.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stop_line_val.front().x() + stop_line_val.back().x()) / 2.0; + stopline_center.position.y = (stop_line_val.front().y() + stop_line_val.back().y()) / 2.0; + stopline_center.position.z = (stop_line_val.front().z() + stop_line_val.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); + dist_to_stop_line = (stopline_arc_coords.length - object_arc_coords.length); +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index a75545353fb7a..5faacd4325b06 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -70,7 +70,8 @@ std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double peeking_offset, + const double stop_line_margin, const double max_accel, const double max_jerk, + const double delay_response_time, const double peeking_offset, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); std::optional getFirstPointInsidePolygon( @@ -107,7 +108,7 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); TrafficPrioritizedLevel getTrafficPrioritizedLevel( lanelet::ConstLanelet lane, const std::map & tl_infos); -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, const double curvature_threshold, const double curvature_calculation_ds); @@ -125,18 +126,23 @@ bool checkStuckVehicleInIntersection( const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, DebugData * debug_data); +bool checkYieldStuckVehicleInIntersection( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, + const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, + DebugData * debug_data); + Polygon2d generateStuckVehicleDetectAreaPolygon( const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); -bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, - const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, - const bool consider_wrong_direction_vehicle, const double dist_margin, - const double parked_vehicle_speed_threshold); +std::optional checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, + const double dist_margin, const bool is_parked_vehicle); void cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, - const rclcpp::Clock::SharedPtr clock, const double time_thr); + util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, + const double time_thr); TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index aed81d771e480..fdcf05a97a7a9 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -15,6 +15,8 @@ #ifndef UTIL_TYPE_HPP_ #define UTIL_TYPE_HPP_ +#include + #include #include #include @@ -40,19 +42,21 @@ struct DebugData std::optional pass_judge_wall_pose{std::nullopt}; std::optional> attention_area{std::nullopt}; std::optional> occlusion_attention_area{std::nullopt}; - std::optional intersection_area{std::nullopt}; std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; std::optional candidate_collision_ego_lane_polygon{std::nullopt}; std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; }; struct InterpolatedPathInfo @@ -67,11 +71,17 @@ struct InterpolatedPathInfo struct IntersectionLanelets { public: - void update(const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info); + void update( + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint); const lanelet::ConstLanelets & attention() const { return is_prioritized_ ? attention_non_preceding_ : attention_; } + const std::vector> & attention_stop_lines() const + { + return is_prioritized_ ? attention_non_preceding_stop_lines_ : attention_stop_lines_; + } const lanelet::ConstLanelets & conflicting() const { return conflicting_; } const lanelet::ConstLanelets & adjacent() const { return adjacent_; } const lanelet::ConstLanelets & occlusion_attention() const @@ -91,38 +101,48 @@ struct IntersectionLanelets { return occlusion_attention_area_; } + const std::optional & first_conflicting_lane() const + { + return first_conflicting_lane_; + } const std::optional & first_conflicting_area() const { return first_conflicting_area_; } + const std::optional & first_attention_lane() const + { + return first_attention_lane_; + } const std::optional & first_attention_area() const { return first_attention_area_; } - lanelet::ConstLanelets attention_; + lanelet::ConstLanelets attention_; // topologically merged lanelets + std::vector> + attention_stop_lines_; // the stop lines for each attention_ lanelets lanelet::ConstLanelets attention_non_preceding_; + std::vector> + attention_non_preceding_stop_lines_; // the stop lines for each attention_non_preceding_ + // lanelets lanelet::ConstLanelets conflicting_; lanelet::ConstLanelets adjacent_; - lanelet::ConstLanelets occlusion_attention_; // for occlusion detection - std::vector attention_area_; + lanelet::ConstLanelets occlusion_attention_; // topologically merged lanelets + std::vector occlusion_attention_size_; // the area() of each occlusion attention lanelets + std::vector attention_area_; // topologically merged lanelets std::vector attention_non_preceding_area_; std::vector conflicting_area_; std::vector adjacent_area_; - std::vector occlusion_attention_area_; + std::vector + occlusion_attention_area_; // topologically merged lanelets // the first area intersecting with the path // even if lane change/re-routing happened on the intersection, these areas area are supposed to // be invariant under the 'associative' lanes. + std::optional first_conflicting_lane_{std::nullopt}; + std::optional first_conflicting_area_{std::nullopt}; + std::optional first_attention_lane_{std::nullopt}; + std::optional first_attention_area_{std::nullopt}; bool is_prioritized_ = false; - std::optional first_conflicting_area_ = std::nullopt; - std::optional first_attention_area_ = std::nullopt; -}; - -struct DiscretizedLane -{ - int lane_id{0}; - // discrete fine lines from left to right - std::vector divisions{}; }; struct IntersectionStopLines @@ -156,6 +176,24 @@ struct PathLanelets // conflicting lanelets plus the next lane part of the path }; +struct TargetObject +{ + autoware_auto_perception_msgs::msg::PredictedObject object; + std::optional attention_lanelet{std::nullopt}; + std::optional stop_line{std::nullopt}; + std::optional dist_to_stop_line{std::nullopt}; + void calc_dist_to_stop_line(); +}; + +struct TargetObjects +{ + std_msgs::msg::Header header; + std::vector attention_objects; + std::vector parked_attention_objects; + std::vector intersection_area_objects; + std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy +}; + enum class TrafficPrioritizedLevel { // The target lane's traffic signal is red or the ego's traffic signal has an arrow. FULLY_PRIORITIZED = 0, diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/behavior_velocity_run_out_module/README.md index ee762653245c6..a36cfdf6485c6 100644 --- a/planning/behavior_velocity_run_out_module/README.md +++ b/planning/behavior_velocity_run_out_module/README.md @@ -217,6 +217,11 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab | `max_jerk` | double | [m/s^3] minimum jerk deceleration for safe brake. | | `max_acc` | double | [m/s^2] minimum accel deceleration for safe brake. | +| Parameter /ignore_momentary_detection | Type | Description | +| ------------------------------------- | ------ | ----------------------------------------------------------------- | +| `enable` | bool | [-] whether to ignore momentary detection | +| `time_threshold` | double | [sec] ignores detections that persist for less than this duration | + ### Future extensions / Unimplemented parts - Calculate obstacle's min velocity and max velocity from covariance diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml index f9668549f2fb2..2641214ac5ad4 100644 --- a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml +++ b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml @@ -48,3 +48,8 @@ enable: true max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. + + # prevent abrupt stops caused by false positives in perception + ignore_momentary_detection: + enable: true + time_threshold: 0.15 # [sec] ignores detections that persist for less than this duration diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index b262bf00cb57a..a5253f59b60f9 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -119,6 +119,13 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) p.max_acc = getOrDeclareParameter(node, ns_m + ".max_acc"); } + { + auto & p = planner_param_.ignore_momentary_detection; + const std::string ns_param = ns + ".ignore_momentary_detection"; + p.enable = getOrDeclareParameter(node, ns_param + ".enable"); + p.time_threshold = getOrDeclareParameter(node, ns_param + ".time_threshold"); + } + debug_ptr_ = std::make_shared(node); setDynamicObstacleCreator(node, debug_ptr_); } diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index b91d1d1fe1ae7..54902f52c47da 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -137,7 +137,7 @@ bool RunOutModule::modifyPathVelocity( } boost::optional RunOutModule::detectCollision( - const std::vector & dynamic_obstacles, const PathWithLaneId & path) const + const std::vector & dynamic_obstacles, const PathWithLaneId & path) { if (path.points.size() < 2) { RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points."); @@ -181,6 +181,11 @@ boost::optional RunOutModule::detectCollision( continue; } + // ignore momentary obstacle detection to avoid sudden stopping by false positive + if (isMomentaryDetection()) { + return {}; + } + debug_ptr_->pushCollisionPoints(obstacle_selected->collision_points); debug_ptr_->pushNearestCollisionPoint(obstacle_selected->nearest_collision_point); @@ -188,6 +193,7 @@ boost::optional RunOutModule::detectCollision( } // no collision detected + first_detected_time_.reset(); return {}; } @@ -812,4 +818,21 @@ void RunOutModule::publishDebugValue( debug_ptr_->publishDebugValue(); } +bool RunOutModule::isMomentaryDetection() +{ + if (!planner_param_.ignore_momentary_detection.enable) { + return false; + } + + if (!first_detected_time_) { + first_detected_time_ = std::make_shared(clock_->now()); + return true; + } + + const auto now = clock_->now(); + const double elapsed_time_since_detection = (now - *first_detected_time_).seconds(); + RCLCPP_DEBUG_STREAM(logger_, "elapsed_time_since_detection: " << elapsed_time_since_detection); + + return elapsed_time_since_detection < planner_param_.ignore_momentary_detection.time_threshold; +} } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index 20bd8ffc799ff..925d8ea8b234c 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -62,13 +62,13 @@ class RunOutModule : public SceneModuleInterface std::unique_ptr dynamic_obstacle_creator_; std::shared_ptr debug_ptr_; std::unique_ptr state_machine_; + std::shared_ptr first_detected_time_; // Function Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const; boost::optional detectCollision( - const std::vector & dynamic_obstacles, - const PathWithLaneId & path_points) const; + const std::vector & dynamic_obstacles, const PathWithLaneId & path_points); float calcCollisionPositionOfVehicleSide( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const; @@ -141,6 +141,8 @@ class RunOutModule : public SceneModuleInterface const PathWithLaneId & path, const std::vector extracted_obstacles, const boost::optional & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose) const; + + bool isMomentaryDetection(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 8fc3d48de4858..b25a8fc94bff3 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -126,6 +126,12 @@ struct DynamicObstacleParam float points_interval; // [m] }; +struct IgnoreMomentaryDetection +{ + bool enable; + double time_threshold; +}; + struct PlannerParam { CommonParam common; @@ -138,6 +144,7 @@ struct PlannerParam DynamicObstacleParam dynamic_obstacle; SlowDownLimit slow_down_limit; Smoother smoother; + IgnoreMomentaryDetection ignore_momentary_detection; }; enum class DetectionMethod { diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp index 8bf46c697104a..2f8babf103877 100644 --- a/planning/obstacle_avoidance_planner/src/debug_marker.cpp +++ b/planning/obstacle_avoidance_planner/src/debug_marker.cpp @@ -47,21 +47,22 @@ MarkerArray getFootprintsMarkerArray( const auto & traj_point = mpt_traj.at(i); - const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; + const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) .position); marker.points.push_back(marker.points.front()); @@ -71,8 +72,8 @@ MarkerArray getFootprintsMarkerArray( } MarkerArray getBoundsWidthMarkerArray( - const std::vector & ref_points, const double vehicle_width, - const size_t sampling_num) + const std::vector & ref_points, + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num) { const auto current_time = rclcpp::Clock().now(); MarkerArray marker_array; @@ -105,8 +106,10 @@ MarkerArray getBoundsWidthMarkerArray( } const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx); + const double base_to_right = + (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double lb_y = - ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - vehicle_width / 2.0; + ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - base_to_right; const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(pose.position); @@ -125,8 +128,10 @@ MarkerArray getBoundsWidthMarkerArray( } const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx); + const double base_to_left = + (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double ub_y = - ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + vehicle_width / 2.0; + ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + base_to_left; const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(pose.position); @@ -140,7 +145,8 @@ MarkerArray getBoundsWidthMarkerArray( } MarkerArray getBoundsLineMarkerArray( - const std::vector & ref_points, const double vehicle_width) + const std::vector & ref_points, + const vehicle_info_util::VehicleInfo & vehicle_info) { MarkerArray marker_array; @@ -158,12 +164,13 @@ MarkerArray getBoundsLineMarkerArray( for (size_t i = 0; i < ref_points.size(); i++) { const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose; - - const double ub_y = ref_points.at(i).bounds.upper_bound + vehicle_width / 2.0; + const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; + const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; + const double ub_y = ref_points.at(i).bounds.upper_bound + base_to_left; const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(ub); - const double lb_y = ref_points.at(i).bounds.lower_bound - vehicle_width / 2.0; + const double lb_y = ref_points.at(i).bounds.lower_bound - base_to_right; const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(lb); } @@ -175,8 +182,9 @@ MarkerArray getBoundsLineMarkerArray( MarkerArray getVehicleCircleLinesMarkerArray( const std::vector & ref_points, - const std::vector & vehicle_circle_longitudinal_offsets, const double vehicle_width, - const size_t sampling_num, const std::string & ns) + const std::vector & vehicle_circle_longitudinal_offsets, + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num, + const std::string & ns) { const auto current_time = rclcpp::Clock().now(); MarkerArray msg; @@ -206,11 +214,13 @@ MarkerArray getVehicleCircleLinesMarkerArray( auto base_pose = tier4_autoware_utils::calcOffsetPose(pose_with_deviation, d, 0.0, 0.0); base_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha); - + const double base_to_right = + (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; + const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const auto ub = - tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, vehicle_width / 2.0, 0.0).position; + tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, base_to_left, 0.0).position; const auto lb = - tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, -vehicle_width / 2.0, 0.0).position; + tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, -base_to_right, 0.0).position; marker.points.push_back(ub); marker.points.push_back(lb); @@ -368,13 +378,12 @@ MarkerArray getDebugMarker( MarkerArray marker_array; // bounds line - appendMarkerArray( - getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info.vehicle_width_m), &marker_array); + appendMarkerArray(getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info), &marker_array); // bounds width appendMarkerArray( getBoundsWidthMarkerArray( - debug_data.ref_points, vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num), + debug_data.ref_points, vehicle_info, debug_data.mpt_visualize_sampling_num), &marker_array); // current vehicle circles @@ -404,9 +413,8 @@ MarkerArray getDebugMarker( // vehicle circle line appendMarkerArray( getVehicleCircleLinesMarkerArray( - debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, - vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num, - "vehicle_circle_lines"), + debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, vehicle_info, + debug_data.mpt_visualize_sampling_num, "vehicle_circle_lines"), &marker_array); // footprint by drivable area diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 359485fadcfff..765136b3cf6f6 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -36,9 +36,11 @@ std::tuple, std::vector> calcVehicleCirclesByUniform const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num, const double radius_ratio) { + const double lateral_offset = + abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; const double radius = std::hypot( vehicle_info.vehicle_length_m / static_cast(circle_num) / 2.0, - vehicle_info.vehicle_width_m / 2.0) * + vehicle_info.vehicle_width_m / 2.0 + lateral_offset) * radius_ratio; const std::vector radiuses(circle_num, radius); @@ -59,16 +61,18 @@ std::tuple, std::vector> calcVehicleCirclesByBicycle if (circle_num < 2) { throw std::invalid_argument("circle_num is less than 2."); } - + const double lateral_offset = + abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; // 1st circle (rear wheel) - const double rear_radius = vehicle_info.vehicle_width_m / 2.0 * rear_radius_ratio; + const double rear_radius = + vehicle_info.vehicle_width_m / 2.0 + lateral_offset * rear_radius_ratio; const double rear_lon_offset = 0.0; // 2nd circle (front wheel) const double front_radius = std::hypot( vehicle_info.vehicle_length_m / static_cast(circle_num) / 2.0, - vehicle_info.vehicle_width_m / 2.0) * + vehicle_info.vehicle_width_m / 2.0 + lateral_offset) * front_radius_ratio; const double unit_lon_length = vehicle_info.vehicle_length_m / static_cast(circle_num); @@ -84,8 +88,9 @@ std::tuple, std::vector> calcVehicleCirclesByFitting if (circle_num < 2) { throw std::invalid_argument("circle_num is less than 2."); } - - const double radius = vehicle_info.vehicle_width_m / 2.0; + const double lateral_offset = + abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; + const double radius = vehicle_info.vehicle_width_m / 2.0 + lateral_offset; std::vector radiuses(circle_num, radius); const double unit_lon_length = diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index b33342b9dea4f..c0f57489078d6 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -427,9 +427,12 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject( obj_vel_norm = std::hypot( obj.kinematics.initial_twist_with_covariance.twist.linear.x, obj.kinematics.initial_twist_with_covariance.twist.linear.y); - obj_vel_yaw = std::atan2( - obj.kinematics.initial_twist_with_covariance.twist.linear.y, - obj.kinematics.initial_twist_with_covariance.twist.linear.x); + + const double obj_yaw = + tf2::getYaw(obj.kinematics.initial_pose_with_covariance.pose.orientation); + obj_vel_yaw = obj_yaw + std::atan2( + obj.kinematics.initial_twist_with_covariance.twist.linear.y, + obj.kinematics.initial_twist_with_covariance.twist.linear.x); get_obj = true; break; } @@ -451,9 +454,13 @@ void AdaptiveCruiseController::calculateProjectedVelocityFromObject( double obj_vel_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y); - double obj_vel_yaw = std::atan2( - object.kinematics.initial_twist_with_covariance.twist.linear.y, - object.kinematics.initial_twist_with_covariance.twist.linear.x); + + const double obj_yaw = + tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation); + const double obj_vel_yaw = + obj_yaw + std::atan2( + object.kinematics.initial_twist_with_covariance.twist.linear.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x); *velocity = obj_vel_norm * std::cos(tier4_autoware_utils::normalizeRadian(obj_vel_yaw - traj_yaw)); diff --git a/planning/planning_debug_tools/CMakeLists.txt b/planning/planning_debug_tools/CMakeLists.txt index bf7e36c4c0c0a..58f73ca3836c2 100644 --- a/planning/planning_debug_tools/CMakeLists.txt +++ b/planning/planning_debug_tools/CMakeLists.txt @@ -48,6 +48,7 @@ ament_auto_package( ) install(PROGRAMS + scripts/processing_time_checker.py scripts/trajectory_visualizer.py scripts/closest_velocity_checker.py scripts/perception_replayer/perception_reproducer.py diff --git a/planning/planning_debug_tools/README.md b/planning/planning_debug_tools/README.md index fe9615614de81..aa46c0e2fc7ef 100644 --- a/planning/planning_debug_tools/README.md +++ b/planning/planning_debug_tools/README.md @@ -4,6 +4,8 @@ This package contains several planning-related debug tools. - **Trajectory analyzer**: visualizes the information (speed, curvature, yaw, etc) along the trajectory - **Closest velocity checker**: prints the velocity information indicated by each modules +- **Perception reproducer**: generates detected objects from rosbag data in planning simulator environment +- **processing time checker**: displays processing_time of modules on the terminal ## Trajectory analyzer @@ -241,3 +243,24 @@ ros2 run planning_debug_tools perception_replayer.py -b ``` Instead of publishing predicted objects, you can publish detected/tracked objects by designating `-d` or `-t`, respectively. + +## Processing time checker + +The purpose of the Processing Time Subscriber is to monitor and visualize the processing times of various ROS 2 topics in a system. By providing a real-time terminal-based visualization, users can easily confirm the processing time performance as in the picture below. + +![processing_time_checker](image/processing_time_checker.png) + +You can run the program by the following command. + +```bash +ros2 run planning_debug_tools processing_time_checker.py -f -m +``` + +This program subscribes to ROS 2 topics that have a suffix of `processing_time_ms`. + +The program allows users to customize two parameters via command-line arguments: + +- --max_display_time (or -m): This sets the maximum display time in milliseconds. The default value is 150ms. +- --display_frequency (or -f): This sets the frequency at which the terminal UI updates. The default value is 5Hz. + +By adjusting these parameters, users can tailor the display to their specific monitoring needs. diff --git a/planning/planning_debug_tools/image/processing_time_checker.png b/planning/planning_debug_tools/image/processing_time_checker.png new file mode 100644 index 0000000000000..fb064372796ae Binary files /dev/null and b/planning/planning_debug_tools/image/processing_time_checker.png differ diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml index deadcd54fffdd..14db1277cc02a 100644 --- a/planning/planning_debug_tools/package.xml +++ b/planning/planning_debug_tools/package.xml @@ -6,6 +6,7 @@ The planning_debug_tools package Takamasa Horibe Taiki Tanaka + Takayuki Murooka Apache License 2.0 Takamasa Horibe diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index 01079fdc3f8c1..7bf54c0278a27 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -153,6 +153,11 @@ def kill_online_perception_node(self): pass def binary_search(self, data, timestamp): + if data[-1][0] < timestamp: + return data[-1][1] + elif data[0][0] > timestamp: + return data[0][1] + low, high = 0, len(data) - 1 while low <= high: @@ -175,10 +180,4 @@ def find_topics_by_timestamp(self, timestamp): return objects_data, traffic_signals_data def find_ego_odom_by_timestamp(self, timestamp): - ego_odom_data = None - for data in self.rosbag_ego_odom_data: - if timestamp < data[0]: - ego_odom_data = data[1] - break - - return ego_odom_data + return self.binary_search(self.rosbag_ego_odom_data, timestamp) diff --git a/planning/planning_debug_tools/scripts/processing_time_checker.py b/planning/planning_debug_tools/scripts/processing_time_checker.py new file mode 100755 index 0000000000000..fa6bc57fb7ae5 --- /dev/null +++ b/planning/planning_debug_tools/scripts/processing_time_checker.py @@ -0,0 +1,155 @@ +#!/usr/bin/env python3 + +# 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. + + +import argparse +from collections import deque +import os +import sys + +import rclpy +from rclpy.node import Node +from tier4_debug_msgs.msg import Float64Stamped + + +class ProcessingTimeSubscriber(Node): + def __init__( + self, + max_display_time=150, + display_frequency=5.0, + window_size=1, + bar_scale=2, + display_character="|", + ): + super().__init__("processing_time_subscriber") + self.data_map = {} # {topic_name: data_value} + self.data_queue_map = {} # {topic_name: deque} + self.window_size = window_size + self.max_display_time = max_display_time + self.bar_scale = bar_scale + self.display_character = display_character + + # Initialize a set to keep track of subscribed topics + self.subscribed_topics = set() + + # Call get_topic_list immediately and set a timer to call it regularly + self.get_topic_list() + self.create_timer(1.0, self.get_topic_list) # update topic list every 1 second + + # Timer to print data at given frequency + self.create_timer(1.0 / display_frequency, self.print_data) + + def get_topic_list(self): + # Get list of existing topics in the system + topic_list = self.get_topic_names_and_types() + + # Subscribe to topics with 'processing_time_ms' suffix + for topic, types in topic_list: + if topic.endswith("processing_time_ms") and topic not in self.subscribed_topics: + self.create_subscription( + Float64Stamped, topic, lambda msg, topic=topic: self.callback(msg, topic), 1 + ) + self.get_logger().info(f"Subscribed to {topic} | Type: {types}") + self.subscribed_topics.add(topic) + + def callback(self, msg, topic): + # Add the new data to the queue + if topic not in self.data_queue_map: + self.data_queue_map[topic] = deque(maxlen=self.window_size) + self.data_queue_map[topic].append(msg.data) + + # Calculate the average + avg_value = sum(self.data_queue_map[topic]) / len(self.data_queue_map[topic]) + self.data_map[topic] = avg_value + + def print_data(self): + # Clear terminal + os.system("cls" if os.name == "nt" else "clear") + + if not self.data_map: + print("No topics available.") + return + + # Get the maximum topic name length for alignment + max_topic_length = max(map(len, self.data_map.keys())) + + # Generate the header based on the max_display_time + header_str = f"topics (window size = {self.window_size})".ljust(max_topic_length) + ":" + for i in range(0, self.max_display_time + 1, 10): + header_str += f" {i}ms".ljust(self.bar_scale * 10) + + # Print the header + print(header_str) + print("-" * len(header_str)) + + # Print each topic's data + for topic, data in self.data_map.items(): + # Round the data to the third decimal place for display + data_rounded = round(data, 3) + # Calculate the number of bars to be displayed (clamped to max_display_time) + num_bars = ( + int(min(data * self.bar_scale, self.max_display_time * self.bar_scale - 1)) + 1 + ) # Convert bar_scale's reciprocal to milliseconds + print( + f"{topic.ljust(max_topic_length)}: {self.display_character * num_bars} [{data_rounded}ms]" + ) + + +def main(args=None): + # Get the command line arguments before argparse processes them + cmd_args = sys.argv[1:] + + parser = argparse.ArgumentParser(description="Processing Time Subscriber Parameters") + parser.add_argument( + "-m", "--max_display_time", type=int, default=50, help="Maximum display time in ms" + ) + parser.add_argument( + "-f", "--display_frequency", type=float, default=5.0, help="Display frequency in Hz" + ) + parser.add_argument( + "-w", "--window_size", type=int, default=5, help="Number of samples to average" + ) + parser.add_argument( + "-bs", + "--bar_scale", + type=int, + default=2, + help="Number of bars per second. Default is 2 bars per second.", + ) + parser.add_argument( + "-dc", + "--display_character", + type=str, + default="|", + help="Character to use for the display. Default is '|'.", + ) + args = parser.parse_args() + + rclpy.init(args=cmd_args) # Use the original command line arguments here + subscriber = ProcessingTimeSubscriber( + max_display_time=args.max_display_time, + display_frequency=args.display_frequency, + window_size=args.window_size, + bar_scale=args.bar_scale, + display_character=args.display_character, + ) + rclpy.spin(subscriber) + subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index ba9c46f157f2b..d74a8712a6e3f 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -14,6 +14,7 @@ #include "route_handler/route_handler.hpp" +#include #include #include #include @@ -855,12 +856,11 @@ bool RouteHandler::getNextLaneletWithinRoute( return false; } - lanelet::ConstLanelet start_lanelet; - const bool flag_check = getClosestLaneletWithinRoute(route_ptr_->start_pose, &start_lanelet); + const auto start_lane_id = route_ptr_->segments.front().preferred_primitive.id; const auto following_lanelets = routing_graph_ptr_->following(lanelet); for (const auto & llt : following_lanelets) { - if (!(flag_check && start_lanelet.id() == llt.id()) && exists(route_lanelets_, llt)) { + if (start_lane_id != llt.id() && exists(route_lanelets_, llt)) { *next_lanelet = llt; return true; } @@ -2128,10 +2128,24 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints( double shortest_path_length2d = std::numeric_limits::max(); for (const auto & st_llt : start_lanelets) { + // check if the angle difference between start_checkpoint and start lanelet center line + // orientation is in yaw_threshold range + double yaw_threshold = M_PI / 2.0; + bool is_proper_angle = false; + { + double lanelet_angle = lanelet::utils::getLaneletAngle(st_llt, start_checkpoint.position); + double pose_yaw = tf2::getYaw(start_checkpoint.orientation); + double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw)); + + if (angle_diff <= std::abs(yaw_threshold)) { + is_proper_angle = true; + } + } + optional_route = routing_graph_ptr_->getRoute(st_llt, goal_lanelet, 0); - if (!optional_route) { + if (!optional_route || !is_proper_angle) { RCLCPP_ERROR_STREAM( - logger_, "Failed to find a proper path!" + logger_, "Failed to find a proper route!" << std::endl << " - start checkpoint: " << toString(start_checkpoint) << std::endl << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 599a017bffed7..3a18dca815f12 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -139,11 +139,11 @@ void GNSSPoser::callbackNavSatFix( tf2::Transform tf_map2gnss_antenna{}; tf2::fromMsg(gnss_antenna_pose, tf_map2gnss_antenna); - // get TF from base_link to gnss_antenna + // get TF from gnss_antenna to base_link auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared(); getStaticTransform( - gnss_frame_, base_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); + base_frame_, gnss_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); tf2::Transform tf_gnss_antenna2base_link{}; tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link); diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index b47ae1500f43b..7af82c1129aff 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -51,10 +51,16 @@ Note that the node calculates bias from the gyroscope data by averaging the data ### Input -| Name | Type | Description | -| ----------------- | ------------------------------------------------ | ---------------- | -| `~/input/imu_raw` | `sensor_msgs::msg::Imu` | **raw** imu data | -| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | vehicle velocity | +| Name | Type | Description | +| ----------------- | ----------------------------------------------- | ---------------- | +| `~/input/imu_raw` | `sensor_msgs::msg::Imu` | **raw** imu data | +| `~/input/pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | ndt pose | + +Note that the input pose is assumed to be accurate enough. For example when using NDT, we assume that the NDT is appropriately converged. + +Currently, it is possible to use methods other than NDT as a `pose_source` for Autoware, but less accurate methods are not suitable for IMU bias estimation. + +In the future, with careful implementation for pose errors, the IMU bias estimated by NDT could potentially be used not only for validation but also for online calibration. ### Output @@ -64,12 +70,11 @@ Note that the node calculates bias from the gyroscope data by averaging the data ### Parameters -| Name | Type | Description | -| --------------------------- | ------ | ----------------------------------------------------------------------------- | -| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] | -| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] | -| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] | -| `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] | -| `velocity_threshold` | double | threshold of the vehicle velocity to determine if the vehicle is stopped[m/s] | -| `timestamp_threshold` | double | threshold of the timestamp diff between IMU and twist [s] | -| `data_num_threshold` | int | number of data used to calculate bias | +| Name | Type | Description | +| ------------------------------------- | ------ | ------------------------------------------------------------------------------------------- | +| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] | +| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] | +| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] | +| `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] | +| `timer_callback_interval_sec` | double | seconds about the timer callback function [sec] | +| `straight_motion_ang_vel_upper_limit` | double | upper limit of yaw angular velocity, beyond which motion is not considered straight [rad/s] | diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml index d5868e1df416a..e10329c920137 100644 --- a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -1,6 +1,5 @@ /**: ros__parameters: gyro_bias_threshold: 0.0015 # [rad/s] - velocity_threshold: 0.0 # [m/s] - timestamp_threshold: 0.1 # [s] - data_num_threshold: 200 # [num] + timer_callback_interval_sec: 0.5 # [sec] + straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml index a25ce5f90ed27..e16ccef446aba 100644 --- a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml +++ b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml @@ -1,14 +1,14 @@ - + - + diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp index 2deb6088f6542..9b5719de69524 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp @@ -14,58 +14,106 @@ #include "gyro_bias_estimation_module.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + namespace imu_corrector { -GyroBiasEstimationModule::GyroBiasEstimationModule( - const double velocity_threshold, const double timestamp_threshold, - const size_t data_num_threshold) -: velocity_threshold_(velocity_threshold), - timestamp_threshold_(timestamp_threshold), - data_num_threshold_(data_num_threshold), - is_stopped_(false) -{ -} -void GyroBiasEstimationModule::update_gyro( - const double time, const geometry_msgs::msg::Vector3 & gyro) +/** + * @brief perform dead reckoning based on "gyro_list" and return a relative pose (in RPY) + */ +geometry_msgs::msg::Vector3 integrate_orientation( + const std::vector & gyro_list, + const geometry_msgs::msg::Vector3 & gyro_bias) { - if (time - last_velocity_time_ > timestamp_threshold_) { - return; - } - if (!is_stopped_) { - return; - } - gyro_buffer_.push_back(gyro); - if (gyro_buffer_.size() > data_num_threshold_) { - gyro_buffer_.pop_front(); + geometry_msgs::msg::Vector3 d_rpy{}; + double t_prev = rclcpp::Time(gyro_list.front().header.stamp).seconds(); + for (std::size_t i = 0; i < gyro_list.size() - 1; ++i) { + double t_cur = rclcpp::Time(gyro_list[i + 1].header.stamp).seconds(); + + d_rpy.x += (t_cur - t_prev) * (gyro_list[i].vector.x - gyro_bias.x); + d_rpy.y += (t_cur - t_prev) * (gyro_list[i].vector.y - gyro_bias.y); + d_rpy.z += (t_cur - t_prev) * (gyro_list[i].vector.z - gyro_bias.z); + + t_prev = t_cur; } + return d_rpy; } -void GyroBiasEstimationModule::update_velocity(const double time, const double velocity) +/** + * @brief calculate RPY error on dead-reckoning (calculated from "gyro_list") compared to the + * ground-truth pose from "pose_list". + */ +geometry_msgs::msg::Vector3 calculate_error_rpy( + const std::vector & pose_list, + const std::vector & gyro_list, + const geometry_msgs::msg::Vector3 & gyro_bias) { - is_stopped_ = velocity <= velocity_threshold_; - last_velocity_time_ = time; + const geometry_msgs::msg::Vector3 rpy_0 = + tier4_autoware_utils::getRPY(pose_list.front().pose.orientation); + const geometry_msgs::msg::Vector3 rpy_1 = + tier4_autoware_utils::getRPY(pose_list.back().pose.orientation); + const geometry_msgs::msg::Vector3 d_rpy = integrate_orientation(gyro_list, gyro_bias); + + geometry_msgs::msg::Vector3 error_rpy; + error_rpy.x = tier4_autoware_utils::normalizeRadian(-rpy_1.x + rpy_0.x + d_rpy.x); + error_rpy.y = tier4_autoware_utils::normalizeRadian(-rpy_1.y + rpy_0.y + d_rpy.y); + error_rpy.z = tier4_autoware_utils::normalizeRadian(-rpy_1.z + rpy_0.z + d_rpy.z); + return error_rpy; } -geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias() const +/** + * @brief update gyroscope bias based on a given trajectory data + */ +void GyroBiasEstimationModule::update_bias( + const std::vector & pose_list, + const std::vector & gyro_list) { - if (gyro_buffer_.size() < data_num_threshold_) { - throw std::runtime_error("Bias estimation is not yet ready because of insufficient data."); + const double dt_pose = + (rclcpp::Time(pose_list.back().header.stamp) - rclcpp::Time(pose_list.front().header.stamp)) + .seconds(); + const double dt_gyro = + (rclcpp::Time(gyro_list.back().header.stamp) - rclcpp::Time(gyro_list.front().header.stamp)) + .seconds(); + if (dt_pose == 0 || dt_gyro == 0) { + throw std::runtime_error("dt_pose or dt_gyro is zero"); } - geometry_msgs::msg::Vector3 bias; - bias.x = 0.0; - bias.y = 0.0; - bias.z = 0.0; - for (const auto & gyro : gyro_buffer_) { - bias.x += gyro.x; - bias.y += gyro.y; - bias.z += gyro.z; + auto error_rpy = calculate_error_rpy(pose_list, gyro_list, geometry_msgs::msg::Vector3{}); + error_rpy.x *= dt_pose / dt_gyro; + error_rpy.y *= dt_pose / dt_gyro; + error_rpy.z *= dt_pose / dt_gyro; + + gyro_bias_pair_.first.x += dt_pose * error_rpy.x; + gyro_bias_pair_.first.y += dt_pose * error_rpy.y; + gyro_bias_pair_.first.z += dt_pose * error_rpy.z; + gyro_bias_pair_.second.x += dt_pose * dt_pose; + gyro_bias_pair_.second.y += dt_pose * dt_pose; + gyro_bias_pair_.second.z += dt_pose * dt_pose; + + geometry_msgs::msg::Vector3 gyro_bias; + gyro_bias.x = error_rpy.x / dt_pose; + gyro_bias.y = error_rpy.y / dt_pose; + gyro_bias.z = error_rpy.z / dt_pose; +} + +/** + * @brief getter function for current estimated bias + */ +geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias_base_link() const +{ + geometry_msgs::msg::Vector3 gyro_bias_base; + if ( + gyro_bias_pair_.second.x == 0 || gyro_bias_pair_.second.y == 0 || + gyro_bias_pair_.second.z == 0) { + throw std::runtime_error("gyro_bias_pair_.second is zero"); } - bias.x /= gyro_buffer_.size(); - bias.y /= gyro_buffer_.size(); - bias.z /= gyro_buffer_.size(); - return bias; + gyro_bias_base.x = gyro_bias_pair_.first.x / gyro_bias_pair_.second.x; + gyro_bias_base.y = gyro_bias_pair_.first.y / gyro_bias_pair_.second.y; + gyro_bias_base.z = gyro_bias_pair_.first.z / gyro_bias_pair_.second.z; + return gyro_bias_base; } } // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp index 6ebae942fff5d..dfa152d32c0d9 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp @@ -11,33 +11,30 @@ // 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 GYRO_BIAS_ESTIMATION_MODULE_HPP_ #define GYRO_BIAS_ESTIMATION_MODULE_HPP_ -#include +#include +#include #include +#include +#include namespace imu_corrector { class GyroBiasEstimationModule { public: - GyroBiasEstimationModule( - const double velocity_threshold, const double timestamp_threshold, - const size_t data_num_threshold); - geometry_msgs::msg::Vector3 get_bias() const; - void update_gyro(const double time, const geometry_msgs::msg::Vector3 & gyro); - void update_velocity(const double time, const double velocity); + GyroBiasEstimationModule() = default; + void update_bias( + const std::vector & pose_list, + const std::vector & gyro_list); + geometry_msgs::msg::Vector3 get_bias_base_link() const; private: - const double velocity_threshold_; - const double timestamp_threshold_; - const size_t data_num_threshold_; - bool is_stopped_; - std::deque gyro_buffer_; - - double last_velocity_time_; + std::pair gyro_bias_pair_; }; } // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 5f41a9089b6ee..50e4a702ec949 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -14,6 +14,13 @@ #include "gyro_bias_estimator.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include +#include + +#include + namespace imu_corrector { GyroBiasEstimator::GyroBiasEstimator() @@ -22,59 +29,145 @@ GyroBiasEstimator::GyroBiasEstimator() angular_velocity_offset_x_(declare_parameter("angular_velocity_offset_x")), angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), angular_velocity_offset_z_(declare_parameter("angular_velocity_offset_z")), + timer_callback_interval_sec_(declare_parameter("timer_callback_interval_sec")), + straight_motion_ang_vel_upper_limit_( + declare_parameter("straight_motion_ang_vel_upper_limit")), updater_(this), gyro_bias_(std::nullopt) { updater_.setHardwareID(get_name()); updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); - const double velocity_threshold = declare_parameter("velocity_threshold"); - const double timestamp_threshold = declare_parameter("timestamp_threshold"); - const size_t data_num_threshold = - static_cast(declare_parameter("data_num_threshold")); - gyro_bias_estimation_module_ = std::make_unique( - velocity_threshold, timestamp_threshold, data_num_threshold); + gyro_bias_estimation_module_ = std::make_unique(); imu_sub_ = create_subscription( "~/input/imu_raw", rclcpp::SensorDataQoS(), [this](const Imu::ConstSharedPtr msg) { callback_imu(msg); }); - twist_sub_ = create_subscription( - "~/input/twist", rclcpp::SensorDataQoS(), - [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { callback_twist(msg); }); - + odom_sub_ = create_subscription( + "~/input/odom", rclcpp::SensorDataQoS(), + [this](const Odometry::ConstSharedPtr msg) { callback_odom(msg); }); gyro_bias_pub_ = create_publisher("~/output/gyro_bias", rclcpp::SensorDataQoS()); + + auto timer_callback = std::bind(&GyroBiasEstimator::timer_callback, this); + auto period_control = std::chrono::duration_cast( + std::chrono::duration(timer_callback_interval_sec_)); + timer_ = std::make_shared>( + this->get_clock(), period_control, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); + + transform_listener_ = std::make_shared(this); } void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) { - // Update gyro data - gyro_bias_estimation_module_->update_gyro( - rclcpp::Time(imu_msg_ptr->header.stamp).seconds(), imu_msg_ptr->angular_velocity); - - // Estimate gyro bias - try { - gyro_bias_ = gyro_bias_estimation_module_->get_bias(); - } catch (const std::runtime_error & e) { - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *(this->get_clock()), 1000, e.what()); + imu_frame_ = imu_msg_ptr->header.frame_id; + geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = + transform_listener_->getLatestTransform(imu_frame_, output_frame_); + if (!tf_imu2base_ptr) { + RCLCPP_ERROR( + this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), + (imu_frame_).c_str()); + return; } + geometry_msgs::msg::Vector3Stamped gyro; + gyro.header.stamp = imu_msg_ptr->header.stamp; + gyro.vector = transform_vector3(imu_msg_ptr->angular_velocity, *tf_imu2base_ptr); + + gyro_all_.push_back(gyro); + // Publish results for debugging if (gyro_bias_ != std::nullopt) { Vector3Stamped gyro_bias_msg; + gyro_bias_msg.header.stamp = this->now(); gyro_bias_msg.vector = gyro_bias_.value(); + gyro_bias_pub_->publish(gyro_bias_msg); } +} + +void GyroBiasEstimator::callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr) +{ + geometry_msgs::msg::PoseStamped pose; + pose.header = odom_msg_ptr->header; + pose.pose = odom_msg_ptr->pose.pose; + pose_buf_.push_back(pose); +} + +void GyroBiasEstimator::timer_callback() +{ + if (pose_buf_.empty()) { + return; + } + + // Copy data + const std::vector pose_buf = pose_buf_; + const std::vector gyro_all = gyro_all_; + pose_buf_.clear(); + gyro_all_.clear(); + + // Check time + const rclcpp::Time t0_rclcpp_time = rclcpp::Time(pose_buf.front().header.stamp); + const rclcpp::Time t1_rclcpp_time = rclcpp::Time(pose_buf.back().header.stamp); + if (t1_rclcpp_time <= t0_rclcpp_time) { + return; + } + + // Filter gyro data + std::vector gyro_filtered; + for (const auto & gyro : gyro_all) { + const rclcpp::Time t = rclcpp::Time(gyro.header.stamp); + if (t0_rclcpp_time <= t && t < t1_rclcpp_time) { + gyro_filtered.push_back(gyro); + } + } + + // Check gyro data size + // Data size must be greater than or equal to 2 since the time difference will be taken later + if (gyro_filtered.size() <= 1) { + return; + } + + // Check if the vehicle is moving straight + const geometry_msgs::msg::Vector3 rpy_0 = + tier4_autoware_utils::getRPY(pose_buf.front().pose.orientation); + const geometry_msgs::msg::Vector3 rpy_1 = + tier4_autoware_utils::getRPY(pose_buf.back().pose.orientation); + const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(rpy_1.z - rpy_0.z)); + const double time_diff = (t1_rclcpp_time - t0_rclcpp_time).seconds(); + const double yaw_vel = yaw_diff / time_diff; + const bool is_straight = (yaw_vel < straight_motion_ang_vel_upper_limit_); + if (!is_straight) { + return; + } + + // Calculate gyro bias + gyro_bias_estimation_module_->update_bias(pose_buf, gyro_filtered); + + geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_base2imu_ptr = + transform_listener_->getLatestTransform(output_frame_, imu_frame_); + if (!tf_base2imu_ptr) { + RCLCPP_ERROR( + this->get_logger(), "Please publish TF %s to %s", imu_frame_.c_str(), output_frame_.c_str()); + return; + } + gyro_bias_ = + transform_vector3(gyro_bias_estimation_module_->get_bias_base_link(), *tf_base2imu_ptr); - // Update diagnostics updater_.force_update(); } -void GyroBiasEstimator::callback_twist( - const TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr) +geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( + const geometry_msgs::msg::Vector3 & vec, const geometry_msgs::msg::TransformStamped & transform) { - gyro_bias_estimation_module_->update_velocity( - rclcpp::Time(twist_msg_ptr->header.stamp).seconds(), twist_msg_ptr->twist.twist.linear.x); + geometry_msgs::msg::Vector3Stamped vec_stamped; + vec_stamped.vector = vec; + + geometry_msgs::msg::Vector3Stamped vec_stamped_transformed; + tf2::doTransform(vec_stamped, vec_stamped_transformed, transform); + return vec_stamped_transformed.vector; } void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index e74a0390af3aa..821cba0b213ff 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -15,17 +15,20 @@ #define GYRO_BIAS_ESTIMATOR_HPP_ #include "gyro_bias_estimation_module.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include #include -#include +#include #include +#include #include #include #include #include +#include namespace imu_corrector { @@ -33,9 +36,10 @@ class GyroBiasEstimator : public rclcpp::Node { private: using Imu = sensor_msgs::msg::Imu; - using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using Vector3Stamped = geometry_msgs::msg::Vector3Stamped; using Vector3 = geometry_msgs::msg::Vector3; + using Odometry = nav_msgs::msg::Odometry; public: GyroBiasEstimator(); @@ -43,12 +47,19 @@ class GyroBiasEstimator : public rclcpp::Node private: void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr); - void callback_twist(const TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); + void callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr); + void timer_callback(); - rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription::SharedPtr twist_sub_; + static geometry_msgs::msg::Vector3 transform_vector3( + const geometry_msgs::msg::Vector3 & vec, + const geometry_msgs::msg::TransformStamped & transform); + + const std::string output_frame_ = "base_link"; + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Publisher::SharedPtr gyro_bias_pub_; + rclcpp::TimerBase::SharedPtr timer_; std::unique_ptr gyro_bias_estimation_module_; @@ -56,10 +67,19 @@ class GyroBiasEstimator : public rclcpp::Node const double angular_velocity_offset_x_; const double angular_velocity_offset_y_; const double angular_velocity_offset_z_; + const double timer_callback_interval_sec_; + const double straight_motion_ang_vel_upper_limit_; diagnostic_updater::Updater updater_; std::optional gyro_bias_; + + std::shared_ptr transform_listener_; + + std::string imu_frame_; + + std::vector gyro_all_; + std::vector pose_buf_; }; } // namespace imu_corrector diff --git a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp index a0da4d0e24e17..78558feeb7936 100644 --- a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp +++ b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp @@ -14,6 +14,8 @@ #include "../src/gyro_bias_estimation_module.hpp" +#include + #include namespace imu_corrector @@ -21,45 +23,67 @@ namespace imu_corrector class GyroBiasEstimationModuleTest : public ::testing::Test { protected: - double velocity_threshold = 1.0; - double timestamp_threshold = 5.0; - size_t data_num_threshold = 10; - GyroBiasEstimationModule module = - GyroBiasEstimationModule(velocity_threshold, timestamp_threshold, data_num_threshold); + GyroBiasEstimationModule module; }; TEST_F(GyroBiasEstimationModuleTest, GetBiasEstimationWhenVehicleStopped) { - geometry_msgs::msg::Vector3 gyro; - gyro.x = 0.1; - gyro.y = 0.2; - gyro.z = 0.3; - for (size_t i = 0; i < data_num_threshold + 1; ++i) { - module.update_velocity( - i * 0.1 * timestamp_threshold, 0.0); // velocity = 0.0 < 1.0 = velocity_threshold - module.update_gyro(i * 0.1 * timestamp_threshold, gyro); + std::vector pose_list; + std::vector gyro_list; + geometry_msgs::msg::PoseStamped pose1; + pose1.header.stamp = rclcpp::Time(0); + pose1.pose.orientation.w = 1.0; + pose_list.push_back(pose1); + geometry_msgs::msg::PoseStamped pose2; + pose2.header.stamp = rclcpp::Time(1e9); + pose2.pose.orientation.w = 1.0; + pose_list.push_back(pose2); + + geometry_msgs::msg::Vector3Stamped gyro1; + gyro1.header.stamp = rclcpp::Time(0.25 * 1e9); + gyro1.vector.x = 0.1; + gyro1.vector.y = 0.2; + gyro1.vector.z = 0.3; + gyro_list.push_back(gyro1); + geometry_msgs::msg::Vector3Stamped gyro2; + gyro2.header.stamp = rclcpp::Time(0.5 * 1e9); + gyro2.vector.x = 0.1; + gyro2.vector.y = 0.2; + gyro2.vector.z = 0.3; + gyro_list.push_back(gyro2); + + for (size_t i = 0; i < 10; ++i) { + module.update_bias(pose_list, gyro_list); } - ASSERT_NEAR(module.get_bias().x, gyro.x, 0.0001); - ASSERT_NEAR(module.get_bias().y, gyro.y, 0.0001); - ASSERT_NEAR(module.get_bias().z, gyro.z, 0.0001); + const geometry_msgs::msg::Vector3 result = module.get_bias_base_link(); + ASSERT_DOUBLE_EQ(result.x, 0.1); + ASSERT_DOUBLE_EQ(result.y, 0.2); + ASSERT_DOUBLE_EQ(result.z, 0.3); } TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataException) { - ASSERT_THROW(module.get_bias(), std::runtime_error); + ASSERT_THROW(module.get_bias_base_link(), std::runtime_error); } TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataExceptionWhenVehicleMoving) { - geometry_msgs::msg::Vector3 gyro; - gyro.x = 0.1; - gyro.y = 0.2; - gyro.z = 0.3; - for (size_t i = 0; i < data_num_threshold + 1; ++i) { - module.update_velocity( - i * 0.1 * timestamp_threshold, 5.0); // velocity = 5.0 > 1.0 = velocity_threshold - module.update_gyro(i * 0.1 * timestamp_threshold, gyro); + std::vector pose_list; + std::vector gyro_list; + geometry_msgs::msg::PoseStamped pose1; + pose1.header.stamp = rclcpp::Time(0); + pose1.pose.orientation.w = 1.0; + pose_list.push_back(pose1); + + geometry_msgs::msg::Vector3Stamped gyro1; + gyro1.header.stamp = rclcpp::Time(0); + gyro1.vector.x = 0.1; + gyro1.vector.y = 0.2; + gyro1.vector.z = 0.3; + gyro_list.push_back(gyro1); + + for (size_t i = 0; i < 10; ++i) { + ASSERT_THROW(module.update_bias(pose_list, gyro_list), std::runtime_error); } - ASSERT_THROW(module.get_bias(), std::runtime_error); } } // namespace imu_corrector diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index 0cae2c90c2f16..20d1f5c8b3d6d 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -119,7 +119,8 @@ void CropBoxFilterComponent::faster_filter( stop_watch_ptr_->toc("processing_time", true); if (indices) { - RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, "Indices are not supported and will be ignored"); } int x_offset = input->fields[pcl::getFieldIndex(*input, "x")].offset; @@ -129,6 +130,8 @@ void CropBoxFilterComponent::faster_filter( output.data.resize(input->data.size()); size_t output_size = 0; + int skipped_count = 0; + for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); global_offset += input->point_step) { Eigen::Vector4f point; @@ -138,7 +141,7 @@ void CropBoxFilterComponent::faster_filter( point[3] = 1; if (!std::isfinite(point[0]) || !std::isfinite(point[1]) || !std::isfinite(point[2])) { - RCLCPP_WARN(this->get_logger(), "Ignoring point containing NaN values"); + skipped_count++; continue; } @@ -162,6 +165,12 @@ void CropBoxFilterComponent::faster_filter( } } + if (skipped_count > 0) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, "%d points contained NaN values and have been ignored", + skipped_count); + } + output.data.resize(output_size); // Note that tf_input_orig_frame_ is the input frame, while tf_input_frame_ is the frame of the diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 208fd80d57a08..fb6a5457e8f05 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -638,6 +638,7 @@ void SimplePlanningSimulator::publish_acceleration() msg.header.frame_id = "/base_link"; msg.header.stamp = get_clock()->now(); msg.accel.accel.linear.x = vehicle_model_ptr_->getAx(); + msg.accel.accel.linear.y = vehicle_model_ptr_->getWz() * vehicle_model_ptr_->getVx(); using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; constexpr auto COV = 0.001; @@ -658,6 +659,7 @@ void SimplePlanningSimulator::publish_imu() imu.header.frame_id = "base_link"; imu.header.stamp = now(); imu.linear_acceleration.x = vehicle_model_ptr_->getAx(); + imu.linear_acceleration.y = vehicle_model_ptr_->getWz() * vehicle_model_ptr_->getVx(); constexpr auto COV = 0.001; imu.linear_acceleration_covariance.at(COV_IDX::X_X) = COV; imu.linear_acceleration_covariance.at(COV_IDX::Y_Y) = COV; diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index a61a1b75697ab..829585ed4b8b4 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -46,12 +46,11 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) for (size_t i = 0; i < module_names.size(); ++i) { const auto name = "/system/component_state_monitor/component/autonomous/" + module_names[i]; const auto qos = rclcpp::QoS(1).transient_local(); - const auto callback = [this, i](const ModeChangeAvailable::ConstSharedPtr msg) { - module_states_[i] = msg->available; + const auto callback = [this, i, module_names](const ModeChangeAvailable::ConstSharedPtr msg) { + module_states_[module_names[i]] = msg->available; }; sub_module_states_.push_back(create_subscription(name, qos, callback)); } - module_states_.resize(module_names.size()); timer_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(5.0).period(), std::bind(&OperationModeNode::on_timer, this)); @@ -137,11 +136,22 @@ void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedP void OperationModeNode::on_timer() { bool autonomous_available = true; + std::string unhealthy_components = ""; for (const auto & state : module_states_) { - autonomous_available &= state; + if (!state.second) { + unhealthy_components += unhealthy_components.empty() ? state.first : ", " + state.first; + } + autonomous_available &= state.second; } mode_available_[OperationModeState::Message::AUTONOMOUS] = autonomous_available; + if (!unhealthy_components.empty()) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 3000, + "%s component state is unhealthy. Autonomous is not available.", + unhealthy_components.c_str()); + } + update_state(); } diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp index 5f5699f42cab7..1830b7041b566 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/default_ad_api/src/operation_mode.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -65,7 +66,7 @@ class OperationModeNode : public rclcpp::Node Cli cli_mode_; Cli cli_control_; - std::vector module_states_; + std::unordered_map module_states_; std::vector::SharedPtr> sub_module_states_; void on_change_to_stop( diff --git a/system/duplicated_node_checker/CMakeLists.txt b/system/duplicated_node_checker/CMakeLists.txt new file mode 100644 index 0000000000000..6a8089fa85c96 --- /dev/null +++ b/system/duplicated_node_checker/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(duplicated_node_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(rclcpp REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/duplicated_node_checker_core.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "duplicated_node_checker::DuplicatedNodeChecker" + EXECUTABLE duplicated_node_checker_node +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/duplicated_node_checker/README.md b/system/duplicated_node_checker/README.md new file mode 100644 index 0000000000000..81b53e36c9f6d --- /dev/null +++ b/system/duplicated_node_checker/README.md @@ -0,0 +1,37 @@ +# Duplicated Node Checker + +## Purpose + +This node monitors the ROS 2 environments and detect duplication of node names in the environment. +The result is published as diagnostics. + +### Standalone Startup + +```bash +ros2 launch duplicated_node_checker duplicated_node_checker.launch.xml +``` + +## Inner-workings / Algorithms + +The types of topic status and corresponding diagnostic status are following. + +| Duplication status | Diagnostic status | Description | +| --------------------- | ----------------- | -------------------------- | +| `OK` | OK | No duplication is detected | +| `Duplicated Detected` | ERROR | Duplication is detected | + +## Inputs / Outputs + +### Output + +| Name | Type | Description | +| -------------- | --------------------------------- | ------------------- | +| `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | Diagnostics outputs | + +## Parameters + +{{ json_to_markdown("system/duplicated_node_checker/schema/duplicated_node_checker.schema.json") }} + +## Assumptions / Known limits + +TBD. diff --git a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml new file mode 100644 index 0000000000000..54b4f691b6eb1 --- /dev/null +++ b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + update_rate: 10.0 + add_duplicated_node_names_to_msg: false # if true, duplicated node names are added to msg diff --git a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp new file mode 100644 index 0000000000000..9d806754f3c20 --- /dev/null +++ b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp @@ -0,0 +1,55 @@ +// 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 DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ +#define DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ + +#include +#include + +#include +#include +#include + +namespace duplicated_node_checker +{ +class DuplicatedNodeChecker : public rclcpp::Node +{ +public: + explicit DuplicatedNodeChecker(const rclcpp::NodeOptions & node_options); + std::vector findIdenticalNames(const std::vector input_name_lists) + { + std::unordered_set unique_names; + std::vector identical_names; + for (auto name : input_name_lists) { + if (unique_names.find(name) != unique_names.end()) { + identical_names.push_back(name); + } else { + unique_names.insert(name); + } + } + return identical_names; + } + +private: + void onTimer(); + void produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + + diagnostic_updater::Updater updater_{this}; + rclcpp::TimerBase::SharedPtr timer_; + bool add_duplicated_node_names_to_msg_; +}; +} // namespace duplicated_node_checker + +#endif // DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ diff --git a/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml b/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml new file mode 100644 index 0000000000000..87f41f045545d --- /dev/null +++ b/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/system/duplicated_node_checker/package.xml b/system/duplicated_node_checker/package.xml new file mode 100644 index 0000000000000..e2ba225b16330 --- /dev/null +++ b/system/duplicated_node_checker/package.xml @@ -0,0 +1,24 @@ + + + + duplicated_node_checker + 1.0.0 + A package to find out nodes with common names + Shumpei Wakabayashi + yliuhb + Apache 2.0 + + ament_cmake + autoware_cmake + + diagnostic_updater + rclcpp + rclcpp_components + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/system/duplicated_node_checker/schema/duplicated_node_checker.schema.json b/system/duplicated_node_checker/schema/duplicated_node_checker.schema.json new file mode 100644 index 0000000000000..6ee933ecf1a77 --- /dev/null +++ b/system/duplicated_node_checker/schema/duplicated_node_checker.schema.json @@ -0,0 +1,31 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Duplicated Node Checker", + "type": "object", + "definitions": { + "duplicated_node_checker": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": 10, + "exclusiveMinimum": 2, + "description": "The scanning and update frequency of the checker." + } + }, + "required": ["update_rate"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/duplicated_node_checker" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp new file mode 100644 index 0000000000000..46c02d9d6e133 --- /dev/null +++ b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -0,0 +1,88 @@ +// 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 "duplicated_node_checker/duplicated_node_checker_core.hpp" + +#include +#include + +#include +#include +#include + +namespace duplicated_node_checker +{ + +DuplicatedNodeChecker::DuplicatedNodeChecker(const rclcpp::NodeOptions & node_options) +: Node("duplicated_node_checker", node_options) +{ + double update_rate = declare_parameter("update_rate"); + add_duplicated_node_names_to_msg_ = declare_parameter("add_duplicated_node_names_to_msg"); + updater_.setHardwareID("duplicated_node_checker"); + updater_.add("duplicated_node_checker", this, &DuplicatedNodeChecker::produceDiagnostics); + + const auto period_ns = rclcpp::Rate(update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&DuplicatedNodeChecker::onTimer, this)); +} + +std::string get_fullname_from_name_ns_pair(std::pair name_and_ns_pair) +{ + std::string full_name; + const std::string & name = name_and_ns_pair.first; + const std::string & ns = name_and_ns_pair.second; + if (ns.back() == '/') { + full_name = ns + name; + } else { + full_name = ns + "/" + name; + } + return full_name; +} + +void DuplicatedNodeChecker::onTimer() +{ + updater_.force_update(); +} + +void DuplicatedNodeChecker::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + using diagnostic_msgs::msg::DiagnosticStatus; + + std::vector node_names = this->get_node_names(); + std::vector identical_names = findIdenticalNames(node_names); + std::string msg; + int level; + if (identical_names.size() > 0) { + level = DiagnosticStatus::ERROR; + msg = "Error: Duplicated nodes detected"; + if (add_duplicated_node_names_to_msg_) { + std::set unique_identical_names(identical_names.begin(), identical_names.end()); + for (const auto & name : unique_identical_names) { + msg += " " + name; + } + } + for (auto name : identical_names) { + stat.add("Duplicated Node Name", name); + } + } else { + msg = "OK"; + level = DiagnosticStatus::OK; + } + stat.summary(level, msg); +} + +} // namespace duplicated_node_checker + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(duplicated_node_checker::DuplicatedNodeChecker) diff --git a/system/system_diagnostic_graph/CMakeLists.txt b/system/system_diagnostic_graph/CMakeLists.txt index 54aebe6f37f73..81d8ba39b3d1b 100644 --- a/system/system_diagnostic_graph/CMakeLists.txt +++ b/system/system_diagnostic_graph/CMakeLists.txt @@ -5,13 +5,13 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_executable(aggregator - src/main.cpp src/core/config.cpp src/core/debug.cpp src/core/graph.cpp - src/core/node.cpp - src/core/expr.cpp - src/core/update.cpp + src/core/nodes.cpp + src/core/exprs.cpp + src/core/modes.cpp + src/main.cpp ) ament_auto_add_executable(converter diff --git a/system/system_diagnostic_graph/README.md b/system/system_diagnostic_graph/README.md index c2c99938348e2..df22121a778fb 100644 --- a/system/system_diagnostic_graph/README.md +++ b/system/system_diagnostic_graph/README.md @@ -2,28 +2,42 @@ ## Overview -The system diagnostic graph node subscribes to diagnostic status and publishes aggregated diagnostic status. +The system diagnostic graph node subscribes to diagnostic array and publishes aggregated diagnostic graph. As shown in the diagram below, this node introduces extra diagnostic status for intermediate functional unit. Diagnostic status dependencies will be directed acyclic graph (DAG). ![overview](./doc/overview.drawio.svg) +## Operation mode availability + +For MRM, this node publishes the status of the top-level functional units in the dedicated message. +Therefore, the diagnostic graph must contain functional units with the following names. +This feature breaks the generality of the graph and may be changed to a plugin or another node in the future. + +- /autoware/operation/stop +- /autoware/operation/autonomous +- /autoware/operation/local +- /autoware/operation/remote +- /autoware/operation/emergency-stop +- /autoware/operation/comfortable-stop +- /autoware/operation/pull-over + ## Interface -| Interface Type | Interface Name | Data Type | Description | -| -------------- | --------------------------- | --------------------------------------- | ------------------ | -| subscription | `/diagnostics` | `diagnostic_msgs/msg/DiagnosticArray` | Input diagnostics. | -| publisher | `/diagnostics_graph/status` | `diagnostic_msgs/msg/DiagnosticArray` | Graph status. | -| publisher | `/diagnostics_graph/struct` | `tier4_system_msgs/msg/DiagnosticGraph` | Graph structure. | +| Interface Type | Interface Name | Data Type | Description | +| -------------- | ------------------------------------- | ------------------------------------------------- | ------------------ | +| subscription | `/diagnostics` | `diagnostic_msgs/msg/DiagnosticArray` | Diagnostics input. | +| publisher | `/diagnostics_graph` | `tier4_system_msgs/msg/DiagnosticGraph` | Diagnostics graph. | +| publisher | `/system/operation_mode/availability` | `tier4_system_msgs/msg/OperationModeAvailability` | mode availability. | ## Parameters -| Parameter Name | Data Type | Description | -| ------------------ | --------- | ------------------------------------------ | -| `graph_file` | `string` | Path of the config file. | -| `rate` | `double` | Rate of aggregation and topic publication. | -| `status_qos_depth` | `uint` | QoS depth of status topic. | -| `source_qos_depth` | `uint` | QoS depth of source topic. | +| Parameter Name | Data Type | Description | +| ----------------- | --------- | ------------------------------------------ | +| `graph_file` | `string` | Path of the config file. | +| `rate` | `double` | Rate of aggregation and topic publication. | +| `input_qos_depth` | `uint` | QoS depth of input array topic. | +| `graph_qos_depth` | `uint` | QoS depth of output graph topic. | ## Graph file format diff --git a/system/system_diagnostic_graph/config/default.param.yaml b/system/system_diagnostic_graph/config/default.param.yaml index f02247374b14e..9fd572c7926fa 100644 --- a/system/system_diagnostic_graph/config/default.param.yaml +++ b/system/system_diagnostic_graph/config/default.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + mode_availability: true + mode: psim rate: 1.0 input_qos_depth: 1000 graph_qos_depth: 1 diff --git a/system/system_diagnostic_graph/doc/overview.drawio.svg b/system/system_diagnostic_graph/doc/overview.drawio.svg index aa6be5a48b08e..e971c052dedc8 100644 --- a/system/system_diagnostic_graph/doc/overview.drawio.svg +++ b/system/system_diagnostic_graph/doc/overview.drawio.svg @@ -4,13 +4,13 @@ xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="921px" - height="561px" - viewBox="-0.5 -0.5 921 561" - content="<mxfile><diagram id="pC5EcGJeU-t8W7RoQB87" name="Page-1">3VtNc6M4EP01vpsPYfvoOJ7Z2sruTiVbtccpBRTQDtAuIcfx/vqVQBiQcGKXGdDkZGgakF4/tfpJeOZtsrevDO+SPyAi6cydR28z737mug5yFuJHWo6VBS1RZYgZjZRTY3ii/xFlnCvrnkak6DhygJTTXdcYQp6TkHdsmDE4dN1eIO2+dYdjYhieQpya1n9oxJPKunQXjf03QuOkfrMTrKorGa6dVU+KBEdwaJm87czbMABeHWVvG5JK8Gpcqvu+nLl6ahgjOb/kBtev7njF6V51TjhHFMc5FJyGxfdYhvB7wTHfF6rN/FgDwWCfR0Q+az7z7g4J5eRph0N59SDuE7aEZ6k4c8QhTmmci+OUvIi23b0SJl6A07Uyc5D+hbid5vFD6XOPhCXCRVK+Qj7D7J/qsnwaeWuZVH+/EsgIZ0fhoq76Cvpj9/TQBHJVRydpBREFyogVeeLTkxt8xYGC+Azc6AO4PwXCSw3hHoiXfg/EjjsExIGBIYnE6FWnwHgCMeQ43TbWuy7Kjc8DSMhKXP4lnB9VKsJ7Dl3kz6JWwJ6Fqh11fuCYxUS5eYoRso3vYstIijl97WagPqDKW9eM4WPLYQc050Xryd+koQmZp48KT8sb1/mLg6oFTchOXbksigtjoPwtQuHOH+j9+vG2UTIAww2KL02KO31ZxB+C4UtrGa5Y0GW4ZwXDkaPn/fcZHgTv+t/O8NUvFUW0tCKK/uq6KH7gf3MUa6haeeoLg7IFdmQqX0/UY2YqzzH6bw3HvZ5MtbCC4wHqhqyebC6dizX/2znu/lpRXNkRxcV1UfzA//YoemczFTwLeReKsAlFTrgQzBTyyRNXMO/i4bljJi7fXsojaxOXkYiCKxNXMDDlTbX9DQpJcyLEdoat4LkuJcblub1i2esRy2hwKXExUKYeXYt+5ZCBXBabZxAR66jkzsekksWqdGUVlVYGlTaQvYjO4+dyDi7kOt3UXNKn31G5VD/DQi759X6DFVyqW9Pi0jYjLM7Doy1EOpFkEiLZK118ewWoo88jEwtQ35Quj7Dn00+4OrdHXVzx7dUofo9GmTBJmjLgdzjK7bYftlRui2DKJGmvCPB7RIA/+I7ZxUCZIqAhkmUMGjcVLQ1g1n/eG5iIrvBuxwvO4AfZQApMWHLIJbVeaJpqpnpjOxQAEdaztZ3RKCp52Yd0NxYDgG1MeMgEu2+De4j9bX+SfaPeQJ1FcoSa5sMixHE0sKu8ou5q8L51kcvxryuONP+bi6Oaeu11XZrKUeLOn2XH9uWKV4jLH5xHkjsiaZGsJBHuKaM+zThFmoh1PHOcnj4qG3qgIns3v5BrjtPp6rO6NS0GP5XK1Y7aTN9BHbU2Q6b02TIGrIzDTvBjcnh0DdRTd5xqk8HRsVcCob5tGju+hNG/bPEW130Jo/nfPoOZ+kztTAY4k4TNn4tyesARZpPzXV8YHbXQRoEB1V+Pn3f+Pse8MepstLA3tyx7cosdS4d6xTX10iEylWnPVw87RiJqx2cPerXxM7eDZxKL+p8GFbjN/zW87f8=</diagram></mxfile>" + height="521px" + viewBox="-0.5 -0.5 921 521" + content="<mxfile><diagram id="pC5EcGJeU-t8W7RoQB87" name="Page-1">3VpNb6MwEP01ufMdckyTtntod1ftSnusXHDBrWEi43ywv37tYELAtE0UCqiXyDzGxn7zPB6PMrEXye6WoVV8DyGmE8sIdxN7ObEs3zXErwTyAvBMtwAiRsICMivgkfzDClT9ojUJcVYz5ACUk1UdDCBNccBrGGIMtnWzF6D1r65QhDXgMUBUR/+SkMdqWda0wn9gEsXll01vVrxJUGmsVpLFKITtEWRfT+wFA+BFK9ktMJXclbwU/W7eeXuYGMMpP6WD5RQ9Noiu1eKEcUhQlELGSZA9RdKDarI8LxlgsE5DLAcxJvbVNiYcP65QIN9uRQeBxTyh4skUTURJlIo2xS9iUlcbzMTIiM4VzEHaZ6I7SaO7vc3SFUiIsnj/CTmGvjC1Vjka3h1BaqG3GBLMWS5M1FtHcZ7XH7eVB2elW+Ij77kliJRqosPIFbGiobht59mdtfGc5RnHiWjACjPECaRPidgrAkAbRCh6JpTw/FuQ79fJ93XyfaeFfLML8i33E5F/R4Ydo1eKPY1DHIqYqR6B8RgiSBG9rtCrOsuVzR1Iyva8vGLOc3UAoDWHOvPvspbBmgVqHmVU5ohFWJnZShFyjh9yyzAV+3JTj/ttRO27zhlD+ZHBCkjKs6ORf0ugcplr1H1m+41ofZ69aBQzqFx2WMppXpxqG+WPcIVl3JHl/OGyXdJFAG9G8JYQbraFcKcLhfujVbhttCjcHoXCvWlDsd7HCp86H9pfrPCSqiOF3zDYz2AcGveMATVum9r6R6Nxu0Xj01FovKnZ0hWnRvGG/eUatzWNw3PGUSDcJe5cmIsrkUg1h5d6I2NpSwq/TurOeKXujlbqmnSNM6VudCx1PbNfQSZljkVin6BR6LyZtvSrcz2lk7JMIYH1hfeeL6DmQEMf1DimRg3DCXA8OC3Nu1yvtHjWxZHx3IjntBzupXe6i3gn60I/P0tdGAEkCUrD0Smk1zTR0aMuhQDRwVk5rHiQcDLeEojTUgJxOi+BnEyUfiS9Qi4LcW8j2mFNLfW7w3yNovnPpcaJWAqvLzzjDN7wAigwgaSQSpG9EEobUFmzDARBmLVULRMShnuFtjFd90UHZGt5oqmT3Va77KJ06cyG2LetjnqXyR6uAp/m7tMG10WAUZ0qui+9UphnXinMjq8UrqVtvUyW8IeOR83CUK9nmzve27LbdlseR/HTbxwhQxc/XT1vU8VPDyVSselztg/vKERscMFPvQEPYNfTqPr18H3PX015dn/nr6dTff9wPzr5fWW8nch9WP7lo9jY1f9m7Ov/</diagram></mxfile>" > - + @@ -20,243 +20,231 @@ >
- /diagnostics_graph_status + /diagnostics_graph
- /diagnostics_graph_status + /diagnostics_graph - +
- /diagnostics + /system/operation_mode/availability
- /diagnostics + /system/operation_mode/availability - - - +
-
+
- Top LiDAR + /diagnostics
- Top LiDAR + /diagnostics - - - - - + + +
- Front LiDAR + Top LiDAR
- Front LiDAR + Top LiDAR - - - - - + + +
- Front obstacle detection + Front LiDAR
- Front obstacle detec... + Front LiDAR - - - + + +
- Pose estimation + obstacle detection
- Pose estimation + obstacle detection - - - + + +
- Autonomous mode + pose estimation
- Autonomous mode + pose estimation - - - +
- Comfortable stop + autonomous
- Comfortable stop + autonomous - - - +
- Emergncy stop + remote
- Emergncy stop + remote - - - + + +
- Route + remote command
- Route + remote command - - - +
- Joystick mode + local
- Joystick mode + local - - - + + +
- Joystick + joystick command
- Joystick + joystick command - +
@@ -265,73 +253,38 @@
- AND + AND - - - + + +
- Filter by use case and system state -
-
-
- - Filter by use case and system state - - - - - - - - -
-
-
- Stop mode -
-
-
- - Stop mode - - - - - - -
-
-
- Error report + stop
- Error report + stop - - - + + +
@@ -340,16 +293,16 @@
- Front radar + Front radar - +
@@ -358,27 +311,25 @@
- OR + OR - - - +
- Front obstacle prediction + MRM
- Front obstacle predi... + MRM diff --git a/system/system_diagnostic_graph/example/example.launch.xml b/system/system_diagnostic_graph/example/example.launch.xml index 461842f020ded..1456bfd5c6593 100644 --- a/system/system_diagnostic_graph/example/example.launch.xml +++ b/system/system_diagnostic_graph/example/example.launch.xml @@ -1,6 +1,6 @@ - + - + diff --git a/system/system_diagnostic_graph/example/example1.yaml b/system/system_diagnostic_graph/example/example1.yaml deleted file mode 100644 index 25053b5f7e12c..0000000000000 --- a/system/system_diagnostic_graph/example/example1.yaml +++ /dev/null @@ -1,41 +0,0 @@ -files: - - { package: system_diagnostic_graph, path: example/example2.yaml } - -nodes: - - name: /autoware/diagnostics - type: and - list: - - { type: unit, name: /autoware/operation/stop } - - { type: unit, name: /autoware/operation/autonomous } - - { type: unit, name: /autoware/operation/local } - - { type: unit, name: /autoware/operation/remote } - - { type: unit, name: /autoware/operation/emergency-stop } - - { type: unit, name: /autoware/operation/comfortable-stop } - - - name: /autoware/operation/stop - type: debug-ok - - - name: /autoware/operation/autonomous - type: and - list: - - { type: unit, name: /autoware/localization } - - { type: unit, name: /autoware/planning } - - { type: unit, name: /autoware/perception } - - - name: /autoware/operation/local - type: debug-ok - - - name: /autoware/operation/remote - type: and - list: - - { type: diag, hardware: test, name: /external/remote_command } - - - name: /autoware/operation/emergency-stop - type: debug-ok - - - name: /autoware/operation/comfortable-stop - type: and - list: - - { type: unit, name: /autoware/localization } - - { type: unit, name: /autoware/planning } - - { type: unit, name: /autoware/perception/front-obstacle-detection } diff --git a/system/system_diagnostic_graph/example/example2.yaml b/system/system_diagnostic_graph/example/example2.yaml deleted file mode 100644 index 2af09c0cb69a2..0000000000000 --- a/system/system_diagnostic_graph/example/example2.yaml +++ /dev/null @@ -1,32 +0,0 @@ -nodes: - - name: /autoware/localization - type: and - list: - - { type: diag, hardware: test, name: /sensing/lidars/top } - - - name: /autoware/planning - type: and - list: - - { type: unit, name: /autoware/planning/route } - - - name: /autoware/planning/route - type: and - list: - - { type: diag, hardware: test, name: /planning/route } - - - name: /autoware/perception - type: and - list: - - { type: unit, name: /autoware/perception/front-obstacle-detection } - - { type: unit, name: /autoware/perception/front-obstacle-prediction } - - - name: /autoware/perception/front-obstacle-detection - type: or - list: - - { type: diag, hardware: test, name: /sensing/lidars/front } - - { type: diag, hardware: test, name: /sensing/radars/front } - - - name: /autoware/perception/front-obstacle-prediction - type: and - list: - - { type: diag, hardware: test, name: /sensing/lidars/front } diff --git a/system/system_diagnostic_graph/example/example_0.yaml b/system/system_diagnostic_graph/example/example_0.yaml new file mode 100644 index 0000000000000..0ee6e59c8e121 --- /dev/null +++ b/system/system_diagnostic_graph/example/example_0.yaml @@ -0,0 +1,32 @@ +files: + - { package: system_diagnostic_graph, path: example/example_1.yaml } + - { package: system_diagnostic_graph, path: example/example_2.yaml } + +nodes: + - path: /autoware/modes/stop + type: debug-ok + + - path: /autoware/modes/autonomous + type: and + list: + - { type: link, path: /functions/pose_estimation } + - { type: link, path: /functions/obstacle_detection } + + - path: /autoware/modes/local + type: and + list: + - { type: link, path: /external/joystick_command } + + - path: /autoware/modes/remote + type: and + list: + - { type: link, path: /external/remote_command } + + - path: /autoware/modes/emergency-stop + type: debug-ok + + - path: /autoware/modes/comfortable-stop + type: debug-ok + + - path: /autoware/modes/pull-over + type: debug-ok diff --git a/system/system_diagnostic_graph/example/example_1.yaml b/system/system_diagnostic_graph/example/example_1.yaml new file mode 100644 index 0000000000000..5ba93ec3059e4 --- /dev/null +++ b/system/system_diagnostic_graph/example/example_1.yaml @@ -0,0 +1,23 @@ +nodes: + - path: /functions/pose_estimation + type: and + list: + - { type: link, path: /sensing/lidars/top } + + - path: /functions/obstacle_detection + type: or + list: + - { type: link, path: /sensing/lidars/front } + - { type: link, path: /sensing/radars/front } + + - path: /sensing/lidars/top + type: diag + name: "lidar_driver/top: status" + + - path: /sensing/lidars/front + type: diag + name: "lidar_driver/front: status" + + - path: /sensing/radars/front + type: diag + name: "radar_driver/front: status" diff --git a/system/system_diagnostic_graph/example/example_2.yaml b/system/system_diagnostic_graph/example/example_2.yaml new file mode 100644 index 0000000000000..f61f4accbfec8 --- /dev/null +++ b/system/system_diagnostic_graph/example/example_2.yaml @@ -0,0 +1,8 @@ +nodes: + - path: /external/joystick_command + type: diag + name: "external_command_checker: joystick_command" + + - path: /external/remote_command + type: diag + name: "external_command_checker: remote_command" diff --git a/system/system_diagnostic_graph/example/example_diags.py b/system/system_diagnostic_graph/example/example_diags.py new file mode 100755 index 0000000000000..52d6189a63f30 --- /dev/null +++ b/system/system_diagnostic_graph/example/example_diags.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 + +# 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. + +from diagnostic_msgs.msg import DiagnosticArray +from diagnostic_msgs.msg import DiagnosticStatus +import rclpy +import rclpy.node +import rclpy.qos + + +class DummyDiagnostics(rclpy.node.Node): + def __init__(self, names): + super().__init__("dummy_diagnostics") + qos = rclpy.qos.qos_profile_system_default + self.diags = self.create_publisher(DiagnosticArray, "/diagnostics", qos) + self.timer = self.create_timer(0.5, self.on_timer) + self.count = 0 + self.array = [self.create_status(name) for name in names] + + def on_timer(self): + error_index = int(self.count / 10) + for index, status in enumerate(self.array, 1): + if index == error_index: + status.level = DiagnosticStatus.ERROR + status.message = "ERROR" + else: + status.level = DiagnosticStatus.OK + status.message = "OK" + + diagnostics = DiagnosticArray() + diagnostics.header.stamp = self.get_clock().now().to_msg() + diagnostics.status = self.array + self.count = (self.count + 1) % 60 + self.diags.publish(diagnostics) + + @staticmethod + def create_status(name: str): + return DiagnosticStatus(name=name, hardware_id="example") + + +if __name__ == "__main__": + diags = [ + "lidar_driver/top: status", + "lidar_driver/front: status", + "radar_driver/front: status", + "external_command_checker: joystick_command", + "external_command_checker: remote_command", + ] + rclpy.init() + rclpy.spin(DummyDiagnostics(diags)) + rclpy.shutdown() diff --git a/system/system_diagnostic_graph/example/publisher.py b/system/system_diagnostic_graph/example/publisher.py deleted file mode 100755 index 2e73237284508..0000000000000 --- a/system/system_diagnostic_graph/example/publisher.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/usr/bin/env python3 - -# 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. - -import argparse - -from diagnostic_msgs.msg import DiagnosticArray -from diagnostic_msgs.msg import DiagnosticStatus -import rclpy -import rclpy.node -import rclpy.qos - - -class DummyDiagnostics(rclpy.node.Node): - def __init__(self, array): - super().__init__("dummy_diagnostics") - qos = rclpy.qos.qos_profile_system_default - self.diags = self.create_publisher(DiagnosticArray, "/diagnostics", qos) - self.timer = self.create_timer(0.5, self.on_timer) - self.array = [self.create_status(*data) for data in array] - - def on_timer(self): - diagnostics = DiagnosticArray() - diagnostics.header.stamp = self.get_clock().now().to_msg() - diagnostics.status = self.array - self.diags.publish(diagnostics) - - @staticmethod - def create_status(name: str, level: int): - return DiagnosticStatus(level=level, name=name, message="OK", hardware_id="test") - - -if __name__ == "__main__": - data = { - "ok": [ - ("/sensing/lidars/top", DiagnosticStatus.OK), - ("/sensing/lidars/front", DiagnosticStatus.OK), - ("/sensing/radars/front", DiagnosticStatus.OK), - ("/planning/route", DiagnosticStatus.OK), - ("/external/remote_command", DiagnosticStatus.OK), - ], - "front-lidar": [ - ("/sensing/lidars/top", DiagnosticStatus.OK), - ("/sensing/lidars/front", DiagnosticStatus.ERROR), - ("/sensing/radars/front", DiagnosticStatus.OK), - ("/planning/route", DiagnosticStatus.OK), - ("/external/remote_command", DiagnosticStatus.OK), - ], - "front-radar": [ - ("/sensing/lidars/top", DiagnosticStatus.OK), - ("/sensing/lidars/front", DiagnosticStatus.OK), - ("/sensing/radars/front", DiagnosticStatus.ERROR), - ("/planning/route", DiagnosticStatus.OK), - ("/external/remote_command", DiagnosticStatus.OK), - ], - "front": [ - ("/sensing/lidars/top", DiagnosticStatus.OK), - ("/sensing/lidars/front", DiagnosticStatus.ERROR), - ("/sensing/radars/front", DiagnosticStatus.ERROR), - ("/planning/route", DiagnosticStatus.OK), - ("/external/remote_command", DiagnosticStatus.OK), - ], - } - - parser = argparse.ArgumentParser() - parser.add_argument("--data", default="ok") - args = parser.parse_args() - - rclpy.init() - rclpy.spin(DummyDiagnostics(data[args.data])) - rclpy.shutdown() diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp index b33a7deb66fb6..305860af32840 100644 --- a/system/system_diagnostic_graph/src/core/config.cpp +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -17,106 +17,245 @@ #include #include -#include #include #include +#include + +// DEBUG +#include namespace system_diagnostic_graph { -ConfigError create_error(const FileConfig & config, const std::string & message) +ErrorMarker::ErrorMarker(const std::string & file) +{ + file_ = file; +} + +std::string ErrorMarker::str() const +{ + if (type_.empty()) { + return file_; + } + + std::string result = type_; + for (const auto & index : indices_) { + result += "-" + std::to_string(index); + } + return result + " in " + file_; +} + +ErrorMarker ErrorMarker::type(const std::string & type) const { - const std::string marker = config ? "File:" + config->path : "Parameter"; - return ConfigError(message + " (" + marker + ")"); + ErrorMarker mark = *this; + mark.type_ = type; + return mark; } -ConfigError create_error(const NodeConfig & config, const std::string & message) +ErrorMarker ErrorMarker::index(size_t index) const { - const std::string marker = "File:" + config->path + ", Node:" + config->name; - return ConfigError(message + " (" + marker + ")"); + ErrorMarker mark = *this; + mark.indices_.push_back(index); + return mark; } -NodeConfig parse_config_node(YAML::Node yaml, const FileConfig & scope) +ConfigObject::ConfigObject(const ErrorMarker & mark, YAML::Node yaml, const std::string & type) { if (!yaml.IsMap()) { - throw create_error(scope, "node object is not a dict"); + throw create_error(mark, type + " is not a dict type"); } - if (!yaml["name"]) { - throw create_error(scope, "node object has no 'name' field"); + for (const auto & kv : yaml) { + dict_[kv.first.as()] = kv.second; } + mark_ = mark; + type_ = type; +} - const auto config = std::make_shared(); - config->path = scope->path; - config->name = take(yaml, "name"); - config->yaml = yaml; - return config; +ErrorMarker ConfigObject::mark() const +{ + return mark_; } -FileConfig parse_config_path(YAML::Node yaml, const FileConfig & scope) +std::optional ConfigObject::take_yaml(const std::string & name) { - if (!yaml.IsMap()) { - throw create_error(scope, "file object is not a dict"); + if (!dict_.count(name)) { + return std::nullopt; } - if (!yaml["package"]) { - throw create_error(scope, "file object has no 'package' field"); + const auto yaml = dict_.at(name); + dict_.erase(name); + return yaml; +} + +std::string ConfigObject::take_text(const std::string & name) +{ + if (!dict_.count(name)) { + throw create_error(mark_, "object has no '" + name + "' field"); } - if (!yaml["path"]) { - throw create_error(scope, "file object has no 'path' field"); + + const auto yaml = dict_.at(name); + dict_.erase(name); + return yaml.as(); +} + +std::string ConfigObject::take_text(const std::string & name, const std::string & fail) +{ + if (!dict_.count(name)) { + return fail; } - const auto package_name = yaml["package"].as(); - const auto package_path = ament_index_cpp::get_package_share_directory(package_name); - return parse_config_path(package_path + "/" + yaml["path"].as(), scope); + const auto yaml = dict_.at(name); + dict_.erase(name); + return yaml.as(); } -FileConfig parse_config_path(const std::string & path, const FileConfig & scope) +std::vector ConfigObject::take_list(const std::string & name) { - if (!std::filesystem::exists(path)) { - throw create_error(scope, "config file '" + path + "' does not exist"); + if (!dict_.count(name)) { + return std::vector(); } - return parse_config_file(path); + + const auto yaml = dict_.at(name); + dict_.erase(name); + + if (!yaml.IsSequence()) { + throw ConfigError("the '" + name + "' field is not a list type"); + } + return std::vector(yaml.begin(), yaml.end()); +} + +bool ConfigFilter::check(const std::string & mode) const +{ + if (!excludes.empty() && excludes.count(mode) != 0) return false; + if (!includes.empty() && includes.count(mode) == 0) return false; + return true; } -FileConfig parse_config_file(const std::string & path) +ConfigError create_error(const ErrorMarker & mark, const std::string & message) { - const auto config = std::make_shared(); - config->path = path; + (void)mark; + return ConfigError(message); +} - const auto yaml = YAML::LoadFile(path); - if (!yaml.IsMap()) { - throw create_error(config, "config file is not a dict"); +ConfigFilter parse_mode_filter(const ErrorMarker & mark, std::optional yaml) +{ + std::unordered_set excludes; + std::unordered_set includes; + if (yaml) { + ConfigObject dict(mark, yaml.value(), "mode filter"); + + for (const auto & mode : dict.take_list("except")) { + excludes.emplace(mode.as()); + } + for (const auto & mode : dict.take_list("only")) { + includes.emplace(mode.as()); + } } + return ConfigFilter{excludes, includes}; +} + +FileConfig parse_file_config(const ErrorMarker & mark, YAML::Node yaml) +{ + ConfigObject dict(mark, yaml, "file object"); + const auto relative_path = dict.take_text("path"); + const auto package_name = dict.take_text("package"); + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return FileConfig{mark, package_path + "/" + relative_path}; +} + +NodeConfig parse_node_config(const ErrorMarker & mark, YAML::Node yaml) +{ + ConfigObject dict(mark, yaml, "node object"); + const auto path = dict.take_text("path"); + const auto mode = parse_mode_filter(dict.mark(), dict.take_yaml("mode")); + return NodeConfig{path, mode, dict}; +} + +ExprConfig parse_expr_config(const ErrorMarker & mark, YAML::Node yaml) +{ + ConfigObject dict(mark, yaml, "expr object"); + return parse_expr_config(dict); +} + +ExprConfig parse_expr_config(ConfigObject & dict) +{ + const auto type = dict.take_text("type"); + const auto mode = parse_mode_filter(dict.mark(), dict.take_yaml("mode")); + return ExprConfig{type, mode, dict}; +} - const auto files = yaml["files"] ? yaml["files"] : YAML::Node(YAML::NodeType::Sequence); - if (!files.IsSequence()) { - throw create_error(config, "files section is not a list"); +void dump(const ConfigFile & config) +{ + std::cout << "=================================================================" << std::endl; + std::cout << config.mark.str() << std::endl; + for (const auto & file : config.files) { + std::cout << " - f: " << file.path << " (" << file.mark.str() << ")" << std::endl; + } + for (const auto & unit : config.units) { + std::cout << " - u: " << unit.path << " (" << unit.dict.mark().str() << ")" << std::endl; } - for (const auto file : files) { - config->files.push_back(parse_config_path(file, config)); + for (const auto & diag : config.diags) { + std::cout << " - d: " << diag.path << " (" << diag.dict.mark().str() << ")" << std::endl; } +} - const auto nodes = yaml["nodes"] ? yaml["nodes"] : YAML::Node(YAML::NodeType::Sequence); - if (!nodes.IsSequence()) { - throw create_error(config, "nodes section is not a list"); +template +auto apply(const ErrorMarker & mark, F & func, const std::vector & list) +{ + std::vector result; + for (size_t i = 0; i < list.size(); ++i) { + result.push_back(func(mark.index(i), list[i])); } - for (const auto node : nodes) { - config->nodes.push_back(parse_config_node(node, config)); + return result; +} + +ConfigFile load_config_file(const FileConfig & file) +{ + if (!std::filesystem::exists(file.path)) { + throw create_error(file.mark, "config file '" + file.path + "' does not exist"); } + const auto yaml = YAML::LoadFile(file.path); + const auto mark = ErrorMarker(file.path); + auto dict = ConfigObject(mark, yaml, "config file"); + + std::vector units; + std::vector diags; + for (const auto & node : dict.take_list("nodes")) { + const auto type = node["type"].as(); + if (type == "diag") { + diags.push_back(node); + } else { + units.push_back(node); + } + } + + ConfigFile config(mark); + config.files = apply(mark.type("file"), parse_file_config, dict.take_list("files")); + config.units = apply(mark.type("unit"), parse_node_config, units); + config.diags = apply(mark.type("diag"), parse_node_config, diags); return config; } -void walk_config_tree(const FileConfig & config, std::vector & nodes) +ConfigFile load_config_root(const std::string & path) { - nodes.insert(nodes.end(), config->nodes.begin(), config->nodes.end()); - for (const auto & file : config->files) walk_config_tree(file, nodes); -} + const auto mark = ErrorMarker("root file"); + std::vector configs; + configs.push_back(load_config_file(FileConfig{mark, path})); -std::vector load_config_file(const std::string & path) -{ - std::vector nodes; - walk_config_tree(parse_config_path(path, nullptr), nodes); - return nodes; + // Use an index because updating the vector invalidates the iterator. + for (size_t i = 0; i < configs.size(); ++i) { + for (const auto & file : configs[i].files) { + configs.push_back(load_config_file(file)); + } + } + + ConfigFile result(mark); + for (const auto & config : configs) { + result.files.insert(result.files.end(), config.files.begin(), config.files.end()); + result.units.insert(result.units.end(), config.units.begin(), config.units.end()); + result.diags.insert(result.diags.end(), config.diags.begin(), config.diags.end()); + } + return result; } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/config.hpp b/system/system_diagnostic_graph/src/core/config.hpp index 6393cb906b119..4d679ef575944 100644 --- a/system/system_diagnostic_graph/src/core/config.hpp +++ b/system/system_diagnostic_graph/src/core/config.hpp @@ -18,8 +18,11 @@ #include #include +#include #include #include +#include +#include #include namespace system_diagnostic_graph @@ -30,38 +33,79 @@ struct ConfigError : public std::runtime_error using runtime_error::runtime_error; }; -struct NodeConfig_ +class ErrorMarker +{ +public: + explicit ErrorMarker(const std::string & file = ""); + std::string str() const; + ErrorMarker type(const std::string & type) const; + ErrorMarker index(size_t index) const; + +private: + std::string file_; + std::string type_; + std::vector indices_; +}; + +class ConfigObject +{ +public: + ConfigObject(const ErrorMarker & mark, YAML::Node yaml, const std::string & type); + ErrorMarker mark() const; + std::optional take_yaml(const std::string & name); + std::string take_text(const std::string & name); + std::string take_text(const std::string & name, const std::string & fail); + std::vector take_list(const std::string & name); + +private: + ErrorMarker mark_; + std::string type_; + std::unordered_map dict_; +}; + +struct ConfigFilter +{ + bool check(const std::string & mode) const; + std::unordered_set excludes; + std::unordered_set includes; +}; + +struct ExprConfig +{ + std::string type; + ConfigFilter mode; + ConfigObject dict; +}; + +struct NodeConfig { std::string path; - std::string name; - YAML::Node yaml; + ConfigFilter mode; + ConfigObject dict; }; -struct FileConfig_ +struct FileConfig { + ErrorMarker mark; std::string path; - std::vector> files; - std::vector> nodes; }; -template -T take(YAML::Node yaml, const std::string & field) +struct ConfigFile { - const auto result = yaml[field].as(); - yaml.remove(field); - return result; -} - -using NodeConfig = std::shared_ptr; -using FileConfig = std::shared_ptr; -ConfigError create_error(const FileConfig & config, const std::string & message); -ConfigError create_error(const NodeConfig & config, const std::string & message); -std::vector load_config_file(const std::string & path); - -NodeConfig parse_config_node(YAML::Node yaml, const FileConfig & scope); -FileConfig parse_config_path(YAML::Node yaml, const FileConfig & scope); -FileConfig parse_config_path(const std::string & path, const FileConfig & scope); -FileConfig parse_config_file(const std::string & path); + explicit ConfigFile(const ErrorMarker & mark) : mark(mark) {} + ErrorMarker mark; + std::vector files; + std::vector units; + std::vector diags; +}; + +using ConfigDict = std::unordered_map; + +ConfigError create_error(const ErrorMarker & mark, const std::string & message); +ConfigFile load_config_root(const std::string & path); + +ExprConfig parse_expr_config(const ErrorMarker & mark, YAML::Node yaml); +ExprConfig parse_expr_config(ConfigObject & dict); } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/debug.cpp b/system/system_diagnostic_graph/src/core/debug.cpp index 337be8c74aded..ed696573521a7 100644 --- a/system/system_diagnostic_graph/src/core/debug.cpp +++ b/system/system_diagnostic_graph/src/core/debug.cpp @@ -14,9 +14,9 @@ #include "debug.hpp" -#include "node.hpp" +#include "graph.hpp" +#include "nodes.hpp" #include "types.hpp" -#include "update.hpp" #include #include @@ -32,10 +32,10 @@ const std::unordered_map level_names = { {DiagnosticStatus::ERROR, "ERROR"}, {DiagnosticStatus::STALE, "STALE"}}; -void DiagGraph::debug() +void Graph::debug() { std::vector lines; - for (const auto & node : graph_.nodes()) { + for (const auto & node : nodes_) { lines.push_back(node->debug()); } @@ -59,17 +59,23 @@ void DiagGraph::debug() DiagDebugData UnitNode::debug() const { - const auto & level = node_.status.level; - const auto & name = node_.status.name; - return DiagDebugData{std::to_string(index()), "unit", name, "-----", level_names.at(level)}; + const auto level_name = level_names.at(level()); + const auto index_name = std::to_string(index()); + return {"unit", index_name, level_name, path_, "-----"}; } DiagDebugData DiagNode::debug() const { - const auto & level = node_.status.level; - const auto & name = node_.status.name; - const auto & hardware = node_.status.hardware_id; - return DiagDebugData{std::to_string(index()), "diag", name, hardware, level_names.at(level)}; + const auto level_name = level_names.at(level()); + const auto index_name = std::to_string(index()); + return {"diag", index_name, level_name, path_, name_}; +} + +DiagDebugData UnknownNode::debug() const +{ + const auto level_name = level_names.at(level()); + const auto index_name = std::to_string(index()); + return {"test", index_name, level_name, path_, "-----"}; } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/expr.cpp b/system/system_diagnostic_graph/src/core/expr.cpp deleted file mode 100644 index dc7ebcf8dd859..0000000000000 --- a/system/system_diagnostic_graph/src/core/expr.cpp +++ /dev/null @@ -1,284 +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 "expr.hpp" - -#include "config.hpp" -#include "graph.hpp" -#include "node.hpp" - -#include -#include -#include -#include - -// -#include - -namespace system_diagnostic_graph -{ - -using LinkStatus = std::vector>; - -void extend(LinkStatus & a, const LinkStatus & b) -{ - a.insert(a.end(), b.begin(), b.end()); -} - -void extend_false(LinkStatus & a, const LinkStatus & b) -{ - for (const auto & p : b) { - a.push_back(std::make_pair(p.first, false)); - } -} - -std::unique_ptr BaseExpr::create(Graph & graph, YAML::Node yaml) -{ - if (!yaml.IsMap()) { - throw ConfigError("expr object is not a dict"); - } - if (!yaml["type"]) { - throw ConfigError("expr object has no 'type' field"); - } - - const auto type = take(yaml, "type"); - - if (type == "unit") { - return std::make_unique(graph, yaml); - } - if (type == "diag") { - return std::make_unique(graph, yaml); - } - if (type == "and") { - return std::make_unique(graph, yaml); - } - if (type == "or") { - return std::make_unique(graph, yaml); - } - if (type == "if") { - return std::make_unique(graph, yaml); - } - if (type == "debug-ok") { - return std::make_unique(DiagnosticStatus::OK); - } - if (type == "debug-warn") { - return std::make_unique(DiagnosticStatus::WARN); - } - if (type == "debug-error") { - return std::make_unique(DiagnosticStatus::ERROR); - } - if (type == "debug-stale") { - return std::make_unique(DiagnosticStatus::STALE); - } - throw ConfigError("unknown expr type: " + type); -} - -ConstExpr::ConstExpr(const DiagnosticLevel level) -{ - level_ = level; -} - -ExprStatus ConstExpr::eval() const -{ - ExprStatus status; - status.level = level_; - return status; -} - -std::vector ConstExpr::get_dependency() const -{ - return {}; -} - -UnitExpr::UnitExpr(Graph & graph, YAML::Node yaml) -{ - if (!yaml["name"]) { - throw ConfigError("unit object has no 'name' field"); - } - const auto name = take(yaml, "name"); - node_ = graph.find_unit(name); - if (!node_) { - throw ConfigError("unit node '" + name + "' does not exist"); - } -} - -ExprStatus UnitExpr::eval() const -{ - ExprStatus status; - status.level = node_->level(); - status.links.push_back(std::make_pair(node_, true)); - return status; -} - -std::vector UnitExpr::get_dependency() const -{ - return {node_}; -} - -DiagExpr::DiagExpr(Graph & graph, YAML::Node yaml) -{ - if (!yaml["name"]) { - throw ConfigError("diag object has no 'name' field"); - } - const auto name = yaml["name"].as(); - const auto hardware = yaml["hardware"].as(""); - node_ = graph.find_diag(name, hardware); - if (!node_) { - node_ = graph.make_diag(name, hardware); - } -} - -ExprStatus DiagExpr::eval() const -{ - ExprStatus status; - status.level = node_->level(); - status.links.push_back(std::make_pair(node_, true)); - return status; -} - -std::vector DiagExpr::get_dependency() const -{ - return {node_}; -} - -AndExpr::AndExpr(Graph & graph, YAML::Node yaml) -{ - if (!yaml["list"]) { - throw ConfigError("expr object has no 'list' field"); - } - if (!yaml["list"].IsSequence()) { - throw ConfigError("list field is not a list"); - } - - for (const auto & node : yaml["list"]) { - list_.push_back(BaseExpr::create(graph, node)); - } -} - -ExprStatus AndExpr::eval() const -{ - std::vector results; - for (const auto & expr : list_) { - results.push_back(expr->eval()); - } - std::vector levels; - for (const auto & result : results) { - levels.push_back(result.level); - } - ExprStatus status; - for (const auto & result : results) { - extend(status.links, result.links); - } - const auto level = *std::max_element(levels.begin(), levels.end()); - status.level = std::min(level, DiagnosticStatus::ERROR); - return status; -} - -std::vector AndExpr::get_dependency() const -{ - std::vector depends; - for (const auto & expr : list_) { - const auto nodes = expr->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - return depends; -} - -OrExpr::OrExpr(Graph & graph, YAML::Node yaml) -{ - if (!yaml["list"]) { - throw ConfigError("expr object has no 'list' field"); - } - if (!yaml["list"].IsSequence()) { - throw ConfigError("list field is not a list"); - } - - for (const auto & node : yaml["list"]) { - list_.push_back(BaseExpr::create(graph, node)); - } -} - -ExprStatus OrExpr::eval() const -{ - std::vector results; - for (const auto & expr : list_) { - results.push_back(expr->eval()); - } - std::vector levels; - for (const auto & result : results) { - levels.push_back(result.level); - } - ExprStatus status; - for (const auto & result : results) { - extend(status.links, result.links); - } - const auto level = *std::min_element(levels.begin(), levels.end()); - status.level = std::min(level, DiagnosticStatus::ERROR); - return status; -} - -std::vector OrExpr::get_dependency() const -{ - std::vector depends; - for (const auto & expr : list_) { - const auto nodes = expr->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - return depends; -} - -IfExpr::IfExpr(Graph & graph, YAML::Node yaml) -{ - if (!yaml["cond"]) { - throw ConfigError("expr object has no 'cond' field"); - } - if (!yaml["then"]) { - throw ConfigError("expr object has no 'then' field"); - } - cond_ = BaseExpr::create(graph, yaml["cond"]); - then_ = BaseExpr::create(graph, yaml["then"]); -} - -ExprStatus IfExpr::eval() const -{ - const auto cond = cond_->eval(); - const auto then = then_->eval(); - ExprStatus status; - if (cond.level == DiagnosticStatus::OK) { - status.level = std::min(then.level, DiagnosticStatus::ERROR); - extend(status.links, cond.links); - extend(status.links, then.links); - } else { - status.level = std::min(cond.level, DiagnosticStatus::ERROR); - extend(status.links, cond.links); - extend_false(status.links, then.links); - } - return status; -} - -std::vector IfExpr::get_dependency() const -{ - std::vector depends; - { - const auto nodes = cond_->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - { - const auto nodes = then_->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - return depends; -} - -} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/exprs.cpp b/system/system_diagnostic_graph/src/core/exprs.cpp new file mode 100644 index 0000000000000..22281f939cad2 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/exprs.cpp @@ -0,0 +1,216 @@ +// 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 "exprs.hpp" + +#include "nodes.hpp" + +#include +#include +#include +#include +#include + +// DEBUG +#include + +namespace system_diagnostic_graph +{ + +using LinkStatus = std::vector>; + +void extend(LinkStatus & a, const LinkStatus & b) +{ + a.insert(a.end(), b.begin(), b.end()); +} + +void extend_false(LinkStatus & a, const LinkStatus & b) +{ + for (const auto & p : b) { + a.push_back(std::make_pair(p.first, false)); + } +} + +auto create_expr_list(ExprInit & exprs, ConfigObject & config) +{ + std::vector> result; + const auto list = config.take_list("list"); + for (size_t i = 0; i < list.size(); ++i) { + auto dict = parse_expr_config(config.mark().index(i), list[i]); + auto expr = exprs.create(dict); + if (expr) { + result.push_back(std::move(expr)); + } + } + return result; +} + +ConstExpr::ConstExpr(const DiagnosticLevel level) +{ + level_ = level; +} + +ExprStatus ConstExpr::eval() const +{ + ExprStatus status; + status.level = level_; + return status; +} + +std::vector ConstExpr::get_dependency() const +{ + return {}; +} + +LinkExpr::LinkExpr(ExprInit & exprs, ConfigObject & config) +{ + // TODO(Takagi, Isamu): remove + (void)config; + (void)exprs; +} + +void LinkExpr::init(ConfigObject & config, std::unordered_map nodes) +{ + const auto path = config.take_text("path"); + if (!nodes.count(path)) { + throw ConfigError("node path '" + path + "' does not exist"); + } + node_ = nodes.at(path); +} + +ExprStatus LinkExpr::eval() const +{ + ExprStatus status; + status.level = node_->level(); + status.links.push_back(std::make_pair(node_, true)); + return status; +} + +std::vector LinkExpr::get_dependency() const +{ + return {node_}; +} + +AndExpr::AndExpr(ExprInit & exprs, ConfigObject & config, bool short_circuit) +{ + list_ = create_expr_list(exprs, config); + short_circuit_ = short_circuit; +} + +ExprStatus AndExpr::eval() const +{ + if (list_.empty()) { + ExprStatus status; + status.level = DiagnosticStatus::OK; + return status; + } + + ExprStatus status; + status.level = DiagnosticStatus::OK; + for (const auto & expr : list_) { + const auto result = expr->eval(); + status.level = std::max(status.level, result.level); + extend(status.links, result.links); + if (short_circuit_ && status.level != DiagnosticStatus::OK) { + break; + } + } + status.level = std::min(status.level, DiagnosticStatus::ERROR); + return status; +} + +std::vector AndExpr::get_dependency() const +{ + std::vector depends; + for (const auto & expr : list_) { + const auto nodes = expr->get_dependency(); + depends.insert(depends.end(), nodes.begin(), nodes.end()); + } + return depends; +} + +OrExpr::OrExpr(ExprInit & exprs, ConfigObject & config) +{ + list_ = create_expr_list(exprs, config); +} + +ExprStatus OrExpr::eval() const +{ + if (list_.empty()) { + ExprStatus status; + status.level = DiagnosticStatus::OK; + return status; + } + + ExprStatus status; + status.level = DiagnosticStatus::ERROR; + for (const auto & expr : list_) { + const auto result = expr->eval(); + status.level = std::min(status.level, result.level); + extend(status.links, result.links); + } + status.level = std::min(status.level, DiagnosticStatus::ERROR); + return status; +} + +std::vector OrExpr::get_dependency() const +{ + std::vector depends; + for (const auto & expr : list_) { + const auto nodes = expr->get_dependency(); + depends.insert(depends.end(), nodes.begin(), nodes.end()); + } + return depends; +} + +ExprInit::ExprInit(const std::string & mode) +{ + mode_ = mode; +} + +std::unique_ptr ExprInit::create(ExprConfig config) +{ + if (!config.mode.check(mode_)) { + return nullptr; + } + if (config.type == "link") { + auto expr = std::make_unique(*this, config.dict); + links_.push_back(std::make_pair(expr.get(), config.dict)); + return expr; + } + if (config.type == "and") { + return std::make_unique(*this, config.dict, false); + } + if (config.type == "short-circuit-and") { + return std::make_unique(*this, config.dict, true); + } + if (config.type == "or") { + return std::make_unique(*this, config.dict); + } + if (config.type == "debug-ok") { + return std::make_unique(DiagnosticStatus::OK); + } + if (config.type == "debug-warn") { + return std::make_unique(DiagnosticStatus::WARN); + } + if (config.type == "debug-error") { + return std::make_unique(DiagnosticStatus::ERROR); + } + if (config.type == "debug-stale") { + return std::make_unique(DiagnosticStatus::STALE); + } + throw ConfigError("unknown expr type: " + config.type + " " + config.dict.mark().str()); +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/expr.hpp b/system/system_diagnostic_graph/src/core/exprs.hpp similarity index 68% rename from system/system_diagnostic_graph/src/core/expr.hpp rename to system/system_diagnostic_graph/src/core/exprs.hpp index 541841582180a..f582e019d5691 100644 --- a/system/system_diagnostic_graph/src/core/expr.hpp +++ b/system/system_diagnostic_graph/src/core/exprs.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CORE__EXPR_HPP_ -#define CORE__EXPR_HPP_ +#ifndef CORE__EXPRS_HPP_ +#define CORE__EXPRS_HPP_ +#include "config.hpp" #include "types.hpp" -#include - #include #include +#include #include #include @@ -36,7 +36,6 @@ struct ExprStatus class BaseExpr { public: - static std::unique_ptr create(Graph & graph, YAML::Node yaml); virtual ~BaseExpr() = default; virtual ExprStatus eval() const = 0; virtual std::vector get_dependency() const = 0; @@ -53,43 +52,34 @@ class ConstExpr : public BaseExpr DiagnosticLevel level_; }; -class UnitExpr : public BaseExpr +class LinkExpr : public BaseExpr { public: - UnitExpr(Graph & graph, YAML::Node yaml); + LinkExpr(ExprInit & exprs, ConfigObject & config); + void init(ConfigObject & config, std::unordered_map nodes); ExprStatus eval() const override; std::vector get_dependency() const override; private: - UnitNode * node_; -}; - -class DiagExpr : public BaseExpr -{ -public: - DiagExpr(Graph & graph, YAML::Node yaml); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - DiagNode * node_; + BaseNode * node_; }; class AndExpr : public BaseExpr { public: - AndExpr(Graph & graph, YAML::Node yaml); + AndExpr(ExprInit & exprs, ConfigObject & config, bool short_circuit); ExprStatus eval() const override; std::vector get_dependency() const override; private: + bool short_circuit_; std::vector> list_; }; class OrExpr : public BaseExpr { public: - OrExpr(Graph & graph, YAML::Node yaml); + OrExpr(ExprInit & exprs, ConfigObject & config); ExprStatus eval() const override; std::vector get_dependency() const override; @@ -97,18 +87,18 @@ class OrExpr : public BaseExpr std::vector> list_; }; -class IfExpr : public BaseExpr +class ExprInit { public: - IfExpr(Graph & graph, YAML::Node yaml); - ExprStatus eval() const override; - std::vector get_dependency() const override; + explicit ExprInit(const std::string & mode); + std::unique_ptr create(ExprConfig config); + auto get() const { return links_; } private: - std::unique_ptr cond_; - std::unique_ptr then_; + std::string mode_; + std::vector> links_; }; } // namespace system_diagnostic_graph -#endif // CORE__EXPR_HPP_ +#endif // CORE__EXPRS_HPP_ diff --git a/system/system_diagnostic_graph/src/core/graph.cpp b/system/system_diagnostic_graph/src/core/graph.cpp index ba0e3cfd2e016..b4fd1d15827f3 100644 --- a/system/system_diagnostic_graph/src/core/graph.cpp +++ b/system/system_diagnostic_graph/src/core/graph.cpp @@ -14,58 +14,32 @@ #include "graph.hpp" -#include "node.hpp" +#include "config.hpp" +#include "exprs.hpp" +#include "nodes.hpp" #include -#include #include +#include +#include #include +#include -// +// DEBUG #include namespace system_diagnostic_graph { -UnitNode * Graph::make_unit(const std::string & name) +void topological_sort(std::vector> & input) { - const auto key = name; - auto unit = std::make_unique(key); - units_[key] = unit.get(); - nodes_.emplace_back(std::move(unit)); - return units_[key]; -} - -UnitNode * Graph::find_unit(const std::string & name) -{ - const auto key = name; - return units_.count(key) ? units_.at(key) : nullptr; -} - -DiagNode * Graph::make_diag(const std::string & name, const std::string & hardware) -{ - const auto key = std::make_pair(name, hardware); - auto diag = std::make_unique(name, hardware); - diags_[key] = diag.get(); - nodes_.emplace_back(std::move(diag)); - return diags_[key]; -} - -DiagNode * Graph::find_diag(const std::string & name, const std::string & hardware) -{ - const auto key = std::make_pair(name, hardware); - return diags_.count(key) ? diags_.at(key) : nullptr; -} - -void Graph::topological_sort() -{ - std::map degrees; + std::unordered_map degrees; std::deque nodes; std::deque result; std::deque buffer; // Create a list of raw pointer nodes. - for (const auto & node : nodes_) { + for (const auto & node : input) { nodes.push_back(node.get()); } @@ -104,15 +78,127 @@ void Graph::topological_sort() result = std::deque(result.rbegin(), result.rend()); // Replace node vector. - std::map indices; + std::unordered_map indices; for (size_t i = 0; i < result.size(); ++i) { indices[result[i]] = i; } - std::vector> temp(nodes_.size()); - for (auto & node : nodes_) { - temp[indices[node.get()]] = std::move(node); + std::vector> sorted(input.size()); + for (auto & node : input) { + sorted[indices[node.get()]] = std::move(node); + } + input = std::move(sorted); +} + +Graph::Graph() +{ + // for unique_ptr +} + +Graph::~Graph() +{ + // for unique_ptr +} + +void Graph::init(const std::string & file, const std::string & mode) +{ + ConfigFile root = load_config_root(file); + + std::vector> nodes; + std::unordered_map diags; + std::unordered_map paths; + ExprInit exprs(mode); + + for (auto & config : root.units) { + if (config.mode.check(mode)) { + auto node = std::make_unique(config.path); + nodes.push_back(std::move(node)); + } + } + for (auto & config : root.diags) { + if (config.mode.check(mode)) { + auto node = std::make_unique(config.path, config.dict); + diags[node->name()] = node.get(); + nodes.push_back(std::move(node)); + } + } + + // TODO(Takagi, Isamu): unknown diag names + { + auto node = std::make_unique("/unknown"); + unknown_ = node.get(); + nodes.push_back(std::move(node)); + } + + for (const auto & node : nodes) { + paths[node->path()] = node.get(); + } + + for (auto & config : root.units) { + if (paths.count(config.path)) { + paths.at(config.path)->create(config.dict, exprs); + } + } + for (auto & config : root.diags) { + if (paths.count(config.path)) { + paths.at(config.path)->create(config.dict, exprs); + } + } + + for (auto & [link, config] : exprs.get()) { + link->init(config, paths); + } + + // Sort unit nodes in topological order for update dependencies. + topological_sort(nodes); + + // Set the link index for the ros message. + for (size_t i = 0; i < nodes.size(); ++i) { + nodes[i]->set_index(i); + } + + nodes_ = std::move(nodes); + diags_ = diags; +} + +void Graph::callback(const DiagnosticArray & array, const rclcpp::Time & stamp) +{ + for (const auto & status : array.status) { + const auto iter = diags_.find(status.name); + if (iter != diags_.end()) { + iter->second->callback(status, stamp); + } else { + unknown_->callback(status, stamp); + } + } +} + +void Graph::update(const rclcpp::Time & stamp) +{ + for (const auto & node : nodes_) { + node->update(stamp); + } + stamp_ = stamp; +} + +DiagnosticGraph Graph::message() const +{ + DiagnosticGraph result; + result.stamp = stamp_; + result.nodes.reserve(nodes_.size()); + for (const auto & node : nodes_) { + result.nodes.push_back(node->report()); + } + return result; +} + +std::vector Graph::nodes() const +{ + std::vector result; + result.reserve(nodes_.size()); + for (const auto & node : nodes_) { + result.push_back(node.get()); } - nodes_ = std::move(temp); + return result; } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/graph.hpp b/system/system_diagnostic_graph/src/core/graph.hpp index a3f46760388f7..e0060c9111592 100644 --- a/system/system_diagnostic_graph/src/core/graph.hpp +++ b/system/system_diagnostic_graph/src/core/graph.hpp @@ -17,29 +17,36 @@ #include "types.hpp" -#include +#include + #include #include +#include #include #include namespace system_diagnostic_graph { -class Graph +class Graph final { public: - UnitNode * make_unit(const std::string & name); - UnitNode * find_unit(const std::string & name); - DiagNode * make_diag(const std::string & name, const std::string & hardware); - DiagNode * find_diag(const std::string & name, const std::string & hardware); - void topological_sort(); - const std::vector> & nodes() { return nodes_; } + Graph(); + ~Graph(); + + void init(const std::string & file, const std::string & mode); + void callback(const DiagnosticArray & array, const rclcpp::Time & stamp); + void update(const rclcpp::Time & stamp); + DiagnosticGraph message() const; + std::vector nodes() const; + + void debug(); private: std::vector> nodes_; - std::map units_; - std::map, DiagNode *> diags_; + std::unordered_map diags_; + UnknownNode * unknown_; + rclcpp::Time stamp_; }; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/modes.cpp b/system/system_diagnostic_graph/src/core/modes.cpp new file mode 100644 index 0000000000000..0ca18f84b0407 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/modes.cpp @@ -0,0 +1,75 @@ +// 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 "modes.hpp" + +#include "config.hpp" +#include "nodes.hpp" + +#include +#include +#include + +namespace system_diagnostic_graph +{ + +OperationModes::OperationModes(rclcpp::Node & node, const std::vector & graph) +{ + pub_ = node.create_publisher("/system/operation_mode/availability", rclcpp::QoS(1)); + + using PathNodes = std::unordered_map; + PathNodes paths; + for (const auto & node : graph) { + paths[node->path()] = node; + } + + const auto find_node = [](const PathNodes & paths, const std::string & name) { + const auto iter = paths.find(name); + if (iter != paths.end()) { + return iter->second; + } + throw ConfigError("summary node '" + name + "' does node exist"); + }; + + // clang-format off + stop_mode_ = find_node(paths, "/autoware/modes/stop"); + autonomous_mode_ = find_node(paths, "/autoware/modes/autonomous"); + local_mode_ = find_node(paths, "/autoware/modes/local"); + remote_mode_ = find_node(paths, "/autoware/modes/remote"); + emergency_stop_mrm_ = find_node(paths, "/autoware/modes/emergency-stop"); + comfortable_stop_mrm_ = find_node(paths, "/autoware/modes/comfortable-stop"); + pull_over_mrm_ = find_node(paths, "/autoware/modes/pull-over"); + // clang-format on +} + +void OperationModes::update(const rclcpp::Time & stamp) const +{ + const auto is_ok = [](const BaseNode * node) { return node->level() == DiagnosticStatus::OK; }; + + // clang-format off + Availability message; + message.stamp = stamp; + message.stop = is_ok(stop_mode_); + message.autonomous = is_ok(autonomous_mode_); + message.local = is_ok(local_mode_); + message.remote = is_ok(remote_mode_); + message.emergency_stop = is_ok(emergency_stop_mrm_); + message.comfortable_stop = is_ok(comfortable_stop_mrm_); + message.pull_over = is_ok(pull_over_mrm_); + // clang-format on + + pub_->publish(message); +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/update.hpp b/system/system_diagnostic_graph/src/core/modes.hpp similarity index 53% rename from system/system_diagnostic_graph/src/core/update.hpp rename to system/system_diagnostic_graph/src/core/modes.hpp index 3cba96ad8203a..ead1ec10dce93 100644 --- a/system/system_diagnostic_graph/src/core/update.hpp +++ b/system/system_diagnostic_graph/src/core/modes.hpp @@ -12,47 +12,40 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CORE__UPDATE_HPP_ -#define CORE__UPDATE_HPP_ +#ifndef CORE__MODES_HPP_ +#define CORE__MODES_HPP_ -#include "graph.hpp" -#include "node.hpp" #include "types.hpp" #include -#include +#include + #include namespace system_diagnostic_graph { -struct Summary -{ - UnitNode * stop_mode; - UnitNode * autonomous_mode; - UnitNode * local_mode; - UnitNode * remote_mode; - UnitNode * emergency_stop_mrm; - UnitNode * comfortable_stop_mrm; - UnitNode * pull_over_mrm; -}; - -class DiagGraph +class OperationModes { public: - void create(const std::string & file); - void callback(const DiagnosticArray & array); - DiagnosticGraph report(const rclcpp::Time & stamp); - OperationModeAvailability summary(const rclcpp::Time & stamp); - - void debug(); + explicit OperationModes(rclcpp::Node & node, const std::vector & graph); + void update(const rclcpp::Time & stamp) const; private: - Graph graph_; - Summary summary_; + using Availability = tier4_system_msgs::msg::OperationModeAvailability; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr pub_; + + BaseNode * stop_mode_; + BaseNode * autonomous_mode_; + BaseNode * local_mode_; + BaseNode * remote_mode_; + BaseNode * emergency_stop_mrm_; + BaseNode * comfortable_stop_mrm_; + BaseNode * pull_over_mrm_; }; } // namespace system_diagnostic_graph -#endif // CORE__UPDATE_HPP_ +#endif // CORE__MODES_HPP_ diff --git a/system/system_diagnostic_graph/src/core/node.cpp b/system/system_diagnostic_graph/src/core/node.cpp deleted file mode 100644 index 1439188d93e18..0000000000000 --- a/system/system_diagnostic_graph/src/core/node.cpp +++ /dev/null @@ -1,94 +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 "node.hpp" - -#include "expr.hpp" - -#include - -#include -#include - -namespace system_diagnostic_graph -{ - -UnitNode::UnitNode(const std::string & name) -{ - node_.status.level = DiagnosticStatus::STALE; - node_.status.name = name; - node_.status.hardware_id = ""; -} - -UnitNode::~UnitNode() -{ - // for unique_ptr -} - -DiagnosticNode UnitNode::report() const -{ - return node_; -} - -void UnitNode::update() -{ - const auto result = expr_->eval(); - node_.status.level = result.level; - node_.links.clear(); - for (const auto & [node, used] : result.links) { - DiagnosticLink link; - link.index = node->index(); - link.used = used; - node_.links.push_back(link); - } -} - -void UnitNode::create(Graph & graph, const NodeConfig & config) -{ - try { - expr_ = BaseExpr::create(graph, config->yaml); - } catch (const ConfigError & error) { - throw create_error(config, error.what()); - } -} - -std::vector UnitNode::links() const -{ - return expr_->get_dependency(); -} - -DiagNode::DiagNode(const std::string & name, const std::string & hardware) -{ - node_.status.level = DiagnosticStatus::STALE; - node_.status.name = name; - node_.status.hardware_id = hardware; -} - -DiagnosticNode DiagNode::report() const -{ - return node_; -} - -void DiagNode::update() -{ - // TODO(Takagi, Isamu): timeout, error hold - // constexpr double timeout = 1.0; // TODO(Takagi, Isamu): parameterize -} - -void DiagNode::callback(const DiagnosticStatus & status) -{ - node_.status = status; -} - -} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/node.hpp b/system/system_diagnostic_graph/src/core/node.hpp deleted file mode 100644 index 359153fc2824a..0000000000000 --- a/system/system_diagnostic_graph/src/core/node.hpp +++ /dev/null @@ -1,84 +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. - -#ifndef CORE__NODE_HPP_ -#define CORE__NODE_HPP_ - -#include "config.hpp" -#include "debug.hpp" -#include "types.hpp" - -#include -#include -#include -#include - -namespace system_diagnostic_graph -{ - -class BaseNode -{ -public: - virtual ~BaseNode() = default; - virtual void update() = 0; - virtual DiagnosticNode report() const = 0; - virtual DiagDebugData debug() const = 0; - virtual std::vector links() const = 0; - - DiagnosticLevel level() const { return node_.status.level; } - std::string name() const { return node_.status.name; } - - size_t index() const { return index_; } - void set_index(const size_t index) { index_ = index; } - -protected: - size_t index_ = 0; - DiagnosticNode node_; -}; - -class UnitNode : public BaseNode -{ -public: - explicit UnitNode(const std::string & name); - ~UnitNode() override; - - DiagnosticNode report() const override; - DiagDebugData debug() const override; - void update() override; - void create(Graph & graph, const NodeConfig & config); - - std::vector links() const override; - -private: - std::unique_ptr expr_; -}; - -class DiagNode : public BaseNode -{ -public: - explicit DiagNode(const std::string & name, const std::string & hardware); - - DiagnosticNode report() const override; - DiagDebugData debug() const override; - void update() override; - void callback(const DiagnosticStatus & status); - - std::vector links() const override { return {}; } - -private: -}; - -} // namespace system_diagnostic_graph - -#endif // CORE__NODE_HPP_ diff --git a/system/system_diagnostic_graph/src/core/nodes.cpp b/system/system_diagnostic_graph/src/core/nodes.cpp new file mode 100644 index 0000000000000..bbc4bb4d42561 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/nodes.cpp @@ -0,0 +1,157 @@ +// 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 "nodes.hpp" + +#include "exprs.hpp" + +#include + +#include +#include + +namespace system_diagnostic_graph +{ + +BaseNode::BaseNode(const std::string & path) : path_(path) +{ + index_ = 0; +} + +UnitNode::UnitNode(const std::string & path) : BaseNode(path) +{ + level_ = DiagnosticStatus::STALE; +} + +UnitNode::~UnitNode() +{ + // for unique_ptr +} + +void UnitNode::create(ConfigObject & config, ExprInit & exprs) +{ + expr_ = exprs.create(parse_expr_config(config)); +} + +void UnitNode::update(const rclcpp::Time &) +{ + const auto result = expr_->eval(); + level_ = result.level; + links_.clear(); + for (const auto & [node, used] : result.links) { + DiagnosticLink link; + link.index = node->index(); + link.used = used; + links_.push_back(link); + } +} + +DiagnosticNode UnitNode::report() const +{ + DiagnosticNode message; + message.status.level = level_; + message.status.name = path_; + message.links = links_; + return message; +} + +DiagnosticLevel UnitNode::level() const +{ + return level_; +} + +std::vector UnitNode::links() const +{ + return expr_->get_dependency(); +} + +DiagNode::DiagNode(const std::string & path, ConfigObject & config) : BaseNode(path) +{ + timeout_ = 3.0; // TODO(Takagi, Isamu): parameterize + name_ = config.take_text("name"); + + status_.level = DiagnosticStatus::STALE; +} + +void DiagNode::create(ConfigObject &, ExprInit &) +{ +} + +void DiagNode::update(const rclcpp::Time & stamp) +{ + if (time_) { + const auto elapsed = (stamp - time_.value()).seconds(); + if (timeout_ < elapsed) { + status_ = DiagnosticStatus(); + status_.level = DiagnosticStatus::STALE; + time_ = std::nullopt; + } + } +} + +DiagnosticNode DiagNode::report() const +{ + DiagnosticNode message; + message.status = status_; + message.status.name = path_; + return message; +} + +DiagnosticLevel DiagNode::level() const +{ + return status_.level; +} + +void DiagNode::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) +{ + status_ = status; + time_ = stamp; +} + +UnknownNode::UnknownNode(const std::string & path) : BaseNode(path) +{ +} + +void UnknownNode::create(ConfigObject &, ExprInit &) +{ +} + +void UnknownNode::update(const rclcpp::Time & stamp) +{ + (void)stamp; +} + +DiagnosticNode UnknownNode::report() const +{ + DiagnosticNode message; + message.status.name = path_; + for (const auto & diag : diagnostics_) { + diagnostic_msgs::msg::KeyValue kv; + kv.key = diag.first; + message.status.values.push_back(kv); + } + return message; +} + +DiagnosticLevel UnknownNode::level() const +{ + return DiagnosticStatus::WARN; +} + +void UnknownNode::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) +{ + diagnostics_[status.name] = stamp; +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/nodes.hpp b/system/system_diagnostic_graph/src/core/nodes.hpp new file mode 100644 index 0000000000000..1a849cf58b268 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/nodes.hpp @@ -0,0 +1,114 @@ +// 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. + +#ifndef CORE__NODES_HPP_ +#define CORE__NODES_HPP_ + +#include "config.hpp" +#include "debug.hpp" +#include "types.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +class BaseNode +{ +public: + explicit BaseNode(const std::string & path); + virtual ~BaseNode() = default; + virtual void create(ConfigObject & config, ExprInit & exprs) = 0; + virtual void update(const rclcpp::Time & stamp) = 0; + virtual DiagnosticNode report() const = 0; + virtual DiagnosticLevel level() const = 0; + virtual DiagDebugData debug() const = 0; + virtual std::vector links() const = 0; + + std::string path() const { return path_; } + + size_t index() const { return index_; } + void set_index(const size_t index) { index_ = index; } + +protected: + const std::string path_; + size_t index_; +}; + +class UnitNode : public BaseNode +{ +public: + explicit UnitNode(const std::string & path); + ~UnitNode() override; + void create(ConfigObject & config, ExprInit & exprs) override; + void update(const rclcpp::Time & stamp) override; + DiagnosticNode report() const override; + DiagnosticLevel level() const override; + DiagDebugData debug() const override; + std::vector links() const override; + +private: + std::unique_ptr expr_; + std::vector links_; + DiagnosticLevel level_; +}; + +class DiagNode : public BaseNode +{ +public: + DiagNode(const std::string & path, ConfigObject & config); + void create(ConfigObject & config, ExprInit & exprs) override; + void update(const rclcpp::Time & stamp) override; + DiagnosticNode report() const override; + DiagnosticLevel level() const override; + DiagDebugData debug() const override; + std::vector links() const override { return {}; } + std::string name() const { return name_; } + + void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); + +private: + double timeout_; + std::optional time_; + std::string name_; + DiagnosticStatus status_; +}; + +class UnknownNode : public BaseNode +{ +public: + explicit UnknownNode(const std::string & path); + void create(ConfigObject & config, ExprInit & exprs) override; + void update(const rclcpp::Time & stamp) override; + DiagnosticNode report() const override; + DiagnosticLevel level() const override; + DiagDebugData debug() const override; + std::vector links() const override { return {}; } + + void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); + +private: + std::map diagnostics_; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__NODES_HPP_ diff --git a/system/system_diagnostic_graph/src/core/types.hpp b/system/system_diagnostic_graph/src/core/types.hpp index 75167958e49bc..2adfbb3f9d4ef 100644 --- a/system/system_diagnostic_graph/src/core/types.hpp +++ b/system/system_diagnostic_graph/src/core/types.hpp @@ -20,7 +20,6 @@ #include #include #include -#include namespace system_diagnostic_graph { @@ -30,15 +29,15 @@ using diagnostic_msgs::msg::DiagnosticStatus; using tier4_system_msgs::msg::DiagnosticGraph; using tier4_system_msgs::msg::DiagnosticLink; using tier4_system_msgs::msg::DiagnosticNode; -using tier4_system_msgs::msg::OperationModeAvailability; - using DiagnosticLevel = DiagnosticStatus::_level_type; -class Graph; class BaseNode; class UnitNode; class DiagNode; +class UnknownNode; + class BaseExpr; +class ExprInit; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/update.cpp b/system/system_diagnostic_graph/src/core/update.cpp deleted file mode 100644 index bb42dcba12192..0000000000000 --- a/system/system_diagnostic_graph/src/core/update.cpp +++ /dev/null @@ -1,116 +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 "update.hpp" - -#include "config.hpp" - -#include -#include -#include -#include -#include -#include - -namespace system_diagnostic_graph -{ - -UnitNode * find_node(Graph & graph, const std::string & name) -{ - const auto node = graph.find_unit(name); - if (!node) { - throw ConfigError("summary node '" + name + "' does node exist"); - } - return node; -}; - -void DiagGraph::create(const std::string & file) -{ - const auto configs = load_config_file(file); - - // Create unit nodes first because it is necessary for the link. - std::vector> units; - for (const auto & config : configs) { - UnitNode * unit = graph_.make_unit(config->name); - units.push_back(std::make_pair(config, unit)); - } - - // Reflect the config after creating all the unit nodes, - for (auto & [config, unit] : units) { - unit->create(graph_, config); - } - - // Sort unit nodes in topological order for update dependencies. - graph_.topological_sort(); - - // Set the link index for the ros message. - const auto & nodes = graph_.nodes(); - for (size_t i = 0; i < nodes.size(); ++i) { - nodes[i]->set_index(i); - } - - // Get reserved unit node for summary. - summary_.stop_mode = find_node(graph_, "/autoware/operation/stop"); - summary_.autonomous_mode = find_node(graph_, "/autoware/operation/autonomous"); - summary_.local_mode = find_node(graph_, "/autoware/operation/local"); - summary_.remote_mode = find_node(graph_, "/autoware/operation/remote"); - summary_.emergency_stop_mrm = find_node(graph_, "/autoware/operation/emergency-stop"); - summary_.comfortable_stop_mrm = find_node(graph_, "/autoware/operation/comfortable-stop"); - summary_.pull_over_mrm = find_node(graph_, "/autoware/operation/pull-over"); -} - -DiagnosticGraph DiagGraph::report(const rclcpp::Time & stamp) -{ - DiagnosticGraph message; - message.stamp = stamp; - message.nodes.reserve(graph_.nodes().size()); - - for (const auto & node : graph_.nodes()) { - node->update(); - } - for (const auto & node : graph_.nodes()) { - message.nodes.push_back(node->report()); - } - return message; -} - -OperationModeAvailability DiagGraph::summary(const rclcpp::Time & stamp) -{ - const auto is_ok = [](const UnitNode * node) { return node->level() == DiagnosticStatus::OK; }; - - OperationModeAvailability message; - message.stamp = stamp; - message.stop = is_ok(summary_.stop_mode); - message.autonomous = is_ok(summary_.autonomous_mode); - message.local = is_ok(summary_.local_mode); - message.remote = is_ok(summary_.remote_mode); - message.emergency_stop = is_ok(summary_.emergency_stop_mrm); - message.comfortable_stop = is_ok(summary_.comfortable_stop_mrm); - message.pull_over = is_ok(summary_.pull_over_mrm); - return message; -} - -void DiagGraph::callback(const DiagnosticArray & array) -{ - for (const auto & status : array.status) { - auto diag = graph_.find_diag(status.name, status.hardware_id); - if (diag) { - diag->callback(status); - } else { - // TODO(Takagi, Isamu): handle unknown diagnostics - } - } -} - -} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/main.cpp b/system/system_diagnostic_graph/src/main.cpp index 38b9fa5bacb3b..722ddcf833577 100644 --- a/system/system_diagnostic_graph/src/main.cpp +++ b/system/system_diagnostic_graph/src/main.cpp @@ -22,6 +22,19 @@ namespace system_diagnostic_graph MainNode::MainNode() : Node("system_diagnostic_graph_aggregator") { + // Init diagnostics graph. + { + const auto file = declare_parameter("graph_file"); + const auto mode = declare_parameter("mode"); + graph_.init(file, mode); + graph_.debug(); + } + + // Init plugins + if (declare_parameter("mode_availability")) { + modes_ = std::make_unique(*this, graph_.nodes()); + } + // Init ros interface. { using std::placeholders::_1; @@ -31,32 +44,30 @@ MainNode::MainNode() : Node("system_diagnostic_graph_aggregator") const auto callback = std::bind(&MainNode::on_diag, this, _1); sub_input_ = create_subscription("/diagnostics", qos_input, callback); pub_graph_ = create_publisher("/diagnostics_graph", qos_graph); - pub_modes_ = create_publisher( - "/system/operation_mode/availability", rclcpp::QoS(1)); const auto rate = rclcpp::Rate(declare_parameter("rate")); timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); } +} - // Init diagnostics graph. - { - const auto file = declare_parameter("graph_file"); - graph_.create(file); - graph_.debug(); - } +MainNode::~MainNode() +{ + // for unique_ptr } void MainNode::on_timer() { - const auto data = graph_.report(now()); + const auto stamp = now(); + graph_.update(stamp); graph_.debug(); - pub_graph_->publish(data); - pub_modes_->publish(graph_.summary(now())); + pub_graph_->publish(graph_.message()); + + if (modes_) modes_->update(stamp); } void MainNode::on_diag(const DiagnosticArray::ConstSharedPtr msg) { - graph_.callback(*msg); + graph_.callback(*msg, now()); } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/main.hpp b/system/system_diagnostic_graph/src/main.hpp index 4cc659c978611..6deb0518cd9d0 100644 --- a/system/system_diagnostic_graph/src/main.hpp +++ b/system/system_diagnostic_graph/src/main.hpp @@ -15,11 +15,14 @@ #ifndef MAIN_HPP_ #define MAIN_HPP_ +#include "core/graph.hpp" +#include "core/modes.hpp" #include "core/types.hpp" -#include "core/update.hpp" #include +#include + namespace system_diagnostic_graph { @@ -27,13 +30,14 @@ class MainNode : public rclcpp::Node { public: MainNode(); + ~MainNode(); private: - DiagGraph graph_; + Graph graph_; + std::unique_ptr modes_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::Subscription::SharedPtr sub_input_; rclcpp::Publisher::SharedPtr pub_graph_; - rclcpp::Publisher::SharedPtr pub_modes_; void on_timer(); void on_diag(const DiagnosticArray::ConstSharedPtr msg); }; diff --git a/system/system_diagnostic_graph/src/tool.hpp b/system/system_diagnostic_graph/src/tool.hpp index 11f6a2632386b..5ad19b8460c4b 100644 --- a/system/system_diagnostic_graph/src/tool.hpp +++ b/system/system_diagnostic_graph/src/tool.hpp @@ -15,7 +15,6 @@ #ifndef TOOL_HPP_ #define TOOL_HPP_ -#include "core/graph.hpp" #include "core/types.hpp" #include diff --git a/system/system_diagnostic_monitor/CMakeLists.txt b/system/system_diagnostic_monitor/CMakeLists.txt deleted file mode 100644 index 909210f99d55e..0000000000000 --- a/system/system_diagnostic_monitor/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(system_diagnostic_monitor) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_package(INSTALL_TO_SHARE config launch script) diff --git a/system/system_diagnostic_monitor/README.md b/system/system_diagnostic_monitor/README.md deleted file mode 100644 index 8dccca34db8c5..0000000000000 --- a/system/system_diagnostic_monitor/README.md +++ /dev/null @@ -1 +0,0 @@ -# System diagnostic monitor diff --git a/system/system_diagnostic_monitor/config/autoware.yaml b/system/system_diagnostic_monitor/config/autoware.yaml deleted file mode 100644 index 892a5da851ba7..0000000000000 --- a/system/system_diagnostic_monitor/config/autoware.yaml +++ /dev/null @@ -1,74 +0,0 @@ -files: - - { package: system_diagnostic_monitor, path: config/map.yaml } - - { package: system_diagnostic_monitor, path: config/localization.yaml } - - { package: system_diagnostic_monitor, path: config/planning.yaml } - - { package: system_diagnostic_monitor, path: config/perception.yaml } - - { package: system_diagnostic_monitor, path: config/control.yaml } - - { package: system_diagnostic_monitor, path: config/vehicle.yaml } - - { package: system_diagnostic_monitor, path: config/system.yaml } - -nodes: - - name: /autoware - type: and - list: - - { type: unit, name: /autoware/operation/stop } - - { type: unit, name: /autoware/operation/autonomous } - - { type: unit, name: /autoware/operation/local } - - { type: unit, name: /autoware/operation/remote } - - { type: unit, name: /autoware/operation/emergency-stop } - - { type: unit, name: /autoware/operation/comfortable-stop } - - { type: unit, name: /autoware/operation/pull-over } - - - name: /autoware/operation/stop - type: debug-ok - - - name: /autoware/operation/autonomous - type: and - list: - - { type: unit, name: /autoware/map } - - { type: unit, name: /autoware/localization } - - { type: unit, name: /autoware/planning } - - { type: unit, name: /autoware/perception } - - { type: unit, name: /autoware/control } - - { type: unit, name: /autoware/vehicle } - - { type: unit, name: /autoware/system } - - - name: /autoware/operation/local - type: and - list: - - { type: unit, name: /autoware/vehicle } - - { type: unit, name: /autoware/system } - - - name: /autoware/operation/remote - type: and - list: - - { type: unit, name: /autoware/vehicle } - - { type: unit, name: /autoware/system } - - - name: /autoware/operation/emergency-stop - type: and - list: - - { type: unit, name: /autoware/vehicle } - - { type: unit, name: /autoware/system } - - - name: /autoware/operation/comfortable-stop - type: and - list: - - { type: unit, name: /autoware/map } - - { type: unit, name: /autoware/localization } - - { type: unit, name: /autoware/planning } - - { type: unit, name: /autoware/perception } - - { type: unit, name: /autoware/control } - - { type: unit, name: /autoware/vehicle } - - { type: unit, name: /autoware/system } - - - name: /autoware/operation/pull-over - type: and - list: - - { type: unit, name: /autoware/map } - - { type: unit, name: /autoware/localization } - - { type: unit, name: /autoware/planning } - - { type: unit, name: /autoware/perception } - - { type: unit, name: /autoware/control } - - { type: unit, name: /autoware/vehicle } - - { type: unit, name: /autoware/system } diff --git a/system/system_diagnostic_monitor/config/control.yaml b/system/system_diagnostic_monitor/config/control.yaml deleted file mode 100644 index 8884a79847c71..0000000000000 --- a/system/system_diagnostic_monitor/config/control.yaml +++ /dev/null @@ -1,10 +0,0 @@ -nodes: - - name: /autoware/control - type: and - list: - - type: diag - name: "topic_state_monitor_trajectory_follower_control_cmd: control_topic_status" - hardware: topic_state_monitor - - type: diag - name: "topic_state_monitor_control_command_control_cmd: control_topic_status" - hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/localization.yaml b/system/system_diagnostic_monitor/config/localization.yaml deleted file mode 100644 index 26d680b6c4f0f..0000000000000 --- a/system/system_diagnostic_monitor/config/localization.yaml +++ /dev/null @@ -1,5 +0,0 @@ -nodes: - - name: /autoware/localization - type: and - list: - - { type: diag, name: "component_state_diagnostics: localization_state" } diff --git a/system/system_diagnostic_monitor/config/map.yaml b/system/system_diagnostic_monitor/config/map.yaml deleted file mode 100644 index 7bee7419200bd..0000000000000 --- a/system/system_diagnostic_monitor/config/map.yaml +++ /dev/null @@ -1,7 +0,0 @@ -nodes: - - name: /autoware/map - type: and - list: - - type: diag - name: "topic_state_monitor_vector_map: map_topic_status" - hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/perception.yaml b/system/system_diagnostic_monitor/config/perception.yaml deleted file mode 100644 index b6b4ec27d5596..0000000000000 --- a/system/system_diagnostic_monitor/config/perception.yaml +++ /dev/null @@ -1,7 +0,0 @@ -nodes: - - name: /autoware/perception - type: and - list: - - type: diag - name: "topic_state_monitor_object_recognition_objects: perception_topic_status" - hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/planning.yaml b/system/system_diagnostic_monitor/config/planning.yaml deleted file mode 100644 index c29059193081b..0000000000000 --- a/system/system_diagnostic_monitor/config/planning.yaml +++ /dev/null @@ -1,15 +0,0 @@ -nodes: - - name: /autoware/planning - type: if - cond: - type: diag - name: "component_state_diagnostics: route_state" - then: - type: and - list: - - type: diag - name: "topic_state_monitor_mission_planning_route: planning_topic_status" - hardware: topic_state_monitor - - type: diag - name: "topic_state_monitor_scenario_planning_trajectory: planning_topic_status" - hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/system.yaml b/system/system_diagnostic_monitor/config/system.yaml deleted file mode 100644 index 0377e68daaae6..0000000000000 --- a/system/system_diagnostic_monitor/config/system.yaml +++ /dev/null @@ -1,7 +0,0 @@ -nodes: - - name: /autoware/system - type: and - list: - - type: diag - name: "topic_state_monitor_system_emergency_control_cmd: system_topic_status" - hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/vehicle.yaml b/system/system_diagnostic_monitor/config/vehicle.yaml deleted file mode 100644 index 4826c96e5f72f..0000000000000 --- a/system/system_diagnostic_monitor/config/vehicle.yaml +++ /dev/null @@ -1,10 +0,0 @@ -nodes: - - name: /autoware/vehicle - type: and - list: - - type: diag - name: "topic_state_monitor_vehicle_status_velocity_status: vehicle_topic_status" - hardware: topic_state_monitor - - type: diag - name: "topic_state_monitor_vehicle_status_steering_status: vehicle_topic_status" - hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml deleted file mode 100644 index a13b1dc9b78bc..0000000000000 --- a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/system/system_diagnostic_monitor/package.xml b/system/system_diagnostic_monitor/package.xml deleted file mode 100644 index d410e75000876..0000000000000 --- a/system/system_diagnostic_monitor/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - system_diagnostic_monitor - 0.1.0 - The system_diagnostic_monitor package - Takagi, Isamu - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - system_diagnostic_graph - - autoware_adapi_v1_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/system/system_diagnostic_monitor/script/component_state_diagnostics.py b/system/system_diagnostic_monitor/script/component_state_diagnostics.py deleted file mode 100755 index 8e12ed6656674..0000000000000 --- a/system/system_diagnostic_monitor/script/component_state_diagnostics.py +++ /dev/null @@ -1,79 +0,0 @@ -#!/usr/bin/env python3 - -# 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. - -from autoware_adapi_v1_msgs.msg import LocalizationInitializationState as LocalizationState -from autoware_adapi_v1_msgs.msg import RouteState -from diagnostic_msgs.msg import DiagnosticArray -from diagnostic_msgs.msg import DiagnosticStatus -import rclpy -import rclpy.node -import rclpy.qos - - -class ComponentStateDiagnostics(rclpy.node.Node): - def __init__(self): - super().__init__("component_state_diagnostics") - durable_qos = rclpy.qos.QoSProfile( - depth=1, - durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, - reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, - ) - - self.timer = self.create_timer(0.5, self.on_timer) - self.pub = self.create_publisher(DiagnosticArray, "/diagnostics", 1) - self.sub1 = self.create_subscription( - LocalizationState, - "/api/localization/initialization_state", - self.on_localization, - durable_qos, - ) - self.sub2 = self.create_subscription( - RouteState, "/api/routing/state", self.on_routing, durable_qos - ) - - self.diags = DiagnosticArray() - self.diags.status.append( - DiagnosticStatus( - level=DiagnosticStatus.STALE, name="component_state_diagnostics: localization_state" - ) - ) - self.diags.status.append( - DiagnosticStatus( - level=DiagnosticStatus.STALE, name="component_state_diagnostics: route_state" - ) - ) - - def on_timer(self): - self.diags.header.stamp = self.get_clock().now().to_msg() - self.pub.publish(self.diags) - - def on_localization(self, msg): - self.diags.status[0].level = ( - DiagnosticStatus.OK - if msg.state == LocalizationState.INITIALIZED - else DiagnosticStatus.ERROR - ) - - def on_routing(self, msg): - self.diags.status[1].level = ( - DiagnosticStatus.OK if msg.state != RouteState.UNSET else DiagnosticStatus.ERROR - ) - - -if __name__ == "__main__": - rclpy.init() - rclpy.spin(ComponentStateDiagnostics()) - rclpy.shutdown() diff --git a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml index 87cf767accc06..f6e13012957aa 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml @@ -26,6 +26,12 @@ contains: ["service_log_checker"] timeout: 5.0 + duplicated_node_checker: + type: diagnostic_aggregator/GenericAnalyzer + path: duplicated_node_checker + contains: ["duplicated_node_checker"] + timeout: 5.0 + resource_monitoring: type: diagnostic_aggregator/AnalyzerGroup path: resource_monitoring diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index 312a2e6186c42..1778f6594f0c3 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -39,6 +39,7 @@ /autoware/system/emergency_stop_operation: default /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" } + /autoware/system/duplicated_node_checker: default /autoware/vehicle/node_alive_monitoring: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index 88431dc406fe3..9784259490ec2 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -38,7 +38,9 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } + /autoware/system/duplicated_node_checker: default # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } + # /autoware/system/duplicated_node_checker: default /autoware/vehicle/node_alive_monitoring: default diff --git a/system/topic_state_monitor/src/topic_state_monitor_core.cpp b/system/topic_state_monitor/src/topic_state_monitor_core.cpp index 326193a58dc4e..68fa3764d8ecd 100644 --- a/system/topic_state_monitor/src/topic_state_monitor_core.cpp +++ b/system/topic_state_monitor/src/topic_state_monitor_core.cpp @@ -151,6 +151,13 @@ void TopicStateMonitorNode::checkTopicStatus(diagnostic_updater::DiagnosticStatu stat.addf("topic", "%s", node_param_.topic.c_str()); } + const auto print_warn = [&](const std::string & msg) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, "%s", msg.c_str()); + }; + const auto print_debug = [&](const std::string & msg) { + RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 3000, "%s", msg.c_str()); + }; + // Judge level int8_t level = DiagnosticStatus::OK; if (topic_status == TopicStatus::Ok) { @@ -159,15 +166,19 @@ void TopicStateMonitorNode::checkTopicStatus(diagnostic_updater::DiagnosticStatu } else if (topic_status == TopicStatus::NotReceived) { level = DiagnosticStatus::ERROR; stat.add("status", "NotReceived"); + print_debug(node_param_.topic + " has not received."); } else if (topic_status == TopicStatus::WarnRate) { level = DiagnosticStatus::WARN; stat.add("status", "WarnRate"); + print_warn(node_param_.topic + " topic rate has dropped to the warning level."); } else if (topic_status == TopicStatus::ErrorRate) { level = DiagnosticStatus::ERROR; stat.add("status", "ErrorRate"); + print_warn(node_param_.topic + " topic rate has dropped to the error level."); } else if (topic_status == TopicStatus::Timeout) { level = DiagnosticStatus::ERROR; stat.add("status", "Timeout"); + print_warn(node_param_.topic + " topic is timeout."); } // Add key-value