diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 5af35b09f52bd..f017e176892e5 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -21,7 +21,6 @@ common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp -common/mission_planner_rviz_plugin/** isamu.takagi@tier4.jp common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @@ -29,27 +28,18 @@ common/path_distance_calculator/** isamu.takagi@tier4.jp common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp -common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/tensorrt_common/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp -common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp -common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp -common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp -common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp -common/tier4_screen_capture_rviz_plugin/** kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp -common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp -common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp +common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp khalil@leodrive.ai common/tier4_system_rviz_plugin/** koji.minoda@tier4.jp -common/tier4_target_object_type_rviz_plugin/** takamasa.horibe@tier4.jp common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -156,13 +146,14 @@ perception/traffic_light_map_based_detector/** shunsuke.miura@tier4.jp tao.zhong perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp +planning/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp -planning/behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp @@ -179,7 +170,6 @@ planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp new file mode 100644 index 0000000000000..09144c1d8ff50 --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp @@ -0,0 +1,36 @@ +// Copyright 2024 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 AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ +#define AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ + +#include + +#include + +namespace autoware_ad_api::system +{ + +struct Heartbeat +{ + using Message = autoware_adapi_v1_msgs::msg::Heartbeat; + static constexpr char name[] = "/api/system/heartbeat"; + static constexpr size_t depth = 10; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace autoware_ad_api::system + +#endif // AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index c0ddd84cd2a30..3715f466e7684 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -1903,20 +1903,21 @@ extern template void insertOrientation -void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2) +void removeFirstInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2) { - for (size_t i = 1; i < points.size();) { - const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1)); - const auto p2 = tier4_autoware_utils::getPose(points.at(i)); + for (auto itr = points.begin(); std::next(itr) != points.end();) { + const auto p1 = tier4_autoware_utils::getPose(*itr); + const auto p2 = tier4_autoware_utils::getPose(*std::next(itr)); const double yaw1 = tf2::getYaw(p1.orientation); const double yaw2 = tf2::getYaw(p2.orientation); if ( max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) || !tier4_autoware_utils::isDrivingForward(p1, p2)) { - points.erase(points.begin() + i); + points.erase(std::next(itr)); + return; } else { - ++i; + itr++; } } } diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 326b6e106e30b..9ebc712b3bf1a 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -5337,10 +5337,10 @@ TEST(trajectory, cropPoints) } } -TEST(Trajectory, removeInvalidOrientationPoints) +TEST(Trajectory, removeFirstInvalidOrientationPoints) { using motion_utils::insertOrientation; - using motion_utils::removeInvalidOrientationPoints; + using motion_utils::removeFirstInvalidOrientationPoints; const double max_yaw_diff = M_PI_2; @@ -5351,7 +5351,7 @@ TEST(Trajectory, removeInvalidOrientationPoints) auto modified_traj = traj; insertOrientation(modified_traj.points, true); modifyTrajectory(modified_traj); - removeInvalidOrientationPoints(modified_traj.points, max_yaw_diff); + removeFirstInvalidOrientationPoints(modified_traj.points, max_yaw_diff); EXPECT_EQ(modified_traj.points.size(), expected_size); for (size_t i = 0; i < modified_traj.points.size() - 1; ++i) { EXPECT_EQ(traj.points.at(i), modified_traj.points.at(i)); @@ -5374,7 +5374,7 @@ TEST(Trajectory, removeInvalidOrientationPoints) [&](Trajectory & t) { auto invalid_point = t.points.back(); invalid_point.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2)); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4)); t.points.push_back(invalid_point); }, traj.points.size()); @@ -5385,21 +5385,10 @@ TEST(Trajectory, removeInvalidOrientationPoints) [&](Trajectory & t) { auto invalid_point = t.points[4]; invalid_point.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2)); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4)); t.points.insert(t.points.begin() + 4, invalid_point); }, traj.points.size()); - - // invalid point at the beginning - testRemoveInvalidOrientationPoints( - traj, - [&](Trajectory & t) { - auto invalid_point = t.points.front(); - invalid_point.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2)); - t.points.insert(t.points.begin(), invalid_point); - }, - 1); // expected size is 1 since only the first point remains } TEST(trajectory, calcYawDeviation) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp index 0c53a9e3941dd..4901e28472acb 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp @@ -15,6 +15,8 @@ #ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ #define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ +#include + namespace tier4_autoware_utils { @@ -22,6 +24,8 @@ float sin(float radian); float cos(float radian); +std::pair sin_and_cos(float radian); + } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/src/math/trigonometry.cpp b/common/tier4_autoware_utils/src/math/trigonometry.cpp index 15f5c71012722..0ce65c7aa5bc8 100644 --- a/common/tier4_autoware_utils/src/math/trigonometry.cpp +++ b/common/tier4_autoware_utils/src/math/trigonometry.cpp @@ -49,4 +49,27 @@ float cos(float radian) return sin(radian + static_cast(tier4_autoware_utils::pi) / 2.f); } +std::pair sin_and_cos(float radian) +{ + constexpr float tmp = + (180.f / static_cast(tier4_autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f); + const float degree = radian * tmp; + size_t idx = + (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % + discrete_arcs_num_360; + + if (idx < discrete_arcs_num_90) { + return {g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]}; + } else if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) { + idx = 2 * discrete_arcs_num_90 - idx; + return {g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]}; + } else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) { + idx = idx - 2 * discrete_arcs_num_90; + return {-g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]}; + } else { // 3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90 + idx = 4 * discrete_arcs_num_90 - idx; + return {-g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]}; + } +} + } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp b/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp index 379418539841c..d7106fd823682 100644 --- a/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp +++ b/common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp @@ -40,3 +40,13 @@ TEST(trigonometry, cos) tier4_autoware_utils::cos(x * static_cast(i))) < 10e-5); } } + +TEST(trigonometry, sin_and_cos) +{ + float x = 4.f * tier4_autoware_utils::pi / 128.f; + for (int i = 0; i < 128; i++) { + const auto sin_and_cos = tier4_autoware_utils::sin_and_cos(x * static_cast(i)); + EXPECT_TRUE(std::abs(std::sin(x * static_cast(i)) - sin_and_cos.first) < 10e-7); + EXPECT_TRUE(std::abs(std::cos(x * static_cast(i)) - sin_and_cos.second) < 10e-7); + } +} diff --git a/common/tier4_state_rviz_plugin/CMakeLists.txt b/common/tier4_state_rviz_plugin/CMakeLists.txt index afe21f66291b2..6b569ddb6d162 100644 --- a/common/tier4_state_rviz_plugin/CMakeLists.txt +++ b/common/tier4_state_rviz_plugin/CMakeLists.txt @@ -13,8 +13,32 @@ add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(${PROJECT_NAME} SHARED src/autoware_state_panel.cpp src/velocity_steering_factors_panel.cpp + src/custom_toggle_switch.cpp + src/custom_slider.cpp + src/custom_container.cpp + src/custom_button.cpp + src/custom_icon_label.cpp + src/custom_segmented_button.cpp + src/custom_segmented_button_item.cpp + src/custom_label.cpp + src/include/material_colors.hpp + src/include/autoware_state_panel.hpp + src/include/custom_button.hpp + src/include/custom_container.hpp + src/include/custom_icon_label.hpp + src/include/custom_label.hpp + src/include/custom_segmented_button_item.hpp + src/include/custom_segmented_button.hpp + src/include/custom_slider.hpp + src/include/custom_toggle_switch.hpp + src/include/velocity_steering_factors_panel.hpp ) +target_include_directories( + ${PROJECT_NAME} PUBLIC +) + + target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ) @@ -22,6 +46,9 @@ target_link_libraries(${PROJECT_NAME} # Export the plugin to be imported by rviz2 pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + + + ament_auto_package( INSTALL_TO_SHARE icons diff --git a/common/tier4_state_rviz_plugin/icons/assets/active.png b/common/tier4_state_rviz_plugin/icons/assets/active.png new file mode 100644 index 0000000000000..db9c81211abd5 Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/active.png differ diff --git a/common/tier4_state_rviz_plugin/icons/assets/crash.png b/common/tier4_state_rviz_plugin/icons/assets/crash.png new file mode 100644 index 0000000000000..18175a823ae4a Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/crash.png differ diff --git a/common/tier4_state_rviz_plugin/icons/assets/danger.png b/common/tier4_state_rviz_plugin/icons/assets/danger.png new file mode 100644 index 0000000000000..baed346deea2d Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/danger.png differ diff --git a/common/tier4_state_rviz_plugin/icons/assets/none.png b/common/tier4_state_rviz_plugin/icons/assets/none.png new file mode 100644 index 0000000000000..c44f9f485dbf1 Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/none.png differ diff --git a/common/tier4_state_rviz_plugin/icons/assets/pending.png b/common/tier4_state_rviz_plugin/icons/assets/pending.png new file mode 100644 index 0000000000000..3925162878fd5 Binary files /dev/null and b/common/tier4_state_rviz_plugin/icons/assets/pending.png differ diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index 6cd134a4a1301..65ccd217d8016 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -6,6 +6,7 @@ The autoware state rviz plugin package Hiroki OTA Takagi, Isamu + Khalil Selyan Apache License 2.0 ament_cmake_auto diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index 8c2889581493c..f9fb7d3dd3958 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -14,15 +14,13 @@ // limitations under the License. // -#include "autoware_state_panel.hpp" +#include "include/autoware_state_panel.hpp" -#include -#include -#include -#include -#include #include +#include +#include + #include #include @@ -35,181 +33,59 @@ namespace rviz_plugins { AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(parent) { - // Gear - auto * gear_prefix_label_ptr = new QLabel("GEAR: "); - gear_prefix_label_ptr->setAlignment(Qt::AlignRight | Qt::AlignVCenter); - gear_label_ptr_ = new QLabel("INIT"); - gear_label_ptr_->setAlignment(Qt::AlignCenter); - auto * gear_layout = new QHBoxLayout; - gear_layout->addWidget(gear_prefix_label_ptr); - gear_layout->addWidget(gear_label_ptr_); - - // Velocity Limit - velocity_limit_button_ptr_ = new QPushButton("Send Velocity Limit"); - pub_velocity_limit_input_ = new QSpinBox(); - pub_velocity_limit_input_->setRange(-100.0, 100.0); - pub_velocity_limit_input_->setValue(0.0); - pub_velocity_limit_input_->setSingleStep(5.0); - connect(velocity_limit_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVelocityLimit())); - - // Emergency Button - emergency_button_ptr_ = new QPushButton("Set Emergency"); - connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton())); + // Panel Configuration + this->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); // Layout - auto * v_layout = new QVBoxLayout; - auto * velocity_limit_layout = new QHBoxLayout(); - v_layout->addWidget(makeOperationModeGroup()); - v_layout->addWidget(makeControlModeGroup()); - { - auto * h_layout = new QHBoxLayout(); - h_layout->addWidget(makeRoutingGroup()); - h_layout->addWidget(makeLocalizationGroup()); - h_layout->addWidget(makeMotionGroup()); - h_layout->addWidget(makeFailSafeGroup()); - v_layout->addLayout(h_layout); - } - - v_layout->addLayout(gear_layout); - velocity_limit_layout->addWidget(velocity_limit_button_ptr_); - velocity_limit_layout->addWidget(pub_velocity_limit_input_); - velocity_limit_layout->addWidget(new QLabel(" [km/h]")); - velocity_limit_layout->addWidget(emergency_button_ptr_); - v_layout->addLayout(velocity_limit_layout); - setLayout(v_layout); -} - -QGroupBox * AutowareStatePanel::makeOperationModeGroup() -{ - auto * group = new QGroupBox("OperationMode"); - auto * grid = new QGridLayout; - - operation_mode_label_ptr_ = new QLabel("INIT"); - operation_mode_label_ptr_->setAlignment(Qt::AlignCenter); - operation_mode_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(operation_mode_label_ptr_, 0, 0, 0, 1); - - auto_button_ptr_ = new QPushButton("AUTO"); - auto_button_ptr_->setCheckable(true); - connect(auto_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutonomous())); - grid->addWidget(auto_button_ptr_, 0, 1); - - stop_button_ptr_ = new QPushButton("STOP"); - stop_button_ptr_->setCheckable(true); - connect(stop_button_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); - grid->addWidget(stop_button_ptr_, 0, 2); - - local_button_ptr_ = new QPushButton("LOCAL"); - local_button_ptr_->setCheckable(true); - connect(local_button_ptr_, SIGNAL(clicked()), SLOT(onClickLocal())); - grid->addWidget(local_button_ptr_, 1, 1); - - remote_button_ptr_ = new QPushButton("REMOTE"); - remote_button_ptr_->setCheckable(true); - connect(remote_button_ptr_, SIGNAL(clicked()), SLOT(onClickRemote())); - grid->addWidget(remote_button_ptr_, 1, 2); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeControlModeGroup() -{ - auto * group = new QGroupBox("AutowareControl"); - auto * grid = new QGridLayout; - - control_mode_label_ptr_ = new QLabel("INIT"); - control_mode_label_ptr_->setAlignment(Qt::AlignCenter); - control_mode_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(control_mode_label_ptr_, 0, 0); - - enable_button_ptr_ = new QPushButton("Enable"); - enable_button_ptr_->setCheckable(true); - connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareControl())); - grid->addWidget(enable_button_ptr_, 0, 1); - - disable_button_ptr_ = new QPushButton("Disable"); - disable_button_ptr_->setCheckable(true); - connect(disable_button_ptr_, SIGNAL(clicked()), SLOT(onClickDirectControl())); - grid->addWidget(disable_button_ptr_, 0, 2); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeRoutingGroup() -{ - auto * group = new QGroupBox("Routing"); - auto * grid = new QGridLayout; - routing_label_ptr_ = new QLabel("INIT"); - routing_label_ptr_->setAlignment(Qt::AlignCenter); - routing_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(routing_label_ptr_, 0, 0); - - clear_route_button_ptr_ = new QPushButton("Clear Route"); - clear_route_button_ptr_->setCheckable(true); - connect(clear_route_button_ptr_, SIGNAL(clicked()), SLOT(onClickClearRoute())); - grid->addWidget(clear_route_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeLocalizationGroup() -{ - auto * group = new QGroupBox("Localization"); - auto * grid = new QGridLayout; - - localization_label_ptr_ = new QLabel("INIT"); - localization_label_ptr_->setAlignment(Qt::AlignCenter); - localization_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(localization_label_ptr_, 0, 0); - - init_by_gnss_button_ptr_ = new QPushButton("Init by GNSS"); - connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); - grid->addWidget(init_by_gnss_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeMotionGroup() -{ - auto * group = new QGroupBox("Motion"); - auto * grid = new QGridLayout; - - motion_label_ptr_ = new QLabel("INIT"); - motion_label_ptr_->setAlignment(Qt::AlignCenter); - motion_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(motion_label_ptr_, 0, 0); - - accept_start_button_ptr_ = new QPushButton("Accept Start"); - accept_start_button_ptr_->setCheckable(true); - connect(accept_start_button_ptr_, SIGNAL(clicked()), SLOT(onClickAcceptStart())); - grid->addWidget(accept_start_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * AutowareStatePanel::makeFailSafeGroup() -{ - auto * group = new QGroupBox("FailSafe"); - auto * grid = new QGridLayout; - - mrm_state_label_ptr_ = new QLabel("INIT"); - mrm_state_label_ptr_->setAlignment(Qt::AlignCenter); - mrm_state_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(mrm_state_label_ptr_, 0, 0); - - mrm_behavior_label_ptr_ = new QLabel("INIT"); - mrm_behavior_label_ptr_->setAlignment(Qt::AlignCenter); - mrm_behavior_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(mrm_behavior_label_ptr_, 1, 0); - - group->setLayout(grid); - return group; + // Create a new container widget + QWidget * containerWidget = new QWidget(this); + containerWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + containerWidget->setStyleSheet( + QString("QWidget { background-color: %1; color: %2; }") + .arg(autoware::state_rviz_plugin::colors::default_colors.background.c_str()) + .arg(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())); + + auto * containerLayout = new QVBoxLayout(containerWidget); + // Set the alignment of the layout + containerLayout->setAlignment(Qt::AlignTop); + containerLayout->setSpacing(1); + + auto * operation_mode_group = makeOperationModeGroup(); + auto * diagnostic_v_layout = new QVBoxLayout; + auto * localization_group = makeLocalizationGroup(); + auto * motion_group = makeMotionGroup(); + auto * fail_safe_group = makeFailSafeGroup(); + auto * routing_group = makeRoutingGroup(); + auto * velocity_limit_group = makeVelocityLimitGroup(); + // auto * diagnostic_group = makeDiagnosticGroup(); + + diagnostic_v_layout->addLayout(routing_group); + // diagnostic_v_layout->addSpacing(5); + diagnostic_v_layout->addLayout(localization_group); + // diagnostic_v_layout->addSpacing(5); + diagnostic_v_layout->addLayout(motion_group); + // diagnostic_v_layout->addSpacing(5); + diagnostic_v_layout->addLayout(fail_safe_group); + + // containerLayout->addLayout(diagnostic_group); + + containerLayout->addLayout(operation_mode_group); + // containerLayout->addSpacing(5); + containerLayout->addLayout(diagnostic_v_layout); + // main_v_layout->addSpacing(5); + containerLayout->addLayout(velocity_limit_group); + + // Create a QScrollArea + QScrollArea * scrollArea = new QScrollArea(this); + scrollArea->setWidgetResizable(true); + scrollArea->setWidget(containerWidget); + + // Main layout for AutowareStatePanel + QVBoxLayout * mainLayout = new QVBoxLayout(this); + mainLayout->addWidget(scrollArea); + setLayout(mainLayout); } void AutowareStatePanel::onInitialize() @@ -268,9 +144,9 @@ void AutowareStatePanel::onInitialize() "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStatePanel::onMRMState, this, _1)); - // Others - sub_gear_ = raw_node_->create_subscription( - "/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1)); + // // Diagnostics + // sub_diag_ = raw_node_->create_subscription( + // "/diagnostics", 10, std::bind(&AutowareStatePanel::onDiagnostics, this, _1)); sub_emergency_ = raw_node_->create_subscription( "/api/autoware/get/emergency", 10, std::bind(&AutowareStatePanel::onEmergencyStatus, this, _1)); @@ -280,174 +156,498 @@ void AutowareStatePanel::onInitialize() pub_velocity_limit_ = raw_node_->create_publisher( "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local()); -} -void AutowareStatePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) -{ - auto changeButtonState = [this]( - QPushButton * button, const bool is_desired_mode_available, - const uint8_t current_mode = OperationModeState::UNKNOWN, - const uint8_t desired_mode = OperationModeState::STOP) { - if (is_desired_mode_available && current_mode != desired_mode) { - activateButton(button); + QObject::connect(segmented_button, &CustomSegmentedButton::buttonClicked, this, [this](int id) { + const QList buttons = segmented_button->getButtonGroup()->buttons(); + + // Check if the button ID is within valid range + if (id < 0 || id >= buttons.size()) { + return; + } + + // Ensure the button is not null + QAbstractButton * abstractButton = segmented_button->getButtonGroup()->button(id); + if (!abstractButton) { + return; + } + + QPushButton * button = qobject_cast(abstractButton); + if (button) { + // Call the corresponding function for each button + if (button == auto_button_ptr_) { + onClickAutonomous(); + } else if (button == local_button_ptr_) { + onClickLocal(); + } else if (button == remote_button_ptr_) { + onClickRemote(); + } else if (button == stop_button_ptr_) { + onClickStop(); + } } else { - deactivateButton(button); + // qDebug() << "Button not found with ID:" << id; } - }; + }); +} - QString text = ""; - QString style_sheet = ""; - // Operation Mode - switch (msg->mode) { - case OperationModeState::AUTONOMOUS: - text = "AUTONOMOUS"; - style_sheet = "background-color: #00FF00;"; // green - break; +QVBoxLayout * AutowareStatePanel::makeOperationModeGroup() +{ + control_mode_switch_ptr_ = new CustomToggleSwitch(this); + connect( + control_mode_switch_ptr_, &QCheckBox::stateChanged, this, + &AutowareStatePanel::onSwitchStateChanged); + + control_mode_label_ptr_ = new QLabel("Autoware Control"); + control_mode_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + CustomContainer * group1 = new CustomContainer(this); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + + horizontal_layout->addWidget(control_mode_switch_ptr_); + horizontal_layout->addWidget(control_mode_label_ptr_); + + // add switch and label to the container + group1->setContentsMargins(0, 0, 0, 10); + group1->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + + // Create the CustomSegmentedButton + segmented_button = new CustomSegmentedButton(this); + auto_button_ptr_ = segmented_button->addButton("Auto"); + local_button_ptr_ = segmented_button->addButton("Local"); + remote_button_ptr_ = segmented_button->addButton("Remote"); + stop_button_ptr_ = segmented_button->addButton("Stop"); + + QVBoxLayout * groupLayout = new QVBoxLayout; + // set these widgets to show up at the left and not stretch more than needed + groupLayout->setAlignment(Qt::AlignCenter); + groupLayout->setContentsMargins(10, 0, 0, 0); + groupLayout->addWidget(group1); + // groupLayout->addSpacing(5); + groupLayout->addWidget(segmented_button, 0, Qt::AlignCenter); + return groupLayout; +} - case OperationModeState::LOCAL: - text = "LOCAL"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; +QVBoxLayout * AutowareStatePanel::makeRoutingGroup() +{ + auto * group = new QVBoxLayout; - case OperationModeState::REMOTE: - text = "REMOTE"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; + auto * custom_container = new CustomContainer(this); - case OperationModeState::STOP: - text = "STOP"; - style_sheet = "background-color: #FFA500;"; // orange - break; + routing_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } + clear_route_button_ptr_ = new CustomElevatedButton("Clear Route"); + clear_route_button_ptr_->setCheckable(true); + clear_route_button_ptr_->setCursor(Qt::PointingHandCursor); + connect(clear_route_button_ptr_, SIGNAL(clicked()), SLOT(onClickClearRoute())); - if (msg->is_in_transition) { - text += "\n(TRANSITION)"; - } + routing_label_ptr_ = new QLabel("Routing | Unknown"); + routing_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); - updateLabel(operation_mode_label_ptr_, text, style_sheet); + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); - // Control Mode - if (msg->is_autoware_control_enabled) { - updateLabel(control_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green - } else { - updateLabel(control_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow - } + horizontal_layout->addWidget(routing_icon); + horizontal_layout->addWidget(routing_label_ptr_); + + custom_container->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + custom_container->getLayout()->addWidget(clear_route_button_ptr_, 0, 2, 1, 4, Qt::AlignRight); + + custom_container->setContentsMargins(10, 0, 0, 0); + + group->addWidget(custom_container); + + return group; +} - // Button - changeButtonState( - auto_button_ptr_, msg->is_autonomous_mode_available, msg->mode, OperationModeState::AUTONOMOUS); - changeButtonState( - stop_button_ptr_, msg->is_stop_mode_available, msg->mode, OperationModeState::STOP); - changeButtonState( - local_button_ptr_, msg->is_local_mode_available, msg->mode, OperationModeState::LOCAL); - changeButtonState( - remote_button_ptr_, msg->is_remote_mode_available, msg->mode, OperationModeState::REMOTE); - - changeButtonState(enable_button_ptr_, !msg->is_autoware_control_enabled); - changeButtonState(disable_button_ptr_, msg->is_autoware_control_enabled); +QVBoxLayout * AutowareStatePanel::makeLocalizationGroup() +{ + auto * group = new QVBoxLayout; + auto * custom_container = new CustomContainer(this); + + init_by_gnss_button_ptr_ = new CustomElevatedButton("Initialize with GNSS"); + init_by_gnss_button_ptr_->setCursor(Qt::PointingHandCursor); + connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); + + localization_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + localization_label_ptr_ = new QLabel("Localization | Unknown"); + localization_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + + horizontal_layout->addWidget(localization_icon); + horizontal_layout->addWidget(localization_label_ptr_); + + custom_container->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + custom_container->getLayout()->addWidget(init_by_gnss_button_ptr_, 0, 2, 1, 4, Qt::AlignRight); + + custom_container->setContentsMargins(10, 0, 0, 0); + + group->addWidget(custom_container); + return group; +} + +QVBoxLayout * AutowareStatePanel::makeMotionGroup() +{ + auto * group = new QVBoxLayout; + auto * custom_container = new CustomContainer(this); + + accept_start_button_ptr_ = new CustomElevatedButton("Accept Start"); + accept_start_button_ptr_->setCheckable(true); + accept_start_button_ptr_->setCursor(Qt::PointingHandCursor); + connect(accept_start_button_ptr_, SIGNAL(clicked()), SLOT(onClickAcceptStart())); + + motion_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + motion_label_ptr_ = new QLabel("Motion | Unknown"); + motion_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + horizontal_layout->setAlignment(Qt::AlignLeft); + + horizontal_layout->addWidget(motion_icon); + horizontal_layout->addWidget(motion_label_ptr_); + + custom_container->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + custom_container->getLayout()->addWidget(accept_start_button_ptr_, 0, 2, 1, 4, Qt::AlignRight); + + custom_container->setContentsMargins(10, 0, 0, 0); + + group->addWidget(custom_container); + + return group; +} + +QVBoxLayout * AutowareStatePanel::makeFailSafeGroup() +{ + auto * group = new QVBoxLayout; + auto * v_layout = new QVBoxLayout; + auto * custom_container1 = new CustomContainer(this); + auto * custom_container2 = new CustomContainer(this); + + mrm_state_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + mrm_behavior_icon = new CustomIconLabel( + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str())); + + mrm_state_label_ptr_ = new QLabel("MRM State | Unknown"); + mrm_behavior_label_ptr_ = new QLabel("MRM Behavior | Unknown"); + + // change text color + mrm_state_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + mrm_behavior_label_ptr_->setStyleSheet( + QString("color: %1; font-weight: bold;") + .arg(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())); + + auto * horizontal_layout = new QHBoxLayout; + horizontal_layout->setSpacing(10); + horizontal_layout->setContentsMargins(0, 0, 0, 0); + + horizontal_layout->addWidget(mrm_state_icon); + horizontal_layout->addWidget(mrm_state_label_ptr_); + + custom_container1->getLayout()->addLayout(horizontal_layout, 0, 0, 1, 1, Qt::AlignLeft); + + auto * horizontal_layout2 = new QHBoxLayout; + horizontal_layout2->setSpacing(10); + horizontal_layout2->setContentsMargins(0, 0, 0, 0); + + horizontal_layout2->addWidget(mrm_behavior_icon); + horizontal_layout2->addWidget(mrm_behavior_label_ptr_); + + custom_container2->getLayout()->addLayout(horizontal_layout2, 0, 0, 1, 1, Qt::AlignLeft); + + v_layout->addWidget(custom_container1); + // v_layout->addSpacing(5); + v_layout->addWidget(custom_container2); + + group->setContentsMargins(10, 0, 0, 0); + + group->addLayout(v_layout); + return group; +} + +/* QVBoxLayout * AutowareStatePanel::makeDiagnosticGroup() +{ + auto * group = new QVBoxLayout; + + // Create the scroll area + QScrollArea * scrollArea = new QScrollArea; + scrollArea->setFixedHeight(66); // Adjust the height as needed + scrollArea->setWidgetResizable(true); + scrollArea->setHorizontalScrollBarPolicy(Qt::ScrollBarAsNeeded); + scrollArea->setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded); + + // Create a widget to contain the layout + QWidget * scrollAreaWidgetContents = new QWidget; + // use layout to contain the diagnostic label and the diagnostic level + diagnostic_layout_ = new QVBoxLayout(); + diagnostic_layout_->setSpacing(5); // Set space between items + diagnostic_layout_->setContentsMargins(5, 5, 5, 5); // Set margins within the layout + + // Add a QLabel to display the title of what this is + auto * tsm_label_title_ptr_ = new QLabel("Topic State Monitor: "); + // Set the layout on the widget + scrollAreaWidgetContents->setLayout(diagnostic_layout_); + + // Set the widget on the scroll area + scrollArea->setWidget(scrollAreaWidgetContents); + + group->addWidget(tsm_label_title_ptr_); + group->addWidget(scrollArea); + + return group; +} */ + +QVBoxLayout * AutowareStatePanel::makeVelocityLimitGroup() +{ + // Velocity Limit + velocity_limit_setter_ptr_ = new QLabel("Set Velocity Limit"); + // set its width to fit the text + velocity_limit_setter_ptr_->setFixedWidth( + velocity_limit_setter_ptr_->fontMetrics().horizontalAdvance("Set Velocity Limit")); + + velocity_limit_value_label_ = new QLabel("0"); + velocity_limit_value_label_->setMaximumWidth( + velocity_limit_value_label_->fontMetrics().horizontalAdvance("0")); + + CustomSlider * pub_velocity_limit_slider_ = new CustomSlider(Qt::Horizontal); + pub_velocity_limit_slider_->setRange(0, 100); + pub_velocity_limit_slider_->setValue(0); + pub_velocity_limit_slider_->setMaximumWidth(300); + + connect(pub_velocity_limit_slider_, &QSlider::sliderPressed, this, [this]() { + sliderIsDragging = true; // User starts dragging the handle + }); + + connect(pub_velocity_limit_slider_, &QSlider::sliderReleased, this, [this]() { + sliderIsDragging = false; // User finished dragging + onClickVelocityLimit(); // Call when handle is released after dragging + }); + + connect(pub_velocity_limit_slider_, &QSlider::valueChanged, this, [this](int value) { + this->velocity_limit_value_label_->setText(QString::number(value)); + velocity_limit_value_label_->setMaximumWidth( + velocity_limit_value_label_->fontMetrics().horizontalAdvance(QString::number(value))); + if (!sliderIsDragging) { // If the value changed without dragging, it's a click on the track + onClickVelocityLimit(); // Call the function immediately since it's not a drag operation + } + }); + + // Emergency Button + emergency_button_ptr_ = new CustomElevatedButton("Set Emergency"); + + emergency_button_ptr_->setCursor(Qt::PointingHandCursor); + // set fixed width to fit the text + connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton())); + auto * utility_layout = new QVBoxLayout; + auto * velocity_limit_layout = new QHBoxLayout; + QLabel * velocity_limit_label = new QLabel("km/h"); + + velocity_limit_layout->addWidget(pub_velocity_limit_slider_); + velocity_limit_layout->addSpacing(5); + velocity_limit_layout->addWidget(velocity_limit_value_label_); + velocity_limit_layout->addWidget(velocity_limit_label); + + // Velocity Limit layout + utility_layout->addSpacing(15); + utility_layout->addWidget(velocity_limit_setter_ptr_); + utility_layout->addSpacing(10); + utility_layout->addLayout(velocity_limit_layout); + utility_layout->addSpacing(25); + utility_layout->addWidget(emergency_button_ptr_); + + utility_layout->setContentsMargins(15, 0, 15, 0); + + return utility_layout; +} + +void AutowareStatePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + auto updateButtonState = []( + CustomSegmentedButtonItem * button, bool is_available, + uint8_t current_mode, uint8_t desired_mode, bool disable) { + bool is_checked = (current_mode == desired_mode); + button->setHovered(false); + + button->setActivated(is_checked); + button->setChecked(is_checked); + button->setDisabledButton(disable || !is_available); + button->setCheckableButton(!disable && is_available && !is_checked); + }; + + bool disable_buttons = msg->is_in_transition; + + updateButtonState( + auto_button_ptr_, msg->is_autonomous_mode_available, msg->mode, OperationModeState::AUTONOMOUS, + disable_buttons); + updateButtonState( + stop_button_ptr_, msg->is_stop_mode_available, msg->mode, OperationModeState::STOP, + disable_buttons); + updateButtonState( + local_button_ptr_, msg->is_local_mode_available, msg->mode, OperationModeState::LOCAL, + disable_buttons); + updateButtonState( + remote_button_ptr_, msg->is_remote_mode_available, msg->mode, OperationModeState::REMOTE, + disable_buttons); + + // toggle switch for control mode + auto changeToggleSwitchState = [](CustomToggleSwitch * toggle_switch, const bool is_enabled) { + // Flick the switch without triggering its function + bool old_state = toggle_switch->blockSignals(true); + toggle_switch->setCheckedState(!is_enabled); + toggle_switch->blockSignals(old_state); + }; + + if (!msg->is_in_transition) { + // would cause an on/off/on flicker if in transition + changeToggleSwitchState(control_mode_switch_ptr_, !msg->is_autoware_control_enabled); + } } void AutowareStatePanel::onRoute(const RouteState::ConstSharedPtr msg) { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString route_state = "Routing | Unknown"; + switch (msg->state) { case RouteState::UNSET: - text = "UNSET"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Pending; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + route_state = "Routing | Unset"; break; case RouteState::SET: - text = "SET"; - style_sheet = "background-color: #00FF00;"; // green + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + route_state = "Routing | Set"; break; case RouteState::ARRIVED: - text = "ARRIVED"; - style_sheet = "background-color: #FFA500;"; // orange + state = Danger; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + route_state = "Routing | Arrived"; break; case RouteState::CHANGING: - text = "CHANGING"; - style_sheet = "background-color: #FFFF00;"; // yellow + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + state = Pending; + route_state = "Routing | Changing"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); break; } - updateLabel(routing_label_ptr_, text, style_sheet); + routing_icon->updateStyle(state, bgColor); + routing_label_ptr_->setText(route_state); if (msg->state == RouteState::SET) { activateButton(clear_route_button_ptr_); } else { + clear_route_button_ptr_->setStyleSheet( + QString("QPushButton {" + "background-color: %1;color: %2;" + "border: 2px solid %3;" + "font-weight: bold;" + "}") + .arg(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()) + .arg(autoware::state_rviz_plugin::colors::default_colors.outline.c_str()) + .arg( + autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str())); deactivateButton(clear_route_button_ptr_); } } void AutowareStatePanel::onLocalization(const LocalizationInitializationState::ConstSharedPtr msg) { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString localization_state = "Localization | Unknown"; + switch (msg->state) { case LocalizationInitializationState::UNINITIALIZED: - text = "UNINITIALIZED"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + localization_state = "Localization | Uninitialized"; break; - case LocalizationInitializationState::INITIALIZING: - text = "INITIALIZING"; - style_sheet = "background-color: #FFA500;"; // orange + case LocalizationInitializationState::INITIALIZED: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + localization_state = "Localization | Initialized"; break; - case LocalizationInitializationState::INITIALIZED: - text = "INITIALIZED"; - style_sheet = "background-color: #00FF00;"; // green + case LocalizationInitializationState::INITIALIZING: + state = Pending; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + localization_state = "Localization | Initializing"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); break; } - updateLabel(localization_label_ptr_, text, style_sheet); + localization_icon->updateStyle(state, bgColor); + localization_label_ptr_->setText(localization_state); } void AutowareStatePanel::onMotion(const MotionState::ConstSharedPtr msg) { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString motion_state = "Motion | Unknown"; + switch (msg->state) { case MotionState::STARTING: - text = "STARTING"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Pending; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + motion_state = "Motion | Starting"; break; - case MotionState::STOPPED: - text = "STOPPED"; - style_sheet = "background-color: #FFA500;"; // orange + case MotionState::MOVING: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + motion_state = "Motion | Moving"; break; - case MotionState::MOVING: - text = "MOVING"; - style_sheet = "background-color: #00FF00;"; // green + case MotionState::STOPPED: + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + motion_state = "Motion | Stopped"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = Danger; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); break; } - updateLabel(motion_label_ptr_, text, style_sheet); + motion_icon->updateStyle(state, bgColor); + motion_label_ptr_->setText(motion_state); if (msg->state == MotionState::STARTING) { activateButton(accept_start_button_ptr_); @@ -458,93 +658,85 @@ void AutowareStatePanel::onMotion(const MotionState::ConstSharedPtr msg) void AutowareStatePanel::onMRMState(const MRMState::ConstSharedPtr msg) { - // state - { - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case MRMState::NONE: - text = "NONE"; - style_sheet = "background-color: #00FF00;"; // green - break; + IconState state; + QColor bgColor; + QString mrm_state = "MRM State | Unknown"; - case MRMState::MRM_OPERATING: - text = "MRM_OPERATING"; - style_sheet = "background-color: #FFA500;"; // orange - break; + switch (msg->state) { + case MRMState::NONE: + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_state = "MRM State | Inactive"; + break; - case MRMState::MRM_SUCCEEDED: - text = "MRM_SUCCEEDED"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; + case MRMState::MRM_OPERATING: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_state = "MRM State | Operating"; + break; - case MRMState::MRM_FAILED: - text = "MRM_FAILED"; - style_sheet = "background-color: #FF0000;"; // red - break; + case MRMState::MRM_SUCCEEDED: + state = Active; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + mrm_state = "MRM State | Successful"; + break; - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } + case MRMState::MRM_FAILED: + state = Danger; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + mrm_state = "MRM State | Failed"; + break; - updateLabel(mrm_state_label_ptr_, text, style_sheet); + default: + state = None; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_state = "MRM State | Unknown"; + break; } + mrm_state_icon->updateStyle(state, bgColor); + mrm_state_label_ptr_->setText(mrm_state); + // behavior { - QString text = ""; - QString style_sheet = ""; + IconState state; + QColor bgColor; + QString mrm_behavior = "MRM Behavior | Unknown"; + switch (msg->behavior) { case MRMState::NONE: - text = "NONE"; - style_sheet = "background-color: #00FF00;"; // green + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_behavior = "MRM Behavior | Inactive"; break; case MRMState::PULL_OVER: - text = "PULL_OVER"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.success.c_str()); + mrm_behavior = "MRM Behavior | Pull Over"; break; case MRMState::COMFORTABLE_STOP: - text = "COMFORTABLE_STOP"; - style_sheet = "background-color: #FFFF00;"; // yellow + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.warning.c_str()); + mrm_behavior = "MRM Behavior | Comfortable Stop"; break; case MRMState::EMERGENCY_STOP: - text = "EMERGENCY_STOP"; - style_sheet = "background-color: #FFA500;"; // orange + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.danger.c_str()); + mrm_behavior = "MRM Behavior | Emergency Stop"; break; default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red + state = Crash; + bgColor = QColor(autoware::state_rviz_plugin::colors::default_colors.info.c_str()); + mrm_behavior = "MRM Behavior | Unknown"; break; } - updateLabel(mrm_behavior_label_ptr_, text, style_sheet); - } -} - -void AutowareStatePanel::onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) -{ - switch (msg->report) { - case autoware_vehicle_msgs::msg::GearReport::PARK: - gear_label_ptr_->setText("PARKING"); - break; - case autoware_vehicle_msgs::msg::GearReport::REVERSE: - gear_label_ptr_->setText("REVERSE"); - break; - case autoware_vehicle_msgs::msg::GearReport::DRIVE: - gear_label_ptr_->setText("DRIVE"); - break; - case autoware_vehicle_msgs::msg::GearReport::NEUTRAL: - gear_label_ptr_->setText("NEUTRAL"); - break; - case autoware_vehicle_msgs::msg::GearReport::LOW: - gear_label_ptr_->setText("LOW"); - break; + mrm_behavior_icon->updateStyle(state, bgColor); + mrm_behavior_label_ptr_->setText(mrm_behavior); } } @@ -553,18 +745,37 @@ void AutowareStatePanel::onEmergencyStatus( { current_emergency_ = msg->emergency; if (msg->emergency) { - emergency_button_ptr_->setText(QString::fromStdString("Clear Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #FF0000;"); + emergency_button_ptr_->updateStyle( + "Clear Emergency", + QColor(autoware::state_rviz_plugin::colors::default_colors.error_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_error_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_error.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_error_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.error_container.c_str())); } else { - emergency_button_ptr_->setText(QString::fromStdString("Set Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #00FF00;"); + emergency_button_ptr_->updateStyle( + "Set Emergency", QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary_container.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary.c_str()), + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_tint.c_str())); + } +} + +void AutowareStatePanel::onSwitchStateChanged(int state) +{ + if (state == 0) { + // call the control mode function + onClickDirectControl(); + } else if (state == 2) { + onClickAutowareControl(); } } void AutowareStatePanel::onClickVelocityLimit() { auto velocity_limit = std::make_shared(); - velocity_limit->max_velocity = pub_velocity_limit_input_->value() / 3.6; + velocity_limit->max_velocity = velocity_limit_value_label_->text().toDouble() / 3.6; pub_velocity_limit_->publish(*velocity_limit); } diff --git a/common/tier4_state_rviz_plugin/src/custom_button.cpp b/common/tier4_state_rviz_plugin/src/custom_button.cpp new file mode 100644 index 0000000000000..0ef2628577135 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_button.cpp @@ -0,0 +1,129 @@ +// Copyright 2024 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 "include/custom_button.hpp" + +#include "src/include/material_colors.hpp" + +CustomElevatedButton::CustomElevatedButton( + const QString & text, const QColor & bgColor, const QColor & textColor, const QColor & hoverColor, + const QColor & disabledBgColor, const QColor & disabledTextColor, QWidget * parent) +: QPushButton(text, parent), + backgroundColor(bgColor), + textColor(textColor), + hoverColor(hoverColor), + disabledBgColor(disabledBgColor), + disabledTextColor(disabledTextColor) +{ + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); + setCursor(Qt::PointingHandCursor); + + // set font weight to bold and size to 12 + QFont font = this->font(); + font.setWeight(QFont::Bold); + font.setFamily("Roboto"); + setFont(font); + + // Set up drop shadow effect + QGraphicsDropShadowEffect * shadowEffect = new QGraphicsDropShadowEffect(this); + shadowEffect->setBlurRadius(15); + shadowEffect->setOffset(3, 3); + shadowEffect->setColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.shadow.c_str())); + setGraphicsEffect(shadowEffect); +} + +QSize CustomElevatedButton::sizeHint() const +{ + QFontMetrics fm(font()); + int textWidth = fm.horizontalAdvance(text()); + int textHeight = fm.height(); + int width = textWidth + 45; // Adding padding + int height = textHeight + 25; // Adding padding + return QSize(width, height); +} + +QSize CustomElevatedButton::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomElevatedButton::updateStyle( + const QString & text, const QColor & bgColor, const QColor & textColor, const QColor & hoverColor, + const QColor & disabledBgColor, const QColor & disabledTextColor) +{ + setText(text); + backgroundColor = bgColor; + this->textColor = textColor; + this->hoverColor = hoverColor; + this->disabledBgColor = disabledBgColor; + this->disabledTextColor = disabledTextColor; + update(); // Force repaint +} + +void CustomElevatedButton::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + QStyleOption opt; + opt.initFrom(this); + QRect r = rect(); + + QColor buttonColor; + QColor currentTextColor = textColor; + if (!isEnabled()) { + buttonColor = disabledBgColor; + currentTextColor = disabledTextColor; + } else if (isHovered) { + buttonColor = hoverColor; + } else { + buttonColor = backgroundColor; + } + + int cornerRadius = height() / 2; // Making the corner radius proportional to the height + + // Draw button background + QPainterPath backgroundPath; + backgroundPath.addRoundedRect(r, cornerRadius, cornerRadius); + if (!isEnabled()) { + painter.setBrush( + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())); + painter.setOpacity(0.12); + } else { + painter.setBrush(buttonColor); + } + painter.setPen(Qt::NoPen); + painter.drawPath(backgroundPath); + + // Draw button text + if (!isEnabled()) { + painter.setOpacity(0.38); + } + painter.setPen(currentTextColor); + painter.drawText(r, Qt::AlignCenter, text()); +} + +void CustomElevatedButton::enterEvent(QEvent * event) +{ + isHovered = true; + update(); + QPushButton::enterEvent(event); +} + +void CustomElevatedButton::leaveEvent(QEvent * event) +{ + isHovered = false; + update(); + QPushButton::leaveEvent(event); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_container.cpp b/common/tier4_state_rviz_plugin/src/custom_container.cpp new file mode 100644 index 0000000000000..0b0aa1ccd6ed5 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_container.cpp @@ -0,0 +1,70 @@ +// Copyright 2024 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 "include/custom_container.hpp" + +#include "src/include/material_colors.hpp" + +CustomContainer::CustomContainer(QWidget * parent) : QFrame(parent), cornerRadius(15) +{ + setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + setContentsMargins(0, 0, 0, 0); + layout = new QGridLayout(this); + layout->setMargin(0); // Margin between the border and child widgets + layout->setSpacing(0); // Spacing between child widgets + layout->setContentsMargins(10, 0, 10, 0); // Margin between the border and the layout + setLayout(layout); +} + +void CustomContainer::setCornerRadius(int radius) +{ + cornerRadius = radius; + update(); +} + +int CustomContainer::getCornerRadius() const +{ + return cornerRadius; +} + +QGridLayout * CustomContainer::getLayout() const +{ + return layout; // Provide access to the layout +} + +QSize CustomContainer::sizeHint() const +{ + QSize size = layout->sizeHint(); + int width = size.width() + 20; // Adding padding + int height = size.height() + 20; // Adding padding + return QSize(width, height); +} + +QSize CustomContainer::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomContainer::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + // Draw background + QPainterPath path; + path.addRoundedRect(rect(), height() / 2, height() / 2); // Use height for rounded corners + painter.setPen(Qt::NoPen); + painter.setBrush(QColor( + autoware::state_rviz_plugin::colors::default_colors.surface.c_str())); // Background color + painter.drawPath(path); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_icon_label.cpp b/common/tier4_state_rviz_plugin/src/custom_icon_label.cpp new file mode 100644 index 0000000000000..6e4d32d40f7fb --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_icon_label.cpp @@ -0,0 +1,80 @@ +// Copyright 2024 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 "include/custom_icon_label.hpp" + +#include + +CustomIconLabel::CustomIconLabel(const QColor & bgColor, QWidget * parent) +: QLabel(parent), backgroundColor(bgColor) +{ + loadIcons(); + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); + setAlignment(Qt::AlignCenter); + setFixedSize(35, 35); +} + +void CustomIconLabel::loadIcons() +{ + std::string packagePath = ament_index_cpp::get_package_share_directory("tier4_state_rviz_plugin"); + + iconMap[Active] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/active.png")); + iconMap[Pending] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/pending.png")); + iconMap[Danger] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/danger.png")); + iconMap[None] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/none.png")); + iconMap[Crash] = QPixmap(QString::fromStdString(packagePath + "/icons/assets/crash.png")); + + icon = iconMap[None]; // Default icon +} + +void CustomIconLabel::updateStyle(IconState state, const QColor & bgColor) +{ + icon = iconMap[state]; + backgroundColor = bgColor; + update(); // Force repaint +} + +QSize CustomIconLabel::sizeHint() const +{ + int size = qMax(icon.width(), icon.height()) + 20; // Adding padding + return QSize(size, size); +} + +QSize CustomIconLabel::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomIconLabel::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + int diameter = qMin(width(), height()); + int radius = diameter / 2; + + // Draw background circle + QPainterPath path; + path.addEllipse(width() / 2 - radius, height() / 2 - radius, diameter, diameter); + painter.setPen(Qt::NoPen); + painter.setBrush(backgroundColor); + painter.drawPath(path); + + // Draw icon in the center + if (!icon.isNull()) { + QSize iconSize = icon.size().scaled(diameter * 0.6, diameter * 0.6, Qt::KeepAspectRatio); + QPoint iconPos((width() - iconSize.width()) / 2, (height() - iconSize.height()) / 2); + painter.drawPixmap( + iconPos, icon.scaled(iconSize, Qt::KeepAspectRatio, Qt::SmoothTransformation)); + } +} diff --git a/common/tier4_state_rviz_plugin/src/custom_label.cpp b/common/tier4_state_rviz_plugin/src/custom_label.cpp new file mode 100644 index 0000000000000..1f96fc0d45095 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_label.cpp @@ -0,0 +1,82 @@ +// Copyright 2024 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 "include/custom_label.hpp" + +#include "src/include/material_colors.hpp" + +#include +#include +#include +#include +#include +#include + +CustomLabel::CustomLabel(const QString & text, QWidget * parent) : QLabel(text, parent) +{ + setFont(QFont("Roboto", 10, QFont::Bold)); // Set the font as needed + setAlignment(Qt::AlignCenter); + + // Add shadow effect + QGraphicsDropShadowEffect * shadowEffect = new QGraphicsDropShadowEffect(this); + shadowEffect->setBlurRadius(15); // Blur radius for a smoother shadow + shadowEffect->setOffset(3, 3); // Offset for the shadow + shadowEffect->setColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.shadow.c_str())); // Shadow color + setGraphicsEffect(shadowEffect); +} + +QSize CustomLabel::sizeHint() const +{ + QFontMetrics fm(font()); + int textWidth = fm.horizontalAdvance(text()); + int textHeight = fm.height(); + int width = textWidth + 40; // Adding padding + int height = textHeight; // Adding padding + return QSize(width, height); +} + +QSize CustomLabel::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomLabel::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + int cornerRadius = height() / 2; // Making the corner radius proportional to the height + + // Draw background + QPainterPath path; + path.addRoundedRect(rect().adjusted(1, 1, -1, -1), cornerRadius, cornerRadius); + + painter.setPen(Qt::NoPen); + painter.setBrush(backgroundColor); + + painter.drawPath(path); + + // Set text color and draw text + painter.setPen(textColor); + painter.drawText(rect(), Qt::AlignCenter, text()); +} + +void CustomLabel::updateStyle( + const QString & text, const QColor & bg_color, const QColor & text_color) +{ + setText(text); + backgroundColor = bg_color; + textColor = text_color; + update(); // Force repaint +} diff --git a/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp b/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp new file mode 100644 index 0000000000000..8063bdc0fbc28 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_segmented_button.cpp @@ -0,0 +1,75 @@ +// Copyright 2024 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 "include/custom_segmented_button.hpp" + +#include + +CustomSegmentedButton::CustomSegmentedButton(QWidget * parent) +: QWidget(parent), buttonGroup(new QButtonGroup(this)), layout(new QHBoxLayout(this)) +{ + setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + layout->setContentsMargins(0, 0, 0, 0); // Ensure no margins around the layout + layout->setSpacing(0); // Ensure no spacing between buttons + + setLayout(layout); + + buttonGroup->setExclusive(true); + + connect( + buttonGroup, QOverload::of(&QButtonGroup::idClicked), this, + &CustomSegmentedButton::buttonClicked); +} + +CustomSegmentedButtonItem * CustomSegmentedButton::addButton(const QString & text) +{ + CustomSegmentedButtonItem * button = new CustomSegmentedButtonItem(text); + layout->addWidget(button); + buttonGroup->addButton(button, layout->count() - 1); + + return button; +} + +QButtonGroup * CustomSegmentedButton::getButtonGroup() const +{ + return buttonGroup; +} + +QSize CustomSegmentedButton::sizeHint() const +{ + return QSize(400, 40); // Adjust the size hint as needed + + // return QSize( + // layout->count() * (layout->itemAt(0)->widget()->width()), + // layout->itemAt(0)->widget()->height() + 10); +} + +QSize CustomSegmentedButton::minimumSizeHint() const +{ + return sizeHint(); +} + +void CustomSegmentedButton::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + // Draw background + QPainterPath path; + path.addRoundedRect(rect(), height() / 2, height() / 2); + + painter.setPen(Qt::NoPen); + painter.setBrush( + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str())); + painter.drawPath(path); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp b/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp new file mode 100644 index 0000000000000..f2d15260c4e41 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp @@ -0,0 +1,186 @@ +// Copyright 2024 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 "include/custom_segmented_button_item.hpp" + +CustomSegmentedButtonItem::CustomSegmentedButtonItem(const QString & text, QWidget * parent) +: QPushButton(text, parent), + bgColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str())), + checkedBgColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.secondary_container.c_str())), + inactiveTextColor(QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())), + activeTextColor( + QColor(autoware::state_rviz_plugin::colors::default_colors.on_secondary_container.c_str())), + isHovered(false), + isActivated(false), + isDisabled(false) + +{ + setCheckable(true); + setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + setCursor(Qt::PointingHandCursor); +} + +void CustomSegmentedButtonItem::setColors( + const QColor & bg, const QColor & checkedBg, const QColor & activeText, + const QColor & inactiveText) +{ + bgColor = bg; + checkedBgColor = checkedBg; + activeTextColor = activeText; + inactiveTextColor = inactiveText; + update(); +} + +// void CustomSegmentedButtonItem::updateSize() +// { +// QFontMetrics fm(font()); +// int width = fm.horizontalAdvance(text()) + 40; // Add padding +// int height = fm.height() + 20; // Add padding +// setFixedSize(width, height); +// } + +void CustomSegmentedButtonItem::setHovered(bool hovered) +{ + isHovered = hovered; + updateCheckableState(); +} + +void CustomSegmentedButtonItem::setCheckableButton(bool checkable) +{ + setCheckable(checkable); + updateCheckableState(); +} + +void CustomSegmentedButtonItem::updateCheckableState() +{ + setCursor( + isDisabled ? Qt::ForbiddenCursor + : (isCheckable() ? Qt::PointingHandCursor : Qt::ForbiddenCursor)); + update(); +} + +void CustomSegmentedButtonItem::setDisabledButton(bool disabled) +{ + isDisabled = disabled; + updateCheckableState(); +} + +void CustomSegmentedButtonItem::setActivated(bool activated) +{ + isActivated = activated; + update(); +} + +void CustomSegmentedButtonItem::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + + // Adjust opacity for disabled state + if (isDisabled) { + painter.setOpacity(0.3); + } else { + painter.setOpacity(1.0); + } + + // Determine the button's color based on its state + QColor buttonColor; + if (isDisabled) { + buttonColor = disabledBgColor; + } else if (isHovered && !isChecked() && isCheckable()) { + buttonColor = hoverColor; + } else if (isActivated) { + buttonColor = checkedBgColor; + } else { + buttonColor = isChecked() ? checkedBgColor : bgColor; + } + + // Determine if this is the first or last button + bool isFirstButton = false; + bool isLastButton = false; + + QHBoxLayout * parentLayout = qobject_cast(parentWidget()->layout()); + if (parentLayout) { + int index = parentLayout->indexOf(this); + isFirstButton = (index == 0); + isLastButton = (index == parentLayout->count() - 1); + } + + // Draw button background + + QRect buttonRect = rect().adjusted(0, 1, 0, -1); + + if (isFirstButton) { + buttonRect.setLeft(buttonRect.left() + 1); + } + if (isLastButton) { + buttonRect.setRight(buttonRect.right() - 1); + } + + QPainterPath path; + double radius = (height() - 2) / 2; + + path.setFillRule(Qt::WindingFill); + if (isFirstButton) { + path.addRoundedRect(buttonRect, radius, radius); + path.addRect(QRect( + (buttonRect.left() + buttonRect.width() - radius), + buttonRect.top() + buttonRect.height() - radius, radius, + radius)); // Bottom Right + path.addRect(QRect( + (buttonRect.left() + buttonRect.width() - radius), buttonRect.top(), radius, + radius)); // Top Right + } else if (isLastButton) { + path.addRoundedRect(buttonRect, radius, radius); + path.addRect(QRect( + (buttonRect.left()), buttonRect.top() + buttonRect.height() - radius, radius, + radius)); // Bottom left + path.addRect(QRect((buttonRect.left()), buttonRect.top(), radius, + radius)); // Top Left + } else { + path.addRect(buttonRect); + } + painter.fillPath(path, buttonColor); + + // Draw button border + QPen pen(QColor(autoware::state_rviz_plugin::colors::default_colors.outline.c_str()), 1); + pen.setJoinStyle(Qt::RoundJoin); + pen.setCapStyle(Qt::RoundCap); + painter.setPen(pen); + painter.drawPath(path.simplified()); + + // Draw button text + QColor textColor = (isChecked() ? activeTextColor : inactiveTextColor); + painter.setPen(textColor); + painter.drawText(rect(), Qt::AlignCenter, text()); +} + +void CustomSegmentedButtonItem::enterEvent(QEvent * event) +{ + if (isCheckable()) { + isHovered = true; + update(); + } + QPushButton::enterEvent(event); +} + +void CustomSegmentedButtonItem::leaveEvent(QEvent * event) +{ + if (isCheckable()) { + isHovered = false; + update(); + } + QPushButton::leaveEvent(event); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_slider.cpp b/common/tier4_state_rviz_plugin/src/custom_slider.cpp new file mode 100644 index 0000000000000..cf3f7c3d4638f --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_slider.cpp @@ -0,0 +1,102 @@ +// Copyright 2024 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 "include/custom_slider.hpp" + +CustomSlider::CustomSlider(Qt::Orientation orientation, QWidget * parent) +: QSlider(orientation, parent) +{ + setMinimumHeight(40); // Ensure there's enough space for the custom track +} + +void CustomSlider::paintEvent(QPaintEvent *) +{ + QPainter painter(this); + painter.setRenderHint(QPainter::Antialiasing); + painter.setPen(Qt::NoPen); + + // Initialize style option + QStyleOptionSlider opt; + initStyleOption(&opt); + + QRect grooveRect = + style()->subControlRect(QStyle::CC_Slider, &opt, QStyle::SC_SliderGroove, this); + int centerY = grooveRect.center().y(); + QRect handleRect = + style()->subControlRect(QStyle::CC_Slider, &opt, QStyle::SC_SliderHandle, this); + + int value = this->value(); + int minValue = this->minimum(); + int maxValue = this->maximum(); + + int trackThickness = 14; + int gap = 8; + + QRect beforeRect( + grooveRect.x(), centerY - trackThickness / 2, handleRect.center().x() - grooveRect.x() - gap, + trackThickness); + + QRect afterRect( + handleRect.center().x() + gap, centerY - trackThickness / 2, + grooveRect.right() - handleRect.center().x() - gap, trackThickness); + + QColor inactiveTrackColor( + autoware::state_rviz_plugin::colors::default_colors.primary_container.c_str()); + QColor activeTrackColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + QColor handleColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + + // only draw the active track if the value is more than the gap from the minimum + if (value > minValue + gap / 2) { + QPainterPath beforePath; + beforePath.moveTo(beforeRect.left(), centerY + trackThickness / 2); // Start from bottom-left + beforePath.quadTo( + beforeRect.left(), centerY - trackThickness / 2, beforeRect.left() + trackThickness * 0.5, + centerY - trackThickness / 2); + beforePath.lineTo(beforeRect.right() - trackThickness * 0.1, centerY - trackThickness / 2); + beforePath.quadTo( + beforeRect.right(), centerY - trackThickness / 2, beforeRect.right(), centerY); + beforePath.quadTo( + beforeRect.right(), centerY + trackThickness / 2, beforeRect.right() - trackThickness * 0.1, + centerY + trackThickness / 2); + beforePath.lineTo(beforeRect.left() + trackThickness * 0.5, centerY + trackThickness / 2); + beforePath.quadTo(beforeRect.left(), centerY + trackThickness / 2, beforeRect.left(), centerY); + painter.fillPath(beforePath, activeTrackColor); + } + + if (value < maxValue - gap / 2) { + QPainterPath afterPath; + afterPath.moveTo(afterRect.left(), centerY + trackThickness / 2); + afterPath.quadTo( + afterRect.left(), centerY - trackThickness / 2, afterRect.left() + trackThickness * 0.1, + centerY - trackThickness / 2); + afterPath.lineTo(afterRect.right() - trackThickness * 0.5, centerY - trackThickness / 2); + afterPath.quadTo(afterRect.right(), centerY - trackThickness / 2, afterRect.right(), centerY); + afterPath.quadTo( + afterRect.right(), centerY + trackThickness / 2, afterRect.right() - trackThickness * 0.5, + centerY + trackThickness / 2); + afterPath.lineTo(afterRect.left() + trackThickness * 0.1, centerY + trackThickness / 2); + afterPath.quadTo(afterRect.left(), centerY + trackThickness / 2, afterRect.left(), centerY); + painter.fillPath(afterPath, inactiveTrackColor); + } + + painter.setBrush(handleColor); + int handleLineHeight = 30; + int handleLineWidth = 4; + int handleLineRadius = 2; + QRect handleLineRect( + handleRect.center().x() - handleLineWidth / 2, centerY - handleLineHeight / 2, handleLineWidth, + handleLineHeight); + QPainterPath handlePath; + handlePath.addRoundedRect(handleLineRect, handleLineRadius, handleLineRadius); + painter.fillPath(handlePath, handleColor); +} diff --git a/common/tier4_state_rviz_plugin/src/custom_toggle_switch.cpp b/common/tier4_state_rviz_plugin/src/custom_toggle_switch.cpp new file mode 100644 index 0000000000000..3b58ded626439 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/custom_toggle_switch.cpp @@ -0,0 +1,87 @@ +// Copyright 2024 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 "include/custom_toggle_switch.hpp" + +CustomToggleSwitch::CustomToggleSwitch(QWidget * parent) : QCheckBox(parent) +{ + setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + setCursor(Qt::PointingHandCursor); + + connect(this, &QCheckBox::stateChanged, this, [this]() { + if (!blockSignalsGuard) { + update(); // Force repaint + } + }); +} + +QSize CustomToggleSwitch::sizeHint() const +{ + return QSize(50, 30); // Preferred size of the toggle switch +} + +void CustomToggleSwitch::paintEvent(QPaintEvent *) +{ + QPainter p(this); + p.setRenderHint(QPainter::Antialiasing); + + int margin = 2; + int circleRadius = height() / 2 - margin * 2; + QRect r = rect().adjusted(margin, margin, -margin, -margin); + bool isChecked = this->isChecked(); + + QColor uncheckedIndicatorColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.outline.c_str()); + QColor checkedIndicatorColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_primary.c_str()); + QColor indicatorColor = isChecked ? checkedIndicatorColor : uncheckedIndicatorColor; + + QColor uncheckedBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()); + QColor checkedBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + + QColor bgColor = isChecked ? checkedBgColor : uncheckedBgColor; + + QRect borderR = r.adjusted(-margin, -margin, margin, margin); + p.setBrush(bgColor); + p.setPen(Qt::NoPen); + p.drawRoundedRect(borderR, circleRadius + 4, circleRadius + 4); + + p.setBrush(bgColor); + p.setPen(Qt::NoPen); + p.drawRoundedRect(r, circleRadius + 4, circleRadius + 4); + + int minX = r.left() + margin * 2; + int maxX = r.right() - circleRadius * 2 - margin; + int circleX = isChecked ? maxX : minX; + QRect circleRect(circleX, r.top() + margin, circleRadius * 2, circleRadius * 2); + p.setBrush(indicatorColor); + p.drawEllipse(circleRect); +} + +void CustomToggleSwitch::mouseReleaseEvent(QMouseEvent * event) +{ + if (event->button() == Qt::LeftButton) { + setCheckedState(!isChecked()); + } + QCheckBox::mouseReleaseEvent(event); +} + +void CustomToggleSwitch::setCheckedState(bool state) +{ + blockSignalsGuard = true; + setChecked(state); + blockSignalsGuard = false; + update(); // Force repaint +} diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp similarity index 72% rename from common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp rename to common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp index 3e27c8a1bbad3..c1ab9018e12f9 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp @@ -17,12 +17,26 @@ #ifndef AUTOWARE_STATE_PANEL_HPP_ #define AUTOWARE_STATE_PANEL_HPP_ -#include +#include "custom_button.hpp" +#include "custom_container.hpp" +#include "custom_icon_label.hpp" +#include "custom_label.hpp" +#include "custom_segmented_button.hpp" +#include "custom_segmented_button_item.hpp" +#include "custom_slider.hpp" +#include "custom_toggle_switch.hpp" +#include "material_colors.hpp" + +#include +#include +#include #include -#include #include +#include #include -#include +#include +#include +#include #include #include @@ -36,11 +50,17 @@ #include #include #include +#include +#include #include #include #include +#include + #include +#include +#include namespace rviz_plugins { @@ -56,6 +76,8 @@ class AutowareStatePanel : public rviz_common::Panel using MotionState = autoware_adapi_v1_msgs::msg::MotionState; using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart; using MRMState = autoware_adapi_v1_msgs::msg::MrmState; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; Q_OBJECT @@ -75,17 +97,19 @@ public Q_SLOTS: // NOLINT for Qt void onClickAcceptStart(); void onClickVelocityLimit(); void onClickEmergencyButton(); + void onSwitchStateChanged(int state); protected: // Layout - QGroupBox * makeOperationModeGroup(); - QGroupBox * makeControlModeGroup(); - QGroupBox * makeRoutingGroup(); - QGroupBox * makeLocalizationGroup(); - QGroupBox * makeMotionGroup(); - QGroupBox * makeFailSafeGroup(); - - void onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); + QVBoxLayout * makeVelocityLimitGroup(); + QVBoxLayout * makeOperationModeGroup(); + QVBoxLayout * makeRoutingGroup(); + QVBoxLayout * makeLocalizationGroup(); + QVBoxLayout * makeMotionGroup(); + QVBoxLayout * makeFailSafeGroup(); + // QVBoxLayout * makeDiagnosticGroup(); + + // void onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; @@ -97,13 +121,15 @@ public Q_SLOTS: // NOLINT for Qt rclcpp::Publisher::SharedPtr pub_velocity_limit_; + QLabel * velocity_limit_value_label_{nullptr}; + bool sliderIsDragging = false; + // Operation Mode - //// Gate Mode QLabel * operation_mode_label_ptr_{nullptr}; - QPushButton * stop_button_ptr_{nullptr}; - QPushButton * auto_button_ptr_{nullptr}; - QPushButton * local_button_ptr_{nullptr}; - QPushButton * remote_button_ptr_{nullptr}; + CustomSegmentedButtonItem * stop_button_ptr_{nullptr}; + CustomSegmentedButtonItem * auto_button_ptr_{nullptr}; + CustomSegmentedButtonItem * local_button_ptr_{nullptr}; + CustomSegmentedButtonItem * remote_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_operation_mode_; rclcpp::Client::SharedPtr client_change_to_autonomous_; @@ -112,6 +138,8 @@ public Q_SLOTS: // NOLINT for Qt rclcpp::Client::SharedPtr client_change_to_remote_; //// Control Mode + CustomSegmentedButton * segmented_button; + CustomToggleSwitch * control_mode_switch_ptr_{nullptr}; QLabel * control_mode_label_ptr_{nullptr}; QPushButton * enable_button_ptr_{nullptr}; QPushButton * disable_button_ptr_{nullptr}; @@ -123,8 +151,9 @@ public Q_SLOTS: // NOLINT for Qt void changeOperationMode(const rclcpp::Client::SharedPtr client); // Routing + CustomIconLabel * routing_icon{nullptr}; + CustomElevatedButton * clear_route_button_ptr_{nullptr}; QLabel * routing_label_ptr_{nullptr}; - QPushButton * clear_route_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Client::SharedPtr client_clear_route_; @@ -132,8 +161,9 @@ public Q_SLOTS: // NOLINT for Qt void onRoute(const RouteState::ConstSharedPtr msg); // Localization + CustomIconLabel * localization_icon{nullptr}; + CustomElevatedButton * init_by_gnss_button_ptr_{nullptr}; QLabel * localization_label_ptr_{nullptr}; - QPushButton * init_by_gnss_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_localization_; rclcpp::Client::SharedPtr client_init_by_gnss_; @@ -141,8 +171,9 @@ public Q_SLOTS: // NOLINT for Qt void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg); // Motion + CustomIconLabel * motion_icon{nullptr}; + CustomElevatedButton * accept_start_button_ptr_{nullptr}; QLabel * motion_label_ptr_{nullptr}; - QPushButton * accept_start_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_motion_; rclcpp::Client::SharedPtr client_accept_start_; @@ -150,7 +181,9 @@ public Q_SLOTS: // NOLINT for Qt void onMotion(const MotionState::ConstSharedPtr msg); // FailSafe + CustomIconLabel * mrm_state_icon{nullptr}; QLabel * mrm_state_label_ptr_{nullptr}; + CustomIconLabel * mrm_behavior_icon{nullptr}; QLabel * mrm_behavior_label_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_mrm_; @@ -158,11 +191,11 @@ public Q_SLOTS: // NOLINT for Qt void onMRMState(const MRMState::ConstSharedPtr msg); // Others - QPushButton * velocity_limit_button_ptr_; + QLabel * velocity_limit_setter_ptr_; QLabel * gear_label_ptr_; QSpinBox * pub_velocity_limit_input_; - QPushButton * emergency_button_ptr_; + CustomElevatedButton * emergency_button_ptr_; bool current_emergency_{false}; @@ -190,6 +223,17 @@ public Q_SLOTS: // NOLINT for Qt label->setText(text); label->setStyleSheet(style_sheet); } + static void updateCustomLabel( + CustomLabel * label, QString text, QColor bg_color, QColor text_color) + { + label->updateStyle(text, bg_color, text_color); + } + + static void updateButton(QPushButton * button, QString text, QString style_sheet) + { + button->setText(text); + button->setStyleSheet(style_sheet); + } static void activateButton(QAbstractButton * button) { diff --git a/common/tier4_state_rviz_plugin/src/include/custom_button.hpp b/common/tier4_state_rviz_plugin/src/include/custom_button.hpp new file mode 100644 index 0000000000000..b10663ce09933 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_button.hpp @@ -0,0 +1,69 @@ +// Copyright 2024 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 CUSTOM_BUTTON_HPP_ +#define CUSTOM_BUTTON_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include + +class CustomElevatedButton : public QPushButton +{ + Q_OBJECT + +public: + explicit CustomElevatedButton( + const QString & text, + const QColor & bgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()), + const QColor & textColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()), + const QColor & hoverColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()), + const QColor & disabledBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_high.c_str()), + const QColor & disabledTextColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface_variant.c_str()), + QWidget * parent = nullptr); + void updateStyle( + const QString & text, const QColor & bgColor, const QColor & textColor, + const QColor & hoverColor, const QColor & disabledBgColor, const QColor & disabledTextColor); + +protected: + void paintEvent(QPaintEvent * event) override; + void enterEvent(QEvent * event) override; + void leaveEvent(QEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + QColor backgroundColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()); + QColor textColor = QColor(autoware::state_rviz_plugin::colors::default_colors.primary.c_str()); + QColor hoverColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()); + QColor disabledBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.disabled_elevated_button_bg.c_str()); + QColor disabledTextColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str()); + bool isHovered = false; +}; + +#endif // CUSTOM_BUTTON_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_container.hpp b/common/tier4_state_rviz_plugin/src/include/custom_container.hpp new file mode 100644 index 0000000000000..5142b409ebc88 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_container.hpp @@ -0,0 +1,52 @@ +// Copyright 2024 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 CUSTOM_CONTAINER_HPP_ +#define CUSTOM_CONTAINER_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include + +class CustomContainer : public QFrame +{ + Q_OBJECT + +public: + explicit CustomContainer(QWidget * parent = nullptr); + + // Methods to set dimensions and corner radius + void setContainerHeight(int height); + void setContainerWidth(int width); + void setCornerRadius(int radius); + + // Getters + int getContainerHeight() const; + int getContainerWidth() const; + int getCornerRadius() const; + QGridLayout * getLayout() const; // Add a method to access the layout + +protected: + void paintEvent(QPaintEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + QGridLayout * layout; + int cornerRadius; +}; + +#endif // CUSTOM_CONTAINER_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_icon_label.hpp b/common/tier4_state_rviz_plugin/src/include/custom_icon_label.hpp new file mode 100644 index 0000000000000..1b3ab9c8e0c6c --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_icon_label.hpp @@ -0,0 +1,54 @@ +// Copyright 2024 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 CUSTOM_ICON_LABEL_HPP_ +#define CUSTOM_ICON_LABEL_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +enum IconState { Active, Pending, Danger, None, Crash }; + +class CustomIconLabel : public QLabel +{ + Q_OBJECT + +public: + explicit CustomIconLabel( + const QColor & bgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_high.c_str()), + QWidget * parent = nullptr); + void updateStyle(IconState state, const QColor & bgColor); + +protected: + void paintEvent(QPaintEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + void loadIcons(); + QPixmap icon; + QColor backgroundColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_high.c_str()); + QMap iconMap; +}; + +#endif // CUSTOM_ICON_LABEL_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_label.hpp b/common/tier4_state_rviz_plugin/src/include/custom_label.hpp new file mode 100644 index 0000000000000..a328c4de56e3d --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_label.hpp @@ -0,0 +1,46 @@ +// Copyright 2024 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 CUSTOM_LABEL_HPP_ +#define CUSTOM_LABEL_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +class CustomLabel : public QLabel +{ + Q_OBJECT + +public: + explicit CustomLabel(const QString & text, QWidget * parent = nullptr); + void updateStyle( + const QString & text, + const QColor & bgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()), + const QColor & textColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str())); + +protected: + void paintEvent(QPaintEvent * event) override; + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +private: + QColor backgroundColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_low.c_str()); + QColor textColor = QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface.c_str()); +}; + +#endif // CUSTOM_LABEL_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_segmented_button.hpp b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button.hpp new file mode 100644 index 0000000000000..c6c589d31b8d4 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button.hpp @@ -0,0 +1,53 @@ +// Copyright 2024 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 CUSTOM_SEGMENTED_BUTTON_HPP_ +#define CUSTOM_SEGMENTED_BUTTON_HPP_ + +#include "custom_segmented_button_item.hpp" +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +class CustomSegmentedButton : public QWidget +{ + Q_OBJECT + +public: + explicit CustomSegmentedButton(QWidget * parent = nullptr); + + CustomSegmentedButtonItem * addButton(const QString & text); + QButtonGroup * getButtonGroup() const; + + QSize sizeHint() const override; + QSize minimumSizeHint() const override; + +Q_SIGNALS: + void buttonClicked(int id); + +protected: + void paintEvent(QPaintEvent * event) override; + +private: + QButtonGroup * buttonGroup; + QHBoxLayout * layout; +}; + +#endif // CUSTOM_SEGMENTED_BUTTON_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_segmented_button_item.hpp b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button_item.hpp new file mode 100644 index 0000000000000..33eb9fe16aa31 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_segmented_button_item.hpp @@ -0,0 +1,64 @@ +// Copyright 2024 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 CUSTOM_SEGMENTED_BUTTON_ITEM_HPP_ +#define CUSTOM_SEGMENTED_BUTTON_ITEM_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +#include +#include + +class CustomSegmentedButtonItem : public QPushButton +{ + Q_OBJECT + +public: + explicit CustomSegmentedButtonItem(const QString & text, QWidget * parent = nullptr); + + void setColors( + const QColor & bg, const QColor & checkedBg, const QColor & activeText, + const QColor & inactiveText); + void setActivated(bool activated); + void setCheckableButton(bool checkable); + void setDisabledButton(bool disabled); + void setHovered(bool hovered); + +protected: + void paintEvent(QPaintEvent * event) override; + void enterEvent(QEvent * event) override; + void leaveEvent(QEvent * event) override; + +private: + void updateCheckableState(); + + QColor bgColor; + QColor checkedBgColor; + QColor hoverColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_container_highest.c_str()); + QColor inactiveTextColor; + QColor activeTextColor; + QColor disabledBgColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.surface_dim.c_str()); + QColor disabledTextColor = + QColor(autoware::state_rviz_plugin::colors::default_colors.on_surface_variant.c_str()); + bool isHovered = false; + bool isActivated = false; + bool isDisabled = false; +}; + +#endif // CUSTOM_SEGMENTED_BUTTON_ITEM_HPP_ diff --git a/localization/pose2twist/src/pose2twist_node.cpp b/common/tier4_state_rviz_plugin/src/include/custom_slider.hpp similarity index 54% rename from localization/pose2twist/src/pose2twist_node.cpp rename to common/tier4_state_rviz_plugin/src/include/custom_slider.hpp index 81f98461ecffd..f0dc9f12aedfe 100644 --- a/localization/pose2twist/src/pose2twist_node.cpp +++ b/common/tier4_state_rviz_plugin/src/include/custom_slider.hpp @@ -1,4 +1,4 @@ -// Copyright 2015-2019 Autoware Foundation +// Copyright 2024 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. @@ -11,19 +11,25 @@ // 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 CUSTOM_SLIDER_HPP_ +#define CUSTOM_SLIDER_HPP_ +#include "material_colors.hpp" -#include "pose2twist/pose2twist_core.hpp" +#include +#include +#include +#include +#include -#include - -#include - -int main(int argc, char ** argv) +class CustomSlider : public QSlider { - rclcpp::init(argc, argv); + Q_OBJECT + +public: + explicit CustomSlider(Qt::Orientation orientation, QWidget * parent = nullptr); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); +protected: + void paintEvent(QPaintEvent * event) override; +}; - return 0; -} +#endif // CUSTOM_SLIDER_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/custom_toggle_switch.hpp b/common/tier4_state_rviz_plugin/src/include/custom_toggle_switch.hpp new file mode 100644 index 0000000000000..107d45af8c3f3 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/custom_toggle_switch.hpp @@ -0,0 +1,40 @@ +// Copyright 2024 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 CUSTOM_TOGGLE_SWITCH_HPP_ +#define CUSTOM_TOGGLE_SWITCH_HPP_ + +#include "material_colors.hpp" + +#include +#include +#include +#include +class CustomToggleSwitch : public QCheckBox +{ + Q_OBJECT + +public: + explicit CustomToggleSwitch(QWidget * parent = nullptr); + QSize sizeHint() const override; + void setCheckedState(bool state); + +protected: + void paintEvent(QPaintEvent * event) override; + void mouseReleaseEvent(QMouseEvent * event) override; + +private: + bool blockSignalsGuard = false; // Guard variable to block signals during updates +}; + +#endif // CUSTOM_TOGGLE_SWITCH_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/include/material_colors.hpp b/common/tier4_state_rviz_plugin/src/include/material_colors.hpp new file mode 100644 index 0000000000000..d146b599ab600 --- /dev/null +++ b/common/tier4_state_rviz_plugin/src/include/material_colors.hpp @@ -0,0 +1,88 @@ +// Copyright 2024 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 MATERIAL_COLORS_HPP_ +#define MATERIAL_COLORS_HPP_ +#include + +namespace autoware +{ +namespace state_rviz_plugin +{ +namespace colors +{ +struct MaterialColors +{ + std::string primary = "#8BD0F0"; + std::string surface_tint = "#8BD0F0"; + std::string on_primary = "#003546"; + std::string primary_container = "#004D64"; + std::string on_primary_container = "#BEE9FF"; + std::string secondary = "#B4CAD6"; + std::string on_secondary = "#1F333C"; + std::string secondary_container = "#354A54"; + std::string on_secondary_container = "#D0E6F2"; + std::string tertiary = "#C6C2EA"; + std::string on_tertiary = "#2F2D4D"; + std::string tertiary_container = "#454364"; + std::string on_tertiary_container = "#E3DFFF"; + std::string error = "#FFB4AB"; + std::string on_error = "#690005"; + std::string error_container = "#93000A"; + std::string on_error_container = "#FFDAD6"; + std::string background = "#0F1417"; + std::string on_background = "#DFE3E7"; + std::string surface = "#0F1417"; + std::string on_surface = "#DFE3E7"; + std::string surface_variant = "#40484C"; + std::string on_surface_variant = "#C0C8CD"; + std::string outline = "#8A9297"; + std::string outline_variant = "#40484C"; + std::string shadow = "#000000"; + std::string scrim = "#000000"; + std::string inverse_surface = "#DFE3E7"; + std::string inverse_on_surface = "#2C3134"; + std::string inverse_primary = "#126682"; + std::string primary_fixed = "#BEE9FF"; + std::string on_primary_fixed = "#001F2A"; + std::string primary_fixed_dim = "#8BD0F0"; + std::string on_primary_fixed_variant = "#004D64"; + std::string secondary_fixed = "#D0E6F2"; + std::string on_secondary_fixed = "#081E27"; + std::string secondary_fixed_dim = "#B4CAD6"; + std::string on_secondary_fixed_variant = "#354A54"; + std::string tertiary_fixed = "#E3DFFF"; + std::string on_tertiary_fixed = "#1A1836"; + std::string tertiary_fixed_dim = "#C6C2EA"; + std::string on_tertiary_fixed_variant = "#454364"; + std::string surface_dim = "#0F1417"; + std::string surface_bright = "#353A3D"; + std::string surface_container_lowest = "#0A0F11"; + std::string surface_container_low = "#171C1F"; + std::string surface_container = "#1B2023"; + std::string surface_container_high = "#262B2E"; + std::string surface_container_highest = "#303538"; + std::string disabled_elevated_button_bg = "#292D30"; + std::string success = "#8DF08B"; + std::string warning = "#EEF08B"; + std::string info = "#8BD0F0"; + std::string danger = "#F08B8B"; +}; + +inline MaterialColors default_colors; +} // namespace colors +} // namespace state_rviz_plugin +} // namespace autoware + +#endif // MATERIAL_COLORS_HPP_ diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp b/common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp similarity index 100% rename from common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp rename to common/tier4_state_rviz_plugin/src/include/velocity_steering_factors_panel.hpp diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp index 6ea142ed66a1b..90ad18fe5af6c 100644 --- a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp @@ -14,7 +14,7 @@ // limitations under the License. // -#include "velocity_steering_factors_panel.hpp" +#include "include/velocity_steering_factors_panel.hpp" #include #include diff --git a/control/autonomous_emergency_braking/README.md b/control/autonomous_emergency_braking/README.md index 7a7bf212320e0..23f4476d9659d 100644 --- a/control/autonomous_emergency_braking/README.md +++ b/control/autonomous_emergency_braking/README.md @@ -16,6 +16,24 @@ This module has following assumptions. ![aeb_range](./image/range.drawio.svg) +### IMU path generation: steering angle vs IMU's angular velocity + +Currently, the IMU-based path is generated using the angular velocity obtained by the IMU itself. It has been suggested that the steering angle could be used instead onf the angular velocity. + +The pros and cons of both approaches are: + +IMU angular velocity: + +- (+) Usually, it has high accuracy +- (-)Vehicle vibration might introduce noise. + +Steering angle: + +- (+) Not so noisy +- (-) May have a steering offset or a wrong gear ratio, and the steering angle of Autoware and the real steering may not be the same. + +For the moment, there are no plans to implement the steering angle on the path creation process of the AEB module. + ### Limitations - AEB might not be able to react with obstacles that are close to the ground. It depends on the performance of the pre-processing methods applied to the point cloud. diff --git a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml index 807f247807d98..16d4b6fe458cd 100644 --- a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml +++ b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml @@ -13,7 +13,7 @@ prediction_time_horizons: [1.0, 2.0, 3.0, 5.0] stopped_velocity_threshold: 1.0 - detection_radius_list: [50.0, 100.0, 150.0, 200.0] + detection_radius_list: [20.0, 40.0, 60.0, 80.0, 100.0, 120.0, 140.0, 160.0, 180.0, 200.0] detection_height_list: [10.0] detection_count_purge_seconds: 36000.0 objects_count_window_seconds: 1.0 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 fed268084792a..cf11c65ac805c 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 @@ -17,6 +17,7 @@ + @@ -26,6 +27,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index d60f54c2e641a..e1a2260f3ee6d 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -22,6 +22,7 @@ + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index aa649710836da..57d0280330641 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -82,12 +82,12 @@ /> - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index d04a405a61c7b..9d39c397f9417 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -57,9 +57,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner autoware_remaining_distance_time_calculator behavior_path_planner - behavior_velocity_planner costmap_generator external_cmd_selector external_velocity_limit_selector diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index a1bbfc883de7c..6253dce864ddf 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -25,11 +25,11 @@ - - - - - + + + + + @@ -57,6 +57,13 @@ + + + + + + + @@ -70,8 +77,20 @@ + + + + + + + + + + + + - + @@ -85,52 +104,31 @@ - + + - - + + + - - - - - - - - - - + + + - + - - - - - - - - - - - - - - - - diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 9944cbb84d97c..13e63ddf3c170 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -13,7 +13,7 @@ include_directories( ament_auto_find_build_dependencies() -ament_auto_add_library(ekf_localizer_lib SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/ekf_localizer.cpp src/covariance.cpp src/diagnostics.cpp @@ -24,21 +24,20 @@ ament_auto_add_library(ekf_localizer_lib SHARED src/ekf_module.cpp ) -target_link_libraries(ekf_localizer_lib Eigen3::Eigen) - -ament_auto_add_executable(ekf_localizer src/ekf_localizer_node.cpp) - -target_compile_options(ekf_localizer PUBLIC -g -Wall -Wextra -Wpedantic -Werror) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "EKFLocalizer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) -target_link_libraries(ekf_localizer ekf_localizer_lib) -target_include_directories(ekf_localizer PUBLIC include) +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) - target_link_libraries("${test_name}" ekf_localizer_lib) + target_link_libraries("${test_name}" ${PROJECT_NAME}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 6925e8b63c66b..8ed925c1bc228 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -102,7 +102,7 @@ class Simple1DFilter class EKFLocalizer : public rclcpp::Node { public: - EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit EKFLocalizer(const rclcpp::NodeOptions & options); private: const std::shared_ptr warning_; diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml index b6a1bd06185c2..3c66fabe34650 100644 --- a/localization/ekf_localizer/launch/ekf_localizer.launch.xml +++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml @@ -17,7 +17,7 @@ - + diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index e9d756e547f26..dad1b0e730711 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -29,6 +29,7 @@ kalman_filter nav_msgs rclcpp + rclcpp_components std_srvs tf2 tf2_ros diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index f77481d45a534..687e812679574 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -40,8 +40,8 @@ using std::placeholders::_1; -EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options), +EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("ekf_localizer", node_options), warning_(std::make_shared(this)), params_(this), ekf_dt_(params_.ekf_dt), @@ -479,3 +479,6 @@ void EKFLocalizer::serviceTriggerNode( res->success = true; return; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(EKFLocalizer) diff --git a/localization/ekf_localizer/src/ekf_localizer_node.cpp b/localization/ekf_localizer/src/ekf_localizer_node.cpp deleted file mode 100644 index 0f75f19c5d50b..0000000000000 --- a/localization/ekf_localizer/src/ekf_localizer_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ekf_localizer/ekf_localizer.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("ekf_localizer", node_options); - - rclcpp::spin(node); - - return 0; -} diff --git a/localization/geo_pose_projector/CMakeLists.txt b/localization/geo_pose_projector/CMakeLists.txt index 6e2cdf32fb6c5..f274e7d23dbab 100644 --- a/localization/geo_pose_projector/CMakeLists.txt +++ b/localization/geo_pose_projector/CMakeLists.txt @@ -4,11 +4,15 @@ project(geo_pose_projector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(geo_pose_projector - src/geo_pose_projector_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/geo_pose_projector.cpp ) -ament_target_dependencies(geo_pose_projector) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "GeoPoseProjector" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml b/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml index d840add1ed1c7..de6785a016858 100644 --- a/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml +++ b/localization/geo_pose_projector/launch/geo_pose_projector.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/localization/geo_pose_projector/package.xml b/localization/geo_pose_projector/package.xml index c0e2db59deb64..36b2aec8384ac 100644 --- a/localization/geo_pose_projector/package.xml +++ b/localization/geo_pose_projector/package.xml @@ -24,6 +24,7 @@ geography_utils geometry_msgs rclcpp + rclcpp_components tf2_geometry_msgs tf2_ros diff --git a/localization/geo_pose_projector/src/geo_pose_projector.cpp b/localization/geo_pose_projector/src/geo_pose_projector.cpp index d147888bb743b..80bb407958de2 100644 --- a/localization/geo_pose_projector/src/geo_pose_projector.cpp +++ b/localization/geo_pose_projector/src/geo_pose_projector.cpp @@ -25,8 +25,8 @@ #include -GeoPoseProjector::GeoPoseProjector() -: Node("geo_pose_projector"), publish_tf_(declare_parameter("publish_tf")) +GeoPoseProjector::GeoPoseProjector(const rclcpp::NodeOptions & options) +: rclcpp::Node("geo_pose_projector", options), publish_tf_(declare_parameter("publish_tf")) { // Subscribe to map_projector_info topic const auto adaptor = component_interface_utils::NodeAdaptor(this); @@ -102,3 +102,6 @@ void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr m tf_broadcaster_->sendTransform(transform_stamped); } } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(GeoPoseProjector) diff --git a/localization/geo_pose_projector/src/geo_pose_projector.hpp b/localization/geo_pose_projector/src/geo_pose_projector.hpp index b0611fec36984..738e805ef8101 100644 --- a/localization/geo_pose_projector/src/geo_pose_projector.hpp +++ b/localization/geo_pose_projector/src/geo_pose_projector.hpp @@ -36,7 +36,7 @@ class GeoPoseProjector : public rclcpp::Node using MapProjectorInfo = map_interface::MapProjectorInfo; public: - GeoPoseProjector(); + explicit GeoPoseProjector(const rclcpp::NodeOptions & options); private: void on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr msg); diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index 57589837dd529..a832383ff4761 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -4,17 +4,12 @@ project(gyro_odometer) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(${PROJECT_NAME} - src/gyro_odometer_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/gyro_odometer_core.cpp ) target_link_libraries(${PROJECT_NAME} fmt) -ament_auto_add_library(gyro_odometer_node SHARED - src/gyro_odometer_core.cpp -) - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_gyro_odometer test/test_main.cpp @@ -25,10 +20,15 @@ if(BUILD_TESTING) rclcpp ) target_link_libraries(test_gyro_odometer - gyro_odometer_node + ${PROJECT_NAME} ) endif() +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::gyro_odometer::GyroOdometerNode" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package(INSTALL_TO_SHARE launch diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index 3a6358e62c21a..2926589bd2da9 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -36,14 +36,17 @@ #include #include -class GyroOdometer : public rclcpp::Node +namespace autoware::gyro_odometer +{ + +class GyroOdometerNode : public rclcpp::Node { private: using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: - explicit GyroOdometer(const rclcpp::NodeOptions & options); - ~GyroOdometer(); + explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); + ~GyroOdometerNode(); private: void callbackVehicleTwist( @@ -75,4 +78,6 @@ class GyroOdometer : public rclcpp::Node std::deque gyro_queue_; }; +} // namespace autoware::gyro_odometer + #endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml index 21aff3e10d26c..aed6050858fe1 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -11,7 +11,7 @@ - + diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index a0a982ddedc16..3c979eb69ac8a 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -19,14 +19,13 @@ fmt geometry_msgs + rclcpp + rclcpp_components sensor_msgs tf2 tf2_geometry_msgs tier4_autoware_utils - rclcpp - rclcpp_components - ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 5de0ecd7cdc0a..bda7af74b8489 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -14,6 +14,8 @@ #include "gyro_odometer/gyro_odometer_core.hpp" +#include + #ifdef ROS_DISTRO_GALACTIC #include #else @@ -25,6 +27,42 @@ #include #include +namespace autoware::gyro_odometer +{ + +GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) +: Node("gyro_odometer", node_options), + output_frame_(declare_parameter("output_frame")), + message_timeout_sec_(declare_parameter("message_timeout_sec")), + vehicle_twist_arrived_(false), + imu_arrived_(false) +{ + transform_listener_ = std::make_shared(this); + logger_configure_ = std::make_unique(this); + + vehicle_twist_sub_ = create_subscription( + "vehicle/twist_with_covariance", rclcpp::QoS{100}, + std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1)); + + imu_sub_ = create_subscription( + "imu", rclcpp::QoS{100}, + std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1)); + + twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); + twist_with_covariance_raw_pub_ = create_publisher( + "twist_with_covariance_raw", rclcpp::QoS{10}); + + twist_pub_ = create_publisher("twist", rclcpp::QoS{10}); + twist_with_covariance_pub_ = create_publisher( + "twist_with_covariance", rclcpp::QoS{10}); + + // TODO(YamatoAndo) createTimer +} + +GyroOdometerNode::~GyroOdometerNode() +{ +} + std::array transformCovariance(const std::array & cov) { using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; @@ -100,39 +138,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( return twist_with_cov; } -GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) -: Node("gyro_odometer", options), - output_frame_(declare_parameter("output_frame")), - message_timeout_sec_(declare_parameter("message_timeout_sec")), - vehicle_twist_arrived_(false), - imu_arrived_(false) -{ - transform_listener_ = std::make_shared(this); - logger_configure_ = std::make_unique(this); - - vehicle_twist_sub_ = create_subscription( - "vehicle/twist_with_covariance", rclcpp::QoS{100}, - std::bind(&GyroOdometer::callbackVehicleTwist, this, std::placeholders::_1)); - - imu_sub_ = create_subscription( - "imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1)); - - twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); - twist_with_covariance_raw_pub_ = create_publisher( - "twist_with_covariance_raw", rclcpp::QoS{10}); - - twist_pub_ = create_publisher("twist", rclcpp::QoS{10}); - twist_with_covariance_pub_ = create_publisher( - "twist_with_covariance", rclcpp::QoS{10}); - - // TODO(YamatoAndo) createTimer -} - -GyroOdometer::~GyroOdometer() -{ -} - -void GyroOdometer::callbackVehicleTwist( +void GyroOdometerNode::callbackVehicleTwist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) { vehicle_twist_arrived_ = true; @@ -173,7 +179,7 @@ void GyroOdometer::callbackVehicleTwist( gyro_queue_.clear(); } -void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) +void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { imu_arrived_ = true; if (!vehicle_twist_arrived_) { @@ -241,7 +247,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m gyro_queue_.clear(); } -void GyroOdometer::publishData( +void GyroOdometerNode::publishData( const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw) { geometry_msgs::msg::TwistStamped twist_raw; @@ -269,3 +275,8 @@ void GyroOdometer::publishData( twist_pub_->publish(twist); twist_with_covariance_pub_->publish(twist_with_covariance); } + +} // namespace autoware::gyro_odometer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gyro_odometer::GyroOdometerNode) diff --git a/localization/gyro_odometer/src/gyro_odometer_node.cpp b/localization/gyro_odometer/src/gyro_odometer_node.cpp deleted file mode 100644 index 5bb6241506fbe..0000000000000 --- a/localization/gyro_odometer/src/gyro_odometer_node.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "gyro_odometer/gyro_odometer_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - auto node = std::make_shared(options); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp index 7f6416fbdda16..b7849ef03bfc5 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -108,7 +108,8 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) expected_output_twist.twist.twist.angular.y = input_imu.angular_velocity.y; expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z; - auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = + std::make_shared(getNodeOptionsWithDefaultParams()); auto imu_generator = std::make_shared(); auto velocity_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); @@ -135,7 +136,8 @@ TEST(GyroOdometer, TestGyroOdometerImuOnly) { Imu input_imu = generateSampleImu(); - auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = + std::make_shared(getNodeOptionsWithDefaultParams()); auto imu_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); diff --git a/localization/localization_error_monitor/CMakeLists.txt b/localization/localization_error_monitor/CMakeLists.txt index 3528367486350..c27e51e6e0359 100644 --- a/localization/localization_error_monitor/CMakeLists.txt +++ b/localization/localization_error_monitor/CMakeLists.txt @@ -4,15 +4,16 @@ project(localization_error_monitor) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(localization_error_monitor_module SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/diagnostics.cpp + src/localization_error_monitor.cpp ) -ament_auto_add_executable(localization_error_monitor - src/main.cpp - src/node.cpp +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "LocalizationErrorMonitor" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor ) -target_link_libraries(localization_error_monitor localization_error_monitor_module) if(BUILD_TESTING) function(add_testcase filepath) @@ -20,17 +21,17 @@ if(BUILD_TESTING) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) - target_link_libraries(${test_name} localization_error_monitor_module) + target_link_libraries(${test_name} ${PROJECT_NAME}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() - find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() add_testcase(test/test_diagnostics.cpp) endif() -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE config launch ) diff --git a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp similarity index 87% rename from localization/localization_error_monitor/include/localization_error_monitor/node.hpp rename to localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp index 296b278004333..6016677d88136 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_ERROR_MONITOR__NODE_HPP_ -#define LOCALIZATION_ERROR_MONITOR__NODE_HPP_ +#ifndef LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ +#define LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ #include #include @@ -58,7 +58,7 @@ class LocalizationErrorMonitor : public rclcpp::Node double measureSizeEllipseAlongBodyFrame(const Eigen::Matrix2d & Pinv, double theta); public: - LocalizationErrorMonitor(); + explicit LocalizationErrorMonitor(const rclcpp::NodeOptions & options); ~LocalizationErrorMonitor() = default; }; -#endif // LOCALIZATION_ERROR_MONITOR__NODE_HPP_ +#endif // LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ diff --git a/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml index eb2b5741b00fc..ad3e8beff92ab 100644 --- a/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml +++ b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index a1b352d911a0d..0138451b069e4 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -23,6 +23,7 @@ diagnostic_msgs diagnostic_updater nav_msgs + rclcpp_components tf2 tf2_geometry_msgs tier4_autoware_utils diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/localization_error_monitor.cpp similarity index 94% rename from localization/localization_error_monitor/src/node.cpp rename to localization/localization_error_monitor/src/localization_error_monitor.cpp index f47372a0158b2..67cdb78c942fb 100644 --- a/localization/localization_error_monitor/src/node.cpp +++ b/localization/localization_error_monitor/src/localization_error_monitor.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_error_monitor/node.hpp" +#include "localization_error_monitor/localization_error_monitor.hpp" #include "localization_error_monitor/diagnostics.hpp" @@ -32,7 +32,8 @@ #include #include -LocalizationErrorMonitor::LocalizationErrorMonitor() : Node("localization_error_monitor") +LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & options) +: Node("localization_error_monitor", options) { scale_ = this->declare_parameter("scale"); error_ellipse_size_ = this->declare_parameter("error_ellipse_size"); @@ -143,3 +144,6 @@ double LocalizationErrorMonitor::measureSizeEllipseAlongBodyFrame( double d = std::sqrt((e.transpose() * Pinv * e)(0, 0) / Pinv.determinant()); return d; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(LocalizationErrorMonitor) diff --git a/localization/localization_error_monitor/src/main.cpp b/localization/localization_error_monitor/src/main.cpp deleted file mode 100644 index f4fa5ab664005..0000000000000 --- a/localization/localization_error_monitor/src/main.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2020 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 "localization_error_monitor/node.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 7ac78514c49fe..5e4db9571c404 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -22,20 +22,24 @@ else() endif() endif() -find_package(glog REQUIRED) find_package(PCL REQUIRED COMPONENTS common io registration) include_directories(${PCL_INCLUDE_DIRS}) -ament_auto_add_executable(ndt_scan_matcher +ament_auto_add_library(${PROJECT_NAME} SHARED src/diagnostics_module.cpp src/map_update_module.cpp src/ndt_scan_matcher_core.cpp - src/ndt_scan_matcher_node.cpp src/particle.cpp ) link_directories(${PCL_LIBRARY_DIRS}) -target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES} glog::glog) +target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES}) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "NDTScanMatcher" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) if(BUILD_TESTING) add_launch_test( @@ -46,19 +50,9 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_auto_add_gtest(standard_sequence_for_initial_pose_estimation test/test_cases/standard_sequence_for_initial_pose_estimation.cpp - src/diagnostics_module.cpp - src/map_update_module.cpp - src/ndt_scan_matcher_core.cpp -# src/ndt_scan_matcher_node.cpp - src/particle.cpp ) ament_auto_add_gtest(once_initialize_at_out_of_map_then_initialize_correctly test/test_cases/once_initialize_at_out_of_map_then_initialize_correctly.cpp - src/diagnostics_module.cpp - src/map_update_module.cpp - src/ndt_scan_matcher_core.cpp -# src/ndt_scan_matcher_node.cpp - src/particle.cpp ) endif() diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index fbc2fa3c9a3f3..a44db7bbaa4bf 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -262,23 +262,23 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num drawing -| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_num`) | -| ----------------------------------------- | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -------------------------------- | ------------------------------------------------------------------------ | -| `topic_time_stamp` | the time stamp of input topic | none | none | no | -| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes | -| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes | -| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes | -| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes | -| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | yes | -| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | -| `is_set_map_points` | whether the map points is set or not | not set | none | yes | -| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | -| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | -| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | -| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes | -| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no | -| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no | -| `skipping_publish_num` | the number of times rejected estimation results consecutively | none | the number of times is 5 or more | - | +| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_num`) | +| ----------------------------------------- | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ----------------------------- | --------------------------------------------------------------------------------------------------- | +| `topic_time_stamp` | the time stamp of input topic | none | none | no | +| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes | +| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes | +| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes | +| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes | +| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | if `is_activated` is false, then estimation is not executed and `skipping_publish_num` is set to 0. | +| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | +| `is_set_map_points` | whether the map points is set or not | not set | none | yes | +| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | +| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | +| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | +| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes | +| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no | +| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no | +| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is 5 or more | none | - | ※The `sensor_points_callback` shares the same callback group as the `trigger_node_service` and `ndt_align_service`. Consequently, if the initial pose estimation takes too long, this diagnostic may become stale. diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index cbcb0a9f74bc4..671e2ace56cad 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index b9a1f7dbad1bd..a37e7c370adf5 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -29,6 +29,7 @@ ndt_omp pcl_conversions rclcpp + rclcpp_components sensor_msgs std_msgs std_srvs 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 8692027e9634a..624eb89671a41 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -275,13 +275,14 @@ void NDTScanMatcher::callback_sensor_points( // check skipping_publish_num static size_t skipping_publish_num = 0; const size_t error_skipping_publish_num = 5; - skipping_publish_num = is_succeed_scan_matching ? 0 : (skipping_publish_num + 1); + skipping_publish_num = + ((is_succeed_scan_matching || !is_activated_) ? 0 : (skipping_publish_num + 1)); diagnostics_scan_points_->addKeyValue("skipping_publish_num", skipping_publish_num); if (skipping_publish_num >= error_skipping_publish_num) { std::stringstream message; message << "skipping_publish_num exceed limit (" << skipping_publish_num << " times)."; diagnostics_scan_points_->updateLevelAndMessage( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } diagnostics_scan_points_->publish(); @@ -1069,3 +1070,6 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( return result_pose_with_cov_msg; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(NDTScanMatcher) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp deleted file mode 100644 index 785055eef3700..0000000000000 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ndt_scan_matcher/ndt_scan_matcher_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); // NOLINT - google::InstallFailureSignalHandler(); - } - - rclcpp::init(argc, argv); - auto ndt_scan_matcher = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor exec; - exec.add_node(ndt_scan_matcher); - exec.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/pose2twist/CMakeLists.txt b/localization/pose2twist/CMakeLists.txt index a2fbbddf7d120..2a586aa9cd049 100644 --- a/localization/pose2twist/CMakeLists.txt +++ b/localization/pose2twist/CMakeLists.txt @@ -4,11 +4,16 @@ project(pose2twist) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose2twist - src/pose2twist_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose2twist_core.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "Pose2Twist" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp index c2a39f7e2ed3d..459a62ea5cd13 100644 --- a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp +++ b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp @@ -24,7 +24,7 @@ class Pose2Twist : public rclcpp::Node { public: - Pose2Twist(); + explicit Pose2Twist(const rclcpp::NodeOptions & options); ~Pose2Twist() = default; private: diff --git a/localization/pose2twist/launch/pose2twist.launch.xml b/localization/pose2twist/launch/pose2twist.launch.xml index a0fae57a163e6..57a41dbfcf017 100644 --- a/localization/pose2twist/launch/pose2twist.launch.xml +++ b/localization/pose2twist/launch/pose2twist.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml index 16c49bb51c218..07e445c72978c 100644 --- a/localization/pose2twist/package.xml +++ b/localization/pose2twist/package.xml @@ -19,6 +19,7 @@ geometry_msgs rclcpp + rclcpp_components tf2_geometry_msgs tier4_debug_msgs diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp index a321a06122816..4b98ec6c81ad4 100644 --- a/localization/pose2twist/src/pose2twist_core.cpp +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -24,7 +24,7 @@ #include #include -Pose2Twist::Pose2Twist() : Node("pose2twist_core") +Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose2twist", options) { using std::placeholders::_1; @@ -129,3 +129,6 @@ void Pose2Twist::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_ms angular_z_msg.data = twist_msg.twist.angular.z; angular_z_pub_->publish(angular_z_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Pose2Twist) diff --git a/localization/pose_instability_detector/CMakeLists.txt b/localization/pose_instability_detector/CMakeLists.txt index 5270df636d791..c6f94ab7df16e 100644 --- a/localization/pose_instability_detector/CMakeLists.txt +++ b/localization/pose_instability_detector/CMakeLists.txt @@ -4,14 +4,19 @@ project(pose_instability_detector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose_instability_detector - src/main.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_instability_detector.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "PoseInstabilityDetector" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_pose_instability_detector + ament_auto_add_gtest(test_${PROJECT_NAME} test/test.cpp src/pose_instability_detector.cpp ) @@ -19,6 +24,6 @@ endif() ament_auto_package( INSTALL_TO_SHARE - launch - config + launch + config ) diff --git a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml index 4a390ee2854d7..5ebe7d7e429e0 100644 --- a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml +++ b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml index bf57e5589b747..d658d1c2d437f 100644 --- a/localization/pose_instability_detector/package.xml +++ b/localization/pose_instability_detector/package.xml @@ -22,6 +22,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components tf2 tf2_geometry_msgs tier4_autoware_utils diff --git a/localization/pose_instability_detector/src/main.cpp b/localization/pose_instability_detector/src/main.cpp deleted file mode 100644 index 34e679e86730f..0000000000000 --- a/localization/pose_instability_detector/src/main.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023- Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "pose_instability_detector.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto pose_instability_detector = std::make_shared(); - rclcpp::spin(pose_instability_detector); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index afb7d6e007db2..c2bce6a3db288 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -23,7 +23,7 @@ #include PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) -: Node("pose_instability_detector", options), +: rclcpp::Node("pose_instability_detector", options), threshold_diff_position_x_(this->declare_parameter("threshold_diff_position_x")), threshold_diff_position_y_(this->declare_parameter("threshold_diff_position_y")), threshold_diff_position_z_(this->declare_parameter("threshold_diff_position_z")), @@ -174,3 +174,6 @@ void PoseInstabilityDetector::callback_timer() prev_odometry_ = latest_odometry_; twist_buffer_.clear(); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(PoseInstabilityDetector) diff --git a/localization/stop_filter/CMakeLists.txt b/localization/stop_filter/CMakeLists.txt index 97a0443195ae5..4776a1f4967b2 100644 --- a/localization/stop_filter/CMakeLists.txt +++ b/localization/stop_filter/CMakeLists.txt @@ -4,11 +4,15 @@ project(stop_filter) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(stop_filter - src/stop_filter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/stop_filter.cpp ) -ament_target_dependencies(stop_filter) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "StopFilter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/stop_filter/include/stop_filter/stop_filter.hpp b/localization/stop_filter/include/stop_filter/stop_filter.hpp index 3c2b91590234f..24145a7920d91 100644 --- a/localization/stop_filter/include/stop_filter/stop_filter.hpp +++ b/localization/stop_filter/include/stop_filter/stop_filter.hpp @@ -37,7 +37,7 @@ class StopFilter : public rclcpp::Node { public: - StopFilter(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit StopFilter(const rclcpp::NodeOptions & options); private: rclcpp::Publisher::SharedPtr pub_odom_; //!< @brief odom publisher diff --git a/localization/stop_filter/launch/stop_filter.launch.xml b/localization/stop_filter/launch/stop_filter.launch.xml index 0ea92d26c9677..c53b37efc9954 100644 --- a/localization/stop_filter/launch/stop_filter.launch.xml +++ b/localization/stop_filter/launch/stop_filter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml index 83eaf1b9fa821..2e6d5c4f8bbe1 100644 --- a/localization/stop_filter/package.xml +++ b/localization/stop_filter/package.xml @@ -21,6 +21,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components tf2 tier4_debug_msgs diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index ac0960b8cb67b..4d6b2c6240867 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -24,8 +24,8 @@ using std::placeholders::_1; -StopFilter::StopFilter(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options) +StopFilter::StopFilter(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("stop_filter", node_options) { vx_threshold_ = declare_parameter("vx_threshold"); wz_threshold_ = declare_parameter("wz_threshold"); @@ -57,3 +57,6 @@ void StopFilter::callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) pub_stop_flag_->publish(stop_flag_msg); pub_odom_->publish(odom_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(StopFilter) diff --git a/localization/stop_filter/src/stop_filter_node.cpp b/localization/stop_filter/src/stop_filter_node.cpp deleted file mode 100644 index 9748214828de2..0000000000000 --- a/localization/stop_filter/src/stop_filter_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2021 TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "stop_filter/stop_filter.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("stop_filter", node_options); - - rclcpp::spin(node); - - return 0; -} diff --git a/localization/twist2accel/CMakeLists.txt b/localization/twist2accel/CMakeLists.txt index 59f23eacb2fb3..9178bf02288a8 100644 --- a/localization/twist2accel/CMakeLists.txt +++ b/localization/twist2accel/CMakeLists.txt @@ -4,11 +4,15 @@ project(twist2accel) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(twist2accel - src/twist2accel_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/twist2accel.cpp ) -ament_target_dependencies(twist2accel) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "Twist2Accel" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/twist2accel/include/twist2accel/twist2accel.hpp b/localization/twist2accel/include/twist2accel/twist2accel.hpp index da5f2b4a3b228..0db69890fbfe8 100644 --- a/localization/twist2accel/include/twist2accel/twist2accel.hpp +++ b/localization/twist2accel/include/twist2accel/twist2accel.hpp @@ -40,7 +40,7 @@ class Twist2Accel : public rclcpp::Node { public: - Twist2Accel(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit Twist2Accel(const rclcpp::NodeOptions & options); private: rclcpp::Publisher::SharedPtr diff --git a/localization/twist2accel/launch/twist2accel.launch.xml b/localization/twist2accel/launch/twist2accel.launch.xml index c4c9a3ed50bfc..c534a288aac3e 100644 --- a/localization/twist2accel/launch/twist2accel.launch.xml +++ b/localization/twist2accel/launch/twist2accel.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml index 08dacf9185769..0dbce08f309ac 100644 --- a/localization/twist2accel/package.xml +++ b/localization/twist2accel/package.xml @@ -21,6 +21,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components signal_processing tf2 tier4_debug_msgs diff --git a/localization/twist2accel/src/twist2accel.cpp b/localization/twist2accel/src/twist2accel.cpp index 68019f27e95fe..0af29445bbeb8 100644 --- a/localization/twist2accel/src/twist2accel.cpp +++ b/localization/twist2accel/src/twist2accel.cpp @@ -24,8 +24,8 @@ using std::placeholders::_1; -Twist2Accel::Twist2Accel(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options) +Twist2Accel::Twist2Accel(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("twist2accel", node_options) { sub_odom_ = create_subscription( "input/odom", 1, std::bind(&Twist2Accel::callbackOdometry, this, _1)); @@ -103,3 +103,6 @@ void Twist2Accel::estimateAccel(const geometry_msgs::msg::TwistStamped::SharedPt pub_accel_->publish(accel_msg); prev_twist_ptr_ = msg; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Twist2Accel) diff --git a/localization/twist2accel/src/twist2accel_node.cpp b/localization/twist2accel/src/twist2accel_node.cpp deleted file mode 100644 index ce8ed31db5c32..0000000000000 --- a/localization/twist2accel/src/twist2accel_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2022 TIER IV -// -// 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 "twist2accel/twist2accel.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("twist2accel", node_options); - - rclcpp::spin(node); - - return 0; -} diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index f6102a1efa795..87f519ab22572 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -6,25 +6,24 @@ autoware_package() ament_auto_find_build_dependencies() -ament_auto_add_library(map_projection_loader_lib SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/map_projection_loader.cpp src/load_info_from_lanelet2_map.cpp ) -target_link_libraries(map_projection_loader_lib yaml-cpp) - -ament_auto_add_executable(map_projection_loader src/map_projection_loader_node.cpp) - -target_compile_options(map_projection_loader PUBLIC -g -Wall -Wextra -Wpedantic -Werror) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "MapProjectionLoader" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) -target_link_libraries(map_projection_loader map_projection_loader_lib) -target_include_directories(map_projection_loader PUBLIC include) +target_link_libraries(${PROJECT_NAME} yaml-cpp) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gmock(${test_name} ${filepath}) - target_link_libraries("${test_name}" map_projection_loader_lib) + target_link_libraries("${test_name}" ${PROJECT_NAME}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() @@ -57,7 +56,8 @@ if(BUILD_TESTING) add_testcase(test/test_load_info_from_lanelet2_map.cpp) endif() -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch config ) diff --git a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp index 54e794e2742bf..05bc6e64e1675 100644 --- a/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp +++ b/map/map_projection_loader/include/map_projection_loader/map_projection_loader.hpp @@ -29,7 +29,7 @@ tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( class MapProjectionLoader : public rclcpp::Node { public: - MapProjectionLoader(); + explicit MapProjectionLoader(const rclcpp::NodeOptions & options); private: using MapProjectorInfo = map_interface::MapProjectorInfo; diff --git a/map/map_projection_loader/launch/map_projection_loader.launch.xml b/map/map_projection_loader/launch/map_projection_loader.launch.xml index a6570b69d3498..13418a7e97423 100644 --- a/map/map_projection_loader/launch/map_projection_loader.launch.xml +++ b/map/map_projection_loader/launch/map_projection_loader.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index 475881577bd58..7a930085cd7b1 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -21,6 +21,7 @@ component_interface_utils lanelet2_extension rclcpp + rclcpp_components tier4_map_msgs yaml-cpp diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 5966baaed8383..383051e8f67a5 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -82,7 +82,8 @@ tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( return msg; } -MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") +MapProjectionLoader::MapProjectionLoader(const rclcpp::NodeOptions & options) +: rclcpp::Node("map_projection_loader", options) { const std::string yaml_filename = this->declare_parameter("map_projector_info_path"); const std::string lanelet2_map_filename = @@ -96,3 +97,6 @@ MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader") adaptor.init_pub(publisher_); publisher_->publish(msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(MapProjectionLoader) diff --git a/map/map_projection_loader/src/map_projection_loader_node.cpp b/map/map_projection_loader/src/map_projection_loader_node.cpp deleted file mode 100644 index 1d9336be0d9dd..0000000000000 --- a/map/map_projection_loader/src/map_projection_loader_node.cpp +++ /dev/null @@ -1,23 +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 "map_projection_loader/map_projection_loader.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py index b8540550ce9da..0d0b5cb31afba 100644 --- a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py index c7697038cc253..6a17ff340b19f 100644 --- a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py index f75beddc6827c..37cfd9936bf20 100644 --- a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py index 765f3cde04679..7bccdc7875454 100644 --- a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py @@ -44,7 +44,7 @@ def generate_test_description(): map_projection_loader_node = Node( package="map_projection_loader", - executable="map_projection_loader", + executable="map_projection_loader_node", output="screen", parameters=[ { diff --git a/perception/detected_object_validation/CMakeLists.txt b/perception/detected_object_validation/CMakeLists.txt index f882c6dd165fe..d023c6c9df328 100644 --- a/perception/detected_object_validation/CMakeLists.txt +++ b/perception/detected_object_validation/CMakeLists.txt @@ -85,6 +85,12 @@ rclcpp_components_register_node(occupancy_grid_based_validator EXECUTABLE occupancy_grid_based_validator_node ) +if(BUILD_TESTING) + ament_auto_add_gtest(detection_object_validation_tests + test/test_utils.cpp + ) +endif() + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/perception/detected_object_validation/test/test_utils.cpp b/perception/detected_object_validation/test/test_utils.cpp new file mode 100644 index 0000000000000..f7077dadb1ef2 --- /dev/null +++ b/perception/detected_object_validation/test/test_utils.cpp @@ -0,0 +1,67 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "detected_object_validation/utils/utils.hpp" + +#include + +#include + +using AutowareLabel = autoware_perception_msgs::msg::ObjectClassification; + +utils::FilterTargetLabel createFilterTargetAll() +{ + utils::FilterTargetLabel filter; + filter.UNKNOWN = true; + filter.CAR = true; + filter.TRUCK = true; + filter.BUS = true; + filter.TRAILER = true; + filter.MOTORCYCLE = true; + filter.BICYCLE = true; + filter.PEDESTRIAN = true; + return filter; +} + +utils::FilterTargetLabel createFilterTargetVehicle() +{ + utils::FilterTargetLabel filter; + filter.UNKNOWN = false; + filter.CAR = true; + filter.TRUCK = true; + filter.BUS = true; + filter.TRAILER = true; + filter.MOTORCYCLE = false; + filter.BICYCLE = false; + filter.PEDESTRIAN = false; + return filter; +} + +TEST(IsTargetTest, AllTarget) +{ + auto filter = createFilterTargetAll(); + auto label = AutowareLabel::CAR; + EXPECT_TRUE(filter.isTarget(label)); +} + +TEST(IsTargetTest, VehicleTarget) +{ + auto filter = createFilterTargetVehicle(); + + auto car_label = AutowareLabel::CAR; + EXPECT_TRUE(filter.isTarget(car_label)); + + auto unknown_label = AutowareLabel::UNKNOWN; + EXPECT_FALSE(filter.isTarget(unknown_label)); +} diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp index f1e275a919a8a..aec3f56936828 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp @@ -32,6 +32,10 @@ class EuclideanCluster : public EuclideanClusterInterface bool cluster( const pcl::PointCloud::ConstPtr & pointcloud, std::vector> & clusters) override; + + bool cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) override; void setTolerance(float tolerance) { tolerance_ = tolerance; } private: diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp index 70c21daabcd66..65b907f747666 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp +++ b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp @@ -16,6 +16,9 @@ #include +#include +#include + #include #include @@ -41,6 +44,10 @@ class EuclideanClusterInterface const pcl::PointCloud::ConstPtr & pointcloud, std::vector> & clusters) = 0; + virtual bool cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_msg, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & output_clusters) = 0; + protected: bool use_height_ = true; int min_cluster_size_; diff --git a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp b/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp index 2ac77aebdea0c..50d2306d48f72 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp +++ b/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp @@ -31,6 +31,9 @@ void convertPointCloudClusters2Msg( const std_msgs::msg::Header & header, const std::vector> & clusters, tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg); +void convertPointCloudClusters2Msg( + const std_msgs::msg::Header & header, const std::vector & clusters, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg); void convertObjectMsg2SensorMsg( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input, sensor_msgs::msg::PointCloud2 & output); diff --git a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp index d6a07503c5ca5..375cc2d19a01f 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp @@ -35,6 +35,9 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface bool cluster( const pcl::PointCloud::ConstPtr & pointcloud, std::vector> & clusters) override; + bool cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) override; void setVoxelLeafSize(float voxel_leaf_size) { voxel_leaf_size_ = voxel_leaf_size; } void setTolerance(float tolerance) { tolerance_ = tolerance; } void setMinPointsNumberPerVoxel(int min_points_number_per_voxel) diff --git a/perception/euclidean_cluster/lib/euclidean_cluster.cpp b/perception/euclidean_cluster/lib/euclidean_cluster.cpp index bd8270bd6b881..5ba1b99403280 100644 --- a/perception/euclidean_cluster/lib/euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/euclidean_cluster.cpp @@ -33,6 +33,15 @@ EuclideanCluster::EuclideanCluster( : EuclideanClusterInterface(use_height, min_cluster_size, max_cluster_size), tolerance_(tolerance) { } +// TODO(badai-nguyen): implement input field copy for euclidean_cluster.cpp +bool EuclideanCluster::cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) +{ + (void)pointcloud_msg; + (void)clusters; + return false; +} bool EuclideanCluster::cluster( const pcl::PointCloud::ConstPtr & pointcloud, diff --git a/perception/euclidean_cluster/lib/utils.cpp b/perception/euclidean_cluster/lib/utils.cpp index 338c4b88e44b7..6d119ef3d01aa 100644 --- a/perception/euclidean_cluster/lib/utils.cpp +++ b/perception/euclidean_cluster/lib/utils.cpp @@ -62,6 +62,25 @@ void convertPointCloudClusters2Msg( msg.feature_objects.push_back(feature_object); } } + +void convertPointCloudClusters2Msg( + const std_msgs::msg::Header & header, const std::vector & clusters, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg) +{ + msg.header = header; + for (const auto & ros_pointcloud : clusters) { + tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object; + feature_object.feature.cluster = ros_pointcloud; + feature_object.object.kinematics.pose_with_covariance.pose.position = + getCentroid(ros_pointcloud); + autoware_perception_msgs::msg::ObjectClassification classification; + classification.label = autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; + classification.probability = 1.0f; + feature_object.object.classification.emplace_back(classification); + msg.feature_objects.push_back(feature_object); + } +} + void convertObjectMsg2SensorMsg( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input, sensor_msgs::msg::PointCloud2 & output) diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp index b8276296dde5f..40699fd6e27ab 100644 --- a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -40,14 +40,27 @@ VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster( min_points_number_per_voxel_(min_points_number_per_voxel) { } - +// TODO(badai-nguyen): remove this function when field copying also implemented for +// euclidean_cluster.cpp bool VoxelGridBasedEuclideanCluster::cluster( const pcl::PointCloud::ConstPtr & pointcloud, std::vector> & clusters) +{ + (void)pointcloud; + (void)clusters; + return false; +} + +bool VoxelGridBasedEuclideanCluster::cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & objects) { // TODO(Saito) implement use_height is false version // create voxel + pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud); + int point_step = pointcloud_msg->point_step; + pcl::fromROSMsg(*pointcloud_msg, *pointcloud); pcl::PointCloud::Ptr voxel_map_ptr(new pcl::PointCloud); voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, 100000.0); voxel_grid_.setMinimumPointsNumberPerVoxel(min_points_number_per_voxel_); @@ -81,36 +94,69 @@ bool VoxelGridBasedEuclideanCluster::cluster( // create map to search cluster index from voxel grid index std::unordered_map map; + std::vector temporary_clusters; // no check about cluster size + std::vector clusters_data_size; + temporary_clusters.resize(cluster_indices.size()); for (size_t cluster_idx = 0; cluster_idx < cluster_indices.size(); ++cluster_idx) { const auto & cluster = cluster_indices.at(cluster_idx); + auto & temporary_cluster = temporary_clusters.at(cluster_idx); for (const auto & point_idx : cluster.indices) { map[point_idx] = cluster_idx; } + temporary_cluster.height = pointcloud_msg->height; + temporary_cluster.fields = pointcloud_msg->fields; + temporary_cluster.point_step = point_step; + temporary_cluster.data.resize(max_cluster_size_ * point_step); + clusters_data_size.push_back(0); } // create vector of point cloud cluster. vector index is voxel grid index. - std::vector> temporary_clusters; // no check about cluster size - temporary_clusters.resize(cluster_indices.size()); - for (const auto & point : pointcloud->points) { + for (size_t i = 0; i < pointcloud->points.size(); ++i) { + const auto & point = pointcloud->points.at(i); const int index = voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z)); if (map.find(index) != map.end()) { - temporary_clusters.at(map[index]).points.push_back(point); + auto & cluster_data_size = clusters_data_size.at(map[index]); + if (cluster_data_size + point_step > std::size_t(max_cluster_size_ * point_step)) { + continue; + } + std::memcpy( + &temporary_clusters.at(map[index]).data[cluster_data_size], + &pointcloud_msg->data[i * point_step], point_step); + cluster_data_size += point_step; } } // build output and check cluster size { - for (const auto & cluster : temporary_clusters) { - if (!(min_cluster_size_ <= static_cast(cluster.points.size()) && - static_cast(cluster.points.size()) <= max_cluster_size_)) { + for (size_t i = 0; i < temporary_clusters.size(); ++i) { + auto & i_cluster_data_size = clusters_data_size.at(i); + if (!(min_cluster_size_ <= static_cast(i_cluster_data_size / point_step) && + static_cast(i_cluster_data_size / point_step) <= max_cluster_size_)) { continue; } - clusters.push_back(cluster); - clusters.back().width = cluster.points.size(); - clusters.back().height = 1; - clusters.back().is_dense = false; + const auto & cluster = temporary_clusters.at(i); + tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object; + feature_object.feature.cluster = cluster; + feature_object.feature.cluster.data.resize(i_cluster_data_size); + feature_object.feature.cluster.header = pointcloud_msg->header; + feature_object.feature.cluster.is_bigendian = pointcloud_msg->is_bigendian; + feature_object.feature.cluster.is_dense = pointcloud_msg->is_dense; + feature_object.feature.cluster.point_step = point_step; + feature_object.feature.cluster.row_step = i_cluster_data_size / pointcloud_msg->height; + feature_object.feature.cluster.width = + i_cluster_data_size / point_step / pointcloud_msg->height; + + feature_object.object.kinematics.pose_with_covariance.pose.position = + getCentroid(feature_object.feature.cluster); + autoware_perception_msgs::msg::ObjectClassification classification; + classification.label = autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; + classification.probability = 1.0f; + feature_object.object.classification.emplace_back(classification); + + objects.feature_objects.push_back(feature_object); } + objects.header = pointcloud_msg->header; } return true; diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index f58d9ac6dcc48..7e6a456561900 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -55,24 +55,15 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( stop_watch_ptr_->toc("processing_time", true); // convert ros to pcl - pcl::PointCloud::Ptr raw_pointcloud_ptr(new pcl::PointCloud); if (input_msg->data.empty()) { // NOTE: prevent pcl log spam RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); - } else { - pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr); } - - // clustering - std::vector> clusters; - if (!raw_pointcloud_ptr->empty()) { - cluster_->cluster(raw_pointcloud_ptr, clusters); - } - - // build output msg + // cluster and build output msg tier4_perception_msgs::msg::DetectedObjectsWithFeature output; - convertPointCloudClusters2Msg(input_msg->header, clusters, output); + + cluster_->cluster(input_msg, output); cluster_pub_->publish(output); // build debug msg diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index b13f68b07181e..7baed86088811 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -160,6 +160,13 @@ else() message("Skipping build of some nodes due to missing dependencies") endif() +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_utils + test/test_utils.cpp + ) +endif() + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index bb96044a245cb..c1bd85cb945de 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -36,6 +36,7 @@ tier4_autoware_utils tier4_perception_msgs + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/perception/image_projection_based_fusion/test/test_utils.cpp b/perception/image_projection_based_fusion/test/test_utils.cpp new file mode 100644 index 0000000000000..e578ce1987cc3 --- /dev/null +++ b/perception/image_projection_based_fusion/test/test_utils.cpp @@ -0,0 +1,170 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include + +using autoware_point_types::PointXYZI; + +void setPointCloud2Fields(sensor_msgs::msg::PointCloud2 & pointcloud) +{ + pointcloud.fields.resize(4); + pointcloud.fields[0].name = "x"; + pointcloud.fields[1].name = "y"; + pointcloud.fields[2].name = "z"; + pointcloud.fields[3].name = "intensity"; + pointcloud.fields[0].offset = 0; + pointcloud.fields[1].offset = 4; + pointcloud.fields[2].offset = 8; + pointcloud.fields[3].offset = 12; + pointcloud.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[0].count = 1; + pointcloud.fields[1].count = 1; + pointcloud.fields[2].count = 1; + pointcloud.fields[3].count = 1; + pointcloud.height = 1; + pointcloud.point_step = 16; + pointcloud.is_bigendian = false; + pointcloud.is_dense = true; + pointcloud.header.frame_id = "dummy_frame_id"; + pointcloud.header.stamp.sec = 0; + pointcloud.header.stamp.nanosec = 0; +} + +// Test case 1: Test case when the input pointcloud is empty +TEST(UtilsTest, closest_cluster_empty_cluster_test_case1) +{ + sensor_msgs::msg::PointCloud2 pointcloud; + setPointCloud2Fields(pointcloud); + pointcloud.data.resize(0); + pointcloud.width = 0; + pointcloud.row_step = 0; + + pcl::PointXYZ center; + center.x = 0.0; + center.y = 0.0; + center.z = 0.0; + + // testing closest_cluster function + double cluster_2d_tolerance = 0.5; + int min_cluster_size = 3; + sensor_msgs::msg::PointCloud2 output_cluster; + image_projection_based_fusion::closest_cluster( + pointcloud, cluster_2d_tolerance, min_cluster_size, center, output_cluster); + EXPECT_EQ(output_cluster.data.size(), std::size_t(0)); +} + +// Test case 2: Test case when the input pointcloud as one cluster +TEST(UtilsTest, closest_cluster_test_case2) +{ + sensor_msgs::msg::PointCloud2 pointcloud; + setPointCloud2Fields(pointcloud); + std::vector dummy_x = {0.0, 0.1, 0.2}; + pointcloud.data.resize(dummy_x.size() * pointcloud.point_step); + size_t global_offset = 0; + for (auto x : dummy_x) { + PointXYZI point; + point.x = x; + point.y = 0.0; + point.z = 0.0; + point.intensity = 0.0; + memcpy(&pointcloud.data[global_offset], &point, pointcloud.point_step); + global_offset += pointcloud.point_step; + } + pcl::PointXYZ center; + center.x = 0.0; + center.y = 0.0; + center.z = 0.0; + + // testing closest_cluster function + double cluster_2d_tolerance = 0.5; + int min_cluster_size = 3; + sensor_msgs::msg::PointCloud2 output_cluster; + image_projection_based_fusion::closest_cluster( + pointcloud, cluster_2d_tolerance, min_cluster_size, center, output_cluster); + EXPECT_EQ(output_cluster.data.size(), dummy_x.size() * pointcloud.point_step); +} + +// Test case 3: Test case when the input pointcloud as two clusters +TEST(UtilsTest, closest_cluster_test_case3) +{ + sensor_msgs::msg::PointCloud2 pointcloud; + setPointCloud2Fields(pointcloud); + std::vector dummy_x = {0.0, 0.1, 0.2, 1.0, 1.1, 1.2, 1.21, 1.22}; + pointcloud.data.resize(dummy_x.size() * pointcloud.point_step); + size_t global_offset = 0; + for (auto x : dummy_x) { + PointXYZI point; + point.x = x; + point.y = 0.0; + point.z = 0.0; + point.intensity = 0.0; + memcpy(&pointcloud.data[global_offset], &point, pointcloud.point_step); + global_offset += pointcloud.point_step; + } + pcl::PointXYZ center; + center.x = 0.0; + center.y = 0.0; + center.z = 0.0; + + // testing closest_cluster function + double cluster_2d_tolerance = 0.5; + int min_cluster_size = 3; + sensor_msgs::msg::PointCloud2 output_cluster; + image_projection_based_fusion::closest_cluster( + pointcloud, cluster_2d_tolerance, min_cluster_size, center, output_cluster); + EXPECT_EQ(output_cluster.data.size(), 3 * pointcloud.point_step); +} + +// Test case 4: Test case when the input pointcloud as two clusters +TEST(UtilsTest, closest_cluster_test_case4) +{ + sensor_msgs::msg::PointCloud2 pointcloud; + setPointCloud2Fields(pointcloud); + std::vector dummy_x = {0.0, 0.1, 1.0, 1.01, 1.1, 1.2, 1.21, 1.22}; + pointcloud.data.resize(dummy_x.size() * pointcloud.point_step); + size_t global_offset = 0; + for (auto x : dummy_x) { + PointXYZI point; + point.x = x; + point.y = 0.0; + point.z = 0.0; + point.intensity = 0.0; + memcpy(&pointcloud.data[global_offset], &point, pointcloud.point_step); + global_offset += pointcloud.point_step; + } + pcl::PointXYZ center; + center.x = 0.0; + center.y = 0.0; + center.z = 0.0; + + // testing closest_cluster function + double cluster_2d_tolerance = 0.5; + int min_cluster_size = 3; + sensor_msgs::msg::PointCloud2 output_cluster; + image_projection_based_fusion::closest_cluster( + pointcloud, cluster_2d_tolerance, min_cluster_size, center, output_cluster); + EXPECT_EQ(output_cluster.data.size(), 6 * pointcloud.point_step); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 46af1f9b9a1de..4f9268583dd34 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -22,8 +22,10 @@ include_directories( # Generate exe file set(MULTI_OBJECT_TRACKER_SRC src/multi_object_tracker_core.cpp - src/debugger.cpp + src/debugger/debugger.cpp + src/debugger/debug_object.cpp src/processor/processor.cpp + src/processor/input_manager.cpp src/data_association/data_association.cpp src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp src/tracker/motion_model/motion_model_base.cpp @@ -42,17 +44,18 @@ set(MULTI_OBJECT_TRACKER_SRC src/tracker/model/pass_through_tracker.cpp ) -ament_auto_add_library(multi_object_tracker_node SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED ${MULTI_OBJECT_TRACKER_SRC} ) -target_link_libraries(multi_object_tracker_node +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen glog::glog ) -ament_auto_add_executable(${PROJECT_NAME} - src/multi_object_tracker_node.cpp +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "multi_object_tracker::MultiObjectTracker" + EXECUTABLE ${PROJECT_NAME}_node ) ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/multi_object_tracker/README.md b/perception/multi_object_tracker/README.md index cba1df5e8ecd5..612b331c585a0 100644 --- a/perception/multi_object_tracker/README.md +++ b/perception/multi_object_tracker/README.md @@ -46,31 +46,38 @@ Example: ### Input -| Name | Type | Description | -| --------- | ------------------------------------------------ | ----------- | -| `~/input` | `autoware_perception_msgs::msg::DetectedObjects` | obstacles | +Multiple inputs are pre-defined in the input channel parameters (described below) and the inputs can be configured + +| Name | Type | Description | +| ------------------------- | -------------------------- | ---------------------- | +| `selected_input_channels` | `std::vector` | array of channel names | + +- default value: `selected_input_channels:="['detected_objects']"`, merged DetectedObject message +- multi-input example: `selected_input_channels:="['lidar_centerpoint','camera_lidar_fusion','detection_by_tracker','radar_far']"` ### Output -| Name | Type | Description | -| ---------- | ----------------------------------------------- | ------------------ | -| `~/output` | `autoware_perception_msgs::msg::TrackedObjects` | modified obstacles | +| Name | Type | Description | +| ---------- | ---------------------------------------------------- | --------------- | +| `~/output` | `autoware_perception_msgs::msg::TrackedObjects` | tracked objects | ## Parameters - +| Name | Type | Description | +| --------------------------------- | ----------------------------------------------------- | ------------------------------------- | +| `` | | the name of channel | +| `.topic` | `autoware_perception_msgs::msg::DetectedObjects` | detected objects | +| `.can_spawn_new_tracker` | `bool` | a switch allow to spawn a new tracker | +| `.optional.name` | `std::string` | channel name for analysis | +| `.optional.short_name` | `std::string` | short name for visualization | ### Core Parameters -Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_object_tracker.param.yaml) and association parameters are defined in [data_association.param.yaml](config/data_association.param.yaml). +Node parameters are defined in [multi_object_tracker_node.param.yaml](config/multi_object_tracker_node.param.yaml) and association parameters are defined in [data_association_matrix.param.yaml](config/data_association_matrix.param.yaml). #### Node parameters @@ -80,6 +87,9 @@ Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_ob | `world_frame_id` | double | object kinematics definition frame | | `enable_delay_compensation` | bool | if True, tracker use timers to schedule publishers and use prediction step to extrapolate object state at desired timestamp | | `publish_rate` | double | Timer frequency to output with delay compensation | +| `publish_processing_time` | bool | enable to publish debug message of process time information | +| `publish_tentative_objects` | bool | enable to publish tentative tracked objects, which have lower confidence | +| `publish_debug_markers` | bool | enable to publish debug markers, which indicates association of multi-inputs, existence probability of each detection | #### Association parameters @@ -93,13 +103,6 @@ Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_ob ## Assumptions / Known limits - - See the [model explanations](models.md). ## (Optional) Error detection and handling diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml new file mode 100644 index 0000000000000..b57f37675d4f1 --- /dev/null +++ b/perception/multi_object_tracker/config/input_channels.param.yaml @@ -0,0 +1,82 @@ +/**: + ros__parameters: + input_channels: + detected_objects: + topic: "/perception/object_recognition/detection/objects" + can_spawn_new_tracker: true + optional: + name: "detected_objects" + short_name: "all" + # LIDAR - rule-based + lidar_clustering: + topic: "/perception/object_recognition/detection/clustering/objects" + can_spawn_new_tracker: true + optional: + name: "clustering" + short_name: "Lcl" + # LIDAR - DNN + lidar_centerpoint: + topic: "/perception/object_recognition/detection/centerpoint/objects" + can_spawn_new_tracker: true + optional: + name: "centerpoint" + short_name: "Lcp" + lidar_centerpoint_validated: + topic: "/perception/object_recognition/detection/centerpoint/validation/objects" + can_spawn_new_tracker: true + optional: + name: "centerpoint" + short_name: "Lcp" + lidar_apollo: + topic: "/perception/object_recognition/detection/apollo/objects" + can_spawn_new_tracker: true + optional: + name: "apollo" + short_name: "Lap" + lidar_apollo_validated: + topic: "/perception/object_recognition/detection/apollo/validation/objects" + can_spawn_new_tracker: true + optional: + name: "apollo" + short_name: "Lap" + # LIDAR-CAMERA - DNN + # cspell:ignore lidar_pointpainitng pointpainting + lidar_pointpainitng: + topic: "/perception/object_recognition/detection/pointpainting/objects" + can_spawn_new_tracker: true + optional: + name: "pointpainting" + short_name: "Lpp" + lidar_pointpainting_validated: + topic: "/perception/object_recognition/detection/pointpainting/validation/objects" + can_spawn_new_tracker: true + optional: + name: "pointpainting" + short_name: "Lpp" + # CAMERA-LIDAR + camera_lidar_fusion: + topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects" + can_spawn_new_tracker: true + optional: + name: "camera_lidar_fusion" + short_name: "CLf" + # CAMERA-LIDAR+TRACKER + detection_by_tracker: + topic: "/perception/object_recognition/detection/detection_by_tracker/objects" + can_spawn_new_tracker: false + optional: + name: "detection_by_tracker" + short_name: "dbT" + # RADAR + radar: + topic: "/sensing/radar/detected_objects" + can_spawn_new_tracker: true + optional: + name: "radar" + short_name: "R" + radar_far: + topic: "/perception/object_recognition/detection/radar/far_objects" + can_spawn_new_tracker: true + optional: + name: "radar_far" + short_name: "Rf" diff --git a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml index ca320c7f58442..09621a40c499f 100644 --- a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml +++ b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml @@ -17,5 +17,6 @@ # debug parameters publish_processing_time: false publish_tentative_objects: false + publish_debug_markers: false diagnostics_warn_delay: 0.5 # [sec] diagnostics_error_delay: 1.0 # [sec] diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp new file mode 100644 index 0000000000000..c685024d6ef8b --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -0,0 +1,102 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ +#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ + +#include "multi_object_tracker/tracker/model/tracker_base.hpp" + +#include +#include + +#include "unique_identifier_msgs/msg/uuid.hpp" +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +struct ObjectData +{ + rclcpp::Time time; + + // object uuid + boost::uuids::uuid uuid; + int uuid_int; + std::string uuid_str; + + // association link, pair of coordinates + // tracker to detection + geometry_msgs::msg::Point tracker_point; + geometry_msgs::msg::Point detection_point; + bool is_associated{false}; + + // existence probabilities + std::vector existence_vector; + + // detection channel id + uint channel_id; +}; + +class TrackerObjectDebugger +{ +public: + explicit TrackerObjectDebugger(std::string frame_id); + +private: + bool is_initialized_{false}; + std::string frame_id_; + visualization_msgs::msg::MarkerArray markers_; + std::unordered_set current_ids_; + std::unordered_set previous_ids_; + rclcpp::Time message_time_; + + std::vector object_data_list_; + std::list unused_marker_ids_; + int32_t marker_id_ = 0; + std::vector> object_data_groups_; + + std::vector channel_names_; + +public: + void setChannelNames(const std::vector & channel_names) + { + channel_names_ = channel_names; + } + void collect( + const rclcpp::Time & message_time, const std::list> & list_tracker, + const uint & channel_index, + const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const std::unordered_map & direct_assignment, + const std::unordered_map & reverse_assignment); + + void reset(); + void draw( + const std::vector> object_data_groups, + visualization_msgs::msg::MarkerArray & marker_array) const; + void process(); + void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const; +}; + +#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp similarity index 66% rename from perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp rename to perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 7382e0ea5f876..9ab5576faccd8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,11 +11,11 @@ // 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 MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ -#define MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ +#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ + +#include "multi_object_tracker/debugger/debug_object.hpp" #include #include @@ -27,7 +27,11 @@ #include #include +#include #include +#include +#include +#include /** * @brief Debugger class for multi object tracker @@ -36,40 +40,68 @@ class TrackerDebugger { public: - explicit TrackerDebugger(rclcpp::Node & node); - void publishTentativeObjects( - const autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const; - void startMeasurementTime( - const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp); - void endMeasurementTime(const rclcpp::Time & now); - void startPublishTime(const rclcpp::Time & now); - void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time); + explicit TrackerDebugger(rclcpp::Node & node, const std::string & frame_id); - void setupDiagnostics(); - void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); +private: struct DEBUG_SETTINGS { bool publish_processing_time; bool publish_tentative_objects; + bool publish_debug_markers; double diagnostics_warn_delay; double diagnostics_error_delay; } debug_settings_; - double pipeline_latency_ms_ = 0.0; - diagnostic_updater::Updater diagnostic_updater_; - bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; } -private: - void loadParameters(); - bool is_initialized_ = false; + // ROS node, publishers rclcpp::Node & node_; rclcpp::Publisher::SharedPtr debug_tentative_objects_pub_; std::unique_ptr processing_time_publisher_; + rclcpp::Publisher::SharedPtr debug_objects_markers_pub_; + + diagnostic_updater::Updater diagnostic_updater_; + // Object debugger + TrackerObjectDebugger object_debugger_; + + // Time measurement + bool is_initialized_ = false; + double pipeline_latency_ms_ = 0.0; rclcpp::Time last_input_stamp_; rclcpp::Time stamp_process_start_; rclcpp::Time stamp_process_end_; rclcpp::Time stamp_publish_start_; rclcpp::Time stamp_publish_output_; + + // Configuration + void setupDiagnostics(); + void loadParameters(); + +public: + // Object publishing + bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; } + void publishTentativeObjects( + const autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const; + + // Time measurement + void startMeasurementTime( + const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp); + void endMeasurementTime(const rclcpp::Time & now); + void startPublishTime(const rclcpp::Time & now); + void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time); + void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); + + // Debug object + void setObjectChannels(const std::vector & channels) + { + object_debugger_.setChannelNames(channels); + } + void collectObjectInfo( + const rclcpp::Time & message_time, const std::list> & list_tracker, + const uint & channel_index, + const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const std::unordered_map & direct_assignment, + const std::unordered_map & reverse_assignment); + void publishObjectsMarkers(); }; -#endif // MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ +#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 4ccad953fca62..569bc4f43a3d4 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -20,7 +20,8 @@ #define MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ #include "multi_object_tracker/data_association/data_association.hpp" -#include "multi_object_tracker/debugger.hpp" +#include "multi_object_tracker/debugger/debugger.hpp" +#include "multi_object_tracker/processor/input_manager.hpp" #include "multi_object_tracker/processor/processor.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" @@ -48,8 +49,16 @@ #include #include #include +#include #include +namespace multi_object_tracker +{ + +using DetectedObject = autoware_perception_msgs::msg::DetectedObject; +using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; +using TrackedObjects = autoware_perception_msgs::msg::TrackedObjects; + class MultiObjectTracker : public rclcpp::Node { public: @@ -57,9 +66,8 @@ class MultiObjectTracker : public rclcpp::Node private: // ROS interface - rclcpp::Publisher::SharedPtr tracked_objects_pub_; - rclcpp::Subscription::SharedPtr - detected_object_sub_; + rclcpp::Publisher::SharedPtr tracked_objects_pub_; + rclcpp::Subscription::SharedPtr detected_object_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -77,15 +85,24 @@ class MultiObjectTracker : public rclcpp::Node std::unique_ptr data_association_; std::unique_ptr processor_; + // input manager + std::unique_ptr input_manager_; + + std::vector input_channels_{}; + size_t input_channel_size_{}; + // callback functions - void onMeasurement( - const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); + void onTrigger(); + void onMessage(const ObjectsList & objects_list); // publish processes + void runProcess(const DetectedObjects & input_objects_msg, const uint & channel_index); void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; +} // namespace multi_object_tracker + #endif // MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp new file mode 100644 index 0000000000000..04651face2246 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp @@ -0,0 +1,158 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ +#define MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "autoware_perception_msgs/msg/detected_objects.hpp" + +#include +#include +#include +#include +#include +#include + +namespace multi_object_tracker +{ +using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; +using ObjectsList = std::vector>; + +struct InputChannel +{ + std::string input_topic; // topic name of the detection, e.g. "/detection/lidar" + std::string long_name = "Detected Object"; // full name of the detection + std::string short_name = "DET"; // abbreviation of the name + bool is_spawn_enabled = true; // enable spawn of the object +}; + +class InputStream +{ +public: + explicit InputStream(rclcpp::Node & node, uint & index); + + void init(const InputChannel & input_channel); + + void setQueueSize(size_t que_size) { que_size_ = que_size; } + void setTriggerFunction(std::function func_trigger) + { + func_trigger_ = func_trigger; + } + + void onMessage(const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); + void updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time); + + bool isTimeInitialized() const { return initial_count_ > 0; } + uint getIndex() const { return index_; } + void getObjectsOlderThan( + const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, + ObjectsList & objects_list); + void getNames(std::string & long_name, std::string & short_name) + { + long_name = long_name_; + short_name = short_name_; + } + bool isSpawnEnabled() const { return is_spawn_enabled_; } + + std::string getLongName() const { return long_name_; } + size_t getObjectsCount() const { return objects_que_.size(); } + void getTimeStatistics( + double & latency_mean, double & latency_var, double & interval_mean, + double & interval_var) const + { + latency_mean = latency_mean_; + latency_var = latency_var_; + interval_mean = interval_mean_; + interval_var = interval_var_; + } + void getLatencyStatistics(double & latency_mean, double & latency_var) const + { + latency_mean = latency_mean_; + latency_var = latency_var_; + } + bool getTimestamps( + rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const; + rclcpp::Time getLatestMeasurementTime() const { return latest_measurement_time_; } + +private: + rclcpp::Node & node_; + uint index_; + + std::string input_topic_; + std::string long_name_; + std::string short_name_; + bool is_spawn_enabled_; + + size_t que_size_{30}; + std::deque objects_que_; + + std::function func_trigger_; + + // bool is_time_initialized_{false}; + int initial_count_{0}; + double expected_interval_; + double latency_mean_; + double latency_var_; + double interval_mean_; + double interval_var_; + + rclcpp::Time latest_measurement_time_; + rclcpp::Time latest_message_time_; +}; + +class InputManager +{ +public: + explicit InputManager(rclcpp::Node & node); + + void init(const std::vector & input_channels); + void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } + + void onTrigger(const uint & index) const; + + void getObjectTimeInterval( + const rclcpp::Time & now, rclcpp::Time & object_latest_time, + rclcpp::Time & object_oldest_time) const; + void optimizeTimings(); + bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list); + bool isChannelSpawnEnabled(const uint & index) const + { + return input_streams_[index]->isSpawnEnabled(); + } + +private: + rclcpp::Node & node_; + std::vector::SharedPtr> sub_objects_array_{}; + + bool is_initialized_{false}; + rclcpp::Time latest_object_time_; + + size_t input_size_; + std::vector> input_streams_; + + std::function func_trigger_; + uint target_stream_idx_{0}; + double target_stream_latency_{0.2}; // [s] + double target_stream_latency_std_{0.04}; // [s] + double target_stream_interval_{0.1}; // [s] + double target_stream_interval_std_{0.02}; // [s] + double target_latency_{0.2}; // [s] + double target_latency_band_{1.0}; // [s] +}; + +} // namespace multi_object_tracker + +#endif // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp index 51cd8124219ed..b2f5f41c81f0a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,8 +11,6 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -// -// #ifndef MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ #define MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ @@ -34,7 +32,8 @@ class TrackerProcessor { public: - explicit TrackerProcessor(const std::map & tracker_map); + explicit TrackerProcessor( + const std::map & tracker_map, const size_t & channel_size); const std::list> & getListTracker() const { return list_tracker_; } // tracker processes @@ -42,11 +41,11 @@ class TrackerProcessor void update( const autoware_perception_msgs::msg::DetectedObjects & transformed_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment); + const std::unordered_map & direct_assignment, const uint & channel_index); void spawn( const autoware_perception_msgs::msg::DetectedObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment); + const std::unordered_map & reverse_assignment, const uint & channel_index); void prune(const rclcpp::Time & time); // output @@ -58,9 +57,12 @@ class TrackerProcessor const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const; + void getExistenceProbabilities(std::vector> & existence_vectors) const; + private: std::map tracker_map_; std::list> list_tracker_; + const size_t channel_size_; // parameters float max_elapsed_time_; // [s] @@ -73,7 +75,7 @@ class TrackerProcessor void removeOverlappedTracker(const rclcpp::Time & time); std::shared_ptr createNewTracker( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) const; + const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const; }; #endif // MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 7d21bc1731327..36176b8202e72 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -56,7 +56,8 @@ class BicycleTracker : public Tracker public: BicycleTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 3ba8a4b087f3f..f284196e1c816 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -61,7 +61,8 @@ class BigVehicleTracker : public Tracker public: BigVehicleTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 52f4f0020a3ec..b3d088da54721 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -35,7 +35,8 @@ class MultipleVehicleTracker : public Tracker public: MultipleVehicleTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index a221a0134c51d..4c9a5a113a340 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -61,7 +61,8 @@ class NormalVehicleTracker : public Tracker public: NormalVehicleTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp index cd380021cef5b..605b3ae462cec 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -34,7 +34,8 @@ class PassThroughTracker : public Tracker public: PassThroughTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 23e2269dd7870..ad81c504d3719 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -34,7 +34,8 @@ class PedestrianAndBicycleTracker : public Tracker public: PedestrianAndBicycleTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 4e74616ecf504..e5d718c4b15ee 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -64,7 +64,8 @@ class PedestrianTracker : public Tracker public: PedestrianTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index 9a8b62ab6af5b..56670e9b54b7e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -35,33 +35,41 @@ class Tracker { -protected: - unique_identifier_msgs::msg::UUID getUUID() const { return uuid_; } - void setClassification( - const std::vector & classification) - { - classification_ = classification; - } - void updateClassification( - const std::vector & classification); - private: unique_identifier_msgs::msg::UUID uuid_; + + // classification std::vector classification_; + + // existence states int no_measurement_count_; int total_no_measurement_count_; int total_measurement_count_; rclcpp::Time last_update_with_measurement_time_; + std::vector existence_probabilities_; + float total_existence_probability_; public: Tracker( const rclcpp::Time & time, - const std::vector & classification); + const std::vector & classification, + const size_t & channel_size); virtual ~Tracker() {} + + void initializeExistenceProbabilities( + const uint & channel_index, const float & existence_probability); + bool getExistenceProbabilityVector(std::vector & existence_vector) const + { + existence_vector = existence_probabilities_; + return existence_vector.size() > 0; + } bool updateWithMeasurement( const autoware_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform); - bool updateWithoutMeasurement(); + const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, + const uint & channel_index); + bool updateWithoutMeasurement(const rclcpp::Time & now); + + // classification std::vector getClassification() const { return classification_; @@ -70,6 +78,8 @@ class Tracker { return object_recognition_utils::getHighestProbLabel(classification_); } + + // existence states int getNoMeasurementCount() const { return no_measurement_count_; } int getTotalNoMeasurementCount() const { return total_no_measurement_count_; } int getTotalMeasurementCount() const { return total_measurement_count_; } @@ -77,13 +87,22 @@ class Tracker { return (current_time - last_update_with_measurement_time_).seconds(); } + +protected: + unique_identifier_msgs::msg::UUID getUUID() const { return uuid_; } + void setClassification( + const std::vector & classification) + { + classification_ = classification; + } + void updateClassification( + const std::vector & classification); + + // virtual functions +public: virtual geometry_msgs::msg::PoseWithCovariance getPoseWithCovariance( const rclcpp::Time & time) const; - /* - * Pure virtual function - */ - protected: virtual bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index a9d0a21ec6dd4..43a1f2fd14842 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -49,7 +49,8 @@ class UnknownTracker : public Tracker public: UnknownTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml index dcd080b851c20..b00ccd8fa623e 100644 --- a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml +++ b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml @@ -1,14 +1,16 @@ - + + - - + + + diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp new file mode 100644 index 0000000000000..f166e83e136af --- /dev/null +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -0,0 +1,389 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "multi_object_tracker/debugger/debug_object.hpp" + +#include "autoware_perception_msgs/msg/tracked_object.hpp" + +#include + +#include +#include +#include +#include + +namespace +{ +std::string uuidToString(const unique_identifier_msgs::msg::UUID & uuid_msg) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +uuid_msg.uuid[i]; + } + return ss.str(); +} + +boost::uuids::uuid uuidToBoostUuid(const unique_identifier_msgs::msg::UUID & uuid_msg) +{ + const std::string uuid_str = uuidToString(uuid_msg); + boost::uuids::string_generator gen; + boost::uuids::uuid uuid = gen(uuid_str); + return uuid; +} + +int32_t uuidToInt(const boost::uuids::uuid & uuid) +{ + return boost::uuids::hash_value(uuid); +} +} // namespace + +TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) +{ + // set frame id + frame_id_ = frame_id; + + // initialize markers + markers_.markers.clear(); + current_ids_.clear(); + previous_ids_.clear(); + message_time_ = rclcpp::Time(0, 0); +} + +void TrackerObjectDebugger::reset() +{ + // maintain previous ids + previous_ids_.clear(); + previous_ids_ = current_ids_; + current_ids_.clear(); + + // clear markers, object data list + object_data_list_.clear(); + markers_.markers.clear(); +} + +void TrackerObjectDebugger::collect( + const rclcpp::Time & message_time, const std::list> & list_tracker, + const uint & channel_index, + const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const std::unordered_map & direct_assignment, + const std::unordered_map & /*reverse_assignment*/) +{ + if (!is_initialized_) is_initialized_ = true; + + message_time_ = message_time; + + int tracker_idx = 0; + for (auto tracker_itr = list_tracker.begin(); tracker_itr != list_tracker.end(); + ++tracker_itr, ++tracker_idx) { + ObjectData object_data; + object_data.time = message_time; + object_data.channel_id = channel_index; + + autoware_perception_msgs::msg::TrackedObject tracked_object; + (*(tracker_itr))->getTrackedObject(message_time, tracked_object); + object_data.uuid = uuidToBoostUuid(tracked_object.object_id); + object_data.uuid_int = uuidToInt(object_data.uuid); + object_data.uuid_str = uuidToString(tracked_object.object_id); + + // tracker + bool is_associated = false; + geometry_msgs::msg::Point tracker_point, detection_point; + tracker_point.x = tracked_object.kinematics.pose_with_covariance.pose.position.x; + tracker_point.y = tracked_object.kinematics.pose_with_covariance.pose.position.y; + tracker_point.z = tracked_object.kinematics.pose_with_covariance.pose.position.z; + + // detection + if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { + const auto & associated_object = + detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); + detection_point.x = associated_object.kinematics.pose_with_covariance.pose.position.x; + detection_point.y = associated_object.kinematics.pose_with_covariance.pose.position.y; + detection_point.z = associated_object.kinematics.pose_with_covariance.pose.position.z; + is_associated = true; + } else { + detection_point.x = tracker_point.x; + detection_point.y = tracker_point.y; + detection_point.z = tracker_point.z; + } + + object_data.tracker_point = tracker_point; + object_data.detection_point = detection_point; + object_data.is_associated = is_associated; + + // existence probabilities + std::vector existence_vector; + (*(tracker_itr))->getExistenceProbabilityVector(existence_vector); + object_data.existence_vector = existence_vector; + + object_data_list_.push_back(object_data); + } +} + +void TrackerObjectDebugger::process() +{ + if (!is_initialized_) return; + + // update uuid_int + for (auto & object_data : object_data_list_) { + current_ids_.insert(object_data.uuid_int); + } + + // sort by uuid, collect the same uuid object_data as a group, and loop for the groups + object_data_groups_.clear(); + { + // sort by uuid + std::sort( + object_data_list_.begin(), object_data_list_.end(), + [](const ObjectData & a, const ObjectData & b) { return a.uuid < b.uuid; }); + + // collect the same uuid object_data as a group + std::vector object_data_group; + boost::uuids::uuid previous_uuid = object_data_list_.front().uuid; + for (const auto & object_data : object_data_list_) { + // if the uuid is different, push the group and clear the group + if (object_data.uuid != previous_uuid) { + // push the group + object_data_groups_.push_back(object_data_group); + object_data_group.clear(); + previous_uuid = object_data.uuid; + } + // push the object_data to the group + object_data_group.push_back(object_data); + } + // push the last group + object_data_groups_.push_back(object_data_group); + } +} + +void TrackerObjectDebugger::draw( + const std::vector> object_data_groups, + visualization_msgs::msg::MarkerArray & marker_array) const +{ + // initialize markers + marker_array.markers.clear(); + + constexpr int PALETTE_SIZE = 16; + constexpr std::array, PALETTE_SIZE> color_array = {{ + {{0.0, 0.0, 1.0}}, // Blue + {{0.0, 1.0, 0.0}}, // Green + {{1.0, 1.0, 0.0}}, // Yellow + {{1.0, 0.0, 0.0}}, // Red + {{0.0, 1.0, 1.0}}, // Cyan + {{1.0, 0.0, 1.0}}, // Magenta + {{1.0, 0.64, 0.0}}, // Orange + {{0.75, 1.0, 0.0}}, // Lime + {{0.0, 0.5, 0.5}}, // Teal + {{0.5, 0.0, 0.5}}, // Purple + {{1.0, 0.75, 0.8}}, // Pink + {{0.65, 0.17, 0.17}}, // Brown + {{0.5, 0.0, 0.0}}, // Maroon + {{0.5, 0.5, 0.0}}, // Olive + {{0.0, 0.0, 0.5}}, // Navy + {{0.5, 0.5, 0.5}} // Grey + }}; + + for (const auto & object_data_group : object_data_groups) { + if (object_data_group.empty()) continue; + const auto object_data_front = object_data_group.front(); + const auto object_data_back = object_data_group.back(); + + // set a reference marker + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id_; + marker.header.stamp = object_data_front.time; + marker.id = object_data_front.uuid_int; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; // white + marker.lifetime = rclcpp::Duration::from_seconds(0); + + // get marker - existence_probability + visualization_msgs::msg::Marker text_marker; + text_marker = marker; + text_marker.ns = "existence_probability"; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.action = visualization_msgs::msg::Marker::ADD; + text_marker.pose.position.z += 1.8; + text_marker.scale.z = 0.5; + text_marker.pose.position.x = object_data_front.tracker_point.x; + text_marker.pose.position.y = object_data_front.tracker_point.y; + text_marker.pose.position.z = object_data_front.tracker_point.z + 2.5; + + // show the last existence probability + // print existence probability with channel name + // probability to text, two digits of percentage + std::string existence_probability_text = ""; + for (size_t i = 0; i < object_data_front.existence_vector.size(); ++i) { + std::stringstream stream; + stream << std::fixed << std::setprecision(0) << object_data_front.existence_vector[i] * 100; + existence_probability_text += channel_names_[i] + stream.str() + ":"; + } + existence_probability_text = + existence_probability_text.substr(0, existence_probability_text.size() - 1); + existence_probability_text += "\n" + object_data_front.uuid_str.substr(0, 6); + + text_marker.text = existence_probability_text; + marker_array.markers.push_back(text_marker); + + // loop for each object_data in the group + // boxed to tracker positions + // and link lines to the detected positions + const double marker_height_offset = 1.0; + const double assign_height_offset = 0.6; + + visualization_msgs::msg::Marker marker_track_boxes; + marker_track_boxes = marker; + marker_track_boxes.ns = "track_boxes"; + marker_track_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker_track_boxes.action = visualization_msgs::msg::Marker::ADD; + marker_track_boxes.scale.x = 0.4; + marker_track_boxes.scale.y = 0.4; + marker_track_boxes.scale.z = 0.4; + + // make detected object markers per channel + std::vector marker_detect_boxes_per_channel; + std::vector marker_detect_lines_per_channel; + + for (size_t idx = 0; idx < channel_names_.size(); idx++) { + // get color - by channel index + std_msgs::msg::ColorRGBA color; + color.a = 1.0; + color.r = color_array[idx % PALETTE_SIZE][0]; + color.g = color_array[idx % PALETTE_SIZE][1]; + color.b = color_array[idx % PALETTE_SIZE][2]; + + visualization_msgs::msg::Marker marker_detect_boxes; + marker_detect_boxes = marker; + marker_detect_boxes.ns = "detect_boxes_" + channel_names_[idx]; + marker_detect_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker_detect_boxes.action = visualization_msgs::msg::Marker::ADD; + marker_detect_boxes.scale.x = 0.2; + marker_detect_boxes.scale.y = 0.2; + marker_detect_boxes.scale.z = 0.2; + marker_detect_boxes.color = color; + marker_detect_boxes_per_channel.push_back(marker_detect_boxes); + + visualization_msgs::msg::Marker marker_lines; + marker_lines = marker; + marker_lines.ns = "association_lines_" + channel_names_[idx]; + marker_lines.type = visualization_msgs::msg::Marker::LINE_LIST; + marker_lines.action = visualization_msgs::msg::Marker::ADD; + marker_lines.scale.x = 0.15; + marker_lines.points.clear(); + marker_lines.color = color; + marker_detect_lines_per_channel.push_back(marker_lines); + } + + bool is_any_associated = false; + + for (const auto & object_data : object_data_group) { + int channel_id = object_data.channel_id; + + // set box + geometry_msgs::msg::Point box_point; + box_point.x = object_data.tracker_point.x; + box_point.y = object_data.tracker_point.y; + box_point.z = object_data.tracker_point.z + marker_height_offset; + marker_track_boxes.points.push_back(box_point); + + // set association marker, if exists + if (!object_data.is_associated) continue; + is_any_associated = true; + + // associated object box + visualization_msgs::msg::Marker & marker_detect_boxes = + marker_detect_boxes_per_channel.at(channel_id); + box_point.x = object_data.detection_point.x; + box_point.y = object_data.detection_point.y; + box_point.z = object_data.detection_point.z + marker_height_offset + assign_height_offset; + marker_detect_boxes.points.push_back(box_point); + + // association line + visualization_msgs::msg::Marker & marker_lines = + marker_detect_lines_per_channel.at(channel_id); + geometry_msgs::msg::Point line_point; + line_point.x = object_data.tracker_point.x; + line_point.y = object_data.tracker_point.y; + line_point.z = object_data.tracker_point.z + marker_height_offset; + marker_lines.points.push_back(line_point); + line_point.x = object_data.detection_point.x; + line_point.y = object_data.detection_point.y; + line_point.z = object_data.detection_point.z + marker_height_offset + assign_height_offset; + marker_lines.points.push_back(line_point); + } + + // add markers + marker_array.markers.push_back(marker_track_boxes); + if (is_any_associated) { + for (size_t i = 0; i < channel_names_.size(); i++) { + if (marker_detect_boxes_per_channel.at(i).points.empty()) continue; + marker_array.markers.push_back(marker_detect_boxes_per_channel.at(i)); + } + for (size_t i = 0; i < channel_names_.size(); i++) { + if (marker_detect_lines_per_channel.at(i).points.empty()) continue; + marker_array.markers.push_back(marker_detect_lines_per_channel.at(i)); + } + } else { + for (size_t i = 0; i < channel_names_.size(); i++) { + marker_detect_boxes_per_channel.at(i).action = visualization_msgs::msg::Marker::DELETE; + marker_array.markers.push_back(marker_detect_boxes_per_channel.at(i)); + } + for (size_t i = 0; i < channel_names_.size(); i++) { + marker_detect_lines_per_channel.at(i).action = visualization_msgs::msg::Marker::DELETE; + marker_array.markers.push_back(marker_detect_lines_per_channel.at(i)); + } + } + } + + return; +} + +void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array) const +{ + if (!is_initialized_) return; + + // draw markers + draw(object_data_groups_, marker_array); + + // remove old markers + for (const auto & previous_id : previous_ids_) { + if (current_ids_.find(previous_id) != current_ids_.end()) { + continue; + } + + visualization_msgs::msg::Marker delete_marker; + delete_marker.header.frame_id = frame_id_; + delete_marker.header.stamp = message_time_; + delete_marker.ns = "existence_probability"; + delete_marker.id = previous_id; + delete_marker.action = visualization_msgs::msg::Marker::DELETE; + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "track_boxes"; + marker_array.markers.push_back(delete_marker); + + for (size_t idx = 0; idx < channel_names_.size(); idx++) { + delete_marker.ns = "detect_boxes_" + channel_names_[idx]; + marker_array.markers.push_back(delete_marker); + delete_marker.ns = "association_lines_" + channel_names_[idx]; + marker_array.markers.push_back(delete_marker); + } + } + + return; +} diff --git a/perception/multi_object_tracker/src/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp similarity index 79% rename from perception/multi_object_tracker/src/debugger.cpp rename to perception/multi_object_tracker/src/debugger/debugger.cpp index 2abeae44050ea..7b4f9d717a927 100644 --- a/perception/multi_object_tracker/src/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,14 +11,13 @@ // 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 "multi_object_tracker/debugger.hpp" +#include "multi_object_tracker/debugger/debugger.hpp" #include -TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&node), node_(node) +TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_id) +: node_(node), diagnostic_updater_(&node), object_debugger_(frame_id) { // declare debug parameters to decide whether to publish debug topics loadParameters(); @@ -31,7 +30,12 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&nod if (debug_settings_.publish_tentative_objects) { debug_tentative_objects_pub_ = node_.create_publisher( - "debug/tentative_objects", rclcpp::QoS{1}); + "~/debug/tentative_objects", rclcpp::QoS{1}); + } + + if (debug_settings_.publish_debug_markers) { + debug_objects_markers_pub_ = node_.create_publisher( + "~/debug/objects_markers", rclcpp::QoS{1}); } // initialize timestamps @@ -46,14 +50,6 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&nod setupDiagnostics(); } -void TrackerDebugger::setupDiagnostics() -{ - diagnostic_updater_.setHardwareID(node_.get_name()); - diagnostic_updater_.add( - "Perception delay check from original header stamp", this, &TrackerDebugger::checkDelay); - diagnostic_updater_.setPeriod(0.1); -} - void TrackerDebugger::loadParameters() { try { @@ -61,6 +57,7 @@ void TrackerDebugger::loadParameters() node_.declare_parameter("publish_processing_time"); debug_settings_.publish_tentative_objects = node_.declare_parameter("publish_tentative_objects"); + debug_settings_.publish_debug_markers = node_.declare_parameter("publish_debug_markers"); debug_settings_.diagnostics_warn_delay = node_.declare_parameter("diagnostics_warn_delay"); debug_settings_.diagnostics_error_delay = @@ -69,11 +66,32 @@ void TrackerDebugger::loadParameters() RCLCPP_WARN(node_.get_logger(), "Failed to declare parameter: %s", e.what()); debug_settings_.publish_processing_time = false; debug_settings_.publish_tentative_objects = false; + debug_settings_.publish_debug_markers = false; debug_settings_.diagnostics_warn_delay = 0.5; debug_settings_.diagnostics_error_delay = 1.0; } } +void TrackerDebugger::setupDiagnostics() +{ + diagnostic_updater_.setHardwareID(node_.get_name()); + diagnostic_updater_.add( + "Perception delay check from original header stamp", this, &TrackerDebugger::checkDelay); + diagnostic_updater_.setPeriod(0.1); +} + +// Object publishing functions + +void TrackerDebugger::publishTentativeObjects( + const autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const +{ + if (debug_settings_.publish_tentative_objects) { + debug_tentative_objects_pub_->publish(tentative_objects); + } +} + +// Time measurement functions + void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat) { if (!is_initialized_) { @@ -97,14 +115,6 @@ void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & s stat.add("Detection delay", delay); } -void TrackerDebugger::publishTentativeObjects( - const autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const -{ - if (debug_settings_.publish_tentative_objects) { - debug_tentative_objects_pub_->publish(tentative_objects); - } -} - void TrackerDebugger::startMeasurementTime( const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp) { @@ -167,3 +177,33 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim } stamp_publish_output_ = now; } + +void TrackerDebugger::collectObjectInfo( + const rclcpp::Time & message_time, const std::list> & list_tracker, + const uint & channel_index, + const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const std::unordered_map & direct_assignment, + const std::unordered_map & reverse_assignment) +{ + if (!debug_settings_.publish_debug_markers) return; + object_debugger_.collect( + message_time, list_tracker, channel_index, detected_objects, direct_assignment, + reverse_assignment); +} + +// ObjectDebugger +void TrackerDebugger::publishObjectsMarkers() +{ + if (!debug_settings_.publish_debug_markers) return; + + visualization_msgs::msg::MarkerArray marker_message; + // process data + object_debugger_.process(); + + // publish markers + object_debugger_.getMessage(marker_message); + debug_objects_markers_pub_->publish(marker_message); + + // reset object data + object_debugger_.reset(); +} diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 66c426c1950eb..70433f16125c9 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -36,8 +36,6 @@ #include #include -using Label = autoware_perception_msgs::msg::ObjectClassification; - namespace { // Function to get the transform between two frames @@ -67,24 +65,85 @@ boost::optional getTransformAnonymous( } // namespace +namespace multi_object_tracker +{ +using Label = autoware_perception_msgs::msg::ObjectClassification; + MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("multi_object_tracker", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_), last_published_time_(this->now()) { - // Create publishers and subscribers - detected_object_sub_ = create_subscription( - "input", rclcpp::QoS{1}, - std::bind(&MultiObjectTracker::onMeasurement, this, std::placeholders::_1)); - tracked_objects_pub_ = - create_publisher("output", rclcpp::QoS{1}); + // glog for debug + if (!google::IsGoogleLoggingInitialized()) { + google::InitGoogleLogging("multi_object_tracker"); + google::InstallFailureSignalHandler(); + } // Get parameters double publish_rate = declare_parameter("publish_rate"); // [hz] world_frame_id_ = declare_parameter("world_frame_id"); bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; + declare_parameter("selected_input_channels", std::vector()); + std::vector selected_input_channels = + get_parameter("selected_input_channels").as_string_array(); + + // ROS interface - Publisher + tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); + + // ROS interface - Input channels + // Get input channels + std::vector input_topic_names; + std::vector input_names_long; + std::vector input_names_short; + std::vector input_is_spawn_enabled; + + if (selected_input_channels.empty()) { + RCLCPP_ERROR(this->get_logger(), "No input topics are specified."); + return; + } + + for (const auto & selected_input_channel : selected_input_channels) { + // required parameters, no default value + const std::string input_topic_name = + declare_parameter("input_channels." + selected_input_channel + ".topic"); + input_topic_names.push_back(input_topic_name); + + // required parameter, but can set a default value + const bool spawn_enabled = declare_parameter( + "input_channels." + selected_input_channel + ".can_spawn_new_tracker", true); + input_is_spawn_enabled.push_back(spawn_enabled); + + // optional parameters + const std::string default_name = selected_input_channel; + const std::string name_long = declare_parameter( + "input_channels." + selected_input_channel + ".optional.name", default_name); + input_names_long.push_back(name_long); + + const std::string default_name_short = selected_input_channel.substr(0, 3); + const std::string name_short = declare_parameter( + "input_channels." + selected_input_channel + ".optional.short_name", default_name_short); + input_names_short.push_back(name_short); + } + + input_channel_size_ = input_topic_names.size(); + input_channels_.resize(input_channel_size_); + + for (size_t i = 0; i < input_channel_size_; i++) { + input_channels_[i].input_topic = input_topic_names[i]; + input_channels_[i].long_name = input_names_long[i]; + input_channels_[i].short_name = input_names_short[i]; + input_channels_[i].is_spawn_enabled = input_is_spawn_enabled[i]; + } + + // Initialize input manager + input_manager_ = std::make_unique(*this); + input_manager_->init(input_channels_); // Initialize input manager, set subscriptions + input_manager_->setTriggerFunction( + std::bind(&MultiObjectTracker::onTrigger, this)); // Set trigger function + // Create tf timer auto cti = std::make_shared( this->get_node_base_interface(), this->get_node_timers_interface()); @@ -119,7 +178,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) tracker_map.insert(std::make_pair( Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); - processor_ = std::make_unique(tracker_map); + processor_ = std::make_unique(tracker_map, input_channel_size_); } // Data association initialization @@ -138,35 +197,96 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) } // Debugger - debugger_ = std::make_unique(*this); + debugger_ = std::make_unique(*this, world_frame_id_); + debugger_->setObjectChannels(input_names_short); published_time_publisher_ = std::make_unique(this); } -void MultiObjectTracker::onMeasurement( - const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) +void MultiObjectTracker::onTrigger() +{ + const rclcpp::Time current_time = this->now(); + // get objects from the input manager and run process + std::vector> objects_list; + const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); + if (!is_objects_ready) return; + + onMessage(objects_list); + const rclcpp::Time latest_time(objects_list.back().second.header.stamp); + + // Publish objects if the timer is not enabled + if (publish_timer_ == nullptr) { + // if the delay compensation is disabled, publish the objects in the latest time + publish(latest_time); + } else { + // Publish if the next publish time is close + const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period + if ((current_time - last_published_time_).seconds() > minimum_publish_interval) { + checkAndPublish(current_time); + } + } +} + +void MultiObjectTracker::onTimer() +{ + const rclcpp::Time current_time = this->now(); + + // Check the publish period + const auto elapsed_time = (current_time - last_published_time_).seconds(); + // If the elapsed time is over the period, publish objects with prediction + constexpr double maximum_latency_ratio = 1.11; // 11% margin + const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio; + if (elapsed_time < maximum_publish_latency) return; + + // get objects from the input manager and run process + ObjectsList objects_list; + const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); + if (is_objects_ready) { + onMessage(objects_list); + } + + // Publish + checkAndPublish(current_time); +} + +void MultiObjectTracker::onMessage(const ObjectsList & objects_list) +{ + const rclcpp::Time current_time = this->now(); + const rclcpp::Time oldest_time(objects_list.front().second.header.stamp); + + // process start + debugger_->startMeasurementTime(this->now(), oldest_time); + // run process for each DetectedObjects + for (const auto & objects_data : objects_list) { + runProcess(objects_data.second, objects_data.first); + } + // process end + debugger_->endMeasurementTime(this->now()); +} + +void MultiObjectTracker::runProcess( + const DetectedObjects & input_objects_msg, const uint & channel_index) { // Get the time of the measurement const rclcpp::Time measurement_time = - rclcpp::Time(input_objects_msg->header.stamp, this->now().get_clock_type()); + rclcpp::Time(input_objects_msg.header.stamp, this->now().get_clock_type()); - /* keep the latest input stamp and check transform*/ - debugger_->startMeasurementTime(this->now(), measurement_time); + // Get the transform of the self frame const auto self_transform = getTransformAnonymous(tf_buffer_, "base_link", world_frame_id_, measurement_time); if (!self_transform) { return; } - /* transform to world coordinate */ - autoware_perception_msgs::msg::DetectedObjects transformed_objects; + + // Transform the objects to the world frame + DetectedObjects transformed_objects; if (!object_recognition_utils::transformObjects( - *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { + input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { return; } - ////// Tracker Process - //// Associate and update /* prediction */ processor_->predict(measurement_time); + /* object association */ std::unordered_map direct_assignment, reverse_assignment; { @@ -176,40 +296,22 @@ void MultiObjectTracker::onMeasurement( Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix( detected_objects, list_tracker); // row : tracker, col : measurement data_association_->assign(score_matrix, direct_assignment, reverse_assignment); + + // Collect debug information - tracker list, existence probabilities, association results + debugger_->collectObjectInfo( + measurement_time, processor_->getListTracker(), channel_index, transformed_objects, + direct_assignment, reverse_assignment); } + /* tracker update */ - processor_->update(transformed_objects, *self_transform, direct_assignment); + processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index); + /* tracker pruning */ processor_->prune(measurement_time); - /* spawn new tracker */ - processor_->spawn(transformed_objects, *self_transform, reverse_assignment); - - // debugger time - debugger_->endMeasurementTime(this->now()); - - // Publish objects if the timer is not enabled - if (publish_timer_ == nullptr) { - // Publish if the delay compensation is disabled - publish(measurement_time); - } else { - // Publish if the next publish time is close - const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period - if ((this->now() - last_published_time_).seconds() > minimum_publish_interval) { - checkAndPublish(this->now()); - } - } -} -void MultiObjectTracker::onTimer() -{ - const rclcpp::Time current_time = this->now(); - // Check the publish period - const auto elapsed_time = (current_time - last_published_time_).seconds(); - // If the elapsed time is over the period, publish objects with prediction - constexpr double maximum_latency_ratio = 1.11; // 11% margin - const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio; - if (elapsed_time > maximum_publish_latency) { - checkAndPublish(current_time); + /* spawn new tracker */ + if (input_manager_->isChannelSpawnEnabled(channel_index)) { + processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); } } @@ -234,7 +336,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const return; } // Create output msg - autoware_perception_msgs::msg::TrackedObjects output_msg, tentative_objects_msg; + TrackedObjects output_msg, tentative_objects_msg; output_msg.header.frame_id = world_frame_id_; processor_->getTrackedObjects(time, output_msg); @@ -246,11 +348,14 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const debugger_->endPublishTime(this->now(), time); if (debugger_->shouldPublishTentativeObjects()) { - autoware_perception_msgs::msg::TrackedObjects tentative_objects_msg; + TrackedObjects tentative_objects_msg; tentative_objects_msg.header.frame_id = world_frame_id_; processor_->getTentativeObjects(time, tentative_objects_msg); debugger_->publishTentativeObjects(tentative_objects_msg); } + debugger_->publishObjectsMarkers(); } -RCLCPP_COMPONENTS_REGISTER_NODE(MultiObjectTracker) +} // namespace multi_object_tracker + +RCLCPP_COMPONENTS_REGISTER_NODE(multi_object_tracker::MultiObjectTracker) diff --git a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp deleted file mode 100644 index c6eed297c02c4..0000000000000 --- a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2024 TIER IV, inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "multi_object_tracker/multi_object_tracker_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); // NOLINT - google::InstallFailureSignalHandler(); - } - - rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - auto multi_object_tracker = std::make_shared(options); - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(multi_object_tracker); - exec.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp new file mode 100644 index 0000000000000..38d7de685f7de --- /dev/null +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -0,0 +1,347 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "multi_object_tracker/processor/input_manager.hpp" + +#include + +namespace multi_object_tracker +{ +/////////////////////////// +/////// InputStream /////// +/////////////////////////// +InputStream::InputStream(rclcpp::Node & node, uint & index) : node_(node), index_(index) +{ +} + +void InputStream::init(const InputChannel & input_channel) +{ + // Initialize parameters + input_topic_ = input_channel.input_topic; + long_name_ = input_channel.long_name; + short_name_ = input_channel.short_name; + is_spawn_enabled_ = input_channel.is_spawn_enabled; + + // Initialize queue + objects_que_.clear(); + + // Initialize latency statistics + latency_mean_ = 0.2; // [s] (initial value) + latency_var_ = 0.0; + interval_mean_ = 0.0; // [s] (initial value) + interval_var_ = 0.0; + + latest_measurement_time_ = node_.now(); + latest_message_time_ = node_.now(); +} + +bool InputStream::getTimestamps( + rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const +{ + if (!isTimeInitialized()) { + return false; + } + latest_measurement_time = latest_measurement_time_; + latest_message_time = latest_message_time_; + return true; +} + +void InputStream::onMessage( + const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) +{ + const DetectedObjects objects = *msg; + objects_que_.push_back(objects); + if (objects_que_.size() > que_size_) { + objects_que_.pop_front(); + } + + // update the timing statistics + rclcpp::Time now = node_.now(); + rclcpp::Time objects_time(objects.header.stamp); + updateTimingStatus(now, objects_time); + + // trigger the function if it is set + if (func_trigger_) { + func_trigger_(index_); + } +} + +void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time) +{ + // Define constants + constexpr int SKIP_COUNT = 4; // Skip the initial messages + constexpr int INITIALIZATION_COUNT = 16; // Initialization process count + + // Update latency statistics + // skip initial messages for the latency statistics + if (initial_count_ > SKIP_COUNT) { + const double latency = (now - objects_time).seconds(); + if (initial_count_ < INITIALIZATION_COUNT) { + // set higher gain for the initial messages + constexpr double initial_gain = 0.5; + latency_mean_ = (1.0 - initial_gain) * latency_mean_ + initial_gain * latency; + } else { + constexpr double gain = 0.05; + latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency; + const double latency_delta = latency - latency_mean_; + latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta; + } + } + + // Calculate interval, Update interval statistics + if (initial_count_ > SKIP_COUNT) { + const double interval = (now - latest_message_time_).seconds(); + if (interval < 0.0) { + RCLCPP_WARN( + node_.get_logger(), + "InputManager::updateTimingStatus %s: Negative interval detected, now: %f, " + "latest_message_time_: %f", + long_name_.c_str(), now.seconds(), latest_message_time_.seconds()); + } else if (initial_count_ < INITIALIZATION_COUNT) { + // Initialization + constexpr double initial_gain = 0.5; + interval_mean_ = (1.0 - initial_gain) * interval_mean_ + initial_gain * interval; + } else { + // The interval is considered regular if it is within 0.5 and 1.5 times the mean interval + bool update_statistics = interval > 0.5 * interval_mean_ && interval < 1.5 * interval_mean_; + if (update_statistics) { + constexpr double gain = 0.05; + interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval; + const double interval_delta = interval - interval_mean_; + interval_var_ = (1.0 - gain) * interval_var_ + gain * interval_delta * interval_delta; + } + } + } + + // Update time + latest_message_time_ = now; + constexpr double delay_threshold = 3.0; // [s] + if (std::abs((latest_measurement_time_ - objects_time).seconds()) > delay_threshold) { + // Reset the latest measurement time if the time difference is too large + latest_measurement_time_ = objects_time; + RCLCPP_WARN( + node_.get_logger(), + "InputManager::updateTimingStatus %s: Resetting the latest measurement time to %f", + long_name_.c_str(), objects_time.seconds()); + } else { + // Aware reversed message arrival + latest_measurement_time_ = + latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_; + } + + // Update the initial count + if (initial_count_ < INITIALIZATION_COUNT) { + initial_count_++; + } +} + +void InputStream::getObjectsOlderThan( + const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, + ObjectsList & objects_list) +{ + assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds()); + + for (const auto & object : objects_que_) { + const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); + + // remove objects older than the specified duration + if (object_time < object_oldest_time) { + objects_que_.pop_front(); + continue; + } + + // Add the object if the object is older than the specified latest time + if (object_latest_time >= object_time) { + std::pair object_pair(index_, object); + objects_list.push_back(object_pair); + // remove the object from the queue + objects_que_.pop_front(); + } + } +} + +//////////////////////////// +/////// InputManager /////// +//////////////////////////// +InputManager::InputManager(rclcpp::Node & node) : node_(node) +{ + latest_object_time_ = node_.now(); +} + +void InputManager::init(const std::vector & input_channels) +{ + // Check input sizes + input_size_ = input_channels.size(); + if (input_size_ == 0) { + RCLCPP_ERROR(node_.get_logger(), "InputManager::init No input streams"); + return; + } + + // Initialize input streams + sub_objects_array_.resize(input_size_); + bool is_any_spawn_enabled = false; + for (size_t i = 0; i < input_size_; i++) { + uint index(i); + InputStream input_stream(node_, index); + input_stream.init(input_channels[i]); + input_stream.setTriggerFunction( + std::bind(&InputManager::onTrigger, this, std::placeholders::_1)); + input_streams_.push_back(std::make_shared(input_stream)); + is_any_spawn_enabled |= input_streams_.at(i)->isSpawnEnabled(); + + // Set subscription + RCLCPP_INFO( + node_.get_logger(), "InputManager::init Initializing %s input stream from %s", + input_channels[i].long_name.c_str(), input_channels[i].input_topic.c_str()); + std::function func = + std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); + sub_objects_array_.at(i) = node_.create_subscription( + input_channels[i].input_topic, rclcpp::QoS{1}, func); + } + + // Check if any spawn enabled input streams + if (!is_any_spawn_enabled) { + RCLCPP_ERROR(node_.get_logger(), "InputManager::init No spawn enabled input streams"); + return; + } + is_initialized_ = true; +} + +void InputManager::onTrigger(const uint & index) const +{ + // when the target stream triggers, call the trigger function + if (index == target_stream_idx_ && func_trigger_) { + func_trigger_(); + } +} + +void InputManager::getObjectTimeInterval( + const rclcpp::Time & now, rclcpp::Time & object_latest_time, + rclcpp::Time & object_oldest_time) const +{ + object_latest_time = + now - rclcpp::Duration::from_seconds( + target_stream_latency_ - + 0.1 * target_stream_latency_std_); // object_latest_time with 0.1 sigma safety margin + // check the target stream can be included in the object time interval + if (input_streams_.at(target_stream_idx_)->isTimeInitialized()) { + rclcpp::Time latest_measurement_time = + input_streams_.at(target_stream_idx_)->getLatestMeasurementTime(); + + // if the object_latest_time is older than the latest measurement time, set it as the latest + // object time + object_latest_time = + object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time; + } + + object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0); + // if the object_oldest_time is older than the latest object time, set it to the latest object + // time + object_oldest_time = + object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_; +} + +void InputManager::optimizeTimings() +{ + double max_latency_mean = 0.0; + uint selected_stream_idx = 0; + double selected_stream_latency_std = 0.1; + double selected_stream_interval = 0.1; + double selected_stream_interval_std = 0.01; + + { + // ANALYSIS: Get the streams statistics + // select the stream that has the maximum latency + double latency_mean, latency_var, interval_mean, interval_var; + for (const auto & input_stream : input_streams_) { + if (!input_stream->isTimeInitialized()) continue; + input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var); + if (latency_mean > max_latency_mean) { + max_latency_mean = latency_mean; + selected_stream_idx = input_stream->getIndex(); + selected_stream_latency_std = std::sqrt(latency_var); + selected_stream_interval = interval_mean; + selected_stream_interval_std = std::sqrt(interval_var); + } + } + } + + // Set the target stream index, which has the maximum latency + // trigger will be called next time + // if no stream is initialized, the target stream index will be 0 and wait for the initialization + target_stream_idx_ = selected_stream_idx; + target_stream_latency_ = max_latency_mean; + target_stream_latency_std_ = selected_stream_latency_std; + target_stream_interval_ = selected_stream_interval; + target_stream_interval_std_ = selected_stream_interval_std; +} + +bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_list) +{ + if (!is_initialized_) { + RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized"); + return false; + } + + // Clear the objects + objects_list.clear(); + + // Get the time interval for the objects + rclcpp::Time object_latest_time, object_oldest_time; + getObjectTimeInterval(now, object_latest_time, object_oldest_time); + + // Optimize the target stream, latency, and its band + // The result will be used for the next time, so the optimization is after getting the time + // interval + optimizeTimings(); + + // Get objects from all input streams + // adds up to the objects vector for efficient processing + for (const auto & input_stream : input_streams_) { + input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_list); + } + + // Sort objects by timestamp + std::sort( + objects_list.begin(), objects_list.end(), + [](const std::pair & a, const std::pair & b) { + return (rclcpp::Time(a.second.header.stamp) - rclcpp::Time(b.second.header.stamp)).seconds() < + 0; + }); + + // Update the latest object time + bool is_any_object = !objects_list.empty(); + if (is_any_object) { + latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); + } else { + // check time jump + if (now < latest_object_time_) { + RCLCPP_WARN( + node_.get_logger(), + "InputManager::getObjects Time jump detected, now: %f, latest_object_time_: %f", + now.seconds(), latest_object_time_.seconds()); + latest_object_time_ = + now - rclcpp::Duration::from_seconds(3.0); // reset the latest object time to 3 seconds ago + } else { + RCLCPP_DEBUG( + node_.get_logger(), + "InputManager::getObjects No objects in the object list, object time band from %f to %f", + (now - object_oldest_time).seconds(), (now - object_latest_time).seconds()); + } + } + + return is_any_object; +} + +} // namespace multi_object_tracker diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp index dab4625a236e5..ff14f7849b161 100644 --- a/perception/multi_object_tracker/src/processor/processor.cpp +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,8 +11,6 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -// -// #include "multi_object_tracker/processor/processor.hpp" @@ -25,8 +23,9 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; -TrackerProcessor::TrackerProcessor(const std::map & tracker_map) -: tracker_map_(tracker_map) +TrackerProcessor::TrackerProcessor( + const std::map & tracker_map, const size_t & channel_size) +: tracker_map_(tracker_map), channel_size_(channel_size) { // Set tracker lifetime parameters max_elapsed_time_ = 1.0; // [s] @@ -50,7 +49,7 @@ void TrackerProcessor::predict(const rclcpp::Time & time) void TrackerProcessor::update( const autoware_perception_msgs::msg::DetectedObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment) + const std::unordered_map & direct_assignment, const uint & channel_index) { int tracker_idx = 0; const auto & time = detected_objects.header.stamp; @@ -59,9 +58,10 @@ void TrackerProcessor::update( if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found const auto & associated_object = detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); - (*(tracker_itr))->updateWithMeasurement(associated_object, time, self_transform); + (*(tracker_itr)) + ->updateWithMeasurement(associated_object, time, self_transform, channel_index); } else { // not found - (*(tracker_itr))->updateWithoutMeasurement(); + (*(tracker_itr))->updateWithoutMeasurement(time); } } } @@ -69,7 +69,7 @@ void TrackerProcessor::update( void TrackerProcessor::spawn( const autoware_perception_msgs::msg::DetectedObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment) + const std::unordered_map & reverse_assignment, const uint & channel_index) { const auto & time = detected_objects.header.stamp; for (size_t i = 0; i < detected_objects.objects.size(); ++i) { @@ -77,34 +77,43 @@ void TrackerProcessor::spawn( continue; } const auto & new_object = detected_objects.objects.at(i); - std::shared_ptr tracker = createNewTracker(new_object, time, self_transform); + std::shared_ptr tracker = + createNewTracker(new_object, time, self_transform, channel_index); if (tracker) list_tracker_.push_back(tracker); } } std::shared_ptr TrackerProcessor::createNewTracker( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) const + const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const { const std::uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); if (tracker_map_.count(label) != 0) { const auto tracker = tracker_map_.at(label); if (tracker == "bicycle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "big_vehicle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "multi_vehicle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "normal_vehicle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "pass_through_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "pedestrian_and_bicycle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "pedestrian_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); } - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); } void TrackerProcessor::prune(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 321ca34d98559..d8a35bcd7d319 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -44,13 +44,17 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("BicycleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty double r_stddev_x = 0.5; // in vehicle coordinate [m] diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index b08f1bf4fe428..115ad2c3b3b6d 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -44,14 +44,18 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("BigVehicleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty float r_stddev_x = 0.5; // in vehicle coordinate [m] diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index 02004a9cc7639..67445e5b0daa2 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp @@ -22,11 +22,14 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; MultipleVehicleTracker::MultipleVehicleTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) -: Tracker(time, object.classification), - normal_vehicle_tracker_(time, object, self_transform), - big_vehicle_tracker_(time, object, self_transform) + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), + normal_vehicle_tracker_(time, object, self_transform, channel_size, channel_index), + big_vehicle_tracker_(time, object, self_transform, channel_size, channel_index) { + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); } bool MultipleVehicleTracker::predict(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index bd421ffd2143f..dd7b3e4f0b75e 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -44,14 +44,18 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("NormalVehicleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty float r_stddev_x = 0.5; // in vehicle coordinate [m] diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index 4b646e5bba9a3..3e4f23cd440bc 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -37,13 +37,17 @@ PassThroughTracker::PassThroughTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PassThroughTracker")), last_update_time_(time) { object_ = object; prev_observed_object_ = object; + + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); } bool PassThroughTracker::predict(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index 2b1834fea37d9..00609934990a3 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -22,11 +22,14 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) -: Tracker(time, object.classification), - pedestrian_tracker_(time, object, self_transform), - bicycle_tracker_(time, object, self_transform) + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), + pedestrian_tracker_(time, object, self_transform, channel_size, channel_index), + bicycle_tracker_(time, object, self_transform, channel_size, channel_index) { + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); } bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 9cd915aea5378..558fbfa40bc58 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -44,13 +44,17 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PedestrianTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // Initialize parameters // measurement noise covariance float r_stddev_x = 0.4; // [m] diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index f64f227a14f66..6d2787ac1d550 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -21,9 +21,18 @@ #include #include +namespace +{ +float updateProbability(float prior, float true_positive, float false_positive) +{ + return (prior * true_positive) / (prior * true_positive + (1 - prior) * false_positive); +} +} // namespace + Tracker::Tracker( const rclcpp::Time & time, - const std::vector & classification) + const std::vector & classification, + const size_t & channel_size) : classification_(classification), no_measurement_count_(0), total_no_measurement_count_(0), @@ -34,23 +43,81 @@ Tracker::Tracker( std::mt19937 gen(std::random_device{}()); std::independent_bits_engine bit_eng(gen); std::generate(uuid_.uuid.begin(), uuid_.uuid.end(), bit_eng); + + // Initialize existence probabilities + existence_probabilities_.resize(channel_size, 0.0); +} + +void Tracker::initializeExistenceProbabilities( + const uint & channel_index, const float & existence_probability) +{ + // The initial existence probability is modeled + // since the incoming object's existence probability is not reliable + // existence probability on each channel + constexpr float initial_existence_probability = 0.1; + existence_probabilities_[channel_index] = initial_existence_probability; + + // total existence probability + total_existence_probability_ = existence_probability; } bool Tracker::updateWithMeasurement( const autoware_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform) + const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, + const uint & channel_index) { - no_measurement_count_ = 0; - ++total_measurement_count_; + // Update existence probability + { + no_measurement_count_ = 0; + ++total_measurement_count_; + + // existence probability on each channel + const double delta_time = (measurement_time - last_update_with_measurement_time_).seconds(); + const double decay_rate = -log(0.5) / 0.3; // 50% decay in 0.3s + constexpr float probability_true_detection = 0.9; + constexpr float probability_false_detection = 0.2; + + // update measured channel probability + existence_probabilities_[channel_index] = updateProbability( + existence_probabilities_[channel_index], probability_true_detection, + probability_false_detection); + // decay other channel probabilities + for (size_t i = 0; i < existence_probabilities_.size(); ++i) { + if (i == channel_index) { + continue; + } + existence_probabilities_[i] *= std::exp(decay_rate * delta_time); + } + + // update total existence probability + const float & existence_probability_from_object = object.existence_probability; + total_existence_probability_ = updateProbability( + total_existence_probability_, existence_probability_from_object, probability_false_detection); + } + last_update_with_measurement_time_ = measurement_time; + + // Update object measure(object, measurement_time, self_transform); + return true; } -bool Tracker::updateWithoutMeasurement() +bool Tracker::updateWithoutMeasurement(const rclcpp::Time & now) { + // Update existence probability ++no_measurement_count_; ++total_no_measurement_count_; + { + // decay existence probability + double const delta_time = (now - last_update_with_measurement_time_).seconds(); + const double decay_rate = -log(0.5) / 0.3; // 50% decay in 0.3s + for (size_t i = 0; i < existence_probabilities_.size(); ++i) { + existence_probabilities_[i] *= std::exp(-decay_rate * delta_time); + } + total_existence_probability_ *= std::exp(-decay_rate * delta_time); + } + return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index f61cbded090d1..f15d5042a2316 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -38,13 +38,17 @@ UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("UnknownTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // initialize params // measurement noise covariance constexpr double r_stddev_x = 1.0; // [m] diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp index 9b440f1dbdfec..fc396ae4b3473 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include #include @@ -61,6 +63,8 @@ class TrtYoloXNode : public rclcpp::Node LabelMap label_map_; std::unique_ptr trt_yolox_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; }; } // namespace tensorrt_yolox diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp index a59c147d14e58..c1c4c42741cee 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -29,6 +29,14 @@ namespace tensorrt_yolox TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) : Node("tensorrt_yolox", node_options) { + { + stop_watch_ptr_ = + std::make_unique>(); + debug_publisher_ = + std::make_unique(this, "tensorrt_yolox"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } using std::placeholders::_1; using std::chrono_literals::operator""ms; @@ -132,6 +140,7 @@ void TrtYoloXNode::onConnect() void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg) { + stop_watch_ptr_->toc("processing_time", true); tier4_perception_msgs::msg::DetectedObjectsWithFeature out_objects; cv_bridge::CvImagePtr in_image_ptr; @@ -173,6 +182,22 @@ void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg) out_objects.header = msg->header; objects_pub_->publish(out_objects); + + if (debug_publisher_) { + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - out_objects.header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + } } bool TrtYoloXNode::readLabelFile(const std::string & label_path) diff --git a/planning/.pages b/planning/.pages index 645a32b4b05fa..e355639eb8da5 100644 --- a/planning/.pages +++ b/planning/.pages @@ -18,7 +18,7 @@ nav: - 'Side Shift': planning/behavior_path_side_shift_module - 'Start Planner': planning/behavior_path_start_planner_module - 'Behavior Velocity Planner': - - 'About Behavior Velocity': planning/behavior_velocity_planner + - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner - 'Template for Custom Module': planning/behavior_velocity_template_module - 'Available Module': - 'Blind Spot': planning/behavior_velocity_blind_spot_module diff --git a/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt b/planning/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt similarity index 88% rename from planning/behavior_path_external_request_lane_change_module/CMakeLists.txt rename to planning/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt index 86dc0ccb61330..662d36618e766 100644 --- a/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_path_external_request_lane_change_module) +project(autoware_behavior_path_external_request_lane_change_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_path_external_request_lane_change_module/package.xml b/planning/autoware_behavior_path_external_request_lane_change_module/package.xml similarity index 89% rename from planning/behavior_path_external_request_lane_change_module/package.xml rename to planning/autoware_behavior_path_external_request_lane_change_module/package.xml index 16216aa1f71e9..c08eb20a2b589 100644 --- a/planning/behavior_path_external_request_lane_change_module/package.xml +++ b/planning/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -1,9 +1,9 @@ - behavior_path_external_request_lane_change_module + autoware_behavior_path_external_request_lane_change_module 0.1.0 - The behavior_path_external_request_lane_change_module package + The autoware_behavior_path_external_request_lane_change_module package Fumiya Watanabe Zulfaqar Azmi diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml b/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..d13dff53e0836 --- /dev/null +++ b/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp similarity index 75% rename from planning/behavior_path_external_request_lane_change_module/src/manager.cpp rename to planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp index 3cfe7aa51f0f1..130cbff29bcf3 100644 --- a/planning/behavior_path_external_request_lane_change_module/src/manager.cpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp @@ -12,13 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_external_request_lane_change_module/manager.hpp" +#include "manager.hpp" -#include "behavior_path_external_request_lane_change_module/scene.hpp" #include "behavior_path_lane_change_module/interface.hpp" +#include "scene.hpp" -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::LaneChangeInterface; std::unique_ptr ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() @@ -38,12 +39,12 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() std::make_unique(parameters_, direction_)); } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, - behavior_path_planner::SceneModuleManagerInterface) + autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, + ::behavior_path_planner::SceneModuleManagerInterface) PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, - behavior_path_planner::SceneModuleManagerInterface) + autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, + ::behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp similarity index 82% rename from planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp rename to planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp index dfcc4f61d8241..a4f3dce188c73 100644 --- a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ -#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ #include "behavior_path_lane_change_module/manager.hpp" #include "route_handler/route_handler.hpp" @@ -23,8 +23,12 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::LaneChangeModuleManager; +using ::behavior_path_planner::LaneChangeModuleType; +using ::behavior_path_planner::SceneModuleInterface; + class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager { public: @@ -49,6 +53,6 @@ class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManage } std::unique_ptr createNewSceneModuleInstance() override; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_path_external_request_lane_change_module/src/scene.cpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp similarity index 84% rename from planning/behavior_path_external_request_lane_change_module/src/scene.cpp rename to planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp index 913266bf79fa3..2ba5a5aea34d4 100644 --- a/planning/behavior_path_external_request_lane_change_module/src/scene.cpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp @@ -12,17 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_external_request_lane_change_module/scene.hpp" +#include "scene.hpp" #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::LaneChangeModuleType; + ExternalRequestLaneChange::ExternalRequestLaneChange( const std::shared_ptr & parameters, Direction direction) : NormalLaneChange(parameters, LaneChangeModuleType::EXTERNAL_REQUEST, direction) { } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp similarity index 80% rename from planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp rename to planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp index c23d6f2f3996c..d2eb20b048a5d 100644 --- a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp @@ -12,15 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ -#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ #include "behavior_path_lane_change_module/scene.hpp" #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::Direction; +using ::behavior_path_planner::LaneChangeParameters; +using ::behavior_path_planner::NormalLaneChange; + class ExternalRequestLaneChange : public NormalLaneChange { public: @@ -33,6 +37,6 @@ class ExternalRequestLaneChange : public NormalLaneChange ExternalRequestLaneChange & operator=(ExternalRequestLaneChange &&) = delete; ~ExternalRequestLaneChange() override = default; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp similarity index 94% rename from planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 1eb5118cd94b2..201d01e9b7fa0 100644 --- a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -22,7 +22,7 @@ #include #include -using behavior_path_planner::BehaviorPathPlannerNode; +using ::behavior_path_planner::BehaviorPathPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() @@ -51,8 +51,10 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); + module_names.emplace_back( + "autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); + module_names.emplace_back( + "autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/autoware_behavior_velocity_planner/CMakeLists.txt similarity index 90% rename from planning/behavior_velocity_planner/CMakeLists.txt rename to planning/autoware_behavior_velocity_planner/CMakeLists.txt index ee7dd7c07a77a..37a02b844dfe9 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_planner/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_planner) +project(autoware_behavior_velocity_planner) find_package(autoware_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) @@ -19,7 +19,7 @@ ament_auto_add_library(${PROJECT_NAME}_lib SHARED ) rclcpp_components_register_node(${PROJECT_NAME}_lib - PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode" + PLUGIN "autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/planning/behavior_velocity_planner/README.md b/planning/autoware_behavior_velocity_planner/README.md similarity index 97% rename from planning/behavior_velocity_planner/README.md rename to planning/autoware_behavior_velocity_planner/README.md index 4c840de8fe4b8..5d3e6e08a8810 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/autoware_behavior_velocity_planner/README.md @@ -67,4 +67,4 @@ The handling of traffic light information varies depending on the usage. In the | traffic_light(sim, `is_simulation = true`)
  • `info` is current information
| GO | traffic_light uses the perceived traffic light information at present directly.
  • If `info` is timeout, STOP whatever the color is
  • If `info` is not timeout, then act according to the color. If `info` is `UNKNOWN`, STOP
{: rowspan=2} | | traffic_light(real, `is_simulation = false`)
  • `info` is current information
| STOP | ⁠ {: style="padding:0"} | | crosswalk with Traffic Light(`is_simulation = *`)
  • `info` is current information
| default |
  • If `disable_yield_for_new_stopped_object` is true, each sub scene_module ignore newly detected pedestrians after module instantiation.
  • If `ignore_with_traffic_light` is true, occlusion detection is skipped.
| -| map_based_prediction(`is_simulation = *`)
  • `info` is current information
| default | If a pedestrian traffic light is
  • RED, surrouding pedestrians are not predicted.
  • GREEN, stopped pedestrians are not predicted.
| +| map_based_prediction(`is_simulation = *`)
  • `info` is current information
| default | If a pedestrian traffic light is
  • RED, surrounding pedestrians are not predicted.
  • GREEN, stopped pedestrians are not predicted.
| diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml rename to planning/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml diff --git a/planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg b/planning/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg similarity index 100% rename from planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg rename to planning/autoware_behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg diff --git a/planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg b/planning/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg similarity index 100% rename from planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg rename to planning/autoware_behavior_velocity_planner/docs/set_stop_velocity.drawio.svg diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml similarity index 97% rename from planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml rename to planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index c1631fdb739de..d4478f87f0610 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -30,7 +30,7 @@ - + diff --git a/planning/behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml similarity index 96% rename from planning/behavior_velocity_planner/package.xml rename to planning/autoware_behavior_velocity_planner/package.xml index 7a3c0ef4eadd9..324f40263a77f 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_planner + autoware_behavior_velocity_planner 0.1.0 - The behavior_velocity_planner package + The autoware_behavior_velocity_planner package Mamoru Sobue Takayuki Murooka diff --git a/planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json b/planning/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json similarity index 100% rename from planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json rename to planning/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp similarity index 96% rename from planning/behavior_velocity_planner/src/node.cpp rename to planning/autoware_behavior_velocity_planner/src/node.cpp index aa0e3e76f7d4b..def296bab94af 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/autoware_behavior_velocity_planner/src/node.cpp @@ -52,7 +52,7 @@ rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) } } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -311,6 +311,7 @@ void BehaviorVelocityPlannerNode::onParam() // lock(mutex_); planner_data_.velocity_smoother_ = std::make_unique(*this); + planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m); } void BehaviorVelocityPlannerNode::onLaneletMap( @@ -437,14 +438,15 @@ autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( std::make_shared(planner_data), *input_path_msg); // screening - const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); + const auto filtered_path = + ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = - interpolatePath(filtered_path, forward_path_length_, behavior_output_path_interval_); + const auto interpolated_path_msg = ::behavior_velocity_planner::interpolatePath( + filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point - output_path_msg = filterStopPathPoint(interpolated_path_msg); + output_path_msg = ::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); @@ -477,7 +479,7 @@ void BehaviorVelocityPlannerNode::publishDebugMarker(const autoware_planning_msg } debug_viz_pub_->publish(output_msg); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include -RCLCPP_COMPONENTS_REGISTER_NODE(behavior_velocity_planner::BehaviorVelocityPlannerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode) diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp similarity index 93% rename from planning/behavior_velocity_planner/src/node.hpp rename to planning/autoware_behavior_velocity_planner/src/node.hpp index 5b9f7c1adb61e..bdef17c14a10b 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/autoware_behavior_velocity_planner/src/node.hpp @@ -18,8 +18,8 @@ #include "planner_manager.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include -#include +#include +#include #include #include #include @@ -44,11 +44,12 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_map_msgs::msg::LaneletMapBin; -using behavior_velocity_planner::srv::LoadPlugin; -using behavior_velocity_planner::srv::UnloadPlugin; +using autoware_behavior_velocity_planner::srv::LoadPlugin; +using autoware_behavior_velocity_planner::srv::UnloadPlugin; +using ::behavior_velocity_planner::TrafficSignalStamped; using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node @@ -133,6 +134,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // NODE_HPP_ diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/planner_manager.cpp rename to planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index bb8c672141a7e..f462fc963f17b 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -19,7 +19,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -50,7 +50,7 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( } // namespace BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() -: plugin_loader_("behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") +: plugin_loader_("autoware_behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") { } @@ -128,4 +128,4 @@ diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::getStopRe { return stop_reason_diag_; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp similarity index 91% rename from planning/behavior_velocity_planner/src/planner_manager.hpp rename to planning/autoware_behavior_velocity_planner/src/planner_manager.hpp index fe48a501c3048..9e7f2942bb067 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -36,8 +36,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::PlannerData; +using ::behavior_velocity_planner::PluginInterface; + class BehaviorVelocityPlannerManager { public: @@ -56,6 +59,6 @@ class BehaviorVelocityPlannerManager pluginlib::ClassLoader plugin_loader_; std::vector> scene_manager_plugins_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // PLANNER_MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/srv/LoadPlugin.srv b/planning/autoware_behavior_velocity_planner/srv/LoadPlugin.srv similarity index 100% rename from planning/behavior_velocity_planner/srv/LoadPlugin.srv rename to planning/autoware_behavior_velocity_planner/srv/LoadPlugin.srv diff --git a/planning/behavior_velocity_planner/srv/UnloadPlugin.srv b/planning/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv similarity index 100% rename from planning/behavior_velocity_planner/srv/UnloadPlugin.srv rename to planning/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp similarity index 98% rename from planning/behavior_velocity_planner/test/src/test_node_interface.cpp rename to planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index f70259b6a1f80..06c1bc48cf529 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -23,7 +23,7 @@ #include #include -using behavior_velocity_planner::BehaviorVelocityPlannerNode; +using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() @@ -50,7 +50,7 @@ std::shared_ptr generateNode() const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto behavior_velocity_planner_dir = - ament_index_cpp::get_package_share_directory("behavior_velocity_planner"); + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); const auto motion_velocity_smoother_dir = ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index b0dd92b62ceb1..45c31da569434 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -43,9 +43,9 @@ ament_cmake_gmock ament_lint_auto + autoware_behavior_velocity_planner autoware_lint_common behavior_path_planner - behavior_velocity_planner ros_testing rosidl_interface_packages diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py index 3011abccd48ca..dc3c26798b12d 100644 --- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py @@ -59,7 +59,7 @@ def generate_test_description(): "config/behavior_path_planner.param.yaml", ), os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("autoware_behavior_velocity_planner"), "config/behavior_velocity_planner.param.yaml", ), os.path.join( diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index dc42af0412254..884812d0db716 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -293,4 +293,11 @@ # for debug debug: - marker: false + enable_other_objects_marker: false + enable_other_objects_info: false + enable_detection_area_marker: false + enable_drivable_bound_marker: false + enable_safety_check_marker: false + enable_shift_line_marker: false + enable_lane_marker: false + enable_misc_marker: false diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 2e1daed38488f..7958a8a2dcbd4 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -337,7 +337,14 @@ struct AvoidanceParameters bool enable_bound_clipping{false}; // debug - bool publish_debug_marker = false; + bool enable_other_objects_marker{false}; + bool enable_other_objects_info{false}; + bool enable_detection_area_marker{false}; + bool enable_drivable_bound_marker{false}; + bool enable_safety_check_marker{false}; + bool enable_shift_line_marker{false}; + bool enable_lane_marker{false}; + bool enable_misc_marker{false}; }; struct ObjectData // avoidance target diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp index 9b23b55acb426..6a4f206ddf309 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp @@ -18,11 +18,13 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/type_alias.hpp" +#include #include namespace behavior_path_planner::utils::avoidance { - + +using behavior_path_planner::AvoidanceParameters; using behavior_path_planner::AvoidancePlanningData; using behavior_path_planner::AvoidLineArray; using behavior_path_planner::DebugData; @@ -42,10 +44,12 @@ MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::st MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); -MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const ObjectInfo & info); +MarkerArray createOtherObjectsMarkerArray( + const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose); MarkerArray createDebugMarkerArray( - const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug); + const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, + const std::shared_ptr & parameters); } // namespace behavior_path_planner::utils::avoidance std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr); diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 8de39d00af130..55d14e424b7f6 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -97,16 +97,18 @@ class AvoidanceHelper return std::max(getEgoSpeed(), values.at(idx)); } - double getMinimumPrepareDistance() const + double getNominalPrepareDistance(const double velocity) const { const auto & p = parameters_; - return std::max(getEgoSpeed() * p->min_prepare_time, p->min_prepare_distance); + const auto & values = p->velocity_map; + const auto idx = getConstraintsMapIndex(velocity, values); + return std::max(values.at(idx) * p->max_prepare_time, p->min_prepare_distance); } double getNominalPrepareDistance() const { const auto & p = parameters_; - return std::max(getEgoSpeed() * p->max_prepare_time, p->min_prepare_distance); + return std::max(getAvoidanceEgoSpeed() * p->max_prepare_time, p->min_prepare_distance); } double getNominalAvoidanceDistance(const double shift_length) const @@ -298,6 +300,13 @@ class AvoidanceHelper return std::numeric_limits::max(); } + bool isEnoughPrepareDistance(const double prepare_distance) const + { + const auto & p = parameters_; + return prepare_distance > + std::max(getEgoSpeed() * p->min_prepare_distance, p->min_prepare_distance); + } + bool isComfortable(const AvoidLineArray & shift_lines) const { const auto JERK_BUFFER = 0.1; // [m/sss] @@ -328,7 +337,7 @@ class AvoidanceHelper const auto desire_shift_length = getShiftLength(object, is_object_on_right, object.avoid_margin.value()); - const auto prepare_distance = getMinimumPrepareDistance(); + const auto prepare_distance = getNominalPrepareDistance(0.0); const auto constant_distance = getFrontConstantDistance(object); const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index 3a6ea950bc79d..66f77cd2d076c 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -383,7 +383,20 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // debug { const std::string ns = "avoidance.debug."; - p.publish_debug_marker = getOrDeclareParameter(*node, ns + "marker"); + p.enable_other_objects_marker = + getOrDeclareParameter(*node, ns + "enable_other_objects_marker"); + p.enable_other_objects_info = + getOrDeclareParameter(*node, ns + "enable_other_objects_info"); + p.enable_detection_area_marker = + getOrDeclareParameter(*node, ns + "enable_detection_area_marker"); + p.enable_drivable_bound_marker = + getOrDeclareParameter(*node, ns + "enable_drivable_bound_marker"); + p.enable_safety_check_marker = + getOrDeclareParameter(*node, ns + "enable_safety_check_marker"); + p.enable_shift_line_marker = + getOrDeclareParameter(*node, ns + "enable_shift_line_marker"); + p.enable_lane_marker = getOrDeclareParameter(*node, ns + "enable_lane_marker"); + p.enable_misc_marker = getOrDeclareParameter(*node, ns + "enable_misc_marker"); } return p; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 6b11d490e8c23..194e5ecdaf86c 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -26,6 +26,7 @@ #include #include +#include #include #include #include @@ -138,9 +139,15 @@ class AvoidanceModule : public SceneModuleInterface void removeCandidateRTCStatus() { if (rtc_interface_ptr_map_.at("left")->isRegistered(candidate_uuid_)) { - rtc_interface_ptr_map_.at("left")->removeCooperateStatus(candidate_uuid_); - } else if (rtc_interface_ptr_map_.at("right")->isRegistered(candidate_uuid_)) { - rtc_interface_ptr_map_.at("right")->removeCooperateStatus(candidate_uuid_); + rtc_interface_ptr_map_.at("left")->updateCooperateStatus( + candidate_uuid_, true, State::FAILED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } + + if (rtc_interface_ptr_map_.at("right")->isRegistered(candidate_uuid_)) { + rtc_interface_ptr_map_.at("right")->updateCooperateStatus( + candidate_uuid_, true, State::FAILED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); } } @@ -362,10 +369,21 @@ class AvoidanceModule : public SceneModuleInterface unlockNewModuleLaunch(); + for (const auto & left_shift : left_shift_array_) { + rtc_interface_ptr_map_.at("left")->updateCooperateStatus( + left_shift.uuid, true, State::FAILED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } + + for (const auto & right_shift : right_shift_array_) { + rtc_interface_ptr_map_.at("right")->updateCooperateStatus( + right_shift.uuid, true, State::FAILED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } + if (!path_shifter_.getShiftLines().empty()) { left_shift_array_.clear(); right_shift_array_.clear(); - removeRTCStatus(); } generator_.reset(); diff --git a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json index 7c0205d2473e5..e0f1156172932 100644 --- a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json +++ b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json @@ -1388,13 +1388,57 @@ "debug": { "type": "object", "properties": { - "marker": { + "enable_other_objects_marker": { "type": "boolean", - "description": "Publish debug marker.", + "description": "Publish other objects marker.", + "default": "false" + }, + "enable_other_objects_info": { + "type": "boolean", + "description": "Publish other objects detail information.", + "default": "false" + }, + "enable_detection_area_marker": { + "type": "boolean", + "description": "Publish detection area.", + "default": "false" + }, + "enable_drivable_bound_marker": { + "type": "boolean", + "description": "Publish drivable area boundary.", + "default": "false" + }, + "enable_safety_check_marker": { + "type": "boolean", + "description": "Publish safety check information.", + "default": "false" + }, + "enable_shift_line_marker": { + "type": "boolean", + "description": "Publish shift line information.", + "default": "false" + }, + "enable_lane_marker": { + "type": "boolean", + "description": "Publish lane information.", + "default": "false" + }, + "enable_misc_marker": { + "type": "boolean", + "description": "Publish misc markers.", "default": "false" } }, - "required": ["marker"], + "required": [ + "enable_other_objects_marker", + "enable_other_objects_info", + "enable_detection_area_marker", + "enable_drivable_bound_marker", + "enable_safety_check_marker", + "enable_shift_line_marker", + "enable_lane_marker", + "enable_misc_marker" + ], "additionalProperties": false } }, diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index da3d4206791b2..a1cfaaeef570e 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -130,7 +130,8 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std:: return msg; } -MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::string && ns) +MarkerArray createObjectInfoMarkerArray( + const ObjectDataArray & objects, std::string && ns, const bool verbose) { MarkerArray msg; @@ -139,7 +140,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); for (const auto & object : objects) { - { + if (verbose) { marker.id = uuidToInt32(object.object.object_id); marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; std::ostringstream string_stream; @@ -160,6 +161,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st { marker.id = uuidToInt32(object.object.object_id); + marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; marker.pose.position.z += 2.0; std::ostringstream string_stream; string_stream << magic_enum::enum_name(object.info) << (object.is_parked ? "(PARKED)" : ""); @@ -201,7 +203,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st createMarkerColor(1.0, 1.0, 0.0, 0.8)), &msg); - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info", true), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); @@ -220,7 +222,7 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std:: createMarkerColor(1.0, 0.0, 0.0, 0.8)), &msg); - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info", true), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); @@ -428,7 +430,8 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons return msg; } -MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const ObjectInfo & info) +MarkerArray createOtherObjectsMarkerArray( + const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose) { const auto filtered_objects = [&objects, &info]() { ObjectDataArray ret{}; @@ -456,7 +459,8 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const filtered_objects, "others_" + ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), createMarkerColor(0.0, 1.0, 0.0, 0.8)), &msg); - appendMarkerArray(createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info"), &msg); + appendMarkerArray( + createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info", verbose), &msg); appendMarkerArray( createOverhangLaneletMarkerArray(filtered_objects, "others_" + ns + "_overhang_lanelet"), &msg); @@ -500,7 +504,8 @@ MarkerArray createDrivableBounds( } MarkerArray createDebugMarkerArray( - const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) + const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, + const std::shared_ptr & parameters) { using behavior_path_planner::utils::transformToLanelets; using lanelet::visualization::laneletsAsTriangleMarkerArray; @@ -534,7 +539,7 @@ MarkerArray createDebugMarkerArray( }; const auto addObjects = [&](const ObjectDataArray & objects, const auto & ns) { - add(createOtherObjectsMarkerArray(objects, ns)); + add(createOtherObjectsMarkerArray(objects, ns, parameters->enable_other_objects_info)); }; const auto addShiftLength = @@ -549,7 +554,7 @@ MarkerArray createDebugMarkerArray( }; // ignore objects - { + if (parameters->enable_other_objects_marker) { addObjects(data.other_objects, ObjectInfo::FURTHER_THAN_THRESHOLD); addObjects(data.other_objects, ObjectInfo::IS_NOT_TARGET_OBJECT); addObjects(data.other_objects, ObjectInfo::FURTHER_THAN_GOAL); @@ -569,7 +574,7 @@ MarkerArray createDebugMarkerArray( } // shift line pre-process - { + if (parameters->enable_shift_line_marker) { addAvoidLine(debug.step1_registered_shift_line, "step1_registered_shift_line", 0.2, 0.2, 1.0); addAvoidLine(debug.step1_current_shift_line, "step1_current_shift_line", 0.2, 0.4, 0.8, 0.3); addAvoidLine(debug.step1_merged_shift_line, "step1_merged_shift_line", 0.2, 0.6, 0.6, 0.3); @@ -578,39 +583,39 @@ MarkerArray createDebugMarkerArray( } // merge process - { + if (parameters->enable_shift_line_marker) { addAvoidLine(debug.step2_merged_shift_line, "step2_merged_shift_line", 0.2, 1.0, 0.0, 0.3); } // trimming process - { + if (parameters->enable_shift_line_marker) { addAvoidLine(debug.step3_grad_filtered_1st, "step3_grad_filtered_1st", 0.2, 0.8, 0.0, 0.3); addAvoidLine(debug.step3_grad_filtered_2nd, "step3_grad_filtered_2nd", 0.4, 0.6, 0.0, 0.3); addAvoidLine(debug.step3_grad_filtered_3rd, "step3_grad_filtered_3rd", 0.6, 0.4, 0.0, 0.3); } // registering process - { + if (parameters->enable_shift_line_marker) { addShiftLine(shifter.getShiftLines(), "step4_old_shift_line", 1.0, 1.0, 0.0, 0.3); addAvoidLine(data.new_shift_line, "step4_new_shift_line", 1.0, 0.0, 0.0, 0.3); } // safety check - { + if (parameters->enable_safety_check_marker) { add(showSafetyCheckInfo(debug.collision_check, "object_debug_info")); add(showPredictedPath(debug.collision_check, "ego_predicted_path")); add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation")); } // shift length - { + if (parameters->enable_shift_line_marker) { addShiftLength(debug.pos_shift, "merged_length_pos", 0.0, 0.7, 0.5); addShiftLength(debug.neg_shift, "merged_length_neg", 0.0, 0.5, 0.7); addShiftLength(debug.total_shift, "merged_length_total", 0.99, 0.4, 0.2); } // shift grad - { + if (parameters->enable_shift_line_marker) { addShiftGrad(debug.pos_shift_grad, debug.pos_shift, "merged_grad_pos", 0.0, 0.7, 0.5); addShiftGrad(debug.neg_shift_grad, debug.neg_shift, "merged_grad_neg", 0.0, 0.5, 0.7); addShiftGrad(debug.total_forward_grad, debug.total_shift, "grad_forward", 0.99, 0.4, 0.2); @@ -618,15 +623,20 @@ MarkerArray createDebugMarkerArray( } // detection area - size_t i = 0; - for (const auto & detection_area : debug.detection_areas) { - add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1)); + if (parameters->enable_detection_area_marker) { + size_t i = 0; + for (const auto & detection_area : debug.detection_areas) { + add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1)); + } } - // misc - { - add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); + // drivable bound + if (parameters->enable_drivable_bound_marker) { add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42)); + } + + // lane + if (parameters->enable_lane_marker) { add(laneletsAsTriangleMarkerArray( "drivable_lanes", transformToLanelets(data.drivable_lanes), createMarkerColor(0.16, 1.0, 0.69, 0.2))); @@ -636,6 +646,11 @@ MarkerArray createDebugMarkerArray( "safety_check_lanes", debug.safety_check_lanes, createMarkerColor(1.0, 0.0, 0.42, 0.2))); } + // misc + if (parameters->enable_misc_marker) { + add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); + } + return msg; } } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 51d671f7db80b..86b05ea50e288 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -261,7 +261,17 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "marker", p->publish_debug_marker); + updateParam( + parameters, ns + "enable_other_objects_marker", p->enable_other_objects_marker); + updateParam(parameters, ns + "enable_other_objects_info", p->enable_other_objects_info); + updateParam( + parameters, ns + "enable_detection_area_marker", p->enable_detection_area_marker); + updateParam( + parameters, ns + "enable_drivable_bound_marker", p->enable_drivable_bound_marker); + updateParam(parameters, ns + "enable_safety_check_marker", p->enable_safety_check_marker); + updateParam(parameters, ns + "enable_shift_line_marker", p->enable_shift_line_marker); + updateParam(parameters, ns + "enable_lane_marker", p->enable_lane_marker); + updateParam(parameters, ns + "enable_misc_marker", p->enable_misc_marker); } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 6858b3809a73b..503438f9f7feb 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -409,9 +409,9 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const const auto registered_lines = path_shifter_.getShiftLines(); if (!registered_lines.empty()) { const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); - const auto to_shift_start_point = motion_utils::calcSignedArcLength( + const auto prepare_distance = motion_utils::calcSignedArcLength( path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx); - if (to_shift_start_point < helper_->getMinimumPrepareDistance()) { + if (!helper_->isEnoughPrepareDistance(prepare_distance)) { RCLCPP_DEBUG( getLogger(), "Distance to shift start point is less than minimum prepare distance. The distance is not " @@ -1283,7 +1283,6 @@ void AvoidanceModule::updateData() void AvoidanceModule::processOnEntry() { initVariables(); - removeRTCStatus(); } void AvoidanceModule::processOnExit() @@ -1361,12 +1360,7 @@ void AvoidanceModule::updateDebugMarker( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { debug_marker_.markers.clear(); - - if (!parameters_->publish_debug_marker) { - return; - } - - debug_marker_ = utils::avoidance::createDebugMarkerArray(data, shifter, debug); + debug_marker_ = utils::avoidance::createDebugMarkerArray(data, shifter, debug, parameters_); } void AvoidanceModule::updateAvoidanceDebugData( @@ -1418,10 +1412,11 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const const auto avoidance_distance = helper_->getMinAvoidanceDistance( helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); const auto constant_distance = helper_->getFrontConstantDistance(object); + const auto prepare_distance = helper_->getNominalPrepareDistance(0.0); return object.longitudinal - std::min( - avoidance_distance + constant_distance + p->min_prepare_distance + p->stop_buffer, + avoidance_distance + constant_distance + prepare_distance + p->stop_buffer, p->stop_max_distance); } @@ -1450,7 +1445,7 @@ void AvoidanceModule::insertReturnDeadLine( const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); const auto min_return_distance = - helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance(); + helper_->getMinAvoidanceDistance(shift_length) + helper_->getNominalPrepareDistance(0.0); const auto to_stop_line = data.to_return_point - min_return_distance - buffer; // If we don't need to consider deceleration constraints, insert a deceleration point diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 84b2b9b61a702..fc3b9e24ff9f6 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -929,7 +929,7 @@ void ShiftLineGenerator::applySmallShiftFilter( continue; } - if (s.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { + if (!helper_->isEnoughPrepareDistance(s.start_longitudinal)) { continue; } @@ -1173,13 +1173,13 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( std::max(nominal_prepare_distance - last_sl_distance, 0.0); double prepare_distance_scaled = std::max( - helper_->getMinimumPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance)); + helper_->getNominalPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance)); double avoid_distance_scaled = nominal_avoid_distance; if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) { const auto scale = (remaining_distance - last_sl_distance) / std::max(nominal_avoid_distance + variable_prepare_distance, 0.1); prepare_distance_scaled = std::max( - helper_->getMinimumPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance); + helper_->getNominalPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance); avoid_distance_scaled *= scale; } @@ -1292,7 +1292,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine( const auto & candidate = shift_lines.at(i); // prevent sudden steering. - if (candidate.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { + if (!helper_->isEnoughPrepareDistance(candidate.start_longitudinal)) { break; } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index dbc1663c7731e..e59193d6fb27b 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1916,7 +1916,7 @@ std::vector getSafetyCheckTargetObjects( std::for_each(objects.begin(), objects.end(), [&p, &ret, ¶meters](const auto & object) { if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) { // check only moving objects - if (filtering_utils::isMovingObject(object, parameters)) { + if (filtering_utils::isMovingObject(object, parameters) || !object.is_parked) { ret.objects.push_back(object.object); } } @@ -2221,7 +2221,7 @@ DrivableLanes generateExpandedDrivableLanes( break; } if (i == max_recursive_search_num - 1) { - RCLCPP_ERROR( + RCLCPP_DEBUG( rclcpp::get_logger(logger_namespace), "Drivable area expansion reaches max iteration."); } } diff --git a/planning/behavior_path_external_request_lane_change_module/plugins.xml b/planning/behavior_path_external_request_lane_change_module/plugins.xml deleted file mode 100644 index f3cc686837c38..0000000000000 --- a/planning/behavior_path_external_request_lane_change_module/plugins.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/planning/behavior_path_lane_change_module/plugins.xml b/planning/behavior_path_lane_change_module/plugins.xml index a598bc8176938..7df36919d8d54 100644 --- a/planning/behavior_path_lane_change_module/plugins.xml +++ b/planning/behavior_path_lane_change_module/plugins.xml @@ -1,6 +1,6 @@ - - + + diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index ef0b5492ad862..ff83c3b2fda51 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -47,6 +47,7 @@ #include #include +#include #include #include #include @@ -186,6 +187,10 @@ class SceneModuleInterface { RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__); + if (getCurrentStatus() == ModuleStatus::SUCCESS) { + updateRTCStatusForSuccess(); + } + clearWaitingApproval(); unlockNewModuleLaunch(); unlockOutputPath(); @@ -499,6 +504,17 @@ class SceneModuleInterface } } + void updateRTCStatusForSuccess() + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->updateCooperateStatus( + uuid_map_.at(module_name), true, State::SUCCEEDED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } + } + } + void setObjectsOfInterestData( const geometry_msgs::msg::Pose & obj_pose, const autoware_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name) diff --git a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index 08bc0ae703a90..7dae0fb32c79f 100644 --- a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -110,6 +110,14 @@ void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyG } } +static IndexXY position2index( + const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Point & position_local) +{ + const int index_x = position_local.x / costmap.info.resolution; + const int index_y = position_local.y / costmap.info.resolution; + return {index_x, index_y}; +} + void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( int theta_index, std::vector & indexes_2d) { @@ -131,12 +139,11 @@ void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( const double offset_x = std::cos(base_theta) * x - std::sin(base_theta) * y; const double offset_y = std::sin(base_theta) * x + std::cos(base_theta) * y; - geometry_msgs::msg::Pose pose_local; - pose_local.position.x = base_pose.position.x + offset_x; - pose_local.position.y = base_pose.position.y + offset_y; + geometry_msgs::msg::Point position_local; + position_local.x = base_pose.position.x + offset_x; + position_local.y = base_pose.position.y + offset_y; - const auto index = pose2index(costmap_, pose_local, param_.theta_size); - const auto index_2d = IndexXY{index.x, index.y}; + const auto index_2d = position2index(costmap_, position_local); indexes_2d.push_back(index_2d); }; diff --git a/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index 38eafb2f38d7d..1ea555f4675ef 100644 --- a/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -53,7 +53,7 @@ namespace behavior_path_planner using motion_utils::findNearestIndex; using motion_utils::insertOrientation; -using motion_utils::removeInvalidOrientationPoints; +using motion_utils::removeFirstInvalidOrientationPoints; using motion_utils::removeOverlapPoints; void PathShifter::setPath(const PathWithLaneId & path) @@ -155,14 +155,14 @@ bool PathShifter::generate( shifted_path->path.points = removeOverlapPoints(shifted_path->path.points); // Use orientation before shift to remove points in reverse order // before setting wrong azimuth orientation - removeInvalidOrientationPoints(shifted_path->path.points); + removeFirstInvalidOrientationPoints(shifted_path->path.points); size_t previous_size{shifted_path->path.points.size()}; do { previous_size = shifted_path->path.points.size(); // Set the azimuth orientation to the next point at each point insertOrientation(shifted_path->path.points, true); // Use azimuth orientation to remove points in reverse order - removeInvalidOrientationPoints(shifted_path->path.points); + removeFirstInvalidOrientationPoints(shifted_path->path.points); } while (previous_size != shifted_path->path.points.size()); // DEBUG diff --git a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt index 682da3ae58d7e..7ff2419eed919 100644 --- a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt @@ -3,12 +3,13 @@ project(behavior_velocity_blind_spot_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp src/manager.cpp src/scene.cpp + src/decisions.cpp ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index 5cc90afb0043d..b733a1ea1e79c 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -71,55 +71,16 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( return msg; } -visualization_msgs::msg::MarkerArray createPoseMarkerArray( - const geometry_msgs::msg::Pose & pose, const StateMachine::State & state, const std::string & ns, - const int64_t id, const double r, const double g, const double b) -{ - visualization_msgs::msg::MarkerArray msg; - - if (state == StateMachine::State::STOP) { - visualization_msgs::msg::Marker marker_line{}; - marker_line.header.frame_id = "map"; - marker_line.ns = ns + "_line"; - marker_line.id = id; - marker_line.lifetime = rclcpp::Duration::from_seconds(0.3); - marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker_line.action = visualization_msgs::msg::Marker::ADD; - marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - marker_line.scale = createMarkerScale(0.1, 0.0, 0.0); - marker_line.color = createMarkerColor(r, g, b, 0.999); - - const double yaw = tf2::getYaw(pose.orientation); - - const double a = 3.0; - geometry_msgs::msg::Point p0; - p0.x = pose.position.x - a * std::sin(yaw); - p0.y = pose.position.y + a * std::cos(yaw); - p0.z = pose.position.z; - marker_line.points.push_back(p0); - - geometry_msgs::msg::Point p1; - p1.x = pose.position.x + a * std::sin(yaw); - p1.y = pose.position.y - a * std::cos(yaw); - p1.z = pose.position.z; - marker_line.points.push_back(p1); - - msg.markers.push_back(marker_line); - } - - return msg; -} - } // namespace motion_utils::VirtualWalls BlindSpotModule::createVirtualWalls() { motion_utils::VirtualWalls virtual_walls; - if (!isActivated() && !is_over_pass_judge_line_) { + if (debug_data_.virtual_wall_pose) { motion_utils::VirtualWall wall; wall.text = "blind_spot"; - wall.pose = debug_data_.virtual_wall_pose; + wall.pose = debug_data_.virtual_wall_pose.value(); wall.ns = std::to_string(module_id_) + "_"; virtual_walls.push_back(wall); } @@ -130,19 +91,8 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; - const auto state = state_machine_.getState(); const auto now = this->clock_->now(); - appendMarkerArray( - createPoseMarkerArray( - debug_data_.stop_point_pose, state, "stop_point_pose", module_id_, 1.0, 0.0, 0.0), - &debug_marker_array, now); - - appendMarkerArray( - createPoseMarkerArray( - debug_data_.judge_point_pose, state, "judge_point_pose", module_id_, 1.0, 1.0, 0.5), - &debug_marker_array, now); - appendMarkerArray( createLaneletPolygonsMarkerArray( debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5), diff --git a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp new file mode 100644 index 0000000000000..961bad2f6b364 --- /dev/null +++ b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp @@ -0,0 +1,154 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene.hpp" + +#include + +namespace behavior_velocity_planner +{ + +/* + * for default + */ +template +void BlindSpotModule::setRTCStatusByDecision( + const T &, const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + static_assert("Unsupported type passed to setRTCStatus"); + return; +} + +template +void BlindSpotModule::reactRTCApprovalByDecision( + [[maybe_unused]] const T & decision, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason) +{ + static_assert("Unsupported type passed to reactRTCApprovalByDecision"); +} + +/* + * for InternalError + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + [[maybe_unused]] const InternalError & decision, + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + [[maybe_unused]] const InternalError & decision, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason) +{ + return; +} + +/* + * For OverPassJudge + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + [[maybe_unused]] const OverPassJudge & decision, + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + [[maybe_unused]] const OverPassJudge & decision, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason) +{ + return; +} + +/* + * for Unsafe + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + const Unsafe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + setSafe(false); + const auto & current_pose = planner_data_->current_odometry->pose; + setDistance( + motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx)); + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason) +{ + if (!isActivated()) { + constexpr double stop_vel = 0.0; + planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path); + debug_data_.virtual_wall_pose = planning_utils::getAheadPose( + decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + + tier4_planning_msgs::msg::StopFactor stop_factor; + const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; + stop_factor.stop_pose = stop_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, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + } + return; +} + +/* + * for Safe + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + const Safe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + setSafe(true); + const auto & current_pose = planner_data_->current_odometry->pose; + setDistance( + motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx)); + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason) +{ + if (!isActivated()) { + constexpr double stop_vel = 0.0; + planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path); + debug_data_.virtual_wall_pose = planning_utils::getAheadPose( + decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + + tier4_planning_msgs::msg::StopFactor stop_factor; + const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; + stop_factor.stop_pose = stop_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, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + } + return; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index 4dd442328c2be..3b7e02527c76c 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -62,14 +62,17 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa } // Is turning lane? - const std::string turn_direction = ll.attributeOr("turn_direction", "else"); - if (turn_direction != "left" && turn_direction != "right") { + const std::string turn_direction_str = ll.attributeOr("turn_direction", "else"); + if (turn_direction_str != "left" && turn_direction_str != "right") { continue; } + const auto turn_direction = turn_direction_str == "left" + ? BlindSpotModule::TurnDirection::LEFT + : BlindSpotModule::TurnDirection::RIGHT; registerModule(std::make_shared( - module_id, lane_id, planner_data_, planner_param_, logger_.get_child("blind_spot_module"), - clock_)); + module_id, lane_id, turn_direction, planner_data_, planner_param_, + logger_.get_child("blind_spot_module"), clock_)); generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index dedb04b0a292e..9aec1d2df44cf 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -44,165 +44,120 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -namespace -{ -[[maybe_unused]] geometry_msgs::msg::Polygon toGeomPoly(const lanelet::CompoundPolygon3d & poly) -{ - geometry_msgs::msg::Polygon geom_poly; - - for (const auto & p : poly) { - geometry_msgs::msg::Point32 geom_point; - geom_point.x = p.x(); - geom_point.y = p.y(); - geom_point.z = p.z(); - geom_poly.points.push_back(geom_point); - } - - return geom_poly; -} -} // namespace - BlindSpotModule::BlindSpotModule( - const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) + const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, + const std::shared_ptr planner_data, const PlannerParam & planner_param, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), planner_param_{planner_param}, - turn_direction_(TurnDirection::INVALID), + turn_direction_(turn_direction), is_over_pass_judge_line_(false) { velocity_factor_.init(PlanningBehavior::REAR_CHECK); - - const auto & assigned_lanelet = - planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); - const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); - if (!turn_direction.compare("left")) { - turn_direction_ = TurnDirection::LEFT; - } else if (!turn_direction.compare("right")) { - turn_direction_ = TurnDirection::RIGHT; - } + sibling_straight_lanelet_ = getSiblingStraightLanelet(planner_data); } -bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +void BlindSpotModule::initializeRTCStatus() { - debug_data_ = DebugData(); - *stop_reason = planning_utils::initializeStopReason(StopReason::BLIND_SPOT); - - const auto input_path = *path; - - const auto current_state = state_machine_.getState(); - RCLCPP_DEBUG( - logger_, "lane_id = %ld, state = %s", lane_id_, StateMachine::toString(current_state).c_str()); - - /* get current pose */ - const auto & current_pose = planner_data_->current_odometry->pose; + setSafe(true); + setDistance(std::numeric_limits::lowest()); +} - /* get lanelet map */ - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); +BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( + PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +{ + const auto & input_path = *path; /* set stop-line and stop-judgement-line for base_link */ const auto interpolated_path_info_opt = generateInterpolatedPathInfo(input_path); if (!interpolated_path_info_opt) { - RCLCPP_DEBUG(logger_, "[BlindSpotModule::run] failed to interpolate path"); - return false; + return InternalError{"Failed to interpolate path"}; } const auto & interpolated_path_info = interpolated_path_info_opt.value(); - if (!sibling_straight_lanelet_) { - sibling_straight_lanelet_ = getSiblingStraightLanelet(); - } - const auto stoplines_idx_opt = generateStopLine(interpolated_path_info, path); if (!stoplines_idx_opt) { - RCLCPP_DEBUG(logger_, "[BlindSpotModule::run] setStopLineIdx fail"); - return false; + return InternalError{"generateStopLine fail"}; } const auto [default_stopline_idx, critical_stopline_idx] = stoplines_idx_opt.value(); - - if (default_stopline_idx <= 0) { - RCLCPP_DEBUG( - logger_, "[Blind Spot] stop line or pass judge line is at path[0], ignore planning."); - *path = input_path; // reset path - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return true; + if (default_stopline_idx == 0) { + return InternalError{"stop line or pass judge line is at path[0], ignore planning."}; } /* calc closest index */ + const auto & current_pose = planner_data_->current_odometry->pose; const auto closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); const auto stop_line_idx = closest_idx > default_stopline_idx ? critical_stopline_idx : default_stopline_idx; - - /* set judge line dist */ - const double current_vel = planner_data_->current_velocity->twist.linear.x; - const double max_acc = planner_data_->max_stop_acceleration_threshold; - const double delay_response_time = planner_data_->delay_response_time; - const double pass_judge_line_dist = - planning_utils::calcJudgeLineDistWithAccLimit(current_vel, max_acc, delay_response_time); - const auto stop_point_pose = path->points.at(stop_line_idx).point.pose; - const auto ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); - const size_t stop_point_segment_idx = - motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); - const auto distance_until_stop = motion_utils::calcSignedArcLength( - input_path.points, current_pose.position, ego_segment_idx, stop_point_pose.position, - stop_point_segment_idx); - - /* get debug info */ const auto stop_line_pose = planning_utils::getAheadPose( - stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); - debug_data_.virtual_wall_pose = stop_line_pose; - const auto stop_pose = path->points.at(stop_line_idx).point.pose; - debug_data_.stop_point_pose = stop_pose; + stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, input_path); - /* if current_state = GO, and current_pose is over judge_line, ignore planning. */ - if (planner_param_.use_pass_judge_line) { - const double eps = 1e-1; // to prevent hunting - if ( - current_state == StateMachine::State::GO && - distance_until_stop + eps < pass_judge_line_dist) { - RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - *path = input_path; // reset path - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return true; // no plan needed. - } + const auto is_over_pass_judge = isOverPassJudge(input_path, stop_line_pose); + if (is_over_pass_judge) { + return is_over_pass_judge.value(); } - /* get dynamic object */ - const auto objects_ptr = planner_data_->predicted_objects; + const auto areas_opt = generateBlindSpotPolygons(input_path, closest_idx, stop_line_pose); + if (!areas_opt) { + return InternalError{"Failed to generate BlindSpotPolygons"}; + } /* calculate dynamic collision around detection area */ - bool has_obstacle = checkObstacleInBlindSpot( - lanelet_map_ptr, routing_graph_ptr, *path, objects_ptr, closest_idx, stop_line_pose); + const auto collision_obstacle = isCollisionDetected(areas_opt.value()); state_machine_.setStateWithMarginTime( - has_obstacle ? StateMachine::State::STOP : StateMachine::State::GO, + collision_obstacle.has_value() ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("state_machine"), *clock_); - /* set stop speed */ - setSafe(state_machine_.getState() != StateMachine::State::STOP); - setDistance(motion_utils::calcSignedArcLength( - path->points, current_pose.position, path->points.at(stop_line_idx).point.pose.position)); - if (!isActivated()) { - constexpr double stop_vel = 0.0; - planning_utils::setVelocityFromIndex(stop_line_idx, stop_vel, path); - - /* get stop point and stop factor */ - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = debug_data_.stop_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, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); - } else { - *path = input_path; // reset path + if (state_machine_.getState() == StateMachine::State::STOP) { + return Unsafe{stop_line_idx, collision_obstacle}; } + + return Safe{stop_line_idx}; +} + +// template-specification based visitor pattern +// https://en.cppreference.com/w/cpp/utility/variant/visit +template +struct VisitorSwitch : Ts... +{ + using Ts::operator()...; +}; +template +VisitorSwitch(Ts...) -> VisitorSwitch; + +void BlindSpotModule::setRTCStatus( + const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + std::visit( + VisitorSwitch{[&](const auto & sub_decision) { setRTCStatusByDecision(sub_decision, path); }}, + decision); +} + +void BlindSpotModule::reactRTCApproval( + const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason) +{ + std::visit( + VisitorSwitch{[&](const auto & sub_decision) { + reactRTCApprovalByDecision(sub_decision, path, stop_reason); + }}, + decision); +} + +bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +{ + debug_data_ = DebugData(); + *stop_reason = planning_utils::initializeStopReason(StopReason::BLIND_SPOT); + + initializeRTCStatus(); + const auto decision = modifyPathVelocityDetail(path, stop_reason); + const auto & input_path = *path; + setRTCStatus(decision, input_path); + reactRTCApproval(decision, path, stop_reason); + return true; } @@ -259,10 +214,11 @@ std::optional BlindSpotModule::generateInterpolatedPathInf return interpolated_path_info; } -std::optional BlindSpotModule::getSiblingStraightLanelet() const +std::optional BlindSpotModule::getSiblingStraightLanelet( + const std::shared_ptr planner_data) const { - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto lanelet_map_ptr = planner_data->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data->route_handler_->getRoutingGraphPtr(); const auto assigned_lane = lanelet_map_ptr->laneletLayer.get(lane_id_); for (const auto & prev : routing_graph_ptr->previous(assigned_lane)) { @@ -324,8 +280,9 @@ static std::optional insertPointIndex( inout_path->points, in_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) - int insert_idx = closest_idx; - tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); + size_t insert_idx = closest_idx; + tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = + inout_path->points.at(closest_idx); if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { ++insert_idx; } else { @@ -346,6 +303,7 @@ std::optional> BlindSpotModule::generateStopLine( const InterpolatedPathInfo & interpolated_path_info, tier4_planning_msgs::msg::PathWithLaneId * path) const { + // NOTE: this is optionally int for later subtraction const int margin_idx_dist = std::ceil(planner_param_.stop_line_margin / interpolated_path_info.ds); @@ -365,7 +323,9 @@ std::optional> BlindSpotModule::generateStopLine( if (!first_conflict_idx_ip_opt) { return std::nullopt; } - const int first_conflict_idx_ip = static_cast(first_conflict_idx_ip_opt.value()); + + // NOTE: this is optionally int for later subtraction + const auto first_conflict_idx_ip = static_cast(first_conflict_idx_ip_opt.value()); stop_idx_default_ip = static_cast(std::max(first_conflict_idx_ip - margin_idx_dist, 0)); stop_idx_critical_ip = static_cast(first_conflict_idx_ip); @@ -426,80 +386,90 @@ void BlindSpotModule::cutPredictPathWithDuration( } } -bool BlindSpotModule::checkObstacleInBlindSpot( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const tier4_planning_msgs::msg::PathWithLaneId & path, - const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) +std::optional BlindSpotModule::isOverPassJudge( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const geometry_msgs::msg::Pose & stop_point_pose) const { - /* get detection area */ - if (turn_direction_ == TurnDirection::INVALID) { - RCLCPP_WARN(logger_, "blind spot detector is running, turn_direction_ = not right or left."); - return false; - } + const auto & current_pose = planner_data_->current_odometry->pose; - const auto areas_opt = generateBlindSpotPolygons( - lanelet_map_ptr, routing_graph_ptr, path, closest_idx, stop_line_pose); - if (areas_opt) { - const auto & detection_areas = areas_opt.value().detection_areas; - const auto & conflict_areas = areas_opt.value().conflict_areas; - const auto & opposite_detection_areas = areas_opt.value().opposite_detection_areas; - const auto & opposite_conflict_areas = areas_opt.value().opposite_conflict_areas; - debug_data_.detection_areas = detection_areas; - debug_data_.conflict_areas = conflict_areas; - debug_data_.detection_areas.insert( - debug_data_.detection_areas.end(), opposite_detection_areas.begin(), - opposite_detection_areas.end()); - debug_data_.conflict_areas.insert( - debug_data_.conflict_areas.end(), opposite_conflict_areas.begin(), - opposite_conflict_areas.end()); + /* set judge line dist */ + const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithAccLimit( + planner_data_->current_velocity->twist.linear.x, planner_data_->max_stop_acceleration_threshold, + planner_data_->delay_response_time); + const auto ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + const size_t stop_point_segment_idx = + motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); + const auto distance_until_stop = motion_utils::calcSignedArcLength( + input_path.points, current_pose.position, ego_segment_idx, stop_point_pose.position, + stop_point_segment_idx); - autoware_perception_msgs::msg::PredictedObjects objects = *objects_ptr; - cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); + /* if current_state = GO, and current_pose is over judge_line, ignore planning. */ + if (planner_param_.use_pass_judge_line) { + const double eps = 1e-1; // to prevent hunting + if (const auto current_state = state_machine_.getState(); + current_state == StateMachine::State::GO && + distance_until_stop + eps < pass_judge_line_dist) { + return OverPassJudge{"over the pass judge line. no plan needed."}; + } + } + return std::nullopt; +} - // check objects in blind spot areas - for (const auto & object : objects.objects) { - if (!isTargetObjectType(object)) { - continue; - } +std::optional +BlindSpotModule::isCollisionDetected(const BlindSpotPolygons & areas) +{ + // TODO(Mamoru Sobue): only do this for target object + autoware_perception_msgs::msg::PredictedObjects objects = + *(planner_data_->predicted_objects); + cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); + + // check objects in blind spot areas + for (const auto & object : objects.objects) { + if (!isTargetObjectType(object)) { + continue; + } - // right direction - const bool exist_in_right_detection_area = - std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) { - return bg::within( - to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), - lanelet::utils::to2D(area)); - }); - // opposite direction - const bool exist_in_opposite_detection_area = std::any_of( - opposite_detection_areas.begin(), opposite_detection_areas.end(), - [&object](const auto & area) { - return bg::within( - to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), - lanelet::utils::to2D(area)); - }); - const bool exist_in_detection_area = - exist_in_right_detection_area || exist_in_opposite_detection_area; - if (!exist_in_detection_area) { - continue; - } - const bool exist_in_right_conflict_area = - isPredictedPathInArea(object, conflict_areas, planner_data_->current_odometry->pose); - const bool exist_in_opposite_conflict_area = isPredictedPathInArea( - object, opposite_conflict_areas, planner_data_->current_odometry->pose); - const bool exist_in_conflict_area = - exist_in_right_conflict_area || exist_in_opposite_conflict_area; - if (exist_in_detection_area && exist_in_conflict_area) { - debug_data_.conflicting_targets.objects.push_back(object); - setObjectsOfInterestData( - object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); - return true; - } + const auto & detection_areas = areas.detection_areas; + const auto & conflict_areas = areas.conflict_areas; + const auto & opposite_detection_areas = areas.opposite_detection_areas; + const auto & opposite_conflict_areas = areas.opposite_conflict_areas; + + // right direction + const bool exist_in_right_detection_area = + std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) { + return bg::within( + to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), + lanelet::utils::to2D(area)); + }); + // opposite direction + const bool exist_in_opposite_detection_area = std::any_of( + opposite_detection_areas.begin(), opposite_detection_areas.end(), + [&object](const auto & area) { + return bg::within( + to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), + lanelet::utils::to2D(area)); + }); + const bool exist_in_detection_area = + exist_in_right_detection_area || exist_in_opposite_detection_area; + if (!exist_in_detection_area) { + continue; + } + const bool exist_in_right_conflict_area = + isPredictedPathInArea(object, conflict_areas, planner_data_->current_odometry->pose); + const bool exist_in_opposite_conflict_area = + isPredictedPathInArea(object, opposite_conflict_areas, planner_data_->current_odometry->pose); + const bool exist_in_conflict_area = + exist_in_right_conflict_area || exist_in_opposite_conflict_area; + if (exist_in_detection_area && exist_in_conflict_area) { + debug_data_.conflicting_targets.objects.push_back(object); + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); + return object; } - return false; - } else { - return false; } + return std::nullopt; } bool BlindSpotModule::isPredictedPathInArea( @@ -618,11 +588,13 @@ lanelet::ConstLanelet BlindSpotModule::generateExtendedOppositeAdjacentLanelet( } std::optional BlindSpotModule::generateBlindSpotPolygons( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - [[maybe_unused]] lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const tier4_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const { + /* get lanelet map */ + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + std::vector lane_ids; lanelet::ConstLanelets blind_spot_lanelets; lanelet::ConstLanelets blind_spot_opposite_lanelets; @@ -741,6 +713,15 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( blind_spot_polygons.opposite_detection_areas.emplace_back( std::move(detection_area_opposite_adj)); } + debug_data_.detection_areas = blind_spot_polygons.detection_areas; + debug_data_.conflict_areas = blind_spot_polygons.conflict_areas; + debug_data_.detection_areas.insert( + debug_data_.detection_areas.end(), blind_spot_polygons.opposite_detection_areas.begin(), + blind_spot_polygons.opposite_detection_areas.end()); + debug_data_.conflict_areas.insert( + debug_data_.conflict_areas.end(), blind_spot_polygons.opposite_conflict_areas.begin(), + blind_spot_polygons.opposite_conflict_areas.end()); + return blind_spot_polygons; } else { return std::nullopt; diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index 843245689ae1e..38646a08e01af 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include namespace behavior_velocity_planner @@ -58,16 +59,40 @@ struct InterpolatedPathInfo std::optional> lane_id_interval{std::nullopt}; }; +/** + * @brief represent action + */ +struct InternalError +{ + const std::string error; +}; + +struct OverPassJudge +{ + const std::string report; +}; + +struct Unsafe +{ + const size_t stop_line_idx; + const std::optional collision_obstacle; +}; + +struct Safe +{ + const size_t stop_line_idx; +}; + +using BlindSpotDecision = std::variant; + class BlindSpotModule : public SceneModuleInterface { public: - enum class TurnDirection { LEFT = 0, RIGHT, INVALID }; + enum class TurnDirection { LEFT, RIGHT }; struct DebugData { - geometry_msgs::msg::Pose virtual_wall_pose; - geometry_msgs::msg::Pose stop_point_pose; - geometry_msgs::msg::Pose judge_point_pose; + std::optional virtual_wall_pose{std::nullopt}; std::vector conflict_areas; std::vector detection_areas; std::vector opposite_conflict_areas; @@ -91,9 +116,9 @@ class BlindSpotModule : public SceneModuleInterface }; BlindSpotModule( - const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, + const std::shared_ptr planner_data, const PlannerParam & planner_param, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path @@ -105,18 +130,39 @@ class BlindSpotModule : public SceneModuleInterface std::vector createVirtualWalls() override; private: + // (semi) const variables const int64_t lane_id_; const PlannerParam planner_param_; - TurnDirection turn_direction_{TurnDirection::INVALID}; - bool is_over_pass_judge_line_{false}; + const TurnDirection turn_direction_; std::optional sibling_straight_lanelet_{std::nullopt}; + // state variables + bool is_over_pass_judge_line_{false}; + // Parameter + void initializeRTCStatus(); + BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + // setSafe(), setDistance() + void setRTCStatus( + const BlindSpotDecision & decision, + const tier4_planning_msgs::msg::PathWithLaneId & path); + template + void setRTCStatusByDecision( + const Decision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); + // stop/GO + void reactRTCApproval( + const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason); + template + void reactRTCApprovalByDecision( + const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason); + std::optional generateInterpolatedPathInfo( const tier4_planning_msgs::msg::PathWithLaneId & input_path) const; - std::optional getSiblingStraightLanelet() const; + std::optional getSiblingStraightLanelet( + const std::shared_ptr planner_data) const; /** * @brief Generate a stop line and insert it into the path. @@ -131,6 +177,10 @@ class BlindSpotModule : public SceneModuleInterface const InterpolatedPathInfo & interpolated_path_info, tier4_planning_msgs::msg::PathWithLaneId * path) const; + std::optional isOverPassJudge( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & stop_point_pose) const; + /** * @brief Check obstacle is in blind spot areas. * Condition1: Object's position is in broad blind spot area. @@ -141,12 +191,8 @@ class BlindSpotModule : public SceneModuleInterface * @param closest_idx closest path point index from ego car in path points * @return true when an object is detected in blind spot */ - bool checkObstacleInBlindSpot( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const tier4_planning_msgs::msg::PathWithLaneId & path, - const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose); + std::optional isCollisionDetected( + const BlindSpotPolygons & area); /** * @brief Create half lanelet @@ -168,9 +214,7 @@ class BlindSpotModule : public SceneModuleInterface * @return Blind spot polygons */ std::optional generateBlindSpotPolygons( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const tier4_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & pose) const; /** diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt index 69dd1ced695ec..f21d5ab786cad 100644 --- a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_crosswalk_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index b69f0856c8020..107a904dd076b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -182,7 +182,7 @@ class CrosswalkModule : public SceneModuleInterface const rclcpp::Time & now, const geometry_msgs::msg::Point & position, const double vel, const bool is_ego_yielding, const std::optional & collision_point, const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon, - const bool is_object_on_ego_path) + const bool is_object_away_from_path) { const bool is_stopped = vel < planner_param.stop_object_velocity; @@ -202,7 +202,7 @@ class CrosswalkModule : public SceneModuleInterface planner_param.timeout_set_for_no_intention_to_walk, distance_to_crosswalk); const bool intent_to_cross = (now - *time_to_start_stopped).seconds() < timeout_no_intention_to_walk; - if (is_ego_yielding && !intent_to_cross && !is_object_on_ego_path) { + if (is_ego_yielding && !intent_to_cross && is_object_away_from_path) { collision_state = CollisionState::IGNORE; return; } @@ -261,15 +261,16 @@ class CrosswalkModule : public SceneModuleInterface // update current uuids current_uuids_.push_back(uuid); - const bool is_object_on_ego_path = - boost::geometry::distance(tier4_autoware_utils::fromMsg(position).to_2d(), attention_area) < - 0.5; + const bool is_object_away_from_path = + !attention_area.outer().empty() && + boost::geometry::distance(tier4_autoware_utils::fromMsg(position).to_2d(), attention_area) > + 0.5; // add new object if (objects.count(uuid) == 0) { if ( has_traffic_light && planner_param.disable_yield_for_new_stopped_object && - !is_object_on_ego_path) { + is_object_away_from_path) { objects.emplace(uuid, ObjectInfo{CollisionState::IGNORE}); } else { objects.emplace(uuid, ObjectInfo{CollisionState::YIELD}); @@ -279,7 +280,7 @@ class CrosswalkModule : public SceneModuleInterface // update object state objects.at(uuid).transitState( now, position, vel, is_ego_yielding, collision_point, planner_param, crosswalk_polygon, - is_object_on_ego_path); + is_object_away_from_path); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; objects.at(uuid).classification = classification; diff --git a/planning/behavior_velocity_detection_area_module/CMakeLists.txt b/planning/behavior_velocity_detection_area_module/CMakeLists.txt index 2a16ed95ba655..e6990d2d28642 100644 --- a/planning/behavior_velocity_detection_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_detection_area_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_detection_area_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt index 20f4b3aa8f4d3..481ac245cb432 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_dynamic_obstacle_stop_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY src diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 07847a08c1209..c02b5a3852722 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_intersection_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) find_package(OpenCV REQUIRED) diff --git a/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt b/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt index ef3073df56c22..265b5b637e7ce 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_no_drivable_lane_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt index 4d14228c4685c..ba3fd0e4f71dd 100644 --- a/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_no_stopping_area_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 704603fe70f38..3f2e7581a4d14 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -86,12 +86,8 @@ boost::optional NoStoppingAreaModule::getStopLineGeometry2d( const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; std::vector collision_points; bg::intersection(area_poly, line, collision_points); - if (collision_points.empty()) { - continue; - } - const double yaw = tier4_autoware_utils::calcAzimuthAngle(p0, p1); if (!collision_points.empty()) { - geometry_msgs::msg::Point left_point; + const double yaw = tier4_autoware_utils::calcAzimuthAngle(p0, p1); const double w = planner_data_->vehicle_info_.vehicle_width_m; const double l = stop_line_margin; stop_line.emplace_back( @@ -304,7 +300,6 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( const int num_ignore_nearest = 1; // Do not consider nearest lane polygon size_t ego_area_start_idx = closest_idx + num_ignore_nearest; - size_t ego_area_end_idx = ego_area_start_idx; // return if area size is not intentional if (no_stopping_area_reg_elem_.noStoppingAreas().size() != 1) { return ego_area; @@ -328,7 +323,7 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( } double dist_from_area_sum = 0.0; // decide end idx with extract distance - ego_area_end_idx = ego_area_start_idx; + size_t ego_area_end_idx = ego_area_start_idx; for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { dist_from_start_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); const auto & p = pp.at(i).point.pose.position; diff --git a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt index 7a062ee854421..8b327291a60f1 100644 --- a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_occlusion_spot_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt index 8ce1a334c4b35..e8191f9b632d5 100644 --- a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_out_of_lane_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY src diff --git a/planning/behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_run_out_module/CMakeLists.txt index 21758c4d0198c..3ee589ba69740 100644 --- a/planning/behavior_velocity_run_out_module/CMakeLists.txt +++ b/planning/behavior_velocity_run_out_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_run_out_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_speed_bump_module/CMakeLists.txt b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt index a96eb3398e5c0..f5d3a7078be9c 100644 --- a/planning/behavior_velocity_speed_bump_module/CMakeLists.txt +++ b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_speed_bump_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_stop_line_module/CMakeLists.txt b/planning/behavior_velocity_stop_line_module/CMakeLists.txt index 3c8cc6a4f9626..bef98aafe6f75 100644 --- a/planning/behavior_velocity_stop_line_module/CMakeLists.txt +++ b/planning/behavior_velocity_stop_line_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_stop_line_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_template_module/CMakeLists.txt b/planning/behavior_velocity_template_module/CMakeLists.txt index e0beb42daf50d..103a3d0a4ceb3 100644 --- a/planning/behavior_velocity_template_module/CMakeLists.txt +++ b/planning/behavior_velocity_template_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_template_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/manager.cpp diff --git a/planning/behavior_velocity_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt index cf9392fa0568f..5737802408996 100644 --- a/planning/behavior_velocity_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_traffic_light_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt index 48ffa35cbae5e..a1c00cf49db29 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_virtual_traffic_light_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/behavior_velocity_walkway_module/CMakeLists.txt b/planning/behavior_velocity_walkway_module/CMakeLists.txt index cd41dec991bc6..351a240743402 100644 --- a/planning/behavior_velocity_walkway_module/CMakeLists.txt +++ b/planning/behavior_velocity_walkway_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(behavior_velocity_walkway_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/debug.cpp diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 95839cdaf5033..5ba152af6611d 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Autoware Foundation +// Copyright 2019-2024 Autoware Foundation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -98,7 +98,7 @@ bool is_in_parking_lot( } double project_goal_to_map( - const lanelet::Lanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point) + const lanelet::ConstLanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point) { const lanelet::ConstLineString3d center_line = lanelet::utils::generateFineCenterline(lanelet_component); @@ -182,12 +182,6 @@ bool DefaultPlanner::ready() const void DefaultPlanner::map_callback(const LaneletMapBin::ConstSharedPtr msg) { route_handler_.setMap(*msg); - lanelet_map_ptr_ = std::make_shared(); - lanelet::utils::conversion::fromBinMsg( - *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); - 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); is_graph_ready_ = true; } @@ -199,7 +193,7 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) for (const auto & route_section : route.segments) { for (const auto & lane_id : route_section.primitives) { - auto lanelet = lanelet_map_ptr_->laneletLayer.get(lane_id.id); + auto lanelet = route_handler_.getLaneletsFromId(lane_id.id); route_lanelets.push_back(lanelet); if (route_section.preferred_primitive.id == lane_id.id) { goal_lanelets.push_back(lanelet); @@ -274,15 +268,15 @@ bool DefaultPlanner::check_goal_footprint_inside_lanes( if (boost::geometry::within(goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon())) { return true; } - + const auto following = route_handler_.getNextLanelets(current_lanelet); // check if goal footprint is in between many lanelets in depth-first search manner - for (const auto & next_lane : routing_graph_ptr_->following(current_lanelet)) { + for (const auto & next_lane : following) { next_lane_length += lanelet::utils::getLaneletLength2d(next_lane); lanelet::ConstLanelets lanelets; lanelets.push_back(combined_prev_lanelet); lanelets.push_back(next_lane); lanelet::ConstLanelet combined_lanelets = - combine_lanelets_with_shoulder(lanelets, shoulder_lanelets_); + combine_lanelets_with_shoulder(lanelets, route_handler_); // if next lanelet length is longer than vehicle longitudinal offset if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) { @@ -317,23 +311,38 @@ bool DefaultPlanner::is_goal_valid( // check if goal is in shoulder lanelet lanelet::Lanelet closest_shoulder_lanelet; + const auto shoulder_lanelets = route_handler_.getShoulderLaneletsAtPose(goal); if (lanelet::utils::query::getClosestLanelet( - shoulder_lanelets_, goal, &closest_shoulder_lanelet)) { - if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) { - const auto lane_yaw = - lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); - const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); - if (std::abs(angle_diff) < th_angle) { - return true; - } + shoulder_lanelets, goal, &closest_shoulder_lanelet)) { + const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); + const auto goal_yaw = tf2::getYaw(goal.orientation); + const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); + const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); + if (std::abs(angle_diff) < th_angle) { + return true; } } - - lanelet::Lanelet closest_lanelet; - if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) { - return false; + lanelet::ConstLanelet closest_lanelet; + const auto road_lanelets_at_goal = route_handler_.getRoadLaneletsAtPose(goal); + if (!lanelet::utils::query::getClosestLanelet(road_lanelets_at_goal, goal, &closest_lanelet)) { + // if no road lanelets directly at the goal, find the closest one + const lanelet::BasicPoint2d goal_point{goal.position.x, goal.position.y}; + auto closest_dist = std::numeric_limits::max(); + const auto closest_road_lanelet_found = + route_handler_.getLaneletMapPtr()->laneletLayer.nearestUntil( + goal_point, [&](const auto & bbox, const auto & ll) { + // this search is done by increasing distance between the bounding box and the goal + // we stop the search when the bounding box is further than the closest dist found + if (lanelet::geometry::distance2d(bbox, goal_point) > closest_dist) + return true; // stop the search + const auto dist = lanelet::geometry::distance2d(goal_point, ll.polygon2d()); + if (route_handler_.isRoadLanelet(ll) && dist < closest_dist) { + closest_dist = dist; + closest_lanelet = ll; + } + return false; // continue the search + }); + if (!closest_road_lanelet_found) return false; } const auto local_vehicle_footprint = vehicle_info_.createFootprint(); @@ -345,7 +354,7 @@ bool DefaultPlanner::is_goal_valid( double next_lane_length = 0.0; // combine calculated route lanelets const lanelet::ConstLanelet combined_prev_lanelet = - combine_lanelets_with_shoulder(path_lanelets, shoulder_lanelets_); + combine_lanelets_with_shoulder(path_lanelets, route_handler_); // check if goal footprint exceeds lane when the goal isn't in parking_lot if ( @@ -353,7 +362,7 @@ bool DefaultPlanner::is_goal_valid( !check_goal_footprint_inside_lanes( closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) && !is_in_parking_lot( - lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_), + lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr()), lanelet::utils::conversion::toLaneletPoint(goal.position))) { RCLCPP_WARN(logger, "Goal's footprint exceeds lane!"); return false; @@ -371,13 +380,15 @@ bool DefaultPlanner::is_goal_valid( } // check if goal is in parking space - const auto parking_spaces = lanelet::utils::query::getAllParkingSpaces(lanelet_map_ptr_); + const auto parking_spaces = + lanelet::utils::query::getAllParkingSpaces(route_handler_.getLaneletMapPtr()); if (is_in_parking_space(parking_spaces, goal_lanelet_pt)) { return true; } // check if goal is in parking lot - const auto parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_); + const auto parking_lots = + lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr()); if (is_in_parking_lot(parking_lots, goal_lanelet_pt)) { return true; } @@ -424,7 +435,8 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) auto goal_pose = points.back(); if (param_.enable_correct_goal_pose) { goal_pose = get_closest_centerline_pose( - lanelet::utils::query::laneletLayer(lanelet_map_ptr_), goal_pose, vehicle_info_); + lanelet::utils::query::laneletLayer(route_handler_.getLaneletMapPtr()), goal_pose, + vehicle_info_); } if (!is_goal_valid(goal_pose, all_route_lanelets)) { @@ -451,7 +463,7 @@ geometry_msgs::msg::Pose DefaultPlanner::refine_goal_height( const Pose & goal, const RouteSections & route_sections) { const auto goal_lane_id = route_sections.back().preferred_primitive.id; - lanelet::Lanelet goal_lanelet = lanelet_map_ptr_->laneletLayer.get(goal_lane_id); + const auto goal_lanelet = route_handler_.getLaneletsFromId(goal_lane_id); const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position); const auto goal_height = project_goal_to_map(goal_lanelet, goal_lanelet_pt); diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index c848482f3eccb..60c232162991d 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -58,11 +58,6 @@ class DefaultPlanner : public mission_planner::PlannerPlugin using RouteSections = std::vector; using Pose = geometry_msgs::msg::Pose; bool is_graph_ready_; - lanelet::LaneletMapPtr lanelet_map_ptr_; - lanelet::routing::RoutingGraphPtr routing_graph_ptr_; - lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; - lanelet::ConstLanelets road_lanelets_; - lanelet::ConstLanelets shoulder_lanelets_; route_handler::RouteHandler route_handler_; DefaultPlannerParameters param_; diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp index 4ef4bd6f20ed0..e7797946aa619 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Autoware Foundation +// Copyright 2019-2024 Autoware Foundation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -42,7 +42,7 @@ void insert_marker_array( } lanelet::ConstLanelet combine_lanelets_with_shoulder( - const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets) + const lanelet::ConstLanelets & lanelets, const route_handler::RouteHandler & route_handler) { lanelet::Points3d lefts; lanelet::Points3d rights; @@ -57,30 +57,18 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder( } } + // lambda to add bound to target_bound + const auto add_bound = [](const auto & bound, auto & target_bound) { + std::transform( + bound.begin(), bound.end(), std::back_inserter(target_bound), + [](const auto & pt) { return lanelet::Point3d(pt); }); + }; for (const auto & llt : lanelets) { - // lambda to check if shoulder lane which share left bound with lanelets exist - const auto find_bound_shared_shoulder = - [&shoulder_lanelets](const auto & lanelet_bound, const auto & get_shoulder_bound) { - return std::find_if( - shoulder_lanelets.begin(), shoulder_lanelets.end(), - [&lanelet_bound, &get_shoulder_bound](const auto & shoulder_llt) { - return lanelet_bound.id() == get_shoulder_bound(shoulder_llt).id(); - }); - }; - - // lambda to add bound to target_bound - const auto add_bound = [](const auto & bound, auto & target_bound) { - std::transform( - bound.begin(), bound.end(), std::back_inserter(target_bound), - [](const auto & pt) { return lanelet::Point3d(pt); }); - }; - // check if shoulder lanelets which has RIGHT bound same to LEFT bound of lanelet exist - const auto left_shared_shoulder_it = find_bound_shared_shoulder( - llt.leftBound(), [](const auto & shoulder_llt) { return shoulder_llt.rightBound(); }); - if (left_shared_shoulder_it != shoulder_lanelets.end()) { + const auto left_shared_shoulder = route_handler.getLeftShoulderLanelet(llt); + if (left_shared_shoulder) { // if exist, add left bound of SHOULDER lanelet to lefts - add_bound(left_shared_shoulder_it->leftBound(), lefts); + add_bound(left_shared_shoulder->leftBound(), lefts); } else if ( // if not exist, add left bound of lanelet to lefts // if the **left** of this lanelet does not match any of the **right** bounds of `lanelets`, @@ -90,11 +78,10 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder( } // check if shoulder lanelets which has LEFT bound same to RIGHT bound of lanelet exist - const auto right_shared_shoulder_it = find_bound_shared_shoulder( - llt.rightBound(), [](const auto & shoulder_llt) { return shoulder_llt.leftBound(); }); - if (right_shared_shoulder_it != shoulder_lanelets.end()) { + const auto right_shared_shoulder = route_handler.getRightShoulderLanelet(llt); + if (right_shared_shoulder) { // if exist, add right bound of SHOULDER lanelet to rights - add_bound(right_shared_shoulder_it->rightBound(), rights); + add_bound(right_shared_shoulder->rightBound(), rights); } else if ( // if not exist, add right bound of lanelet to rights // if the **right** of this lanelet does not match any of the **left** bounds of `lanelets`, diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 3eb38638e17b7..428cd979a2526 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -1,4 +1,4 @@ -// Copyright 2019 Autoware Foundation +// Copyright 2019-2024 Autoware Foundation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,6 +16,7 @@ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #include +#include #include #include @@ -50,10 +51,10 @@ void insert_marker_array( * @brief create a single merged lanelet whose left/right bounds consist of the leftmost/rightmost * bound of the original lanelets or the left/right bound of its adjacent road_shoulder respectively * @param lanelets topologically sorted sequence of lanelets - * @param shoulder_lanelets list of possible road_shoulder lanelets to combine with lanelets + * @param route_handler route handler to query the lanelet map */ lanelet::ConstLanelet combine_lanelets_with_shoulder( - const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets); + const lanelet::ConstLanelets & lanelets, const route_handler::RouteHandler & route_handler); std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet); geometry_msgs::msg::Pose convertBasicPoint3dToPose( diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index f637911dae903..21e3ecb7bf758 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1378,7 +1378,6 @@ void ObstacleCruisePlannerNode::checkConsistency( const auto current_closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles); - // If previous closest obstacle ptr is not set if (!prev_closest_stop_obstacle_ptr_) { if (current_closest_stop_obstacle) { prev_closest_stop_obstacle_ptr_ = @@ -1387,44 +1386,23 @@ void ObstacleCruisePlannerNode::checkConsistency( return; } - // Put previous closest target obstacle if necessary const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), [&](PredictedObject predicted_object) { return tier4_autoware_utils::toHexString(predicted_object.object_id) == prev_closest_stop_obstacle_ptr_->uuid; }); - - // If previous closest obstacle is not in the current perception lists - // just return the current target obstacles + // If previous closest obstacle disappear from the perception result, do nothing anymore. if (predicted_object_itr == predicted_objects.objects.end()) { return; } - // Previous closest obstacle is in the perception lists - const auto obstacle_itr = std::find_if( + const auto is_disappeared_from_stop_obstacle = std::none_of( stop_obstacles.begin(), stop_obstacles.end(), [&](const auto & obstacle) { return obstacle.uuid == prev_closest_stop_obstacle_ptr_->uuid; }); - - // Previous closest obstacle is both in the perception lists and target obstacles - if (obstacle_itr != stop_obstacles.end()) { - if (current_closest_stop_obstacle) { - if ((current_closest_stop_obstacle->uuid == prev_closest_stop_obstacle_ptr_->uuid)) { - // prev_closest_obstacle is current_closest_stop_obstacle just return the target - // obstacles(in target obstacles) - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } else { - // New obstacle becomes new stop obstacle - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } - } else { - // Previous closest stop obstacle becomes cruise obstacle - prev_closest_stop_obstacle_ptr_ = nullptr; - } - } else { - // prev obstacle is not in the target obstacles, but in the perception list + if (is_disappeared_from_stop_obstacle) { + // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" + // condition is satisfied const double elapsed_time = (current_time - prev_closest_stop_obstacle_ptr_->stamp).seconds(); if ( predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < @@ -1433,13 +1411,13 @@ void ObstacleCruisePlannerNode::checkConsistency( stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_); return; } + } - if (current_closest_stop_obstacle) { - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } else { - prev_closest_stop_obstacle_ptr_ = nullptr; - } + if (current_closest_stop_obstacle) { + prev_closest_stop_obstacle_ptr_ = + std::make_shared(*current_closest_stop_obstacle); + } else { + prev_closest_stop_obstacle_ptr_ = nullptr; } } diff --git a/planning/planning_test_utils/CMakeLists.txt b/planning/planning_test_utils/CMakeLists.txt index 56e985c7fa2b7..ce746352bb89c 100644 --- a/planning/planning_test_utils/CMakeLists.txt +++ b/planning/planning_test_utils/CMakeLists.txt @@ -4,6 +4,9 @@ project(planning_test_utils) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_library(planning_test_utils SHARED + src/planning_test_utils.cpp) + ament_auto_add_library(mock_data_parser SHARED src/mock_data_parser.cpp) diff --git a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp index f46b9641de6dd..533c8f5b173df 100644 --- a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp @@ -79,20 +79,248 @@ using tier4_autoware_utils::createQuaternionFromRPY; using tier4_planning_msgs::msg::Scenario; using unique_identifier_msgs::msg::UUID; +/** + * @brief Creates a Pose message with the specified position and orientation. + * + * This function initializes a geometry_msgs::msg::Pose message with the + * given position (x, y, z) and orientation (roll, pitch, yaw). + * + * @param x The x-coordinate of the position. + * @param y The y-coordinate of the position. + * @param z The z-coordinate of the position. + * @param roll The roll component of the orientation (in radians). + * @param pitch The pitch component of the orientation (in radians). + * @param yaw The yaw component of the orientation (in radians). + * @return A geometry_msgs::msg::Pose message with the specified position + * and orientation. + */ geometry_msgs::msg::Pose createPose( - double x, double y, double z, double roll, double pitch, double yaw) -{ - geometry_msgs::msg::Pose p; - p.position = createPoint(x, y, z); - p.orientation = createQuaternionFromRPY(roll, pitch, yaw); - return p; -} - -geometry_msgs::msg::Pose createPose(const std::array & pose3d) -{ - return createPose(pose3d[0], pose3d[1], pose3d[2], 0.0, 0.0, pose3d[3]); -} - + double x, double y, double z, double roll, double pitch, double yaw); + +/** + * @brief Creates a Pose message from a 4-element array representing a 3D pose. + * + * This function initializes a geometry_msgs::msg::Pose message using a + * std::array of four doubles, where the first three elements represent the + * position (x, y, z) and the fourth element represents the yaw orientation. + * The roll and pitch orientations are assumed to be zero. + * + * @param pose3d A std::array of four doubles representing the 3D pose. The + * first element is the x-coordinate, the second is the y-coordinate, the + * third is the z-coordinate, and the fourth is the yaw orientation. + * @return A geometry_msgs::msg::Pose message with the specified position + * and yaw orientation, with roll and pitch set to zero. + */ +geometry_msgs::msg::Pose createPose(const std::array & pose3d); + +/** + * @brief Creates a LaneletSegment with the specified ID. + * + * Initializes a LaneletSegment containing a single LaneletPrimitive with the + * given ID and a primitive type of "lane". + * + * @param id The ID for the LaneletPrimitive and preferred primitive. + * @return A LaneletSegment with the specified ID. + */ +LaneletSegment createLaneletSegment(int id); + +/** + * @brief Loads a Lanelet map from a specified file. + * + * This function loads a Lanelet2 map using the given filename. It uses the MGRS + * projector and checks for any errors during the loading process. If errors + * are found, they are logged, and the function returns nullptr. + * + * @param lanelet2_filename The filename of the Lanelet2 map to load. + * @return A LaneletMapPtr to the loaded map, or nullptr if there were errors. + */ +lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename); + +/** + * @brief Converts a Lanelet map to a LaneletMapBin message. + * + * This function converts a given Lanelet map to a LaneletMapBin message. It also + * parses the format and map versions from the specified filename and includes + * them in the message. The timestamp for the message is set to the provided time. + * + * @param map The Lanelet map to convert. + * @param lanelet2_filename The filename of the Lanelet2 map, used to parse format and map versions. + * @param now The current time to set in the message header. + * @return A LaneletMapBin message containing the converted map data. + */ +LaneletMapBin convertToMapBinMsg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, + const rclcpp::Time & now); + +/** + * @brief Creates a normal Lanelet route with predefined start and goal poses. + * + * This function initializes a LaneletRoute with a predefined start and goal pose. + * + * @return A LaneletRoute with the specified start and goal poses. + */ +LaneletRoute makeNormalRoute(); + +/** + * @brief Creates an OccupancyGrid message with the specified dimensions and resolution. + * + * This function initializes an OccupancyGrid message with given width, height, and resolution. + * All cells in the grid are initialized to zero. + * + * @param width The width of the occupancy grid. + * @param height The height of the occupancy grid. + * @param resolution The resolution of the occupancy grid. + * @return An OccupancyGrid message with the specified dimensions and resolution. + */ +OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double resolution = 0.2); + +/** + * @brief Creates a LaneletMapBin message from a Lanelet map file. + * + * This function loads a Lanelet map from the given file, overwrites the + * centerline with the specified resolution, and converts the map to a LaneletMapBin message. + * + * @param absolute_path The absolute path to the Lanelet2 map file. + * @param center_line_resolution The resolution for the centerline. + * @return A LaneletMapBin message containing the map data. + */ +LaneletMapBin make_map_bin_msg( + const std::string & absolute_path, const double center_line_resolution = 5.0); + +/** + * @brief Creates a LaneletMapBin message using a predefined Lanelet2 map file. + * + * This function loads a lanelet2_map.osm from the test_map folder in the + * planning_test_utils package, overwrites the centerline with a resolution of 5.0, + * and converts the map to a LaneletMapBin message. + * + * @return A LaneletMapBin message containing the map data. + */ +LaneletMapBin makeMapBinMsg(); + +/** + * @brief Creates an Odometry message with a specified shift. + * + * This function initializes an Odometry message with a pose shifted by the given amount at y + * direction. x pose, z pose, and yaw angle remains zero. + * + * @param shift The amount by which to shift the pose. + * @return An Odometry message with the specified shift. + */ +Odometry makeOdometry(const double shift = 0.0); + +/** + * @brief Creates an initial Odometry message with a specified shift. + * + * This function initializes an Odometry message with a pose shifted by the given amount, + * accounting for a specific yaw angle. + * + * @param shift The amount by which to shift the pose. + * @return An Odometry message with the specified shift. + */ +Odometry makeInitialPose(const double shift = 0.0); + +/** + * @brief Creates a TFMessage with the specified frame IDs. + * + * This function initializes a TFMessage with a transform containing the given frame IDs. + * The transform includes a specific translation and rotation. + * + * @param target_node The node to use for obtaining the current time. + * @param frame_id The ID of the parent frame. + * @param child_frame_id The ID of the child frame. + * @return A TFMessage containing the transform. + */ +TFMessage makeTFMsg( + rclcpp::Node::SharedPtr target_node, std::string && frame_id = "", + std::string && child_frame_id = ""); + +/** + * @brief Creates a Scenario message with the specified scenario. + * + * This function initializes a Scenario message with the current scenario and a list of activating + * scenarios. + * + * @param scenario The name of the current scenario. + * @return A Scenario message with the specified scenario. + */ +Scenario makeScenarioMsg(const std::string & scenario); + +/** + * @brief Combines two sets of consecutive RouteSections. + * + * This function combines two sets of RouteSections, removing the overlapping end section from the + * first set. + * + * @param route_sections1 The first set of RouteSections. + * @param route_sections2 The second set of RouteSections. + * @return A combined set of RouteSections. + */ +RouteSections combineConsecutiveRouteSections( + const RouteSections & route_sections1, const RouteSections & route_sections2); + +/** + * @brief Creates a predefined behavior Lanelet route. + * + * This function initializes a LaneletRoute with predefined start and goal poses, + * a list of lanelet segment IDs, and a fixed UUID. + * this is for the test lanelet2_map.osm + * file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 + * + * @return A LaneletRoute with the specified attributes. + */ +LaneletRoute makeBehaviorNormalRoute(); + +/** + * @brief Spins multiple ROS nodes a specified number of times. + * + * This function spins the given test and target nodes for the specified number of iterations. + * + * @param test_node The test node to spin. + * @param target_node The target node to spin. + * @param repeat_count The number of times to spin the nodes. + */ +void spinSomeNodes( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, + const int repeat_count = 1); + +/** + * @brief Updates node options with parameter files. + * + * This function updates the given node options to include the specified parameter files. + * + * @param node_options The node options to update. + * @param params_files A vector of parameter file paths to add to the node options. + */ +void updateNodeOptions( + rclcpp::NodeOptions & node_options, const std::vector & params_files); + +/** + * @brief Loads a PathWithLaneId message from a YAML file. + * + * This function loads a PathWithLaneId message from a YAML file located in the + * planning_test_utils package. + * + * @return A PathWithLaneId message containing the loaded data. + */ +PathWithLaneId loadPathWithLaneIdInYaml(); + +/** + * @brief Generates a trajectory with specified parameters. + * + * This function generates a trajectory of type T with a given number of points, + * point interval, velocity, initial theta, delta theta, and optionally an + * overlapping point index. + * + * @tparam T The type of the trajectory. + * @param num_points The number of points in the trajectory. + * @param point_interval The distance between consecutive points. + * @param velocity The longitudinal velocity for each point. + * @param init_theta The initial theta angle. + * @param delta_theta The change in theta per point. + * @param overlapping_point_index The index of the point to overlap (default is max size_t value). + * @return A trajectory of type T with the specified parameters. + */ template T generateTrajectory( const size_t num_points, const double point_interval, const double velocity = 0.0, @@ -121,197 +349,17 @@ T generateTrajectory( return traj; } -LaneletSegment createLaneletSegment(int id) -{ - LaneletPrimitive primitive; - primitive.id = id; - primitive.primitive_type = "lane"; - LaneletSegment segment; - segment.preferred_primitive.id = id; - segment.primitives.push_back(primitive); - return segment; -} - -lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) -{ - lanelet::ErrorMessages errors{}; - lanelet::projection::MGRSProjector projector{}; - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - if (errors.empty()) { - return map; - } - - for (const auto & error : errors) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); - } - return nullptr; -} - -LaneletMapBin convertToMapBinMsg( - const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) -{ - std::string format_version{}, map_version{}; - lanelet::io_handlers::AutowareOsmParser::parseVersions( - lanelet2_filename, &format_version, &map_version); - - LaneletMapBin map_bin_msg; - map_bin_msg.header.stamp = now; - map_bin_msg.header.frame_id = "map"; - map_bin_msg.version_map_format = format_version; - map_bin_msg.version_map = map_version; - lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); - - return map_bin_msg; -} - -LaneletRoute makeNormalRoute() -{ - const std::array start_pose{5.5, 4., 0., M_PI_2}; - const std::array goal_pose{8.0, 26.3, 0, 0}; - LaneletRoute route; - route.header.frame_id = "map"; - route.start_pose = createPose(start_pose); - route.goal_pose = createPose(goal_pose); - return route; -} - -OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double resolution = 0.2) -{ - nav_msgs::msg::OccupancyGrid costmap_msg; - - // create info - costmap_msg.header.frame_id = "map"; - costmap_msg.info.width = width; - costmap_msg.info.height = height; - costmap_msg.info.resolution = resolution; - - // create data - const size_t n_elem = width * height; - for (size_t i = 0; i < n_elem; ++i) { - costmap_msg.data.push_back(0.0); - } - return costmap_msg; -} - -LaneletMapBin make_map_bin_msg( - const std::string & absolute_path, const double center_line_resolution = 5.0) -{ - const auto map = loadMap(absolute_path); - if (!map) { - return autoware_map_msgs::msg::LaneletMapBin_>{}; - } - - // overwrite centerline - lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); - - // create map bin msg - const auto map_bin_msg = - convertToMapBinMsg(map, absolute_path, rclcpp::Clock(RCL_ROS_TIME).now()); - return map_bin_msg; -} - -LaneletMapBin makeMapBinMsg() -{ - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm"; - double center_line_resolution = 5.0; - - return make_map_bin_msg(lanelet2_path, center_line_resolution); -} - -Odometry makeOdometry(const double shift = 0.0) -{ - Odometry odometry; - const std::array start_pose{0.0, shift, 0.0, 0.0}; - odometry.pose.pose = createPose(start_pose); - odometry.header.frame_id = "map"; - return odometry; -} - -Odometry makeInitialPose(const double shift = 0.0) -{ - Odometry current_odometry; - const auto yaw = 0.9724497591854532; - const auto shift_x = shift * std::sin(yaw); - const auto shift_y = shift * std::cos(yaw); - const std::array start_pose{ - 3722.16015625 + shift_x, 73723.515625 + shift_y, 0.233112560494183, yaw}; - current_odometry.pose.pose = test_utils::createPose(start_pose); - current_odometry.header.frame_id = "map"; - return current_odometry; -} - -TFMessage makeTFMsg( - rclcpp::Node::SharedPtr target_node, std::string frame_id = "", std::string child_frame_id = "") -{ - TFMessage tf_msg; - geometry_msgs::msg::Quaternion quaternion; - quaternion.x = 0.; - quaternion.y = 0.; - quaternion.z = 0.23311256049418302; - quaternion.w = 0.9724497591854532; - - TransformStamped tf; - tf.header.stamp = target_node->get_clock()->now(); - tf.header.frame_id = frame_id; - tf.child_frame_id = child_frame_id; - tf.transform.translation.x = 3722.16015625; - tf.transform.translation.y = 73723.515625; - tf.transform.translation.z = 0; - tf.transform.rotation = quaternion; - - tf_msg.transforms.emplace_back(std::move(tf)); - return tf_msg; -} - -Scenario makeScenarioMsg(const std::string scenario) -{ - Scenario scenario_msg; - scenario_msg.current_scenario = scenario; - scenario_msg.activating_scenarios = {scenario}; - return scenario_msg; -} - -RouteSections combineConsecutiveRouteSections( - const RouteSections & route_sections1, const RouteSections & route_sections2) -{ - RouteSections route_sections; - route_sections.reserve(route_sections1.size() + route_sections2.size()); - if (!route_sections1.empty()) { - // remove end route section because it is overlapping with first one in next route_section - route_sections.insert(route_sections.end(), route_sections1.begin(), route_sections1.end() - 1); - } - if (!route_sections2.empty()) { - route_sections.insert(route_sections.end(), route_sections2.begin(), route_sections2.end()); - } - return route_sections; -} - -// this is for the test lanelet2_map.osm -// file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 -LaneletRoute makeBehaviorNormalRoute() -{ - LaneletRoute route; - route.header.frame_id = "map"; - route.start_pose = - createPose({3722.16015625, 73723.515625, 0.233112560494183, 0.9724497591854532}); - route.goal_pose = - createPose({3778.362060546875, 73721.2734375, -0.5107480274693206, 0.8597304533609347}); - - std::vector primitive_ids = {9102, 9178, 54, 112}; - for (int id : primitive_ids) { - route.segments.push_back(createLaneletSegment(id)); - } - - std::array uuid_bytes{210, 87, 16, 126, 98, 151, 58, 28, - 252, 221, 230, 92, 122, 170, 46, 6}; - route.uuid.uuid = uuid_bytes; - - route.allow_modification = false; - return route; -} - +/** + * @brief Creates a publisher with appropriate QoS settings. + * + * This function creates a publisher for a given topic name and message type with appropriate + * QoS settings, depending on the message type. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param topic_name The name of the topic to publish to. + * @param publisher A reference to the publisher to be created. + */ template void createPublisherWithQoS( rclcpp::Node::SharedPtr test_node, std::string topic_name, @@ -329,6 +377,16 @@ void createPublisherWithQoS( } } +/** + * @brief Sets up a publisher for a given topic. + * + * This function sets up a publisher for a given topic using createPublisherWithQoS. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param topic_name The name of the topic to publish to. + * @param publisher A reference to the publisher to be set. + */ template void setPublisher( rclcpp::Node::SharedPtr test_node, std::string topic_name, @@ -337,6 +395,18 @@ void setPublisher( createPublisherWithQoS(test_node, topic_name, publisher); } +/** + * @brief Creates a subscription with appropriate QoS settings. + * + * This function creates a subscription for a given topic name and message type with appropriate + * QoS settings, depending on the message type. + * + * @tparam T The type of the message to subscribe to. + * @param test_node The node to create the subscription on. + * @param topic_name The name of the topic to subscribe to. + * @param callback The callback function to call when a message is received. + * @param subscriber A reference to the subscription to be created. + */ template void createSubscription( rclcpp::Node::SharedPtr test_node, std::string topic_name, @@ -350,6 +420,17 @@ void createSubscription( } } +/** + * @brief Sets up a subscriber for a given topic. + * + * This function sets up a subscriber for a given topic using createSubscription. + * + * @tparam T The type of the message to subscribe to. + * @param test_node The node to create the subscription on. + * @param topic_name The name of the topic to subscribe to. + * @param subscriber A reference to the subscription to be set. + * @param count A reference to a counter that increments on message receipt. + */ template void setSubscriber( rclcpp::Node::SharedPtr test_node, std::string topic_name, @@ -359,32 +440,32 @@ void setSubscriber( test_node, topic_name, [&count](const typename T::ConstSharedPtr) { count++; }, subscriber); } -void spinSomeNodes( - rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, - const int repeat_count = 1) -{ - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(test_node); - executor.add_node(target_node); - for (int i = 0; i < repeat_count; i++) { - executor.spin_some(std::chrono::milliseconds(100)); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } -} - +/** + * @brief Publishes data to a target node. + * + * This function publishes data to a target node on a given topic, ensuring that the topic has + * subscribers and retrying a specified number of times. + * + * @tparam T The type of the message to publish. + * @param test_node The node to create the publisher on. + * @param target_node The target node to publish the message to. + * @param topic_name The name of the topic to publish to. + * @param publisher A shared pointer to the publisher. + * @param data The data to publish. + * @param repeat_count The number of times to retry publishing. + */ template void publishToTargetNode( rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, std::string topic_name, typename rclcpp::Publisher::SharedPtr publisher, T data, const int repeat_count = 3) { if (topic_name.empty()) { - int status; + int status = 1; char * demangled_name = abi::__cxa_demangle(typeid(data).name(), nullptr, nullptr, &status); if (status == 0) { throw std::runtime_error(std::string("Topic name for ") + demangled_name + " is empty"); - } else { - throw std::runtime_error(std::string("Topic name for ") + typeid(data).name() + " is empty"); } + throw std::runtime_error(std::string("Topic name for ") + typeid(data).name() + " is empty"); } test_utils::setPublisher(test_node, topic_name, publisher); @@ -396,83 +477,6 @@ void publishToTargetNode( test_utils::spinSomeNodes(test_node, target_node, repeat_count); } -void updateNodeOptions( - rclcpp::NodeOptions & node_options, const std::vector & params_files) -{ - std::vector arguments; - arguments.push_back("--ros-args"); - arguments.push_back("--params-file"); - for (const auto & param_file : params_files) { - arguments.push_back(param_file.c_str()); - arguments.push_back("--params-file"); - } - arguments.pop_back(); - - node_options.arguments(std::vector{arguments.begin(), arguments.end()}); -} - -PathWithLaneId loadPathWithLaneIdInYaml() -{ - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto yaml_path = planning_test_utils_dir + "/config/path_with_lane_id_data.yaml"; - YAML::Node yaml_node = YAML::LoadFile(yaml_path); - PathWithLaneId path_msg; - - // Convert YAML data to PathWithLaneId message - // Fill the header - path_msg.header.stamp.sec = yaml_node["header"]["stamp"]["sec"].as(); - path_msg.header.stamp.nanosec = yaml_node["header"]["stamp"]["nanosec"].as(); - path_msg.header.frame_id = yaml_node["header"]["frame_id"].as(); - - // Fill the points - for (const auto & point_node : yaml_node["points"]) { - PathPointWithLaneId point; - // Fill the PathPoint data - point.point.pose.position.x = point_node["point"]["pose"]["position"]["x"].as(); - point.point.pose.position.y = point_node["point"]["pose"]["position"]["y"].as(); - point.point.pose.position.z = point_node["point"]["pose"]["position"]["z"].as(); - point.point.pose.orientation.x = point_node["point"]["pose"]["orientation"]["x"].as(); - point.point.pose.orientation.y = point_node["point"]["pose"]["orientation"]["y"].as(); - point.point.pose.orientation.z = point_node["point"]["pose"]["orientation"]["z"].as(); - point.point.pose.orientation.w = point_node["point"]["pose"]["orientation"]["w"].as(); - point.point.longitudinal_velocity_mps = - point_node["point"]["longitudinal_velocity_mps"].as(); - point.point.lateral_velocity_mps = point_node["point"]["lateral_velocity_mps"].as(); - point.point.heading_rate_rps = point_node["point"]["heading_rate_rps"].as(); - point.point.is_final = point_node["point"]["is_final"].as(); - // Fill the lane_ids - for (const auto & lane_id_node : point_node["lane_ids"]) { - point.lane_ids.push_back(lane_id_node.as()); - } - - path_msg.points.push_back(point); - } - - // Fill the left_bound - for (const auto & point_node : yaml_node["left_bound"]) { - Point point; - // Fill the Point data (left_bound) - point.x = point_node["x"].as(); - point.y = point_node["y"].as(); - point.z = point_node["z"].as(); - - path_msg.left_bound.push_back(point); - } - - // Fill the right_bound - for (const auto & point_node : yaml_node["right_bound"]) { - Point point; - // Fill the Point data - point.x = point_node["x"].as(); - point.y = point_node["y"].as(); - point.z = point_node["z"].as(); - - path_msg.right_bound.push_back(point); - } - return path_msg; -} - } // namespace test_utils #endif // PLANNING_TEST_UTILS__PLANNING_TEST_UTILS_HPP_ diff --git a/planning/planning_test_utils/src/planning_test_utils.cpp b/planning/planning_test_utils/src/planning_test_utils.cpp new file mode 100644 index 0000000000000..170777d5e2147 --- /dev/null +++ b/planning/planning_test_utils/src/planning_test_utils.cpp @@ -0,0 +1,313 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "planning_test_utils/planning_test_utils.hpp" +namespace test_utils +{ + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = createPoint(x, y, z); + p.orientation = createQuaternionFromRPY(roll, pitch, yaw); + return p; +} + +geometry_msgs::msg::Pose createPose(const std::array & pose3d) +{ + return createPose(pose3d[0], pose3d[1], pose3d[2], 0.0, 0.0, pose3d[3]); +} + +LaneletSegment createLaneletSegment(int id) +{ + LaneletPrimitive primitive; + primitive.id = id; + primitive.primitive_type = "lane"; + LaneletSegment segment; + segment.preferred_primitive.id = id; + segment.primitives.push_back(primitive); + return segment; +} + +lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) +{ + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); + if (errors.empty()) { + return map; + } + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + } + return nullptr; +} + +LaneletMapBin convertToMapBinMsg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) +{ + std::string format_version{}; + std::string map_version{}; + lanelet::io_handlers::AutowareOsmParser::parseVersions( + lanelet2_filename, &format_version, &map_version); + + LaneletMapBin map_bin_msg; + map_bin_msg.header.stamp = now; + map_bin_msg.header.frame_id = "map"; + map_bin_msg.version_map_format = format_version; + map_bin_msg.version_map = map_version; + lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); + + return map_bin_msg; +} + +LaneletRoute makeNormalRoute() +{ + const std::array start_pose{5.5, 4., 0., M_PI_2}; + const std::array goal_pose{8.0, 26.3, 0, 0}; + LaneletRoute route; + route.header.frame_id = "map"; + route.start_pose = createPose(start_pose); + route.goal_pose = createPose(goal_pose); + return route; +} + +OccupancyGrid makeCostMapMsg(size_t width, size_t height, double resolution) +{ + nav_msgs::msg::OccupancyGrid costmap_msg; + + // create info + costmap_msg.header.frame_id = "map"; + costmap_msg.info.width = width; + costmap_msg.info.height = height; + costmap_msg.info.resolution = static_cast(resolution); + + // create data + const size_t n_elem = width * height; + for (size_t i = 0; i < n_elem; ++i) { + costmap_msg.data.push_back(0.0); + } + return costmap_msg; +} + +LaneletMapBin make_map_bin_msg(const std::string & absolute_path, const double center_line_resolution) +{ + const auto map = loadMap(absolute_path); + if (!map) { + return autoware_map_msgs::msg::LaneletMapBin_>{}; + } + + // overwrite centerline + lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); + + // create map bin msg + const auto map_bin_msg = + convertToMapBinMsg(map, absolute_path, rclcpp::Clock(RCL_ROS_TIME).now()); + return map_bin_msg; +} + +LaneletMapBin makeMapBinMsg() +{ + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto lanelet2_path = planning_test_utils_dir + "/test_map/lanelet2_map.osm"; + double center_line_resolution = 5.0; + + return make_map_bin_msg(lanelet2_path, center_line_resolution); +} + +Odometry makeOdometry(const double shift) +{ + Odometry odometry; + const std::array start_pose{0.0, shift, 0.0, 0.0}; + odometry.pose.pose = createPose(start_pose); + odometry.header.frame_id = "map"; + return odometry; +} + +Odometry makeInitialPose(const double shift) +{ + Odometry current_odometry; + const auto yaw = 0.9724497591854532; + const auto shift_x = shift * std::sin(yaw); + const auto shift_y = shift * std::cos(yaw); + const std::array start_pose{ + 3722.16015625 + shift_x, 73723.515625 + shift_y, 0.233112560494183, yaw}; + current_odometry.pose.pose = test_utils::createPose(start_pose); + current_odometry.header.frame_id = "map"; + return current_odometry; +} + +TFMessage makeTFMsg( + rclcpp::Node::SharedPtr target_node, std::string && frame_id, std::string && child_frame_id) +{ + TFMessage tf_msg; + geometry_msgs::msg::Quaternion quaternion; + quaternion.x = 0.; + quaternion.y = 0.; + quaternion.z = 0.23311256049418302; + quaternion.w = 0.9724497591854532; + + TransformStamped tf; + tf.header.stamp = target_node->get_clock()->now(); + tf.header.frame_id = frame_id; + tf.child_frame_id = child_frame_id; + tf.transform.translation.x = 3722.16015625; + tf.transform.translation.y = 73723.515625; + tf.transform.translation.z = 0; + tf.transform.rotation = quaternion; + + tf_msg.transforms.emplace_back(std::move(tf)); + return tf_msg; +} + +Scenario makeScenarioMsg(const std::string & scenario) +{ + Scenario scenario_msg; + scenario_msg.current_scenario = scenario; + scenario_msg.activating_scenarios = {scenario}; + return scenario_msg; +} + +RouteSections combineConsecutiveRouteSections( + const RouteSections & route_sections1, const RouteSections & route_sections2) +{ + RouteSections route_sections; + route_sections.reserve(route_sections1.size() + route_sections2.size()); + if (!route_sections1.empty()) { + // remove end route section because it is overlapping with first one in next route_section + route_sections.insert(route_sections.end(), route_sections1.begin(), route_sections1.end() - 1); + } + if (!route_sections2.empty()) { + route_sections.insert(route_sections.end(), route_sections2.begin(), route_sections2.end()); + } + return route_sections; +} + +// this is for the test lanelet2_map.osm +// file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 +LaneletRoute makeBehaviorNormalRoute() +{ + LaneletRoute route; + route.header.frame_id = "map"; + route.start_pose = + createPose({3722.16015625, 73723.515625, 0.233112560494183, 0.9724497591854532}); + route.goal_pose = + createPose({3778.362060546875, 73721.2734375, -0.5107480274693206, 0.8597304533609347}); + + std::vector primitive_ids = {9102, 9178, 54, 112}; + for (int id : primitive_ids) { + route.segments.push_back(createLaneletSegment(id)); + } + + std::array uuid_bytes{210, 87, 16, 126, 98, 151, 58, 28, + 252, 221, 230, 92, 122, 170, 46, 6}; + route.uuid.uuid = uuid_bytes; + + route.allow_modification = false; + return route; +} + +void spinSomeNodes( + rclcpp::Node::SharedPtr test_node, rclcpp::Node::SharedPtr target_node, const int repeat_count) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node); + executor.add_node(target_node); + for (int i = 0; i < repeat_count; i++) { + executor.spin_some(std::chrono::milliseconds(100)); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } +} + +void updateNodeOptions( + rclcpp::NodeOptions & node_options, const std::vector & params_files) +{ + std::vector arguments; + arguments.push_back("--ros-args"); + arguments.push_back("--params-file"); + for (const auto & param_file : params_files) { + arguments.push_back(param_file.c_str()); + arguments.push_back("--params-file"); + } + arguments.pop_back(); + + node_options.arguments(std::vector{arguments.begin(), arguments.end()}); +} + +PathWithLaneId loadPathWithLaneIdInYaml() +{ + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto yaml_path = planning_test_utils_dir + "/config/path_with_lane_id_data.yaml"; + YAML::Node yaml_node = YAML::LoadFile(yaml_path); + PathWithLaneId path_msg; + + // Convert YAML data to PathWithLaneId message + // Fill the header + path_msg.header.stamp.sec = yaml_node["header"]["stamp"]["sec"].as(); + path_msg.header.stamp.nanosec = yaml_node["header"]["stamp"]["nanosec"].as(); + path_msg.header.frame_id = yaml_node["header"]["frame_id"].as(); + + // Fill the points + for (const auto & point_node : yaml_node["points"]) { + PathPointWithLaneId point; + // Fill the PathPoint data + point.point.pose.position.x = point_node["point"]["pose"]["position"]["x"].as(); + point.point.pose.position.y = point_node["point"]["pose"]["position"]["y"].as(); + point.point.pose.position.z = point_node["point"]["pose"]["position"]["z"].as(); + point.point.pose.orientation.x = point_node["point"]["pose"]["orientation"]["x"].as(); + point.point.pose.orientation.y = point_node["point"]["pose"]["orientation"]["y"].as(); + point.point.pose.orientation.z = point_node["point"]["pose"]["orientation"]["z"].as(); + point.point.pose.orientation.w = point_node["point"]["pose"]["orientation"]["w"].as(); + point.point.longitudinal_velocity_mps = + point_node["point"]["longitudinal_velocity_mps"].as(); + point.point.lateral_velocity_mps = point_node["point"]["lateral_velocity_mps"].as(); + point.point.heading_rate_rps = point_node["point"]["heading_rate_rps"].as(); + point.point.is_final = point_node["point"]["is_final"].as(); + // Fill the lane_ids + for (const auto & lane_id_node : point_node["lane_ids"]) { + point.lane_ids.push_back(lane_id_node.as()); + } + + path_msg.points.push_back(point); + } + + // Fill the left_bound + for (const auto & point_node : yaml_node["left_bound"]) { + Point point; + // Fill the Point data (left_bound) + point.x = point_node["x"].as(); + point.y = point_node["y"].as(); + point.z = point_node["z"].as(); + + path_msg.left_bound.push_back(point); + } + + // Fill the right_bound + for (const auto & point_node : yaml_node["right_bound"]) { + Point point; + // Fill the Point data + point.x = point_node["x"].as(); + point.y = point_node["y"].as(); + point.z = point_node["z"].as(); + + path_msg.right_bound.push_back(point); + } + return path_msg; +} + +} // namespace test_utils diff --git a/planning/planning_test_utils/test/test_mock_data_parser.cpp b/planning/planning_test_utils/test/test_mock_data_parser.cpp index 4582762ac53c7..cda09522d8e6e 100644 --- a/planning/planning_test_utils/test/test_mock_data_parser.cpp +++ b/planning/planning_test_utils/test/test_mock_data_parser.cpp @@ -80,11 +80,12 @@ TEST(ParseFunctions, CompleteYAMLTest) // Test parsing of segments std::vector segments = parse_segments(config["segments"]); - ASSERT_EQ(segments.size(), 1); // Assuming only one segment in the provided YAML for this test + ASSERT_EQ( + segments.size(), uint64_t(1)); // Assuming only one segment in the provided YAML for this test const auto & segment0 = segments[0]; EXPECT_EQ(segment0.preferred_primitive.id, 11); - EXPECT_EQ(segment0.primitives.size(), 2); + EXPECT_EQ(segment0.primitives.size(), uint64_t(2)); EXPECT_EQ(segment0.primitives[0].id, 22); EXPECT_EQ(segment0.primitives[0].primitive_type, "lane"); EXPECT_EQ(segment0.primitives[1].id, 33); @@ -118,10 +119,10 @@ TEST(ParseFunction, CompleteFromFilename) ASSERT_EQ( lanelet_route.segments.size(), - 2); // Assuming only one segment in the provided YAML for this test + uint64_t(2)); // Assuming only one segment in the provided YAML for this test const auto & segment1 = lanelet_route.segments[1]; EXPECT_EQ(segment1.preferred_primitive.id, 44); - EXPECT_EQ(segment1.primitives.size(), 4); + EXPECT_EQ(segment1.primitives.size(), uint64_t(4)); EXPECT_EQ(segment1.primitives[0].id, 55); EXPECT_EQ(segment1.primitives[0].primitive_type, "lane"); EXPECT_EQ(segment1.primitives[1].id, 66); diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 7180812c7500b..7ab796321e041 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -84,6 +84,7 @@ class RTCInterface rclcpp::Service::SharedPtr srv_auto_mode_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Clock::SharedPtr clock_; rclcpp::Logger logger_; Module module_; diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index 623f899f55c70..5e4b202071e49 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -81,7 +81,8 @@ Module getModuleType(const std::string & module_name) namespace rtc_interface { RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc) -: logger_{node->get_logger().get_child("RTCInterface[" + name + "]")}, +: clock_{node->get_clock()}, + logger_{node->get_logger().get_child("RTCInterface[" + name + "]")}, is_auto_mode_enabled_{!enable_rtc}, is_locked_{false} { @@ -270,9 +271,7 @@ void RTCInterface::removeExpiredCooperateStatus() std::lock_guard lock(mutex_); const auto itr = std::remove_if( registered_status_.statuses.begin(), registered_status_.statuses.end(), - [](const auto & status) { - return (rclcpp::Clock{RCL_ROS_TIME}.now() - status.stamp).seconds() > 10.0; - }); + [this](const auto & status) { return (clock_->now() - status.stamp).seconds() > 10.0; }); registered_status_.statuses.erase(itr, registered_status_.statuses.end()); } diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 982112189412e..7975937f5b2c1 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -5,7 +5,9 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED + src/diagnostics.cpp src/fail_safe.cpp + src/heartbeat.cpp src/interface.cpp src/localization.cpp src/motion.cpp @@ -22,7 +24,9 @@ ament_auto_add_library(${PROJECT_NAME} SHARED rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::AutowareStateNode" + "default_ad_api::DiagnosticsNode" "default_ad_api::FailSafeNode" + "default_ad_api::HeartbeatNode" "default_ad_api::InterfaceNode" "default_ad_api::LocalizationNode" "default_ad_api::MotionNode" diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index cf8fbe7d001bf..ca1575a528db2 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -42,7 +42,9 @@ def get_default_config(): def generate_launch_description(): components = [ create_api_node("autoware_state", "AutowareStateNode"), + create_api_node("diagnostics", "DiagnosticsNode"), create_api_node("fail_safe", "FailSafeNode"), + create_api_node("heartbeat", "HeartbeatNode"), create_api_node("interface", "InterfaceNode"), create_api_node("localization", "LocalizationNode"), create_api_node("motion", "MotionNode"), diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index cadba99a22c47..77276c7acb0e3 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -20,6 +20,7 @@ autoware_vehicle_msgs component_interface_specs component_interface_utils + diagnostic_graph_utils geographic_msgs geography_utils motion_utils diff --git a/system/default_ad_api/src/diagnostics.cpp b/system/default_ad_api/src/diagnostics.cpp new file mode 100644 index 0000000000000..f4e58b5a4d441 --- /dev/null +++ b/system/default_ad_api/src/diagnostics.cpp @@ -0,0 +1,81 @@ +// Copyright 2024 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 "diagnostics.hpp" + +#include +#include + +namespace default_ad_api +{ + +DiagnosticsNode::DiagnosticsNode(const rclcpp::NodeOptions & options) : Node("diagnostics", options) +{ + using std::placeholders::_1; + + pub_struct_ = create_publisher( + "/api/system/diagnostics/struct", rclcpp::QoS(1).transient_local()); + pub_status_ = create_publisher( + "/api/system/diagnostics/status", rclcpp::QoS(1)); + + sub_graph_.register_create_callback(std::bind(&DiagnosticsNode::on_create, this, _1)); + sub_graph_.register_update_callback(std::bind(&DiagnosticsNode::on_update, this, _1)); + sub_graph_.subscribe(*this, 10); +} +void DiagnosticsNode::on_create(DiagGraph::ConstSharedPtr graph) +{ + const auto & units = graph->units(); + const auto & links = graph->links(); + + std::unordered_map unit_indices_; + for (size_t i = 0; i < units.size(); ++i) { + unit_indices_[units[i]] = i; + } + + autoware_adapi_v1_msgs::msg::DiagGraphStruct msg; + msg.stamp = graph->created_stamp(); + msg.id = graph->id(); + msg.nodes.reserve(units.size()); + msg.links.reserve(links.size()); + for (const auto & unit : units) { + msg.nodes.emplace_back(); + msg.nodes.back().path = unit->path(); + } + for (const auto & link : links) { + msg.links.emplace_back(); + msg.links.back().parent = unit_indices_.at(link->parent()); + msg.links.back().child = unit_indices_.at(link->child()); + } + pub_struct_->publish(msg); +} + +void DiagnosticsNode::on_update(DiagGraph::ConstSharedPtr graph) +{ + const auto & units = graph->units(); + + autoware_adapi_v1_msgs::msg::DiagGraphStatus msg; + msg.stamp = graph->updated_stamp(); + msg.id = graph->id(); + msg.nodes.reserve(units.size()); + for (const auto & unit : units) { + msg.nodes.emplace_back(); + msg.nodes.back().level = unit->level(); + } + pub_status_->publish(msg); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::DiagnosticsNode) diff --git a/system/default_ad_api/src/diagnostics.hpp b/system/default_ad_api/src/diagnostics.hpp new file mode 100644 index 0000000000000..302ffe39159df --- /dev/null +++ b/system/default_ad_api/src/diagnostics.hpp @@ -0,0 +1,46 @@ +// Copyright 2024 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 DIAGNOSTICS_HPP_ +#define DIAGNOSTICS_HPP_ + +#include "diagnostic_graph_utils/subscription.hpp" + +#include + +#include +#include + +namespace default_ad_api +{ + +class DiagnosticsNode : public rclcpp::Node +{ +public: + explicit DiagnosticsNode(const rclcpp::NodeOptions & options); + +private: + using DiagGraph = diagnostic_graph_utils::DiagGraph; + using DiagUnit = diagnostic_graph_utils::DiagUnit; + using DiagLink = diagnostic_graph_utils::DiagLink; + void on_create(DiagGraph::ConstSharedPtr graph); + void on_update(DiagGraph::ConstSharedPtr graph); + rclcpp::Publisher::SharedPtr pub_struct_; + rclcpp::Publisher::SharedPtr pub_status_; + diagnostic_graph_utils::DiagGraphSubscription sub_graph_; +}; + +} // namespace default_ad_api + +#endif // DIAGNOSTICS_HPP_ diff --git a/system/default_ad_api/src/heartbeat.cpp b/system/default_ad_api/src/heartbeat.cpp new file mode 100644 index 0000000000000..4550302bb8bae --- /dev/null +++ b/system/default_ad_api/src/heartbeat.cpp @@ -0,0 +1,42 @@ +// Copyright 2024 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 "heartbeat.hpp" + +#include + +namespace default_ad_api +{ + +HeartbeatNode::HeartbeatNode(const rclcpp::NodeOptions & options) : Node("heartbeat", options) +{ + // Move this function so that the timer no longer holds it as a reference. + const auto on_timer = [this]() { + autoware_ad_api::system::Heartbeat::Message heartbeat; + heartbeat.stamp = now(); + heartbeat.seq = ++sequence_; // Wraps at 65535. + pub_->publish(heartbeat); + }; + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(pub_); + + const auto period = rclcpp::Rate(10.0).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period, std::move(on_timer)); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::HeartbeatNode) diff --git a/localization/geo_pose_projector/src/geo_pose_projector_node.cpp b/system/default_ad_api/src/heartbeat.hpp similarity index 52% rename from localization/geo_pose_projector/src/geo_pose_projector_node.cpp rename to system/default_ad_api/src/heartbeat.hpp index 97367d6b9f774..d922b88489639 100644 --- a/localization/geo_pose_projector/src/geo_pose_projector_node.cpp +++ b/system/default_ad_api/src/heartbeat.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Autoware Foundation +// Copyright 2024 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. @@ -12,18 +12,29 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "geo_pose_projector.hpp" +#ifndef HEARTBEAT_HPP_ +#define HEARTBEAT_HPP_ +#include #include -#include +// This file should be included after messages. +#include "utils/types.hpp" -int main(int argc, char ** argv) +namespace default_ad_api { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); +class HeartbeatNode : public rclcpp::Node +{ +public: + explicit HeartbeatNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::TimerBase::SharedPtr timer_; + Pub pub_; + uint16_t sequence_ = 0; +}; + +} // namespace default_ad_api - return 0; -} +#endif // HEARTBEAT_HPP_ diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index 829585ed4b8b4..d4b430ecd0579 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -39,18 +39,15 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) adaptor.init_cli(cli_mode_, group_cli_); adaptor.init_cli(cli_control_, group_cli_); - const std::vector module_names = { - "sensing", "perception", "map", "localization", "planning", "control", "vehicle", "system", + const auto name = "/system/operation_mode/availability"; + const auto qos = rclcpp::QoS(1); + const auto callback = [this](const OperationModeAvailability::ConstSharedPtr msg) { + mode_available_[OperationModeState::Message::STOP] = msg->stop; + mode_available_[OperationModeState::Message::AUTONOMOUS] = msg->autonomous; + mode_available_[OperationModeState::Message::LOCAL] = msg->local; + mode_available_[OperationModeState::Message::REMOTE] = msg->remote; }; - - 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, module_names](const ModeChangeAvailable::ConstSharedPtr msg) { - module_states_[module_names[i]] = msg->available; - }; - sub_module_states_.push_back(create_subscription(name, qos, callback)); - } + sub_availability_ = create_subscription(name, qos, callback); timer_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(5.0).period(), std::bind(&OperationModeNode::on_timer, this)); @@ -60,8 +57,8 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) mode_available_[OperationModeState::Message::UNKNOWN] = false; mode_available_[OperationModeState::Message::STOP] = true; mode_available_[OperationModeState::Message::AUTONOMOUS] = false; - mode_available_[OperationModeState::Message::LOCAL] = true; - mode_available_[OperationModeState::Message::REMOTE] = true; + mode_available_[OperationModeState::Message::LOCAL] = false; + mode_available_[OperationModeState::Message::REMOTE] = false; } template @@ -135,23 +132,6 @@ 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_) { - 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 1830b7041b566..7cd609b5eb852 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/default_ad_api/src/operation_mode.hpp @@ -25,7 +25,7 @@ #include // TODO(Takagi, Isamu): define interface -#include +#include // This file should be included after messages. #include "utils/types.hpp" @@ -47,7 +47,7 @@ class OperationModeNode : public rclcpp::Node using ChangeToRemote = autoware_ad_api::operation_mode::ChangeToRemote; using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; using AutowareControlRequest = system_interface::ChangeAutowareControl::Service::Request; - using ModeChangeAvailable = tier4_system_msgs::msg::ModeChangeAvailable; + using OperationModeAvailability = tier4_system_msgs::msg::OperationModeAvailability; OperationModeState::Message curr_state_; OperationModeState::Message prev_state_; @@ -65,9 +65,7 @@ class OperationModeNode : public rclcpp::Node Sub sub_state_; Cli cli_mode_; Cli cli_control_; - - std::unordered_map module_states_; - std::vector::SharedPtr> sub_module_states_; + rclcpp::Subscription::SharedPtr sub_availability_; void on_change_to_stop( const ChangeToStop::Service::Request::SharedPtr req, diff --git a/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp b/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp index b2c6fc752e463..c5b386dbe2c86 100644 --- a/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp +++ b/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp @@ -55,12 +55,20 @@ class DiagLink public: using DiagLinkStruct = tier4_system_msgs::msg::DiagLinkStruct; using DiagLinkStatus = tier4_system_msgs::msg::DiagLinkStatus; - explicit DiagLink(const DiagLinkStruct & msg) : struct_(msg) {} + DiagLink(const DiagLinkStruct & msg, DiagUnit * parent, DiagUnit * child) : struct_(msg) + { + parent_ = parent; + child_ = child; + } void update(const DiagLinkStatus & msg) { status_ = msg; } + DiagUnit * parent() const { return parent_; } + DiagUnit * child() const { return child_; } private: DiagLinkStruct struct_; DiagLinkStatus status_; + DiagUnit * parent_; + DiagUnit * child_; }; class DiagNode : public DiagUnit @@ -114,6 +122,7 @@ class DiagGraph bool update(const DiagGraphStatus & msg); rclcpp::Time created_stamp() const { return created_stamp_; } rclcpp::Time updated_stamp() const { return updated_stamp_; } + std::string id() const { return id_; } std::vector units() const; std::vector nodes() const; std::vector diags() const; diff --git a/system/diagnostic_graph_utils/src/lib/graph.cpp b/system/diagnostic_graph_utils/src/lib/graph.cpp index f1177c1047bdb..bd4c215a346ed 100644 --- a/system/diagnostic_graph_utils/src/lib/graph.cpp +++ b/system/diagnostic_graph_utils/src/lib/graph.cpp @@ -54,7 +54,7 @@ void DiagGraph::create(const DiagGraphStruct & msg) for (const auto & data : msg.links) { DiagNode * parent = nodes_.at(data.parent).get(); DiagUnit * child = get_child(data.is_leaf, data.child); - const auto link = links_.emplace_back(std::make_unique(data)).get(); + const auto link = links_.emplace_back(std::make_unique(data, parent, child)).get(); parent->add_child({link, child}); } } diff --git a/system/system_diagnostic_monitor/config/map.yaml b/system/system_diagnostic_monitor/config/map.yaml index 45c1db294119f..231ac6eb5fa44 100644 --- a/system/system_diagnostic_monitor/config/map.yaml +++ b/system/system_diagnostic_monitor/config/map.yaml @@ -13,4 +13,4 @@ units: - path: /autoware/map/topic_rate_check/pointcloud_map type: diag node: topic_state_monitor_pointcloud_map - name: map_topic_status" + name: map_topic_status diff --git a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml index b00fcb8a26f68..8e2566a747abf 100644 --- a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml +++ b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml @@ -1,6 +1,8 @@ + +