From ba0f519e5b23600880df616ec829964f22934719 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Wed, 27 Dec 2023 17:46:47 +0900 Subject: [PATCH 01/13] feat(multi_object_tracker, radar_object_tracker, tracker_object_merger): add glog to tracker node (#5770) * add glog to multi_object_tracker Signed-off-by: yoshiri * add glog to radar object trakcer Signed-off-by: yoshiri * feat: add glog to tracking object merger Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- perception/multi_object_tracker/CMakeLists.txt | 2 ++ perception/multi_object_tracker/package.xml | 1 + .../multi_object_tracker/src/multi_object_tracker_core.cpp | 5 +++++ perception/radar_object_tracker/CMakeLists.txt | 2 ++ perception/radar_object_tracker/package.xml | 1 + .../radar_object_tracker_node/radar_object_tracker_node.cpp | 5 +++++ perception/tracking_object_merger/CMakeLists.txt | 2 ++ perception/tracking_object_merger/package.xml | 1 + .../src/decorative_tracker_merger.cpp | 6 ++++++ 9 files changed, 25 insertions(+) diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 3e379bcfd1cf1..f5fbc8ff950e8 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -12,6 +12,7 @@ endif() ### Find Eigen Dependencies find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) +find_package(glog REQUIRED) include_directories( SYSTEM @@ -40,6 +41,7 @@ ament_auto_add_library(multi_object_tracker_node SHARED target_link_libraries(multi_object_tracker_node Eigen3::Eigen + glog::glog ) rclcpp_components_register_node(multi_object_tracker_node diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index e3172dfd22349..3d2e4662f7ea5 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -15,6 +15,7 @@ autoware_auto_perception_msgs eigen kalman_filter + libgoogle-glog-dev mussp object_recognition_utils rclcpp 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 d28833241bd5f..1d4d50c7eab4c 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -16,6 +16,7 @@ #include +#include #include #include @@ -177,6 +178,10 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { + // glog for debug + google::InitGoogleLogging("multi_object_tracker"); + google::InstallFailureSignalHandler(); + // Create publishers and subscribers detected_object_sub_ = create_subscription( "input", rclcpp::QoS{1}, diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index 54ef7b047bf17..7c1b98c10d14b 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -12,6 +12,7 @@ endif() find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(nlohmann_json REQUIRED) # for debug +find_package(glog REQUIRED) include_directories( SYSTEM @@ -32,6 +33,7 @@ target_link_libraries(radar_object_tracker_node Eigen3::Eigen yaml-cpp nlohmann_json::nlohmann_json # for debug + glog::glog ) rclcpp_components_register_node(radar_object_tracker_node diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index ad71e613c1a18..6939c54b5be75 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -17,6 +17,7 @@ eigen kalman_filter lanelet2_extension + libgoogle-glog-dev mussp nlohmann-json-dev object_recognition_utils diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp index 6d801302ab6c5..e4b394256101d 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp @@ -23,6 +23,7 @@ #include +#include #include #include @@ -194,6 +195,10 @@ RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_ tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { + // glog for debug + google::InitGoogleLogging("radar_object_tracker"); + google::InstallFailureSignalHandler(); + // Create publishers and subscribers detected_object_sub_ = create_subscription( "input", rclcpp::QoS{1}, diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt index 5e46b9403462d..9c8e5a321d9be 100644 --- a/perception/tracking_object_merger/CMakeLists.txt +++ b/perception/tracking_object_merger/CMakeLists.txt @@ -6,6 +6,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(autoware_cmake REQUIRED) +find_package(glog REQUIRED) autoware_package() @@ -31,6 +32,7 @@ ament_auto_add_library(decorative_tracker_merger_node SHARED target_link_libraries(decorative_tracker_merger_node Eigen3::Eigen + glog::glog ) rclcpp_components_register_node(decorative_tracker_merger_node diff --git a/perception/tracking_object_merger/package.xml b/perception/tracking_object_merger/package.xml index d74a8449b20e6..58184c090a1e1 100644 --- a/perception/tracking_object_merger/package.xml +++ b/perception/tracking_object_merger/package.xml @@ -14,6 +14,7 @@ autoware_auto_perception_msgs eigen + libgoogle-glog-dev mussp object_recognition_utils rclcpp diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index 47a333691eabf..d2bc643a8eec1 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -20,6 +20,8 @@ #include +#include + #include #include @@ -85,6 +87,10 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio tf_buffer_(get_clock()), tf_listener_(tf_buffer_) { + // glog for debug + google::InitGoogleLogging("decorative_object_merger_node"); + google::InstallFailureSignalHandler(); + // Subscriber sub_main_objects_ = create_subscription( "input/main_object", rclcpp::QoS{1}, From ee401437a5b22a305e317510598ba259470e16f6 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 27 Dec 2023 18:04:04 +0900 Subject: [PATCH 02/13] feat(start_planner): visualize stop line for safety check feature (#5974) assign stop_pose_ to visualize stop line Signed-off-by: kyoichi-sugahara --- .../src/goal_planner_module.cpp | 2 +- .../utils/parking_departure/utils.hpp | 2 +- .../src/utils/parking_departure/utils.cpp | 2 +- .../start_planner_module.hpp | 2 +- .../src/start_planner_module.cpp | 13 +++++++------ 5 files changed, 11 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 3a8a17c3b0966..493e7ec57f063 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -798,7 +798,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto stop_path = behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( - current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, + current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop); if (stop_path) { output.path = *stop_path; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp index bdcc9dcb2cc6a..e1f3402e8dfdf 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp @@ -70,7 +70,7 @@ std::pair getPairsTerminalVelocityAndAccel( std::optional generateFeasibleStopPath( PathWithLaneId & current_path, std::shared_ptr planner_data, - geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, + std::optional & stop_pose, const double maximum_deceleration, const double maximum_jerk); /** diff --git a/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp index a7b1df45c8150..97eba861f658a 100644 --- a/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp @@ -126,7 +126,7 @@ std::pair getPairsTerminalVelocityAndAccel( std::optional generateFeasibleStopPath( PathWithLaneId & current_path, std::shared_ptr planner_data, - geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, + std::optional & stop_pose, const double maximum_deceleration, const double maximum_jerk) { if (current_path.points.empty()) { diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index a143f34ead649..055a46e0bbeae 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -68,7 +68,7 @@ struct PullOutStatus Pose pull_out_start_pose{}; bool prev_is_safe_dynamic_objects{false}; std::shared_ptr prev_stop_path_after_approval{nullptr}; - bool has_stop_point{false}; + std::optional stop_pose{std::nullopt}; PullOutStatus() {} }; diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 30dd48ab383b9..af2c063204cc9 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -371,11 +371,11 @@ BehaviorModuleOutput StartPlannerModule::plan() incrementPathIndex(); } - if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.has_stop_point) { + if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.stop_pose) { auto current_path = getCurrentPath(); const auto stop_path = behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( - current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, + current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop); // Insert stop point in the path if needed @@ -384,17 +384,18 @@ BehaviorModuleOutput StartPlannerModule::plan() getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects"); path = *stop_path; status_.prev_stop_path_after_approval = std::make_shared(path); - status_.has_stop_point = true; + status_.stop_pose = stop_pose_; } else { path = current_path; } - } else if (!isWaitingApproval() && status_.has_stop_point) { + } else if (!isWaitingApproval() && status_.stop_pose) { // Delete stop point if conditions are met if (status_.is_safe_dynamic_objects && isStopped()) { - status_.has_stop_point = false; + status_.stop_pose = std::nullopt; path = getCurrentPath(); } path = *status_.prev_stop_path_after_approval; + stop_pose_ = status_.stop_pose; } else { path = getCurrentPath(); } @@ -1419,7 +1420,7 @@ void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const status_.prev_is_safe_dynamic_objects ? "true" : "false"); logFunc(" Driving Forward: %s", status_.driving_forward ? "true" : "false"); logFunc(" Backward Driving Complete: %s", status_.backward_driving_complete ? "true" : "false"); - logFunc(" Has Stop Point: %s", status_.has_stop_point ? "true" : "false"); + logFunc(" Has Stop Pose: %s", status_.stop_pose ? "true" : "false"); logFunc("[Module State]"); logFunc(" isActivated: %s", isActivated() ? "true" : "false"); From 61ed72396a60d0c203a0cac1f575aeaee1b5bcf5 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 27 Dec 2023 19:46:50 +0900 Subject: [PATCH 03/13] fix(intersection): fix private condition (#5975) --- .../src/scene_intersection.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 5909d607f6351..631c03d923da0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1058,8 +1058,10 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool is_first_conflicting_lane_private = (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); if (stuck_detected) { - if (!(is_first_conflicting_lane_private && - planner_param_.stuck_vehicle.disable_against_private_lane)) { + if ( + is_first_conflicting_lane_private && + planner_param_.stuck_vehicle.disable_against_private_lane) { + // do nothing } else { std::optional stopline_idx = std::nullopt; if (stuck_stopline_idx_opt) { From de2958b93fedaee5ebbb8e5a3d80fd86dcac0ade Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 27 Dec 2023 20:16:39 +0900 Subject: [PATCH 04/13] feat(tier4_logging_level_configure_rviz_plugin): add goal/start planner (#5978) Signed-off-by: kosuke55 --- .../config/logger_config.yaml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml index 17955fc5d0b9b..a298967a31af9 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -38,6 +38,14 @@ Planning: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance + behavior_path_planner_goal_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.goal_planner + + behavior_path_planner_start_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.start_planner + behavior_path_avoidance_by_lane_change: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: lane_change.AVOIDANCE_BY_LANE_CHANGE From 7676228b849e66cd6ff010ab90ee54fba86b20a1 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 27 Dec 2023 23:27:16 +0900 Subject: [PATCH 05/13] refactor(planner_manager): apply clang-tidy (#5981) Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner/planner_manager.hpp | 2 +- .../src/planner_manager.cpp | 57 ++++++++++--------- 2 files changed, 30 insertions(+), 29 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 3ccc92d04025f..8411be45edace 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -435,7 +435,7 @@ class PlannerManager const std::vector & request_modules, const std::shared_ptr & data, const BehaviorModuleOutput & previous_module_output); - std::string getNames(const std::vector & modules) const; + static std::string getNames(const std::vector & modules); std::optional root_lanelet_{std::nullopt}; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 1c8505c4de1be..cd1b0da702dac 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -130,7 +130,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da /** * STEP1: get approved modules' output */ - const auto approved_modules_output = runApprovedModules(data); + auto approved_modules_output = runApprovedModules(data); /** * STEP2: check modules that need to be launched @@ -272,22 +272,22 @@ std::vector PlannerManager::getRequestModules( // if there exists at least one approved module that is simultaneous but not always // executable. (only modules that are either always executable or simultaneous executable can // be added) - conditions.push_back( - {[&](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - getManager(m)->isSimultaneousExecutableAsApprovedModule(); - }, - [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }}); + conditions.emplace_back( + [&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }); // Condition 3: do not add modules that are not always executable if there exists // at least one approved module that is neither always nor simultaneous executable. // (only modules that are always executable can be added) - conditions.push_back( - {[&](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - !getManager(m)->isSimultaneousExecutableAsApprovedModule(); - }, - [&]() { return false; }}); + conditions.emplace_back( + [&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return false; }); bool skip_module = false; for (const auto & condition : conditions) { @@ -484,22 +484,22 @@ std::pair PlannerManager::runRequestModule // Condition 3: Only modules that are always executable can be added // if there exists at least one executable module that is neither always nor simultaneous // executable. - conditions.push_back( - {[this](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - !getManager(m)->isSimultaneousExecutableAsCandidateModule(); - }, - [&]() { return false; }}); + conditions.emplace_back( + [this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return false; }); // Condition 2: Only modules that are either always executable or simultaneous executable can be // added if there exists at least one executable module that is simultaneous but not always // executable. - conditions.push_back( - {[this](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - getManager(m)->isSimultaneousExecutableAsCandidateModule(); - }, - [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }}); + conditions.emplace_back( + [this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }); for (const auto & condition : conditions) { const auto & find_block_module = condition.first; @@ -933,8 +933,9 @@ void PlannerManager::print() const string_stream << "\n" << std::fixed << std::setprecision(1); string_stream << "processing time : "; for (const auto & t : processing_time_) { - string_stream << std::right << "[" << std::setw(max_string_num + 1) << std::left << t.first - << ":" << std::setw(4) << std::right << t.second << "ms]\n" + string_stream << std::right << "[" << std::setw(static_cast(max_string_num) + 1) + << std::left << t.first << ":" << std::setw(4) << std::right << t.second + << "ms]\n" << std::setw(21); } @@ -962,7 +963,7 @@ std::shared_ptr PlannerManager::getDebugMsg() return debug_msg_ptr_; } -std::string PlannerManager::getNames(const std::vector & modules) const +std::string PlannerManager::getNames(const std::vector & modules) { std::stringstream ss; for (const auto & m : modules) { From c625f3c276b9bec0bf5c70a90c01558032307995 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 28 Dec 2023 02:58:04 +0900 Subject: [PATCH 06/13] feat(start_planner): visualize refined start pose and start pose candidates with idx (#5984) Add createFootprintMarkerArray function and update StartGoalPlannerData structure Signed-off-by: kyoichi-sugahara --- .../marker_utils/utils.hpp | 10 +++ .../parking_departure/common_module_data.hpp | 3 + .../src/marker_utils/utils.cpp | 65 ++++++++++++------- .../src/start_planner_module.cpp | 34 ++++++++++ 4 files changed, 87 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp index 1ce3dd3736276..f34c0266ac081 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp @@ -18,6 +18,8 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include + #include #include #include @@ -56,6 +58,14 @@ inline int64_t bitShift(int64_t original_id) return original_id << (sizeof(int32_t) * 8 / 2); } +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const vehicle_info_util::VehicleInfo & vehicle_info); + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); + MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index 74f6b843803df..91683f4547659 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -38,6 +38,9 @@ struct StartGoalPlannerData std::vector ego_predicted_path; // collision check debug map CollisionCheckDebugMap collision_check; + + Pose refined_start_pose; + std::vector start_pose_candidates; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp index 9e20b9c3f8714..7933c21305432 100644 --- a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp @@ -40,6 +40,45 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + marker.points.push_back(marker.points.front()); +} + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) +{ + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.2, 0.2, 0.2), + createMarkerColor(r, g, b, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + MarkerArray marker_array; + + addFootprintMarker(marker, base_link_pose, vehicle_info); + + msg.markers.push_back(marker); + return msg; +} + MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) @@ -347,7 +386,6 @@ MarkerArray createPredictedPathMarkerArray( Marker marker = createDefaultMarker( "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1), - createMarkerColor(r, g, b, 0.999)); marker.lifetime = rclcpp::Duration::from_seconds(1.5); @@ -357,34 +395,11 @@ MarkerArray createPredictedPathMarkerArray( marker.points.clear(); const auto & predicted_path_pose = path.at(i); - const double half_width = vehicle_info.vehicle_width_m / 2.0; - const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; - const double base_to_rear = vehicle_info.rear_overhang_m; - - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, -half_width, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, half_width, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, half_width, 0.0) - .position); - marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, -half_width, 0.0) - .position); - marker.points.push_back(marker.points.front()); + addFootprintMarker(marker, predicted_path_pose, vehicle_info); marker_array.markers.push_back(marker); } return marker_array; - - marker.points.reserve(path.size()); - for (const auto & point : path) { - marker.points.push_back(point.position); - } - msg.markers.push_back(marker); - return msg; } MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index af2c063204cc9..80c08dea48922 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -816,6 +816,8 @@ void StartPlannerModule::updatePullOutStatus() planWithPriority( start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); + start_planner_data_.refined_start_pose = *refined_start_pose; + start_planner_data_.start_pose_candidates = start_pose_candidates; const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); @@ -1274,6 +1276,8 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons void StartPlannerModule::setDebugData() const { + using marker_utils::addFootprintMarker; + using marker_utils::createFootprintMarkerArray; using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; @@ -1298,6 +1302,8 @@ void StartPlannerModule::setDebugData() const add(createPoseMarkerArray(status_.pull_out_start_pose, "back_end_pose", 0, 0.9, 0.3, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); + add(createFootprintMarkerArray( + start_planner_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0)); @@ -1329,6 +1335,34 @@ void StartPlannerModule::setDebugData() const debug_marker_.markers.push_back(marker); } } + // start pose candidates + { + MarkerArray start_pose_footprint_marker_array{}; + MarkerArray start_pose_text_marker_array{}; + const auto purple = createMarkerColor(1.0, 0.0, 1.0, 0.99); + Marker footprint_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "start_pose_candidates", 0, Marker::LINE_STRIP, + createMarkerScale(0.2, 0.2, 0.2), purple); + Marker text_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "start_pose_candidates_idx", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.3, 0.3, 0.3), purple); + footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + text_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + for (size_t i = 0; i < start_planner_data_.start_pose_candidates.size(); ++i) { + footprint_marker.id = i; + text_marker.id = i; + footprint_marker.points.clear(); + text_marker.text = "idx[" + std::to_string(i) + "]"; + text_marker.pose = start_planner_data_.start_pose_candidates.at(i); + addFootprintMarker( + footprint_marker, start_planner_data_.start_pose_candidates.at(i), vehicle_info_); + start_pose_footprint_marker_array.markers.push_back(footprint_marker); + start_pose_text_marker_array.markers.push_back(text_marker); + } + + add(start_pose_footprint_marker_array); + add(start_pose_text_marker_array); + } // safety check if (parameters_->safety_check_params.enable_safety_check) { From 3e6c3a563125f85735dcc68cf3c5edfc2ee3a1ad Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 28 Dec 2023 09:07:20 +0900 Subject: [PATCH 07/13] docs(surround_obstacle_checker): add descriptions of some parameters (#5977) Signed-off-by: tomoya.kimura --- planning/surround_obstacle_checker/README.md | 21 ++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/planning/surround_obstacle_checker/README.md b/planning/surround_obstacle_checker/README.md index 80bb56ff843c4..071701c024685 100644 --- a/planning/surround_obstacle_checker/README.md +++ b/planning/surround_obstacle_checker/README.md @@ -103,16 +103,17 @@ As mentioned in stop condition section, it prevents chattering by changing thres {{ json_to_markdown("planning/surround_obstacle_checker/schema/surround_obstacle_checker.schema.json") }} -| Name | Type | Description | Default value | -| :-------------------------------- | :------- | :------------------------------------------------------------------------------------- | :------------ | -| `use_pointcloud` | `bool` | Use pointcloud as obstacle check | `true` | -| `use_dynamic_object` | `bool` | Use dynamic object as obstacle check | `true` | -| `surround_check_distance` | `double` | If objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] | 0.5 | -| `surround_check_recover_distance` | `double` | If no object exists in this distance, transit to "non-surrounding-obstacle" status [m] | 0.8 | -| `state_clear_time` | `double` | Threshold to clear stop state [s] | 2.0 | -| `stop_state_ego_speed` | `double` | Threshold to check ego vehicle stopped [m/s] | 0.1 | -| `stop_state_entry_duration_time` | `double` | Threshold to check ego vehicle stopped [s] | 0.1 | -| `publish_debug_footprints` | `bool` | Publish vehicle footprint with/without offsets | `true` | +| Name | Type | Description | Default value | +| :----------------------------------- | :------- | :----------------------------------------------------------------------------------------------------------------------------------------------- | :------------------------------------------- | +| `enable_check` | `bool` | Indicates whether each object is considered in the obstacle check target. | `true` for objects; `false` for point clouds | +| `surround_check_front_distance` | `bool` | If there are objects or point clouds within this distance in front, transition to the "exist-surrounding-obstacle" status [m]. | 0.5 | +| `surround_check_side_distance` | `double` | If there are objects or point clouds within this side distance, transition to the "exist-surrounding-obstacle" status [m]. | 0.5 | +| `surround_check_back_distance` | `double` | If there are objects or point clouds within this back distance, transition to the "exist-surrounding-obstacle" status [m]. | 0.5 | +| `surround_check_hysteresis_distance` | `double` | If no object exists within `surround_check_xxx_distance` plus this additional distance, transition to the "non-surrounding-obstacle" status [m]. | 0.3 | +| `state_clear_time` | `double` | Threshold to clear stop state [s] | 2.0 | +| `stop_state_ego_speed` | `double` | Threshold to check ego vehicle stopped [m/s] | 0.1 | +| `stop_state_entry_duration_time` | `double` | Threshold to check ego vehicle stopped [s] | 0.1 | +| `publish_debug_footprints` | `bool` | Publish vehicle footprint with/without offsets | `true` | ## Assumptions / Known limits From 51a8af86ca37f5d681e08fd8c044585149db1c56 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Thu, 28 Dec 2023 09:13:07 +0900 Subject: [PATCH 08/13] fix(landmark_based_localizer): fix to for moving the definition code of landmarks (#5803) * Fixed to use lanelet extension landmark Signed-off-by: Shintaro Sakoda * Fixed to build Signed-off-by: Shintaro Sakoda * Fixed link Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro Sakoda --- .../landmark_based_localizer/README.md | 63 +------------------ .../src/ar_tag_based_localizer.cpp | 14 ++--- .../src/ar_tag_based_localizer.hpp | 3 +- .../landmark_manager/landmark_manager.hpp | 8 ++- .../landmark_manager/src/landmark_manager.cpp | 34 ++-------- 5 files changed, 19 insertions(+), 103 deletions(-) diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md index 86f3750ad11d9..49588a19ac620 100644 --- a/localization/landmark_based_localizer/README.md +++ b/localization/landmark_based_localizer/README.md @@ -43,68 +43,7 @@ So, if the 4 vertices are considered as forming a tetrahedron and its volume exc ## Map specifications -For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_manager` can interpret. - -The four vertices of a landmark are defined counterclockwise. - -The order of the four vertices is defined as follows. In the coordinate system of a landmark, - -- the x-axis is parallel to the vector from the first vertex to the second vertex -- the y-axis is parallel to the vector from the second vertex to the third vertex - -![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg) - -### Example of `lanelet2_map.osm` - -The values provided below are placeholders. -Ensure to input the correct coordinates corresponding to the actual location where the landmark is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`. - -For example, when using the AR tag, it would look like this. - -```xml -... - - - - - - - - - - - - - - - - - - - - - - - - - - -... - - - - - - - - - - - - -... - -``` +See ## About `consider_orientation` diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index a1e2f3ec31dd3..43ac1e1098453 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -142,7 +142,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg) { - landmark_manager_.parse_landmarks(msg, "apriltag_16h5", this->get_logger()); + landmark_manager_.parse_landmarks(msg, "apriltag_16h5"); const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg(); mapped_tag_pose_pub_->publish(marker_msg); } @@ -173,7 +173,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) const Pose self_pose = interpolate_result.value().interpolated_pose.pose.pose; // detect - const std::vector landmarks = detect_landmarks(msg); + const std::vector landmarks = detect_landmarks(msg); if (landmarks.empty()) { return; } @@ -183,7 +183,7 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) PoseArray pose_array_msg; pose_array_msg.header.stamp = sensor_stamp; pose_array_msg.header.frame_id = "map"; - for (const landmark_manager::Landmark & landmark : landmarks) { + for (const Landmark & landmark : landmarks) { const Pose detected_marker_on_map = tier4_autoware_utils::transformPose(landmark.pose, self_pose); pose_array_msg.poses.push_back(detected_marker_on_map); @@ -293,7 +293,7 @@ std::vector ArTagBasedLocalizer::detect_landmarks( cv_ptr->image.copyTo(in_image); } catch (cv_bridge::Exception & e) { RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); - return std::vector{}; + return std::vector{}; } // get transform from base_link to camera @@ -303,7 +303,7 @@ std::vector ArTagBasedLocalizer::detect_landmarks( tf_buffer_->lookupTransform("base_link", msg->header.frame_id, sensor_stamp); } catch (tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what()); - return std::vector{}; + return std::vector{}; } // detect @@ -311,7 +311,7 @@ std::vector ArTagBasedLocalizer::detect_landmarks( detector_.detect(in_image, markers, cam_param_, marker_size_, false); // parse - std::vector landmarks; + std::vector landmarks; for (aruco::Marker & marker : markers) { // convert marker pose to tf @@ -327,7 +327,7 @@ std::vector ArTagBasedLocalizer::detect_landmarks( const double distance = std::hypot(pose.position.x, pose.position.y, pose.position.z); if (distance <= distance_threshold_) { tf2::doTransform(pose, pose, transform_sensor_to_base_link); - landmarks.push_back(landmark_manager::Landmark{std::to_string(marker.id), pose}); + landmarks.push_back(Landmark{std::to_string(marker.id), pose}); } // for debug, drawing the detected markers diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index eb7406f8c77f8..f70821a39ffe8 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -80,6 +80,7 @@ class ArTagBasedLocalizer : public rclcpp::Node using TransformStamped = geometry_msgs::msg::TransformStamped; using MarkerArray = visualization_msgs::msg::MarkerArray; using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + using Landmark = landmark_manager::Landmark; public: explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); @@ -89,7 +90,7 @@ class ArTagBasedLocalizer : public rclcpp::Node void image_callback(const Image::ConstSharedPtr & msg); void cam_info_callback(const CameraInfo::ConstSharedPtr & msg); void ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg); - std::vector detect_landmarks(const Image::ConstSharedPtr & msg); + std::vector detect_landmarks(const Image::ConstSharedPtr & msg); // Parameters float marker_size_{}; diff --git a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp index fba419f746b5e..7e78ed713dddc 100644 --- a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp +++ b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp @@ -15,6 +15,8 @@ #ifndef LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ #define LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ +#include "lanelet2_extension/localization/landmark.hpp" + #include #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" @@ -40,13 +42,13 @@ class LandmarkManager public: void parse_landmarks( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, - const std::string & target_subtype, const rclcpp::Logger & logger); + const std::string & target_subtype); [[nodiscard]] visualization_msgs::msg::MarkerArray get_landmarks_as_marker_array_msg() const; [[nodiscard]] geometry_msgs::msg::Pose calculate_new_self_pose( - const std::vector & detected_landmarks, - const geometry_msgs::msg::Pose & self_pose, const bool consider_orientation) const; + const std::vector & detected_landmarks, const geometry_msgs::msg::Pose & self_pose, + const bool consider_orientation) const; private: // To allow multiple landmarks with the same id to be registered on a vector_map, diff --git a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp index 875f04edd8c47..7ddd66efea0a6 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp @@ -29,32 +29,17 @@ namespace landmark_manager void LandmarkManager::parse_landmarks( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, - const std::string & target_subtype, const rclcpp::Logger & logger) + const std::string & target_subtype) { - RCLCPP_INFO_STREAM(logger, "msg->format_version: " << msg->format_version); - RCLCPP_INFO_STREAM(logger, "msg->map_format: " << msg->map_format); - RCLCPP_INFO_STREAM(logger, "msg->map_version: " << msg->map_version); - RCLCPP_INFO_STREAM(logger, "msg->data.size(): " << msg->data.size()); - lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared()}; - lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr); - - for (const auto & poly : lanelet_map_ptr->polygonLayer) { - const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")}; - if (type != "pose_marker") { - continue; - } - const std::string subtype{poly.attributeOr(lanelet::AttributeName::Subtype, "none")}; - if (subtype != target_subtype) { - continue; - } - + std::vector landmarks = + lanelet::localization::parseLandmarkPolygons(msg, target_subtype); + for (const lanelet::Polygon3d & poly : landmarks) { // Get landmark_id const std::string landmark_id = poly.attributeOr("marker_id", "none"); // Get 4 vertices const auto & vertices = poly.basicPolygon(); if (vertices.size() != 4) { - RCLCPP_WARN_STREAM(logger, "vertices.size() (" << vertices.size() << ") != 4"); continue; } @@ -65,12 +50,8 @@ void LandmarkManager::parse_landmarks( const auto & v2 = vertices[2]; const auto & v3 = vertices[3]; const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0; - RCLCPP_INFO_STREAM(logger, "volume = " << volume); const double volume_threshold = 1e-5; if (volume > volume_threshold) { - RCLCPP_WARN_STREAM( - logger, - "volume (" << volume << ") > threshold (" << volume_threshold << "), This is not plane."); continue; } @@ -99,13 +80,6 @@ void LandmarkManager::parse_landmarks( // Add landmarks_map_[landmark_id].push_back(pose); - RCLCPP_INFO_STREAM(logger, "id: " << landmark_id); - RCLCPP_INFO_STREAM( - logger, - "(x, y, z) = " << pose.position.x << ", " << pose.position.y << ", " << pose.position.z); - RCLCPP_INFO_STREAM( - logger, "q = " << pose.orientation.x << ", " << pose.orientation.y << ", " - << pose.orientation.z << ", " << pose.orientation.w); } } From 6efa91b96080ef4e7c7f0b1758bc6d4f32e98502 Mon Sep 17 00:00:00 2001 From: karishma1911 <95741784+karishma1911@users.noreply.github.com> Date: Thu, 28 Dec 2023 07:04:18 +0530 Subject: [PATCH 09/13] refactor(perception-tensortt-yolo): rework parameters (#5918) perception-tensortt-yolo Signed-off-by: karishma --- .../tensorrt_yolo/schema/tensortt_yolo.json | 108 ++++++++++++++++++ perception/tensorrt_yolo/src/nodelet.cpp | 16 +-- 2 files changed, 116 insertions(+), 8 deletions(-) create mode 100644 perception/tensorrt_yolo/schema/tensortt_yolo.json diff --git a/perception/tensorrt_yolo/schema/tensortt_yolo.json b/perception/tensorrt_yolo/schema/tensortt_yolo.json new file mode 100644 index 0000000000000..0b4724078c994 --- /dev/null +++ b/perception/tensorrt_yolo/schema/tensortt_yolo.json @@ -0,0 +1,108 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for tensorrt_yolo", + "type": "object", + "definitions": { + "tensorrt_yolo": { + "type": "object", + "properties": { + "num_anchors": { + "type": "number", + "default": [ + 10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, + 156.0, 198.0, 373.0, 326.0 + ], + "description": "The anchors to create bounding box candidates." + }, + "scale_x_y": { + "type": "number", + "default": [1.0, 1.0, 1.0], + "description": "scale parameter to eliminate grid sensitivity." + }, + "score_thresh": { + "type": "number", + "default": 0.1, + "description": "If the objectness score is less than this value, the object is ignored in yolo layer." + }, + "iou_thresh": { + "type": "number", + "default": 0.45, + "description": "The iou threshold for NMS method." + }, + "detections_per_im": { + "type": "number", + "default": 100, + "description": " The maximum detection number for one frame." + }, + "use_darknet_layer": { + "type": "boolean", + "default": true, + "description": "The flag to use yolo layer in darknet." + }, + "ignore_thresh": { + "type": "number", + "default": 0.5, + "description": "If the output score is less than this value, this object is ignored." + }, + "onnx_file": { + "type": "string", + "description": "The onnx file name for yolo model." + }, + "engine_file": { + "type": "string", + "description": "The tensorrt engine file name for yolo model." + }, + "label_file": { + "type": "string", + "description": "The label file with label names for detected objects written on it." + }, + "calib_image_directory": { + "type": "string", + "description": "The directory name including calibration images for int8 inference." + }, + "calib_cache_file": { + "type": "string", + "description": "The calibration cache file for int8 inference." + }, + "mode": { + "type": "string", + "default": "FP32", + "description": "The inference mode: FP32, FP16, INT8." + }, + "gpu_id": { + "type": "number", + "default": 0, + "description": "GPU device ID that runs the model." + } + }, + "required": [ + "num_anchors", + "scale_x_y", + "score_thresh", + "iou_thresh", + "detections_per_im", + "use_darknet_layer", + "ignore_thresh", + "onnx_file", + "engine_file", + "label_file", + "calib_image_directory", + "calib_cache_file", + "mode", + "gpu_id" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/tensorrt_yolo" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/tensorrt_yolo/src/nodelet.cpp b/perception/tensorrt_yolo/src/nodelet.cpp index dcb65114ad88f..fdcd8bf12db72 100644 --- a/perception/tensorrt_yolo/src/nodelet.cpp +++ b/perception/tensorrt_yolo/src/nodelet.cpp @@ -50,9 +50,9 @@ TensorrtYoloNodelet::TensorrtYoloNodelet(const rclcpp::NodeOptions & options) std::string label_file = declare_parameter("label_file", ""); std::string calib_image_directory = declare_parameter("calib_image_directory", ""); std::string calib_cache_file = declare_parameter("calib_cache_file", ""); - std::string mode = declare_parameter("mode", "FP32"); - gpu_device_id_ = declare_parameter("gpu_id", 0); - yolo_config_.num_anchors = declare_parameter("num_anchors", 3); + std::string mode = declare_parameter("mode"); + gpu_device_id_ = declare_parameter("gpu_id"); + yolo_config_.num_anchors = declare_parameter("num_anchors"); auto anchors = declare_parameter( "anchors", std::vector{ 10, 13, 16, 30, 33, 23, 30, 61, 62, 45, 59, 119, 116, 90, 156, 198, 373, 326}); @@ -61,11 +61,11 @@ TensorrtYoloNodelet::TensorrtYoloNodelet(const rclcpp::NodeOptions & options) auto scale_x_y = declare_parameter("scale_x_y", std::vector{1.0, 1.0, 1.0}); std::vector scale_x_y_float(scale_x_y.begin(), scale_x_y.end()); yolo_config_.scale_x_y = scale_x_y_float; - yolo_config_.score_thresh = declare_parameter("score_thresh", 0.1); - yolo_config_.iou_thresh = declare_parameter("iou_thresh", 0.45); - yolo_config_.detections_per_im = declare_parameter("detections_per_im", 100); - yolo_config_.use_darknet_layer = declare_parameter("use_darknet_layer", true); - yolo_config_.ignore_thresh = declare_parameter("ignore_thresh", 0.5); + yolo_config_.score_thresh = declare_parameter("score_thresh"); + yolo_config_.iou_thresh = declare_parameter("iou_thresh"); + yolo_config_.detections_per_im = declare_parameter("detections_per_im"); + yolo_config_.use_darknet_layer = declare_parameter("use_darknet_layer"); + yolo_config_.ignore_thresh = declare_parameter("ignore_thresh"); if (!yolo::set_cuda_device(gpu_device_id_)) { RCLCPP_ERROR(this->get_logger(), "Given GPU not exist or suitable"); From e4e129ee5b4a44ee9d1c4010094d78dec7f09907 Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Thu, 28 Dec 2023 10:47:37 +0900 Subject: [PATCH 10/13] fix(motion_velocity_smoother): front wheel steer rate calculation (#5965) #5964 Signed-off-by: Vincent Richard --- .../motion_velocity_smoother/src/smoother/smoother_base.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index 61cc4e9fb1fcc..bcf981be4e493 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -224,8 +224,8 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( auto & steer_back = output.at(i).front_wheel_angle_rad; // calculate the just 2 steering angle - steer_front = std::atan(base_param_.wheel_base * curvature_v.at(i)); - steer_back = std::atan(base_param_.wheel_base * curvature_v.at(i + 1)); + steer_front = std::atan(base_param_.wheel_base * curvature_v.at(i + 1)); + steer_back = std::atan(base_param_.wheel_base * curvature_v.at(i)); const auto mean_vel = 0.5 * (v_front + v_back); const auto dt = std::max(points_interval / mean_vel, std::numeric_limits::epsilon()); From 4cf5a0c0428da97f0ed62b433540dfe8b47238b8 Mon Sep 17 00:00:00 2001 From: OHMAE Takugo Date: Thu, 28 Dec 2023 11:32:18 +0900 Subject: [PATCH 11/13] fix(filt_vector): fix a problem of list index out of range in filt_vector (#5099) fix filt_vector Signed-off-by: tohmae --- .../src/lowpass_filter.cpp | 6 ++--- .../test/test_lowpass_filter.cpp | 24 +++++++++++++++++++ 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/control/mpc_lateral_controller/src/lowpass_filter.cpp b/control/mpc_lateral_controller/src/lowpass_filter.cpp index 07a6e2c6b0d21..8fbf3488c5a2e 100644 --- a/control/mpc_lateral_controller/src/lowpass_filter.cpp +++ b/control/mpc_lateral_controller/src/lowpass_filter.cpp @@ -120,10 +120,8 @@ bool filt_vector(const int num, std::vector & u) double tmp = 0.0; int num_tmp = 0; double count = 0; - if (i - num < 0) { - num_tmp = i; - } else if (i + num > static_cast(u.size()) - 1) { - num_tmp = static_cast(u.size()) - i - 1; + if ((i - num < 0) || (i + num > static_cast(u.size()) - 1)) { + num_tmp = std::min(i, static_cast(u.size()) - i - 1); } else { num_tmp = num; } diff --git a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp b/control/mpc_lateral_controller/test/test_lowpass_filter.cpp index 2cd641c031265..c68513586847b 100644 --- a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp +++ b/control/mpc_lateral_controller/test/test_lowpass_filter.cpp @@ -61,6 +61,30 @@ TEST(TestLowpassFilter, MoveAverageFilter) EXPECT_EQ(filtered_vec[4], 23.0 / 3); EXPECT_EQ(filtered_vec[5], original_vec[5]); } + { + const int window_size = 3; + const std::vector original_vec = {1.0, 1.0, 1.0, 1.0}; + std::vector filtered_vec = original_vec; + EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); + ASSERT_EQ(filtered_vec.size(), original_vec.size()); + EXPECT_EQ(filtered_vec[0], original_vec[0]); + EXPECT_EQ(filtered_vec[1], 1.0); + EXPECT_EQ(filtered_vec[2], 1.0); + EXPECT_EQ(filtered_vec[3], original_vec[3]); + } + { + const int window_size = 4; + const std::vector original_vec = {1.0, 3.0, 4.0, 6.0, 7.0, 10.0}; + std::vector filtered_vec = original_vec; + EXPECT_TRUE(MoveAverageFilter::filt_vector(window_size, filtered_vec)); + ASSERT_EQ(filtered_vec.size(), original_vec.size()); + EXPECT_EQ(filtered_vec[0], original_vec[0]); + EXPECT_EQ(filtered_vec[1], 8.0 / 3); + EXPECT_EQ(filtered_vec[2], 21.0 / 5); + EXPECT_EQ(filtered_vec[3], 30.0 / 5); + EXPECT_EQ(filtered_vec[4], 23.0 / 3); + EXPECT_EQ(filtered_vec[5], original_vec[5]); + } } TEST(TestLowpassFilter, Butterworth2dFilter) { From 743eb0e64c58d43ae5edb99ca08b6d1e1fea08f6 Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Thu, 28 Dec 2023 11:32:43 +0900 Subject: [PATCH 12/13] feat: add support of overwriting signals if harsh backlight is detected (#5852) * feat: add support of overwriting signals if backlit is detected Signed-off-by: ktro2828 * feat: remove default parameter in nodelet and update lauch for composable node Signed-off-by: ktro2828 * docs: update README Signed-off-by: ktro2828 * docs: update README Signed-off-by: ktro2828 * feat: update confidence to 0.0 corresponding signals overwritten by unkonwn Signed-off-by: ktro2828 --------- Signed-off-by: ktro2828 --- .../traffic_light_node_container.launch.py | 19 +++++++++--- perception/traffic_light_classifier/README.md | 9 +++--- .../traffic_light_classifier/nodelet.hpp | 3 ++ .../traffic_light_classifier.launch.xml | 2 ++ .../traffic_light_classifier/src/nodelet.cpp | 30 ++++++++++++++++++- 5 files changed, 54 insertions(+), 9 deletions(-) diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py index a6bcb40e81252..30eeff04985e5 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -42,7 +42,8 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw") add_launch_arg("output/rois", "/perception/traffic_light_recognition/rois") add_launch_arg( - "output/traffic_signals", "/perception/traffic_light_recognition/traffic_signals" + "output/traffic_signals", + "/perception/traffic_light_recognition/traffic_signals", ) # traffic_light_fine_detector @@ -64,14 +65,20 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("classifier_type", "1") add_launch_arg( "classifier_model_path", - os.path.join(classifier_share_dir, "data", "traffic_light_classifier_efficientNet_b1.onnx"), + os.path.join( + classifier_share_dir, + "data", + "traffic_light_classifier_efficientNet_b1.onnx", + ), ) add_launch_arg( - "classifier_label_path", os.path.join(classifier_share_dir, "data", "lamp_labels.txt") + "classifier_label_path", + os.path.join(classifier_share_dir, "data", "lamp_labels.txt"), ) add_launch_arg("classifier_precision", "fp16") add_launch_arg("classifier_mean", "[123.675, 116.28, 103.53]") add_launch_arg("classifier_std", "[58.395, 57.12, 57.375]") + add_launch_arg("backlight_threshold", "0.85") add_launch_arg("use_intra_process", "False") add_launch_arg("use_multithread", "False") @@ -102,6 +109,7 @@ def create_parameter_dict(*args): "classifier_precision", "classifier_mean", "classifier_std", + "backlight_threshold", ) ], remappings=[ @@ -122,7 +130,10 @@ def create_parameter_dict(*args): ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", LaunchConfiguration("output/rois")), ("~/input/rough/rois", "detection/rough/rois"), - ("~/input/traffic_signals", LaunchConfiguration("output/traffic_signals")), + ( + "~/input/traffic_signals", + LaunchConfiguration("output/traffic_signals"), + ), ("~/output/image", "debug/rois"), ("~/output/image/compressed", "debug/rois/compressed"), ("~/output/image/compressedDepth", "debug/rois/compressedDepth"), diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md index 2aecf66f8fb7b..758234f129f2a 100644 --- a/perception/traffic_light_classifier/README.md +++ b/perception/traffic_light_classifier/README.md @@ -52,10 +52,11 @@ These colors and shapes are assigned to the message as follows: ### Node Parameters -| Name | Type | Description | -| ----------------- | ---- | ------------------------------------------- | -| `classifier_type` | int | if the value is `1`, cnn_classifier is used | -| `data_path` | str | packages data and artifacts directory path | +| Name | Type | Description | +| --------------------- | ----- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `classifier_type` | int | if the value is `1`, cnn_classifier is used | +| `data_path` | str | packages data and artifacts directory path | +| `backlight_threshold` | float | If the intensity get grater than this overwrite with UNKNOWN in corresponding RoI. Note that, if the value is much higher, the node only overwrites in the harsher backlight situations. Therefore, If you wouldn't like to use this feature set this value to `1.0`. The value can be `[0.0, 1.0]`. The confidence of overwritten signal is set to `0.0`. | ### Core Parameters diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp b/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp index e076ff5c69378..89b91c5fb666c 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp +++ b/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp @@ -86,6 +86,9 @@ class TrafficLightClassifierNodelet : public rclcpp::Node rclcpp::Publisher::SharedPtr traffic_signal_array_pub_; std::shared_ptr classifier_ptr_; + + double backlight_threshold_; + bool is_harsh_backlight(const cv::Mat & img) const; }; } // namespace traffic_light diff --git a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml index 10aa04cc585af..8ba0990b3efc6 100644 --- a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml +++ b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -13,6 +13,7 @@ + @@ -24,5 +25,6 @@ + diff --git a/perception/traffic_light_classifier/src/nodelet.cpp b/perception/traffic_light_classifier/src/nodelet.cpp index 90cca87245d22..d0d7e13b928c8 100644 --- a/perception/traffic_light_classifier/src/nodelet.cpp +++ b/perception/traffic_light_classifier/src/nodelet.cpp @@ -26,6 +26,8 @@ TrafficLightClassifierNodelet::TrafficLightClassifierNodelet(const rclcpp::NodeO using std::placeholders::_1; using std::placeholders::_2; is_approximate_sync_ = this->declare_parameter("approximate_sync", false); + backlight_threshold_ = this->declare_parameter("backlight_threshold"); + if (is_approximate_sync_) { approximate_sync_.reset(new ApproximateSync(ApproximateSyncPolicy(10), image_sub_, roi_sub_)); approximate_sync_->registerCallback( @@ -94,19 +96,45 @@ void TrafficLightClassifierNodelet::imageRoiCallback( output_msg.signals.resize(input_rois_msg->rois.size()); std::vector images; + std::vector backlight_indices; for (size_t i = 0; i < input_rois_msg->rois.size(); i++) { output_msg.signals[i].traffic_light_id = input_rois_msg->rois.at(i).traffic_light_id; const sensor_msgs::msg::RegionOfInterest & roi = input_rois_msg->rois.at(i).roi; - images.emplace_back(cv_ptr->image, cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height)); + auto roi_img = cv_ptr->image(cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height)); + if (is_harsh_backlight(roi_img)) { + backlight_indices.emplace_back(i); + } + images.emplace_back(roi_img); } if (!classifier_ptr_->getTrafficSignals(images, output_msg)) { RCLCPP_ERROR(this->get_logger(), "failed classify image, abort callback"); return; } + + for (const auto & idx : backlight_indices) { + auto & elements = output_msg.signals.at(idx).elements; + for (auto & element : elements) { + element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.confidence = 0.0; + } + } + output_msg.header = input_image_msg->header; traffic_signal_array_pub_->publish(output_msg); } +bool TrafficLightClassifierNodelet::is_harsh_backlight(const cv::Mat & img) const +{ + cv::Mat y_cr_cb; + cv::cvtColor(img, y_cr_cb, cv::COLOR_RGB2YCrCb); + + const cv::Scalar mean_values = cv::mean(y_cr_cb); + const double intensity = (mean_values[0] - 112.5) / 112.5; + + return backlight_threshold_ <= intensity; +} + } // namespace traffic_light #include From 3b0c2542cb0d3aada933e4b5cd2ec5c5313d64f6 Mon Sep 17 00:00:00 2001 From: karishma1911 <95741784+karishma1911@users.noreply.github.com> Date: Thu, 28 Dec 2023 08:50:59 +0530 Subject: [PATCH 13/13] refactor(control-lane-departure-checker): rework parameters (#5789) * control-lane-departure-checker Signed-off-by: karishma * control-lane-departure-checker Signed-off-by: karishma --------- Signed-off-by: karishma Co-authored-by: KK --- .../schema/lane_departure_checker.json | 79 +++++++++++++++++++ 1 file changed, 79 insertions(+) create mode 100644 control/lane_departure_checker/schema/lane_departure_checker.json diff --git a/control/lane_departure_checker/schema/lane_departure_checker.json b/control/lane_departure_checker/schema/lane_departure_checker.json new file mode 100644 index 0000000000000..c7f39fbf7ef8a --- /dev/null +++ b/control/lane_departure_checker/schema/lane_departure_checker.json @@ -0,0 +1,79 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for lane_departure_checker", + "type": "object", + "definitions": { + "lane_departure_checker": { + "type": "object", + "properties": { + "footprint_margin_scale": { + "type": "number", + "default": 1.0, + "description": "Coefficient for expanding footprint margin. Multiplied by 1 standard deviation." + }, + "resample_interval": { + "type": "number", + "default": 0.3, + "description": "Minimum Euclidean distance between points when resample trajectory.[m]." + }, + "max_deceleration": { + "type": "number", + "default": 2.8, + "description": "Maximum deceleration when calculating braking distance." + }, + "delay_time": { + "type": "number", + "default": 1.3, + "description": "Delay time which took to actuate brake when calculating braking distance. [second]." + }, + "max_lateral_deviation": { + "type": "number", + "default": 2.0, + "description": "Maximum lateral deviation in vehicle coordinate. [m]." + }, + "max_longitudinal_deviation": { + "type": "number", + "default": 2.0, + "description": "Maximum longitudinal deviation in vehicle coordinate. [m]." + }, + "max_yaw_deviation_deg": { + "type": "number", + "default": 60.0, + "description": "Maximum ego yaw deviation from trajectory. [deg]." + }, + "ego_nearest_dist_threshold": { + "type": "number" + }, + "ego_nearest_yaw_threshold": { + "type": "number" + }, + "min_braking_distance": { + "type": "number" + } + }, + "required": [ + "footprint_margin_scale", + "resample_interval", + "max_deceleration", + "max_lateral_deviation", + "max_longitudinal_deviation", + "max_yaw_deviation_deg", + "ego_nearest_dist_threshold", + "ego_nearest_yaw_threshold", + "min_braking_distance" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lane_departure_checker" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +}