From 61ed72396a60d0c203a0cac1f575aeaee1b5bcf5 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 27 Dec 2023 19:46:50 +0900 Subject: [PATCH 01/32] 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 02/32] 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 03/32] 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 04/32] 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 05/32] 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 06/32] 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 07/32] 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 08/32] 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 09/32] 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 10/32] 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 11/32] 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": ["/**"] +} From 12e5c1045cbaaa874d3234677bffb9d5c0902faf Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Thu, 28 Dec 2023 15:11:05 +0900 Subject: [PATCH 12/32] feat!: remove planning factor type (#5793) remove planning factor type Signed-off-by: Takagi, Isamu Co-authored-by: Hiroki OTA --- .../src/velocity_steering_factors_panel.cpp | 3 - .../src/behavior_path_planner_node.cpp | 8 +- system/default_ad_api/src/planning.cpp | 86 ------------------- 3 files changed, 1 insertion(+), 96 deletions(-) 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 e27fc9b6cfa81..6ea142ed66a1b 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 @@ -174,9 +174,6 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray:: case SteeringFactor::APPROACHING: label->setText("APPROACHING"); break; - case SteeringFactor::TRYING: - label->setText("TRYING"); - break; case SteeringFactor::TURNING: label->setText("TURNING"); break; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 9fd397bb8a8b1..12715520e1146 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -490,16 +490,10 @@ void BehaviorPathPlannerNode::publish_steering_factor( const auto [intersection_pose, intersection_distance] = planner_data->turn_signal_decider.getIntersectionPoseAndDistance(); - const uint16_t steering_factor_state = std::invoke([&intersection_flag]() { - if (intersection_flag) { - return SteeringFactor::TURNING; - } - return SteeringFactor::TRYING; - }); steering_factor_interface_ptr_->updateSteeringFactor( {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance}, - PlanningBehavior::INTERSECTION, steering_factor_direction, steering_factor_state, ""); + PlanningBehavior::INTERSECTION, steering_factor_direction, SteeringFactor::TURNING, ""); } else { steering_factor_interface_ptr_->clearSteeringFactors(); } diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index 17491482a18fe..ed2dcee92eb2b 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -63,82 +63,6 @@ T merge_factors(const rclcpp::Time stamp, const std::vector(now(), velocity_factors_); auto steering = merge_factors(now(), steering_factors_); - // Set velocity factor type for compatibility. - for (auto & factor : velocity.factors) { - factor.type = convert_velocity_behavior(factor.behavior); - } - - // Set steering factor type for compatibility. - for (auto & factor : steering.factors) { - factor.type = convert_steering_behavior(factor.behavior); - } - // Set the distance if it is nan. if (trajectory_ && kinematic_state_) { for (auto & factor : velocity.factors) { From a188ee6ec9f1f0cb2643662d1590c87a7a0b84ff Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 28 Dec 2023 19:32:09 +0900 Subject: [PATCH 13/32] feat(start_planner): separate collision check and path planning (#5952) * remove collision check from pull out planner * change order of extract stop objects function Signed-off-by: kyoichi-sugahara --- .../start_planner_module.hpp | 7 +- .../src/geometric_pull_out.cpp | 10 --- .../src/shift_pull_out.cpp | 15 ---- .../src/start_planner_module.cpp | 79 +++++++++++-------- .../src/util.cpp | 1 - 5 files changed, 50 insertions(+), 62 deletions(-) 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 055a46e0bbeae..9f559cabe8951 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 @@ -169,11 +169,10 @@ class StartPlannerModule : public SceneModuleInterface bool isMoving() const; PriorityOrder determinePriorityOrder( - const std::string & search_priority, const size_t candidates_size); + const std::string & search_priority, const size_t start_pose_candidates_num); bool findPullOutPath( - const std::vector & start_pose_candidates, const size_t index, - const std::shared_ptr & planner, const Pose & refined_start_pose, - const Pose & goal_pose); + const Pose & start_pose_candidate, const std::shared_ptr & planner, + const Pose & refined_start_pose, const Pose & goal_pose); void updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type); diff --git a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp index 7d6dd60398e8e..d9ff155f36e6c 100644 --- a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -63,17 +63,7 @@ std::optional GeometricPullOut::plan(const Pose & start_pose, const // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - const auto & stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); - const auto [pull_out_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); - if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, arc_path, pull_out_lane_stop_objects, - parameters_.collision_check_margin)) { - return {}; - } const double velocity = parallel_parking_parameters_.forward_parking_velocity; if (parameters_.divide_pull_out_path) { diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 35281d5bedbcf..1e1e47ba940e8 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -46,7 +46,6 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos { const auto & route_handler = planner_data_->route_handler; const auto & common_parameters = planner_data_->parameters; - const auto & dynamic_objects = planner_data_->dynamic_object; const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_.max_back_distance; @@ -64,13 +63,6 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos return std::nullopt; } - // extract stop objects in pull out lane for collision check - const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *dynamic_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); - const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_out_lane_objects, parameters_.th_moving_object_velocity); - // get safe path for (auto & pull_out_path : pull_out_paths) { auto & shift_path = @@ -143,13 +135,6 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos continue; } - // check collision - if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, path_start_to_end, pull_out_lane_stop_objects, - parameters_.collision_check_margin)) { - continue; - } - shift_path.header = planner_data_->route_handler->getRouteHeader(); return pull_out_path; 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 80c08dea48922..693029e57cb7b 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 @@ -586,7 +586,7 @@ void StartPlannerModule::planWithPriority( determinePriorityOrder(search_priority, start_pose_candidates.size()); for (const auto & [index, planner] : order_priority) { - if (findPullOutPath(start_pose_candidates, index, planner, refined_start_pose, goal_pose)) + if (findPullOutPath(start_pose_candidates[index], planner, refined_start_pose, goal_pose)) return; } @@ -594,17 +594,17 @@ void StartPlannerModule::planWithPriority( } PriorityOrder StartPlannerModule::determinePriorityOrder( - const std::string & search_priority, const size_t candidates_size) + const std::string & search_priority, const size_t start_pose_candidates_num) { PriorityOrder order_priority; if (search_priority == "efficient_path") { for (const auto & planner : start_planners_) { - for (size_t i = 0; i < candidates_size; i++) { + for (size_t i = 0; i < start_pose_candidates_num; i++) { order_priority.emplace_back(i, planner); } } } else if (search_priority == "short_back_distance") { - for (size_t i = 0; i < candidates_size; i++) { + for (size_t i = 0; i < start_pose_candidates_num; i++) { for (const auto & planner : start_planners_) { order_priority.emplace_back(i, planner); } @@ -617,43 +617,62 @@ PriorityOrder StartPlannerModule::determinePriorityOrder( } bool StartPlannerModule::findPullOutPath( - const std::vector & start_pose_candidates, const size_t index, - const std::shared_ptr & planner, const Pose & refined_start_pose, - const Pose & goal_pose) + const Pose & start_pose_candidate, const std::shared_ptr & planner, + const Pose & refined_start_pose, const Pose & goal_pose) { - // Ensure the index is within the bounds of the start_pose_candidates vector - if (index >= start_pose_candidates.size()) return false; + const auto & dynamic_objects = planner_data_->dynamic_object; + const auto pull_out_lanes = start_planner_utils::getPullOutLanes( + planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); + const auto & vehicle_footprint = createVehicleFootprint(vehicle_info_); + // extract stop objects in pull out lane for collision check + const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + *dynamic_objects, parameters_->th_moving_object_velocity); + const auto [pull_out_lane_stop_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); - const Pose & pull_out_start_pose = start_pose_candidates.at(index); - const bool is_driving_forward = - tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; + // if start_pose_candidate is far from refined_start_pose, backward driving is necessary + const bool backward_is_unnecessary = + tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose) < 0.01; planner->setPlannerData(planner_data_); - const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); + const auto pull_out_path = planner->plan(start_pose_candidate, goal_pose); // If no path is found, return false if (!pull_out_path) { return false; } - // If driving forward, update status with the current path and return true - if (is_driving_forward) { - updateStatusWithCurrentPath(*pull_out_path, pull_out_start_pose, planner->getPlannerType()); - return true; - } + // lambda function for combining partial_paths + const auto combine_partial_path = [pull_out_lanes](const auto & path) { + PathWithLaneId combined_path; + for (const auto & partial_path : path.partial_paths) { + combined_path.points.insert( + combined_path.points.end(), partial_path.points.begin(), partial_path.points.end()); + } + // remove the point behind of shift end pose + const auto shift_end_pose_idx = + motion_utils::findNearestIndex(combined_path.points, path.end_pose); + combined_path.points.erase( + combined_path.points.begin() + *shift_end_pose_idx + 1, combined_path.points.end()); + return combined_path; + }; - // If this is the last start pose candidate, return false - if (index == start_pose_candidates.size() - 1) return false; + // check collision + if (utils::checkCollisionBetweenPathFootprintsAndObjects( + vehicle_footprint, combine_partial_path(*pull_out_path), pull_out_lane_stop_objects, + parameters_->collision_check_margin)) { + return false; + } - const Pose & next_pull_out_start_pose = start_pose_candidates.at(index + 1); - const auto next_pull_out_path = planner->plan(next_pull_out_start_pose, goal_pose); + // If start pose candidate + if (backward_is_unnecessary) { + updateStatusWithCurrentPath(*pull_out_path, start_pose_candidate, planner->getPlannerType()); + return true; + } - // If no next path is found, return false - if (!next_pull_out_path) return false; + updateStatusWithNextPath(*pull_out_path, start_pose_candidate, planner->getPlannerType()); - // Update status with the next path and return true - updateStatusWithNextPath( - *next_pull_out_path, next_pull_out_start_pose, planner->getPlannerType()); return true; } @@ -776,13 +795,9 @@ std::vector StartPlannerModule::generateDrivableLanes( void StartPlannerModule::updatePullOutStatus() { - const bool has_received_new_route = - !planner_data_->prev_route_id || - *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); - // skip updating if enough time has not passed for preventing chattering between back and // start_planner - if (!has_received_new_route) { + if (!receivedNewRoute()) { if (!last_pull_out_start_update_time_) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } diff --git a/planning/behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_start_planner_module/src/util.cpp index 9a9466936c422..73a1e94399503 100644 --- a/planning/behavior_path_start_planner_module/src/util.cpp +++ b/planning/behavior_path_start_planner_module/src/util.cpp @@ -92,7 +92,6 @@ lanelet::ConstLanelets getPullOutLanes( const auto start_pose = planner_data->route_handler->getOriginalStartPose(); lanelet::ConstLanelet current_shoulder_lane; - lanelet::ConstLanelets shoulder_lanes; if (route_handler->getPullOutStartLane( route_handler->getShoulderLanelets(), start_pose, vehicle_width, ¤t_shoulder_lane)) { // pull out from shoulder lane From dd1579e3e4d5afe81436e47116c650e5740ba2a8 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 29 Dec 2023 00:05:30 +0900 Subject: [PATCH 14/32] feat(start_planner): apply offset colission check section (#5982) refactor collision check in GeometricPullOut::plan() Signed-off-by: kyoichi-sugahara Co-authored-by: Kosuke Takeuchi --- .../start_planner_module.hpp | 6 +- .../src/shift_pull_out.cpp | 2 +- .../src/start_planner_module.cpp | 67 +++++++++++++------ 3 files changed, 51 insertions(+), 24 deletions(-) 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 9f559cabe8951..a451a92c16ff1 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 @@ -173,6 +173,8 @@ class StartPlannerModule : public SceneModuleInterface bool findPullOutPath( const Pose & start_pose_candidate, const std::shared_ptr & planner, const Pose & refined_start_pose, const Pose & goal_pose); + + PathWithLaneId extractCollisionCheckPath(const PullOutPath & path); void updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type); @@ -226,7 +228,9 @@ class StartPlannerModule : public SceneModuleInterface void updatePullOutStatus(); void updateStatusAfterBackwardDriving(); PredictedObjects filterStopObjectsInPullOutLanes( - const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const; + const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose, + const double velocity_threshold, const double object_check_backward_distance, + const double object_check_forward_distance) const; bool hasFinishedPullOut() const; bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 1e1e47ba940e8..51673410199b8 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -68,7 +68,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos auto & shift_path = pull_out_path.partial_paths.front(); // shift path is not separate but only one. - // check lane_departure and collision with path between pull_out_start to pull_out_end + // check lane_departure with path between pull_out_start to pull_out_end PathWithLaneId path_start_to_end{}; { const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position); 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 693029e57cb7b..9870a324caf64 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 @@ -643,29 +643,13 @@ bool StartPlannerModule::findPullOutPath( return false; } - // lambda function for combining partial_paths - const auto combine_partial_path = [pull_out_lanes](const auto & path) { - PathWithLaneId combined_path; - for (const auto & partial_path : path.partial_paths) { - combined_path.points.insert( - combined_path.points.end(), partial_path.points.begin(), partial_path.points.end()); - } - // remove the point behind of shift end pose - const auto shift_end_pose_idx = - motion_utils::findNearestIndex(combined_path.points, path.end_pose); - combined_path.points.erase( - combined_path.points.begin() + *shift_end_pose_idx + 1, combined_path.points.end()); - return combined_path; - }; - // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint, combine_partial_path(*pull_out_path), pull_out_lane_stop_objects, + vehicle_footprint, extractCollisionCheckPath(*pull_out_path), pull_out_lane_stop_objects, parameters_->collision_check_margin)) { return false; } - // If start pose candidate if (backward_is_unnecessary) { updateStatusWithCurrentPath(*pull_out_path, start_pose_candidate, planner->getPlannerType()); return true; @@ -676,6 +660,33 @@ bool StartPlannerModule::findPullOutPath( return true; } +PathWithLaneId StartPlannerModule::extractCollisionCheckPath(const PullOutPath & path) +{ + PathWithLaneId combined_path; + for (const auto & partial_path : path.partial_paths) { + combined_path.points.insert( + combined_path.points.end(), partial_path.points.begin(), partial_path.points.end()); + } + + // calculate collision check end idx + size_t collision_check_end_idx = 0; + const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( + combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end); + + if (collision_check_end_pose) { + collision_check_end_idx = + motion_utils::findNearestIndex(combined_path.points, collision_check_end_pose->position); + } + + // remove the point behind of collision check end pose + if (collision_check_end_idx + 1 < combined_path.points.size()) { + combined_path.points.erase( + combined_path.points.begin() + collision_check_end_idx + 1, combined_path.points.end()); + } + + return combined_path; +} + void StartPlannerModule::updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type) @@ -892,9 +903,12 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto stop_objects_in_pull_out_lanes = - filterStopObjectsInPullOutLanes(pull_out_lanes, parameters_->th_moving_object_velocity); + const auto stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( + pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, + backward_path_length, std::numeric_limits::max()); // Set the maximum backward distance less than the distance from the vehicle's base_link to the // lane's rearmost point to prevent lane departure. @@ -937,16 +951,25 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( } PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( - const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const + const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_point, + const double velocity_threshold, const double object_check_forward_distance, + const double object_check_backward_distance) const { const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *planner_data_->dynamic_object, velocity_threshold); - // filter for objects located in pull_out_lanes and moving at a speed below the threshold - const auto [stop_objects_in_pull_out_lanes, others] = + // filter for objects located in pull out lanes and moving at a speed below the threshold + auto [stop_objects_in_pull_out_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + const auto path = planner_data_->route_handler->getCenterLinePath( + pull_out_lanes, object_check_backward_distance, object_check_forward_distance); + + utils::path_safety_checker::filterObjectsByPosition( + stop_objects_in_pull_out_lanes, path.points, current_point, object_check_forward_distance, + object_check_backward_distance); + return stop_objects_in_pull_out_lanes; } From 1c42fbee818ad2bf2d5227e4830d1ad7e22863c5 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 29 Dec 2023 01:27:49 +0900 Subject: [PATCH 15/32] chore(pid_longitudinal_controller): change INFO printing frequency (#5890) Signed-off-by: Takayuki Murooka --- .../src/pid_longitudinal_controller.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 698d4a8403069..31c48fb5060a8 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -555,9 +555,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d return; }; - const auto info_throttle = [this](const auto & s) { - RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", s); - }; + const auto info_once = [this](const auto & s) { RCLCPP_INFO_ONCE(logger_, "%s", s); }; const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; @@ -623,10 +621,10 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d if (m_control_state == ControlState::STOPPED) { // -- debug print -- if (has_nonzero_target_vel && !departure_condition_from_stopped) { - info_throttle("target speed > 0, but departure condition is not met. Keep STOPPED."); + info_once("target speed > 0, but departure condition is not met. Keep STOPPED."); } if (has_nonzero_target_vel && keep_stopped_condition) { - info_throttle("target speed > 0, but keep stop condition is met. Keep STOPPED."); + info_once("target speed > 0, but keep stop condition is met. Keep STOPPED."); } // --------------- From 70b88d82a807d6cf72dc60e96fc14e9067502179 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 29 Dec 2023 01:46:02 +0900 Subject: [PATCH 16/32] feat(behavior_path_planner): run keep last approved modules after candidate modules (#5970) Signed-off-by: kosuke55 --- .../behavior_path_planner/planner_manager.hpp | 9 ++++ .../src/planner_manager.cpp | 42 +++++++++++++++---- 2 files changed, 42 insertions(+), 9 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 8411be45edace..f6f95ae3b5e82 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,6 +435,15 @@ class PlannerManager const std::vector & request_modules, const std::shared_ptr & data, const BehaviorModuleOutput & previous_module_output); + /** + * @brief run keep last approved modules + * @param planner data. + * @param previous module output. + * @return planning result. + */ + BehaviorModuleOutput runKeepLastModules( + const std::shared_ptr & data, const BehaviorModuleOutput & previous_output) 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 cd1b0da702dac..262de4764fcfe 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -141,8 +141,9 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da * STEP3: if there is no module that need to be launched, return approved modules' output */ if (request_modules.empty()) { + const auto output = runKeepLastModules(data, approved_modules_output); processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return approved_modules_output; + return output; } /** @@ -150,24 +151,32 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da */ const auto [highest_priority_module, candidate_modules_output] = runRequestModules(request_modules, data, approved_modules_output); + + /** + * STEP5: run keep last approved modules after running candidate modules. + * NOTE: if no candidate module is launched, approved_modules_output used as input for keep + * last modules and return the result immediately. + */ + const auto output = runKeepLastModules( + data, highest_priority_module ? candidate_modules_output : approved_modules_output); if (!highest_priority_module) { processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return approved_modules_output; + return output; } /** - * STEP5: if the candidate module's modification is NOT approved yet, return the result. + * STEP6: if the candidate module's modification is NOT approved yet, return the result. * NOTE: the result is output of the candidate module, but the output path don't contains path * shape modification that needs approval. On the other hand, it could include velocity * profile modification. */ if (highest_priority_module->isWaitingApproval()) { processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return candidate_modules_output; + return output; } /** - * STEP6: if the candidate module is approved, push the module into approved_module_ptrs_ + * STEP7: if the candidate module is approved, push the module into approved_module_ptrs_ */ addApprovedModule(highest_priority_module); clearCandidateModules(); @@ -178,7 +187,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da logger_, clock_, 1000, "Reach iteration limit (max: %ld). Output current result.", max_iteration_num_); processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return candidate_modules_output; + return output; } } @@ -394,6 +403,19 @@ std::vector PlannerManager::getRequestModules( return request_modules; } +BehaviorModuleOutput PlannerManager::runKeepLastModules( + const std::shared_ptr & data, const BehaviorModuleOutput & previous_output) const +{ + auto output = previous_output; + std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) { + if (getManager(m)->isKeepLast()) { + output = run(m, data, output); + } + }); + + return output; +} + BehaviorModuleOutput PlannerManager::getReferencePath( const std::shared_ptr & data) const { @@ -652,11 +674,13 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrname(), output); + if (!getManager(m)->isKeepLast()) { + output = run(m, data, output); + results.emplace(m->name(), output); + } }); /** From e9d3a8e581c3aa0cd072ea5205f3325d12f08686 Mon Sep 17 00:00:00 2001 From: Tao Zhong <55872497+tzhong518@users.noreply.github.com> Date: Fri, 29 Dec 2023 13:39:26 +0900 Subject: [PATCH 17/32] feat(crosswalk_traffic_light): add detector and classifier for pedestrian traffic light (#5871) * add: crosswalk traffic light recognition Signed-off-by: tzhong518 * fix: set conf=0 when occluded Signed-off-by: tzhong518 * fix: clean code Signed-off-by: tzhong518 * fix: refactor Signed-off-by: tzhong518 * fix: occlusion predictor Signed-off-by: tzhong518 * fix: output not detected signals as unknown Signed-off-by: tzhong518 * Revert "fix: output not detected signals as unknown" This reverts commit 7a166596e760d7eb037570e28106dcd105860567. * Revert "fix: occlusion predictor" This reverts commit 47d8cdd7fee8b4432f7a440f87bc35b50a8bc897. * fix: occlusion predictor Signed-off-by: tzhong518 * fix: clean debug code Signed-off-by: tzhong518 * style(pre-commit): autofix * fix: launch file Signed-off-by: tzhong518 * fix: set max angle range for different type Signed-off-by: tzhong518 * fix: precommit Signed-off-by: tzhong518 * fix: cancel the judge of flashing for estimated crosswalk traffic light Signed-off-by: tzhong518 * delete: not necessary judgement on label Signed-off-by: tzhong518 * Update perception/traffic_light_classifier/src/nodelet.cpp Co-authored-by: Yusuke Muramatsu * Update perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp Co-authored-by: Yusuke Muramatsu * Update perception/crosswalk_traffic_light_estimator/src/node.cpp Co-authored-by: Yusuke Muramatsu * style(pre-commit): autofix * fix: topic names and message attribute name Signed-off-by: tzhong518 * style(pre-commit): autofix * fix: model names Signed-off-by: tzhong518 * style(pre-commit): autofix * Update perception/crosswalk_traffic_light_estimator/src/node.cpp Co-authored-by: Yusuke Muramatsu * Update perception/crosswalk_traffic_light_estimator/src/node.cpp Co-authored-by: Yusuke Muramatsu * Update perception/crosswalk_traffic_light_estimator/src/node.cpp Co-authored-by: Yusuke Muramatsu * Update perception/traffic_light_occlusion_predictor/src/nodelet.cpp Co-authored-by: Yusuke Muramatsu * Update perception/traffic_light_occlusion_predictor/src/nodelet.cpp Co-authored-by: Yusuke Muramatsu * Update perception/traffic_light_occlusion_predictor/src/nodelet.cpp Co-authored-by: Yusuke Muramatsu * fix: argument position Signed-off-by: tzhong518 * fix: set classifier type in launch file Signed-off-by: tzhong518 * fix: function and parameter name Signed-off-by: tzhong518 * fix: func name Signed-off-by: tzhong518 * Update launch/tier4_perception_launch/launch/perception.launch.xml Co-authored-by: Yusuke Muramatsu * Update perception/traffic_light_map_based_detector/src/node.cpp Co-authored-by: Yusuke Muramatsu * style(pre-commit): autofix * fix: move max angle range to config Signed-off-by: tzhong518 * Update launch/tier4_perception_launch/launch/perception.launch.xml * Update launch/tier4_perception_launch/launch/perception.launch.xml * Update launch/tier4_perception_launch/launch/perception.launch.xml * Update launch/tier4_perception_launch/launch/perception.launch.xml * Update launch/tier4_perception_launch/launch/perception.launch.xml * fix: model name Signed-off-by: tzhong518 * fix: conflict Signed-off-by: tzhong518 * fix: precommit Signed-off-by: tzhong518 * fix: CI test Signed-off-by: tzhong518 --------- Signed-off-by: tzhong518 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yusuke Muramatsu --- .../src/traffic_light_utils.cpp | 2 +- .../launch/perception.launch.xml | 15 +- .../traffic_light.launch.xml | 41 +++-- .../traffic_light_node_container.launch.py | 80 +++++++-- ...osswalk_traffic_light_estimator.param.yaml | 1 + .../node.hpp | 16 +- .../src/node.cpp | 162 ++++++++++++++++-- .../traffic_light_classifier/nodelet.hpp | 2 + .../traffic_light_classifier.launch.xml | 3 + .../traffic_light_classifier/src/nodelet.cpp | 16 +- .../traffic_light_fine_detector/nodelet.hpp | 6 +- .../src/nodelet.cpp | 16 +- ...raffic_light_map_based_detector.param.yaml | 2 + .../traffic_light_map_based_detector/node.hpp | 6 + .../src/node.cpp | 70 +++++++- .../nodelet.hpp | 10 +- ...affic_light_occlusion_predictor.launch.xml | 6 +- .../src/nodelet.cpp | 70 ++++++-- 18 files changed, 445 insertions(+), 79 deletions(-) diff --git a/common/traffic_light_utils/src/traffic_light_utils.cpp b/common/traffic_light_utils/src/traffic_light_utils.cpp index 5a68223afb5ff..e02798bb41767 100644 --- a/common/traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/traffic_light_utils/src/traffic_light_utils.cpp @@ -46,7 +46,7 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float signal.elements[0].shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; signal.elements[0].color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; // the default value is -1, which means to not set confidence - if (confidence >= 0) { + if (confidence > 0) { signal.elements[0].confidence = confidence; } } diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 7f5f9d61c4bf0..1b5753fb4d5c4 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -86,13 +86,18 @@ default="$(var data_path)/traffic_light_fine_detector" description="options: `tlr_yolox_s_batch_**`. The batch number must be either one of 1, 4, 6" /> - + + @@ -231,6 +236,12 @@ + + + + + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index 58e9e231e4aa0..7b81a42fdb79f 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -7,8 +7,10 @@ - - + + + + @@ -24,8 +26,10 @@ - - + + + + @@ -34,6 +38,8 @@ + + @@ -54,15 +60,18 @@ - - - + + + + + + @@ -72,11 +81,13 @@ - + + + @@ -84,6 +95,8 @@ + + @@ -104,15 +117,18 @@ - - - + + + + + + @@ -122,7 +138,8 @@ - + + 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 30eeff04985e5..dc5a2443d3f45 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 @@ -45,6 +45,13 @@ def add_launch_arg(name: str, default_value=None, description=None): "output/traffic_signals", "/perception/traffic_light_recognition/traffic_signals", ) + add_launch_arg( + "output/car/traffic_signals", "/perception/traffic_light_recognition/car/traffic_signals" + ) + add_launch_arg( + "output/pedestrian/traffic_signals", + "/perception/traffic_light_recognition/pedestrian/traffic_signals", + ) # traffic_light_fine_detector add_launch_arg( @@ -64,16 +71,21 @@ def add_launch_arg(name: str, default_value=None, description=None): # traffic_light_classifier add_launch_arg("classifier_type", "1") add_launch_arg( - "classifier_model_path", + "car_classifier_model_path", + os.path.join(classifier_share_dir, "data", "traffic_light_classifier_efficientNet_b1.onnx"), + ) + add_launch_arg( + "pedestrian_classifier_model_path", os.path.join( - classifier_share_dir, - "data", - "traffic_light_classifier_efficientNet_b1.onnx", + classifier_share_dir, "data", "pedestrian_traffic_light_classifier_efficientNet_b1.onnx" ), ) add_launch_arg( - "classifier_label_path", - os.path.join(classifier_share_dir, "data", "lamp_labels.txt"), + "car_classifier_label_path", os.path.join(classifier_share_dir, "data", "lamp_labels.txt") + ) + add_launch_arg( + "pedestrian_classifier_label_path", + os.path.join(classifier_share_dir, "data", "lamp_labels_ped.txt"), ) add_launch_arg("classifier_precision", "fp16") add_launch_arg("classifier_mean", "[123.675, 116.28, 103.53]") @@ -98,24 +110,56 @@ def create_parameter_dict(*args): ComposableNode( package="traffic_light_classifier", plugin="traffic_light::TrafficLightClassifierNodelet", - name="traffic_light_classifier", + name="car_traffic_light_classifier", + namespace="classification", + parameters=[ + { + "approximate_sync": LaunchConfiguration("approximate_sync"), + "classifier_type": LaunchConfiguration("classifier_type"), + "classify_traffic_light_type": 0, + "classifier_model_path": LaunchConfiguration("car_classifier_model_path"), + "classifier_label_path": LaunchConfiguration("car_classifier_label_path"), + "classifier_precision": LaunchConfiguration("classifier_precision"), + "classifier_mean": LaunchConfiguration("classifier_mean"), + "classifier_std": LaunchConfiguration("classifier_std"), + "backlight_threshold": LaunchConfiguration("backlight_threshold"), + } + ], + remappings=[ + ("~/input/image", LaunchConfiguration("input/image")), + ("~/input/rois", LaunchConfiguration("output/rois")), + ("~/output/traffic_signals", "classified/car/traffic_signals"), + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ComposableNode( + package="traffic_light_classifier", + plugin="traffic_light::TrafficLightClassifierNodelet", + name="pedestrian_traffic_light_classifier", namespace="classification", parameters=[ - create_parameter_dict( - "approximate_sync", - "classifier_type", - "classifier_model_path", - "classifier_label_path", - "classifier_precision", - "classifier_mean", - "classifier_std", - "backlight_threshold", - ) + { + "approximate_sync": LaunchConfiguration("approximate_sync"), + "classifier_type": LaunchConfiguration("classifier_type"), + "classify_traffic_light_type": 1, + "classifier_model_path": LaunchConfiguration( + "pedestrian_classifier_model_path" + ), + "classifier_label_path": LaunchConfiguration( + "pedestrian_classifier_label_path" + ), + "classifier_precision": LaunchConfiguration("classifier_precision"), + "classifier_mean": LaunchConfiguration("classifier_mean"), + "classifier_std": LaunchConfiguration("classifier_std"), + "backlight_threshold": LaunchConfiguration("backlight_threshold"), + } ], remappings=[ ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", LaunchConfiguration("output/rois")), - ("~/output/traffic_signals", "classified/traffic_signals"), + ("~/output/traffic_signals", "classified/pedestrian/traffic_signals"), ], extra_arguments=[ {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} diff --git a/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml b/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml index 33168c87de7eb..2be9074f8d71e 100644 --- a/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml +++ b/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml @@ -16,3 +16,4 @@ ros__parameters: use_last_detect_color: true last_detect_color_hold_time: 2.0 + last_colors_hold_time: 1.0 diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp index 848293f6334bb..0578d671d27b9 100644 --- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp +++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp @@ -34,11 +34,11 @@ #include #include +#include #include #include #include #include - namespace traffic_light { @@ -53,6 +53,8 @@ using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement using TrafficSignalAndTime = std::pair; using TrafficLightIdMap = std::unordered_map; +using TrafficLightIdArray = std::unordered_map>; + class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node { public: @@ -76,8 +78,12 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node void onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg); void updateLastDetectedSignal(const TrafficLightIdMap & traffic_signals); + void updateLastDetectedSignals(const TrafficLightIdMap & traffic_signals); + void updateFlashingState(const TrafficSignal & signal); + uint8_t updateAndGetColorState(const TrafficSignal & signal); void setCrosswalkTrafficSignal( - const lanelet::ConstLanelet & crosswalk, const uint8_t color, TrafficSignalArray & msg) const; + const lanelet::ConstLanelet & crosswalk, const uint8_t color, const TrafficSignalArray & msg, + TrafficSignalArray & output); lanelet::ConstLanelets getNonRedLanelets( const lanelet::ConstLanelets & lanelets, const TrafficLightIdMap & traffic_light_id_map) const; @@ -97,9 +103,15 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node // Node param bool use_last_detect_color_; double last_detect_color_hold_time_; + double last_colors_hold_time_; // Signal history TrafficLightIdMap last_detect_color_; + TrafficLightIdArray last_colors_; + + // State + std::map is_flashing_; + std::map current_color_state_; // Stop watch StopWatch stop_watch_; diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index 6527f0662bcbf..904a365786755 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -73,7 +73,6 @@ bool hasMergeLane( return false; } - } // namespace CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( @@ -84,6 +83,7 @@ CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( use_last_detect_color_ = declare_parameter("use_last_detect_color"); last_detect_color_hold_time_ = declare_parameter("last_detect_color_hold_time"); + last_colors_hold_time_ = declare_parameter("last_colors_hold_time"); sub_map_ = create_subscription( "~/input/vector_map", rclcpp::QoS{1}.transient_local(), @@ -164,19 +164,19 @@ void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray( traffic_light_id_map[traffic_signal.traffic_signal_id] = std::pair(traffic_signal, get_clock()->now()); } - for (const auto & crosswalk : conflicting_crosswalks_) { constexpr int VEHICLE_GRAPH_ID = 0; const auto conflict_lls = overall_graphs_ptr_->conflictingInGraph(crosswalk, VEHICLE_GRAPH_ID); const auto non_red_lanelets = getNonRedLanelets(conflict_lls, traffic_light_id_map); const auto crosswalk_tl_color = estimateCrosswalkTrafficSignal(crosswalk, non_red_lanelets); - setCrosswalkTrafficSignal(crosswalk, crosswalk_tl_color, output); + setCrosswalkTrafficSignal(crosswalk, crosswalk_tl_color, *msg, output); } removeDuplicateIds(output); updateLastDetectedSignal(traffic_light_id_map); + updateLastDetectedSignals(traffic_light_id_map); pub_traffic_light_array_->publish(output); pub_processing_time_->publish("processing_time_ms", stop_watch.toc("Total")); @@ -221,24 +221,160 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal( } } } - for (const auto id : erase_id_list) last_detect_color_.erase(id); + for (const auto id : erase_id_list) { + last_detect_color_.erase(id); + is_flashing_.erase(id); + current_color_state_.erase(id); + } +} + +void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignals( + const TrafficLightIdMap & traffic_light_id_map) +{ + for (const auto & input_traffic_signal : traffic_light_id_map) { + const auto & elements = input_traffic_signal.second.first.elements; + + if (elements.empty()) { + continue; + } + + if ( + elements.front().color == TrafficSignalElement::UNKNOWN && elements.front().confidence == 1) { + continue; + } + + const auto & id = input_traffic_signal.second.first.traffic_signal_id; + + if (last_colors_.count(id) == 0) { + std::vector signal{input_traffic_signal.second}; + last_colors_.insert(std::make_pair(id, signal)); + continue; + } + + last_colors_.at(id).push_back(input_traffic_signal.second); + } + + std::vector erase_id_list; + for (auto & last_traffic_signal : last_colors_) { + const auto & id = last_traffic_signal.first; + for (auto it = last_traffic_signal.second.begin(); it != last_traffic_signal.second.end();) { + auto sig = (*it).first; + rclcpp::Time t = (*it).second; + + // hold signal recognition results for [last_colors_hold_time_] seconds. + const auto time_from_last_detected = (get_clock()->now() - t).seconds(); + if (time_from_last_detected > last_colors_hold_time_) { + it = last_traffic_signal.second.erase(it); + } else { + ++it; + } + } + if (last_traffic_signal.second.empty()) { + erase_id_list.emplace_back(id); + } + } + for (const auto id : erase_id_list) last_colors_.erase(id); } void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( - const lanelet::ConstLanelet & crosswalk, const uint8_t color, TrafficSignalArray & msg) const + const lanelet::ConstLanelet & crosswalk, const uint8_t color, const TrafficSignalArray & msg, + TrafficSignalArray & output) { const auto tl_reg_elems = crosswalk.regulatoryElementsAs(); + std::unordered_map valid_id2idx_map; // detected traffic light + + for (size_t i = 0; i < msg.signals.size(); ++i) { + auto signal = msg.signals[i]; + valid_id2idx_map[signal.traffic_signal_id] = i; + } + for (const auto & tl_reg_elem : tl_reg_elems) { - TrafficSignal output_traffic_signal; - TrafficSignalElement output_traffic_signal_element; - output_traffic_signal_element.color = color; - output_traffic_signal_element.shape = TrafficSignalElement::CIRCLE; - output_traffic_signal_element.confidence = 1.0; - output_traffic_signal.elements.push_back(output_traffic_signal_element); - output_traffic_signal.traffic_signal_id = tl_reg_elem->id(); - msg.signals.push_back(output_traffic_signal); + auto id = tl_reg_elem->id(); + // if valid prediction exists, overwrite the estimation; else, use the estimation + if (valid_id2idx_map.count(id)) { + size_t idx = valid_id2idx_map[id]; + auto signal = msg.signals[idx]; + updateFlashingState(signal); // check if it is flashing + // update output msg according to flashing and current state + output.signals[idx].elements[0].color = updateAndGetColorState(signal); + } else { + TrafficSignal output_traffic_signal; + TrafficSignalElement output_traffic_signal_element; + output_traffic_signal_element.color = color; + output_traffic_signal_element.shape = TrafficSignalElement::CIRCLE; + output_traffic_signal_element.confidence = 1.0; + output_traffic_signal.elements.push_back(output_traffic_signal_element); + output_traffic_signal.traffic_signal_id = id; + output.signals.push_back(output_traffic_signal); + } + } +} + +void CrosswalkTrafficLightEstimatorNode::updateFlashingState(const TrafficSignal & signal) +{ + const auto id = signal.traffic_signal_id; + + // no record of detected color in last_detect_color_hold_time_ + if (is_flashing_.count(id) == 0) { + is_flashing_.insert(std::make_pair(id, false)); + return; + } + + // flashing green + if ( + signal.elements.front().color == TrafficSignalElement::UNKNOWN && + signal.elements.front().confidence != 0 && // not due to occlusion + current_color_state_.at(id) != TrafficSignalElement::UNKNOWN) { + is_flashing_.at(id) = true; + return; + } + + // history exists + if (last_colors_.count(id) > 0) { + std::vector history = last_colors_.at(id); + for (const auto & h : history) { + if (h.first.elements.front().color != signal.elements.front().color) { + // keep the current value if not same with input signal + return; + } + } + // all history is same with input signal + is_flashing_.at(id) = false; + } + + // no record of detected color in last_color_hold_time_ + // keep the current value + return; +} + +uint8_t CrosswalkTrafficLightEstimatorNode::updateAndGetColorState(const TrafficSignal & signal) +{ + const auto id = signal.traffic_signal_id; + const auto color = signal.elements[0].color; + + if (current_color_state_.count(id) == 0) { + current_color_state_.insert(std::make_pair(id, color)); + } else if (is_flashing_.at(id) == false) { + current_color_state_.at(id) = color; + } else if (is_flashing_.at(id) == true) { + if ( + current_color_state_.at(id) == TrafficSignalElement::GREEN && + color == TrafficSignalElement::RED) { + current_color_state_.at(id) = TrafficSignalElement::RED; + } else if ( + current_color_state_.at(id) == TrafficSignalElement::RED && + color == TrafficSignalElement::GREEN) { + current_color_state_.at(id) = TrafficSignalElement::GREEN; + } else if (current_color_state_.at(id) == TrafficSignalElement::UNKNOWN) { + if (color == TrafficSignalElement::GREEN || color == TrafficSignalElement::UNKNOWN) + current_color_state_.at(id) = TrafficSignalElement::GREEN; + if (color == TrafficSignalElement::RED) + current_color_state_.at(id) = TrafficSignalElement::RED; + } } + + return current_color_state_.at(id); } lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getNonRedLanelets( 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 89b91c5fb666c..1281285ee53dd 100644 --- a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp +++ b/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp @@ -66,6 +66,8 @@ class TrafficLightClassifierNodelet : public rclcpp::Node CNN = 1, }; + uint8_t classify_traffic_light_type_; + private: void connectCb(); 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 8ba0990b3efc6..343531f4a00f1 100644 --- a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml +++ b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -11,6 +11,8 @@ + + @@ -21,6 +23,7 @@ + diff --git a/perception/traffic_light_classifier/src/nodelet.cpp b/perception/traffic_light_classifier/src/nodelet.cpp index d0d7e13b928c8..3a00bb013d867 100644 --- a/perception/traffic_light_classifier/src/nodelet.cpp +++ b/perception/traffic_light_classifier/src/nodelet.cpp @@ -23,6 +23,8 @@ namespace traffic_light TrafficLightClassifierNodelet::TrafficLightClassifierNodelet(const rclcpp::NodeOptions & options) : Node("traffic_light_classifier_node", options) { + classify_traffic_light_type_ = this->declare_parameter("classify_traffic_light_type", 0); + using std::placeholders::_1; using std::placeholders::_2; is_approximate_sync_ = this->declare_parameter("approximate_sync", false); @@ -96,16 +98,28 @@ void TrafficLightClassifierNodelet::imageRoiCallback( output_msg.signals.resize(input_rois_msg->rois.size()); std::vector images; + size_t num_valid_roi = 0; 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; + // skip if not the expected type of roi + if (input_rois_msg->rois.at(i).traffic_light_type != classify_traffic_light_type_) { + continue; + } + output_msg.signals[num_valid_roi].traffic_light_id = + input_rois_msg->rois.at(i).traffic_light_id; + output_msg.signals[num_valid_roi].traffic_light_type = + input_rois_msg->rois.at(i).traffic_light_type; const sensor_msgs::msg::RegionOfInterest & roi = input_rois_msg->rois.at(i).roi; + 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); + num_valid_roi++; } + output_msg.signals.resize(num_valid_roi); + if (!classifier_ptr_->getTrafficSignals(images, output_msg)) { RCLCPP_ERROR(this->get_logger(), "failed classify image, abort callback"); return; diff --git a/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp b/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp index f2f1f17be6894..5c89c76a11833 100644 --- a/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp +++ b/perception/traffic_light_fine_detector/include/traffic_light_fine_detector/nodelet.hpp @@ -115,6 +115,7 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node void detectionMatch( std::map & id2expectRoi, std::map & id2detections, TrafficLightRoiArray & out_rois); + /** * @brief convert image message to cv::Mat * @@ -137,7 +138,8 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node * @return true succeed * @return false failed */ - bool readLabelFile(const std::string & filepath, int & tlr_id, int & num_class); + bool readLabelFile( + const std::string & filepath, std::vector & tlr_label_id_, int & num_class); // variables image_transport::SubscriberFilter image_sub_; @@ -162,7 +164,7 @@ class TrafficLightFineDetectorNodelet : public rclcpp::Node bool is_approximate_sync_; double score_thresh_; - int tlr_id_; + std::vector tlr_label_id_; int batch_size_; std::unique_ptr trt_yolox_; diff --git a/perception/traffic_light_fine_detector/src/nodelet.cpp b/perception/traffic_light_fine_detector/src/nodelet.cpp index 86e77bc31cd71..257e9fc7ea99d 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_fine_detector/src/nodelet.cpp @@ -73,7 +73,7 @@ TrafficLightFineDetectorNodelet::TrafficLightFineDetectorNodelet( float nms_threshold = declare_parameter("fine_detector_nms_thresh", 0.65); is_approximate_sync_ = this->declare_parameter("approximate_sync", false); - if (!readLabelFile(label_path, tlr_id_, num_class)) { + if (!readLabelFile(label_path, tlr_label_id_, num_class)) { RCLCPP_ERROR(this->get_logger(), "Could not find tlr id"); } @@ -157,6 +157,7 @@ void TrafficLightFineDetectorNodelet::callback( tensorrt_yolox::ObjectArrays inference_results; std::vector lts; std::vector roi_ids; + for (size_t roi_i = 0; roi_i < rough_roi_msg->rois.size(); roi_i++) { const auto & rough_roi = rough_roi_msg->rois[roi_i]; cv::Point lt(rough_roi.roi.x_offset, rough_roi.roi.y_offset); @@ -178,7 +179,7 @@ void TrafficLightFineDetectorNodelet::callback( trt_yolox_->doMultiScaleInference(original_image, inference_results, rois); for (size_t batch_i = 0; batch_i < true_batch_size; batch_i++) { for (const tensorrt_yolox::Object & detection : inference_results[batch_i]) { - if (detection.score < score_thresh_ || detection.type != tlr_id_) { + if (detection.score < score_thresh_) { continue; } cv::Point lt_roi( @@ -190,6 +191,7 @@ void TrafficLightFineDetectorNodelet::callback( det.y_offset = lt_roi.y; det.width = rb_roi.x - lt_roi.x; det.height = rb_roi.y - lt_roi.y; + det.type = detection.type; id2detections[roi_ids[batch_i]].push_back(det); } } @@ -273,6 +275,7 @@ void TrafficLightFineDetectorNodelet::detectionMatch( for (const auto & p : bestDetections) { TrafficLightRoi tlr; tlr.traffic_light_id = p.first; + tlr.traffic_light_type = id2expectRoi[p.first].traffic_light_type; tlr.roi.x_offset = p.second.x_offset; tlr.roi.y_offset = p.second.y_offset; tlr.roi.width = p.second.width; @@ -318,9 +321,8 @@ bool TrafficLightFineDetectorNodelet::fitInFrame( } bool TrafficLightFineDetectorNodelet::readLabelFile( - const std::string & filepath, int & tlr_id, int & num_class) + const std::string & filepath, std::vector & tlr_label_id_, int & num_class) { - tlr_id = -1; std::ifstream labelsFile(filepath); if (!labelsFile.is_open()) { RCLCPP_ERROR(this->get_logger(), "Could not open label file. [%s]", filepath.c_str()); @@ -329,13 +331,13 @@ bool TrafficLightFineDetectorNodelet::readLabelFile( std::string label; int idx = 0; while (getline(labelsFile, label)) { - if (label == "traffic_light") { - tlr_id = idx; + if (label == "traffic_light" || label == "pedestrian_traffic_light") { + tlr_label_id_.push_back(idx); } idx++; } num_class = idx; - return tlr_id != -1; + return tlr_label_id_.size() != 0; } } // namespace traffic_light diff --git a/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml b/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml index 2b8ff4aa737b7..2eb671c1f5150 100644 --- a/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml +++ b/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml @@ -6,3 +6,5 @@ max_vibration_width: 0.5 # -0.25 ~ 0.25 m max_vibration_depth: 0.5 # -0.25 ~ 0.25 m max_detection_range: 200.0 + car_traffic_light_max_angle_range: 40.0 + pedestrian_traffic_light_max_angle_range: 80.0 diff --git a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp index e46431a7b199e..0d2b7ffeb7597 100644 --- a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp +++ b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp @@ -58,6 +58,8 @@ class MapBasedDetector : public rclcpp::Node double max_timestamp_offset; double timestamp_sample_len; double max_detection_range; + double car_traffic_light_max_angle_range; + double pedestrian_traffic_light_max_angle_range; }; struct IdLessThan @@ -93,10 +95,14 @@ class MapBasedDetector : public rclcpp::Node std::shared_ptr all_traffic_lights_ptr_; std::shared_ptr route_traffic_lights_ptr_; + std::set pedestrian_tl_id_; + lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; lanelet::routing::RoutingGraphPtr routing_graph_ptr_; + std::shared_ptr overall_graphs_ptr_; + Config config_; /** * @brief Calculated the transform from map to frame_id at timestamp t diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index f0a5d7b7b1fde..5689e55dca316 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -137,6 +137,10 @@ MapBasedDetector::MapBasedDetector(const rclcpp::NodeOptions & node_options) config_.max_timestamp_offset = declare_parameter("max_timestamp_offset", 0.0); config_.timestamp_sample_len = declare_parameter("timestamp_sample_len", 0.01); config_.max_detection_range = declare_parameter("max_detection_range", 200.0); + config_.car_traffic_light_max_angle_range = + declare_parameter("car_traffic_light_max_angle_range", 40.0); + config_.pedestrian_traffic_light_max_angle_range = + declare_parameter("pedestrian_traffic_light_max_angle_range", 80.0); if (config_.max_detection_range <= 0) { RCLCPP_ERROR_STREAM( @@ -287,6 +291,11 @@ bool MapBasedDetector::getTrafficLightRoi( tier4_perception_msgs::msg::TrafficLightRoi & roi) const { roi.traffic_light_id = traffic_light.id(); + if (pedestrian_tl_id_.find(traffic_light.id()) != pedestrian_tl_id_.end()) { + roi.traffic_light_type = tier4_perception_msgs::msg::TrafficLightRoi::PEDESTRIAN_TRAFFIC_LIGHT; + } else { + roi.traffic_light_type = tier4_perception_msgs::msg::TrafficLightRoi::CAR_TRAFFIC_LIGHT; + } // for roi.x_offset and roi.y_offset { @@ -392,7 +401,6 @@ void MapBasedDetector::mapCallback( for (auto tl_itr = all_lanelet_traffic_lights.begin(); tl_itr != all_lanelet_traffic_lights.end(); ++tl_itr) { lanelet::AutowareTrafficLightConstPtr tl = *tl_itr; - auto lights = tl->trafficLights(); for (auto lsp : lights) { if (!lsp.isLineString()) { // traffic lights must be linestrings @@ -401,6 +409,28 @@ void MapBasedDetector::mapCallback( all_traffic_lights_ptr_->insert(static_cast(lsp)); } } + + auto crosswalk_lanelets = lanelet::utils::query::crosswalkLanelets(all_lanelets); + for (const auto & tl : lanelet::utils::query::autowareTrafficLights(crosswalk_lanelets)) { + for (const auto & lsp : tl->trafficLights()) { + if (lsp.isLineString()) { // traffic lights must be linestrings + pedestrian_tl_id_.insert(lsp.id()); + } + } + } + + // crosswalk + const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + const auto pedestrian_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Pedestrian); + lanelet::routing::RoutingGraphConstPtr vehicle_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *traffic_rules); + lanelet::routing::RoutingGraphConstPtr pedestrian_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_rules); + lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); + overall_graphs_ptr_ = + std::make_shared(overall_graphs); } void MapBasedDetector::routeCallback( @@ -436,6 +466,34 @@ void MapBasedDetector::routeCallback( route_traffic_lights_ptr_->insert(static_cast(lsp)); } } + + // crosswalk trafficlights + lanelet::ConstLanelets conflicting_crosswalks; + pedestrian_tl_id_.clear(); + + for (const auto & route_lanelet : route_lanelets) { + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto conflict_lls = + overall_graphs_ptr_->conflictingInGraph(route_lanelet, PEDESTRIAN_GRAPH_ID); + for (const auto & lanelet : conflict_lls) { + conflicting_crosswalks.push_back(lanelet); + } + } + std::vector crosswalk_lanelet_traffic_lights = + lanelet::utils::query::autowareTrafficLights(conflicting_crosswalks); + for (auto tl_itr = crosswalk_lanelet_traffic_lights.begin(); + tl_itr != crosswalk_lanelet_traffic_lights.end(); ++tl_itr) { + lanelet::AutowareTrafficLightConstPtr tl = *tl_itr; + + auto lights = tl->trafficLights(); + for (auto lsp : lights) { + if (!lsp.isLineString()) { // traffic lights must be linestrings + continue; + } + route_traffic_lights_ptr_->insert(static_cast(lsp)); + pedestrian_tl_id_.insert(lsp.id()); + } + } } void MapBasedDetector::getVisibleTrafficLights( @@ -445,12 +503,19 @@ void MapBasedDetector::getVisibleTrafficLights( std::vector & visible_traffic_lights) const { for (const auto & traffic_light : all_traffic_lights) { - // some "Traffic Light" are actually not traffic lights if ( traffic_light.hasAttribute("subtype") == false || traffic_light.attribute("subtype").value() == "solid") { continue; } + // set different max angle range for ped and car traffic light + double max_angle_range; + if (pedestrian_tl_id_.find(traffic_light.id()) != pedestrian_tl_id_.end()) { + max_angle_range = + tier4_autoware_utils::deg2rad(config_.pedestrian_traffic_light_max_angle_range); + } else { + max_angle_range = tier4_autoware_utils::deg2rad(config_.car_traffic_light_max_angle_range); + } // traffic light bottom left const auto & tl_bl = traffic_light.front(); // traffic light bottom right @@ -467,7 +532,6 @@ void MapBasedDetector::getVisibleTrafficLights( // check angle range const double tl_yaw = tier4_autoware_utils::normalizeRadian( std::atan2(tl_br.y() - tl_bl.y(), tl_br.x() - tl_bl.x()) + M_PI_2); - constexpr double max_angle_range = tier4_autoware_utils::deg2rad(40.0); // get direction of z axis tf2::Vector3 camera_z_dir(0, 0, 1); diff --git a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp index 71930c65f88c9..ae6ba8c1feedb 100644 --- a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp +++ b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/nodelet.hpp @@ -37,6 +37,7 @@ #include #include #include +#include namespace traffic_light { @@ -71,7 +72,8 @@ class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node const tier4_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr in_signal_msg, const tier4_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr in_roi_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr in_cam_info_msg, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr in_cloud_msg); + const sensor_msgs::msg::PointCloud2::ConstSharedPtr in_cloud_msg, + const uint8_t traffic_light_type); rclcpp::Subscription::SharedPtr map_sub_; /** @@ -89,7 +91,6 @@ class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node * */ std::shared_ptr cloud_occlusion_predictor_; - typedef perception_utils::PrimeSynchronizer< tier4_perception_msgs::msg::TrafficSignalArray, tier4_perception_msgs::msg::TrafficLightRoiArray, sensor_msgs::msg::CameraInfo, @@ -97,6 +98,11 @@ class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node SynchronizerType; std::shared_ptr synchronizer_; + std::shared_ptr synchronizer_ped_; + + std::vector subscribed_; + std::vector occlusion_ratios_; + tier4_perception_msgs::msg::TrafficSignalArray out_msg_; }; } // namespace traffic_light #endif // TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__NODELET_HPP_ diff --git a/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml b/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml index 795267b920e24..aa7e34f8a4a09 100644 --- a/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml +++ b/perception/traffic_light_occlusion_predictor/launch/traffic_light_occlusion_predictor.launch.xml @@ -4,7 +4,8 @@ - + + @@ -13,7 +14,8 @@ - + + diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index c460f6a623bc4..97fa98ab18a72 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -57,6 +58,7 @@ TrafficLightOcclusionPredictorNodelet::TrafficLightOcclusionPredictorNodelet( // publishers signal_pub_ = create_publisher("~/output/traffic_signals", 1); + // configuration parameters config_.azimuth_occlusion_resolution_deg = declare_parameter("azimuth_occlusion_resolution_deg", 0.15); @@ -72,12 +74,26 @@ TrafficLightOcclusionPredictorNodelet::TrafficLightOcclusionPredictorNodelet( config_.elevation_occlusion_resolution_deg); const std::vector topics{ - "~/input/traffic_signals", "~/input/rois", "~/input/camera_info", "~/input/cloud"}; + "~/input/car/traffic_signals", "~/input/rois", "~/input/camera_info", "~/input/cloud"}; const std::vector qos(topics.size(), rclcpp::SensorDataQoS()); synchronizer_ = std::make_shared( this, topics, qos, - std::bind(&TrafficLightOcclusionPredictorNodelet::syncCallback, this, _1, _2, _3, _4), + std::bind( + &TrafficLightOcclusionPredictorNodelet::syncCallback, this, _1, _2, _3, _4, + tier4_perception_msgs::msg::TrafficLightRoi::CAR_TRAFFIC_LIGHT), + config_.max_image_cloud_delay, config_.max_wait_t); + + const std::vector topics_ped{ + "~/input/pedestrian/traffic_signals", "~/input/rois", "~/input/camera_info", "~/input/cloud"}; + const std::vector qos_ped(topics_ped.size(), rclcpp::SensorDataQoS()); + synchronizer_ped_ = std::make_shared( + this, topics_ped, qos_ped, + std::bind( + &TrafficLightOcclusionPredictorNodelet::syncCallback, this, _1, _2, _3, _4, + tier4_perception_msgs::msg::TrafficLightRoi::PEDESTRIAN_TRAFFIC_LIGHT), config_.max_image_cloud_delay, config_.max_wait_t); + + subscribed_.resize(2, false); } void TrafficLightOcclusionPredictorNodelet::mapCallback( @@ -109,27 +125,53 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( const tier4_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr in_signal_msg, const tier4_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr in_roi_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr in_cam_info_msg, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr in_cloud_msg) + const sensor_msgs::msg::PointCloud2::ConstSharedPtr in_cloud_msg, + const uint8_t traffic_light_type) { - tier4_perception_msgs::msg::TrafficSignalArray out_msg = *in_signal_msg; std::vector occlusion_ratios; - if ( - in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr || - in_roi_msg->rois.size() != in_signal_msg->signals.size()) { - occlusion_ratios.resize(out_msg.signals.size(), 0); + if (in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr) { + occlusion_ratios.resize(in_signal_msg->signals.size(), 0); } else { - cloud_occlusion_predictor_->predict( - in_cam_info_msg, in_roi_msg, in_cloud_msg, tf_buffer_, traffic_light_position_map_, - occlusion_ratios); + tier4_perception_msgs::msg::TrafficLightRoiArray selected_roi_msg; + selected_roi_msg.rois.reserve(in_roi_msg->rois.size()); + for (size_t i = 0; i < in_roi_msg->rois.size(); ++i) { + if (in_roi_msg->rois.at(i).traffic_light_type == traffic_light_type) { + selected_roi_msg.rois.push_back(in_roi_msg->rois.at(i)); + } + } + + tier4_perception_msgs::msg::TrafficSignalArray out_msg = *in_signal_msg; + + if (selected_roi_msg.rois.size() != in_signal_msg->signals.size()) { + occlusion_ratios.resize(in_signal_msg->signals.size(), 0); + } else { + auto selected_roi_msg_ptr = + std::make_shared(selected_roi_msg); + cloud_occlusion_predictor_->predict( + in_cam_info_msg, selected_roi_msg_ptr, in_cloud_msg, tf_buffer_, + traffic_light_position_map_, occlusion_ratios); + } } + + size_t predicted_num = out_msg_.signals.size(); + for (size_t i = 0; i < occlusion_ratios.size(); i++) { + out_msg_.signals.push_back(in_signal_msg->signals.at(i)); + if (occlusion_ratios[i] >= config_.max_occlusion_ratio) { - traffic_light_utils::setSignalUnknown(out_msg.signals[i]); + traffic_light_utils::setSignalUnknown(out_msg_.signals.at(predicted_num + i), 0.0); } } - signal_pub_->publish(out_msg); + subscribed_.at(traffic_light_type) = true; + + if (std::all_of(subscribed_.begin(), subscribed_.end(), [](bool v) { return v; })) { + auto pub_msg = std::make_unique(out_msg_); + pub_msg->header = in_signal_msg->header; + signal_pub_->publish(std::move(pub_msg)); + out_msg_.signals.clear(); + std::fill(subscribed_.begin(), subscribed_.end(), false); + } } - } // namespace traffic_light #include From b076eb7889f24792540206c339c2e47a1b11c15c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 29 Dec 2023 15:37:41 +0900 Subject: [PATCH 18/32] fix(surround_obstacle_checker): remove the virtual wall from the node (#5991) Signed-off-by: Takayuki Murooka --- .../debug_marker.hpp | 2 -- .../src/debug_marker.cpp | 23 ------------------- 2 files changed, 25 deletions(-) diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp index e71ff302e1b60..07a23a4ac2e67 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp @@ -71,7 +71,6 @@ class SurroundObstacleCheckerDebugNode const double back_distance); private: - rclcpp::Publisher::SharedPtr debug_virtual_wall_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr stop_reason_pub_; rclcpp::Publisher::SharedPtr velocity_factor_pub_; @@ -89,7 +88,6 @@ class SurroundObstacleCheckerDebugNode double surround_check_hysteresis_distance_; geometry_msgs::msg::Pose self_pose_; - MarkerArray makeVirtualWallMarker(); MarkerArray makeVisualizationMarker(); StopReasonArray makeStopReasonArray(); VelocityFactorArray makeVelocityFactorArray(); diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/surround_obstacle_checker/src/debug_marker.cpp index 6c005aea5d21c..a0ca15d490e9c 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/surround_obstacle_checker/src/debug_marker.cpp @@ -15,7 +15,6 @@ #include "surround_obstacle_checker/debug_marker.hpp" #include -#include #include #include #ifdef ROS_DISTRO_GALACTIC @@ -52,7 +51,6 @@ Polygon2d createSelfPolygon( } } // namespace -using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::createDefaultMarker; @@ -76,8 +74,6 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( self_pose_(self_pose), clock_(clock) { - debug_virtual_wall_pub_ = - node.create_publisher("~/virtual_wall", 1); debug_viz_pub_ = node.create_publisher("~/debug/marker", 1); stop_reason_pub_ = node.create_publisher("~/output/stop_reasons", 1); velocity_factor_pub_ = @@ -141,10 +137,6 @@ void SurroundObstacleCheckerDebugNode::publishFootprints() void SurroundObstacleCheckerDebugNode::publish() { - /* publish virtual_wall marker for rviz */ - const auto virtual_wall_msg = makeVirtualWallMarker(); - debug_virtual_wall_pub_->publish(virtual_wall_msg); - /* publish debug marker for rviz */ const auto visualization_msg = makeVisualizationMarker(); debug_viz_pub_->publish(visualization_msg); @@ -160,21 +152,6 @@ void SurroundObstacleCheckerDebugNode::publish() stop_obstacle_point_ptr_ = nullptr; } -MarkerArray SurroundObstacleCheckerDebugNode::makeVirtualWallMarker() -{ - MarkerArray msg; - rclcpp::Time current_time = this->clock_->now(); - - // visualize stop line - if (stop_pose_ptr_ != nullptr) { - const auto p = calcOffsetPose(*stop_pose_ptr_, base_link2front_, 0.0, 0.0); - const auto markers = createStopVirtualWallMarker(p, "surround obstacle", current_time, 0); - appendMarkerArray(markers, &msg); - } - - return msg; -} - MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() { MarkerArray msg; From 3b6dad33c7ea6a31036ac099db3d1d7f03712ca2 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Sun, 31 Dec 2023 13:55:04 +0100 Subject: [PATCH 19/32] feat(joy_controller): add xbox one joy mapping (#5999) * feat(joy_controller): add xbox one joy mapping Signed-off-by: amadeuszsz * fix(joy_controller): change to XBOX joy name Signed-off-by: amadeuszsz * fix(joy_controller): buttons naming Signed-off-by: amadeuszsz --------- Signed-off-by: amadeuszsz --- control/joy_controller/README.md | 22 +++++ .../joy_converter/xbox_joy_converter.hpp | 81 +++++++++++++++++++ .../launch/joy_controller.launch.xml | 2 +- .../schema/joy_controller.schema.json | 2 +- .../joy_controller/joy_controller_node.cpp | 3 + 5 files changed, 108 insertions(+), 2 deletions(-) create mode 100644 control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index 4518de37b7034..d1215481188c7 100644 --- a/control/joy_controller/README.md +++ b/control/joy_controller/README.md @@ -85,3 +85,25 @@ | Autoware Disengage | â—‹ | | Vehicle Engage | â–³ | | Vehicle Disengage | â–³ | + +## XBOX Joystick Key Map + +| Action | Button | +| -------------------- | --------------------- | +| Acceleration | RT | +| Brake | LT | +| Steering | Left Stick Left Right | +| Shift up | Cursor Up | +| Shift down | Cursor Down | +| Shift Drive | Cursor Left | +| Shift Reverse | Cursor Right | +| Turn Signal Left | LB | +| Turn Signal Right | RB | +| Clear Turn Signal | A | +| Gate Mode | B | +| Emergency Stop | View | +| Clear Emergency Stop | Menu | +| Autoware Engage | X | +| Autoware Disengage | Y | +| Vehicle Engage | Left Stick Button | +| Vehicle Disengage | Right Stick Button | diff --git a/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp b/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp new file mode 100644 index 0000000000000..a009ee1e12975 --- /dev/null +++ b/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp @@ -0,0 +1,81 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#define JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ + +#include "joy_controller/joy_converter/joy_converter_base.hpp" + +#include + +namespace joy_controller +{ +class XBOXJoyConverter : public JoyConverterBase +{ +public: + explicit XBOXJoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} + + float accel() const { return std::max(0.0f, -((RT() - 1.0f) / 2.0f)); } + + float brake() const { return std::max(0.0f, -((LT() - 1.0f) / 2.0f)); } + + float steer() const { return LStickLeftRight(); } + + bool shift_up() const { return CursorUpDown() == 1.0f; } + bool shift_down() const { return CursorUpDown() == -1.0f; } + bool shift_drive() const { return CursorLeftRight() == 1.0f; } + bool shift_reverse() const { return CursorLeftRight() == -1.0f; } + + bool turn_signal_left() const { return LB(); } + bool turn_signal_right() const { return RB(); } + bool clear_turn_signal() const { return A(); } + + bool gate_mode() const { return B(); } + + bool emergency_stop() const { return ChangeView(); } + bool clear_emergency_stop() const { return Menu(); } + + bool autoware_engage() const { return X(); } + bool autoware_disengage() const { return Y(); } + + bool vehicle_engage() const { return LStickButton(); } + bool vehicle_disengage() const { return RStickButton(); } + +private: + float LStickLeftRight() const { return j_.axes.at(0); } + float LStickUpDown() const { return j_.axes.at(1); } + float RStickLeftRight() const { return j_.axes.at(2); } + float RStickUpDown() const { return j_.axes.at(3); } + float RT() const { return j_.axes.at(4); } + float LT() const { return j_.axes.at(5); } + float CursorLeftRight() const { return j_.axes.at(6); } + float CursorUpDown() const { return j_.axes.at(7); } + + bool A() const { return j_.buttons.at(0); } + bool B() const { return j_.buttons.at(1); } + bool X() const { return j_.buttons.at(3); } + bool Y() const { return j_.buttons.at(4); } + bool LB() const { return j_.buttons.at(6); } + bool RB() const { return j_.buttons.at(7); } + bool Menu() const { return j_.buttons.at(11); } + bool LStickButton() const { return j_.buttons.at(13); } + bool RStickButton() const { return j_.buttons.at(14); } + bool ChangeView() const { return j_.buttons.at(15); } + bool Xbox() const { return j_.buttons.at(16); } + + const sensor_msgs::msg::Joy j_; +}; +} // namespace joy_controller + +#endif // JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/launch/joy_controller.launch.xml b/control/joy_controller/launch/joy_controller.launch.xml index f719d8bd78cb5..d2804a7339046 100644 --- a/control/joy_controller/launch/joy_controller.launch.xml +++ b/control/joy_controller/launch/joy_controller.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/control/joy_controller/schema/joy_controller.schema.json b/control/joy_controller/schema/joy_controller.schema.json index c67c575a6bd41..803997ced6dae 100644 --- a/control/joy_controller/schema/joy_controller.schema.json +++ b/control/joy_controller/schema/joy_controller.schema.json @@ -10,7 +10,7 @@ "type": "string", "description": "Joy controller type", "default": "DS4", - "enum": ["P65", "DS4", "G29"] + "enum": ["P65", "DS4", "G29", "XBOX"] }, "update_rate": { "type": "number", diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index 897986a7a41cf..d82967324aef7 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -16,6 +16,7 @@ #include "joy_controller/joy_converter/ds4_joy_converter.hpp" #include "joy_controller/joy_converter/g29_joy_converter.hpp" #include "joy_controller/joy_converter/p65_joy_converter.hpp" +#include "joy_controller/joy_converter/xbox_joy_converter.hpp" #include @@ -154,6 +155,8 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt joy_ = std::make_shared(*msg); } else if (joy_type_ == "DS4") { joy_ = std::make_shared(*msg); + } else if (joy_type_ == "XBOX") { + joy_ = std::make_shared(*msg); } else { joy_ = std::make_shared(*msg); } From ed8ffea6843ada32afea7f3ec8e9c7df111306f5 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Sun, 31 Dec 2023 13:58:31 +0100 Subject: [PATCH 20/32] feat(joy_controller): allow to control without odometry (#6000) * feat(joy_controller): allow to control without odometry Signed-off-by: amadeuszsz * style(pre-commit): autofix --------- Signed-off-by: amadeuszsz Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- control/joy_controller/README.md | 1 + .../config/joy_controller.param.yaml | 1 + .../include/joy_controller/joy_controller.hpp | 1 + .../schema/joy_controller.schema.json | 6 ++++++ .../src/joy_controller/joy_controller_node.cpp | 15 ++++++++++----- 5 files changed, 19 insertions(+), 5 deletions(-) diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index d1215481188c7..b8ee8ad79a33d 100644 --- a/control/joy_controller/README.md +++ b/control/joy_controller/README.md @@ -37,6 +37,7 @@ | `steering_angle_velocity` | double | steering angle velocity for operation | | `accel_sensitivity` | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | | `brake_sensitivity` | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | +| `raw_control` | bool | skip input odometry if true | | `velocity_gain` | double | ratio to calculate velocity by acceleration | | `max_forward_velocity` | double | absolute max velocity to go forward | | `max_backward_velocity` | double | absolute max velocity to go backward | diff --git a/control/joy_controller/config/joy_controller.param.yaml b/control/joy_controller/config/joy_controller.param.yaml index 8d48948a2d133..73a5d028985c5 100644 --- a/control/joy_controller/config/joy_controller.param.yaml +++ b/control/joy_controller/config/joy_controller.param.yaml @@ -9,6 +9,7 @@ accel_sensitivity: 1.0 brake_sensitivity: 1.0 control_command: + raw_control: false velocity_gain: 3.0 max_forward_velocity: 20.0 max_backward_velocity: 3.0 diff --git a/control/joy_controller/include/joy_controller/joy_controller.hpp b/control/joy_controller/include/joy_controller/joy_controller.hpp index c6de42afc38d9..22064f9cefaad 100644 --- a/control/joy_controller/include/joy_controller/joy_controller.hpp +++ b/control/joy_controller/include/joy_controller/joy_controller.hpp @@ -59,6 +59,7 @@ class AutowareJoyControllerNode : public rclcpp::Node double brake_sensitivity_; // ControlCommand Parameter + bool raw_control_; double velocity_gain_; double max_forward_velocity_; double max_backward_velocity_; diff --git a/control/joy_controller/schema/joy_controller.schema.json b/control/joy_controller/schema/joy_controller.schema.json index 803997ced6dae..d4c6351324935 100644 --- a/control/joy_controller/schema/joy_controller.schema.json +++ b/control/joy_controller/schema/joy_controller.schema.json @@ -55,6 +55,11 @@ "control_command": { "type": "object", "properties": { + "raw_control": { + "type": "boolean", + "description": "Whether to skip input odometry", + "default": false + }, "velocity_gain": { "type": "number", "description": "Ratio to calculate velocity by acceleration", @@ -79,6 +84,7 @@ } }, "required": [ + "raw_control", "velocity_gain", "max_forward_velocity", "max_backward_velocity", diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index d82967324aef7..5eec438032410 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -220,7 +220,7 @@ bool AutowareJoyControllerNode::isDataReady() } // Twist - { + if (!raw_control_) { if (!twist_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(), @@ -461,6 +461,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & steering_angle_velocity_ = declare_parameter("steering_angle_velocity"); accel_sensitivity_ = declare_parameter("accel_sensitivity"); brake_sensitivity_ = declare_parameter("brake_sensitivity"); + raw_control_ = declare_parameter("control_command.raw_control"); velocity_gain_ = declare_parameter("control_command.velocity_gain"); max_forward_velocity_ = declare_parameter("control_command.max_forward_velocity"); max_backward_velocity_ = declare_parameter("control_command.max_backward_velocity"); @@ -480,10 +481,14 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & sub_joy_ = this->create_subscription( "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), subscriber_option); - sub_odom_ = this->create_subscription( - "input/odometry", 1, - std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), - subscriber_option); + if (!raw_control_) { + sub_odom_ = this->create_subscription( + "input/odometry", 1, + std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), + subscriber_option); + } else { + twist_ = std::make_shared(); + } // Publisher pub_control_command_ = From 9fe2e15650fd02647a31d4726d7f4fb34bf7ee09 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 4 Jan 2024 17:13:04 +0900 Subject: [PATCH 21/32] refactor(intersection): cleanup utility function (#5972) Signed-off-by: Mamoru Sobue --- .../src/scene_intersection.cpp | 1422 +++++++++++++++-- .../src/scene_intersection.hpp | 490 +++++- .../src/scene_merge_from_private_road.cpp | 77 +- .../src/scene_merge_from_private_road.hpp | 8 +- .../src/util.cpp | 1389 +--------------- .../src/util.hpp | 176 +- .../src/util_type.hpp | 172 +- 7 files changed, 1885 insertions(+), 1849 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 631c03d923da0..292a6e6a8843d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -17,7 +17,11 @@ #include "util.hpp" #include +#include #include +#include +#include +#include #include #include #include @@ -37,10 +41,27 @@ #include #include +#include #include #include #include #include + +namespace tier4_autoware_utils +{ + +template <> +inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) +{ + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + return point; +} + +} // namespace tier4_autoware_utils + namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -71,6 +92,25 @@ Polygon2d createOneStepPolygon( } } // namespace +static bool isTargetStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) +{ + if ( + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + return true; + } + return false; +} + static bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) { @@ -376,7 +416,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const T & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); return; @@ -392,7 +432,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, - [[maybe_unused]] util::DebugData * debug_data) + [[maybe_unused]] DebugData * debug_data) { return; } @@ -403,7 +443,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -451,7 +491,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -483,7 +523,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -526,7 +566,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -577,7 +617,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -634,7 +674,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -681,7 +721,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -734,7 +774,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -776,7 +816,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -818,7 +858,7 @@ void reactRTCApproval( const IntersectionModule::DecisionResult & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { std::visit( VisitorSwitch{[&](const auto & decision) { @@ -868,7 +908,7 @@ static std::string formatDecisionResult(const IntersectionModule::DecisionResult bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { - debug_data_ = util::DebugData(); + debug_data_ = DebugData(); *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); // set default RTC @@ -938,6 +978,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + const auto & current_pose = planner_data_->current_odometry->pose; // spline interpolation const auto interpolated_path_info_opt = util::generateInterpolatedPath( @@ -952,25 +994,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } // cache intersection lane information because it is invariant - const auto & current_pose = planner_data_->current_odometry->pose; const auto lanelets_on_path = planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); if (!intersection_lanelets_) { - intersection_lanelets_ = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, - planner_param_.common.attention_area_length, - planner_param_.occlusion.occlusion_attention_area_length, - planner_param_.collision_detection.consider_wrong_direction_vehicle); + intersection_lanelets_ = + getObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path); } auto & intersection_lanelets = intersection_lanelets_.value(); - // at the very first time of registration of this module, the path may not be conflicting with the + // at the very first time of regisTration of this module, the path may not be conflicting with the // attention area, so update() is called to update the internal data as well as traffic light info const auto traffic_prioritized_level = - util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); + getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); const bool is_prioritized = - traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; - const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint, baselink2front); // this is abnormal @@ -985,21 +1022,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // generate all stop line candidates // see the doc for struct IntersectionStopLines - const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); /// even if the attention area is null, stuck vehicle stop line needs to be generated from /// conflicting lanes - const auto & dummy_first_attention_area = - first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area; - const auto & dummy_first_attention_lane_centerline = - intersection_lanelets.first_attention_lane() - ? intersection_lanelets.first_attention_lane().value().centerline2d() - : first_conflicting_lane.centerline2d(); - const auto intersection_stoplines_opt = util::generateIntersectionStopLines( - first_conflicting_area, dummy_first_attention_area, dummy_first_attention_lane_centerline, - planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, - planner_param_.common.default_stopline_margin, planner_param_.common.max_accel, - planner_param_.common.max_jerk, planner_param_.common.delay_response_time, - planner_param_.occlusion.peeking_offset, path); + const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() + ? intersection_lanelets.first_attention_lane().value() + : first_conflicting_lane; + + const auto intersection_stoplines_opt = generateIntersectionStopLines( + assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, interpolated_path_info, + path); if (!intersection_stoplines_opt) { return IntersectionModule::Indecisive{"failed to generate intersection_stoplines"}; } @@ -1010,15 +1041,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( default_pass_judge_line_idx, occlusion_wo_tl_pass_judge_line_idx] = intersection_stoplines; // see the doc for struct PathLanelets + const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); const auto & conflicting_area = intersection_lanelets.conflicting_area(); - const auto path_lanelets_opt = util::generatePathLanelets( - lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area, - conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx, - planner_data_->vehicle_info_.vehicle_width_m); + const auto path_lanelets_opt = generatePathLanelets( + lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, + first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); if (!path_lanelets_opt.has_value()) { return IntersectionModule::Indecisive{"failed to generate PathLanelets"}; } - const auto path_lanelets = path_lanelets_opt.value(); + const auto & path_lanelets = path_lanelets_opt.value(); // utility functions auto fromEgoDist = [&](const size_t index) { @@ -1054,7 +1085,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation - const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); + const bool stuck_detected = checkStuckVehicleInIntersection(path_lanelets, &debug_data_); const bool is_first_conflicting_lane_private = (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); if (stuck_detected) { @@ -1119,11 +1150,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // check occlusion on detection lane if (!occlusion_attention_divisions_) { - occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( + occlusion_attention_divisions_ = generateDetectionLaneDivisions( occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution, - planner_param_.occlusion.attention_lane_crop_curvature_threshold, - planner_param_.occlusion.attention_lane_curvature_calculation_ds); + planner_data_->occupancy_grid->info.resolution); } const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); @@ -1132,8 +1161,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // filter objects auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - const bool yield_stuck_detected = checkYieldStuckVehicle( - target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding_); + const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( + target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding(), + &debug_data_); if (yield_stuck_detected) { std::optional stopline_idx = std::nullopt; const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; @@ -1159,15 +1189,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } const double is_amber_or_red = - (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || - (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED); + (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || + (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); auto occlusion_status = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_amber_or_red) ? getOcclusionStatus( - *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, - first_attention_area, interpolated_path_info, occlusion_attention_divisions, - target_objects, current_pose, - planner_param_.occlusion.occlusion_required_clearance_distance) + occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, + occlusion_attention_divisions, target_objects) : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime( occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, @@ -1385,9 +1413,672 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } -bool IntersectionModule::checkStuckVehicle( - const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) +TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel( + lanelet::ConstLanelet lane, const std::map & tl_infos) +{ + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + + std::optional tl_id = std::nullopt; + for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { + tl_id = tl_reg_elem->id(); + break; + } + if (!tl_id) { + // this lane has no traffic light + return TrafficPrioritizedLevel::NOT_PRIORITIZED; + } + const auto tl_info_it = tl_infos.find(tl_id.value()); + if (tl_info_it == tl_infos.end()) { + // the info of this traffic light is not available + return TrafficPrioritizedLevel::NOT_PRIORITIZED; + } + const auto & tl_info = tl_info_it->second; + bool has_amber_signal{false}; + for (auto && tl_light : tl_info.signal.elements) { + if (tl_light.color == TrafficSignalElement::AMBER) { + has_amber_signal = true; + } + if (tl_light.color == TrafficSignalElement::RED) { + // NOTE: Return here since the red signal has the highest priority. + return TrafficPrioritizedLevel::FULLY_PRIORITIZED; + } + } + if (has_amber_signal) { + return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; + } + return TrafficPrioritizedLevel::NOT_PRIORITIZED; +} + +static std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec) +{ + std::vector polys; + for (auto && ll : ll_vec) { + polys.push_back(ll.polygon3d()); + } + return polys; +} + +IntersectionLanelets IntersectionModule::getObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path) +{ + const double detection_area_length = planner_param_.common.attention_area_length; + const double occlusion_detection_area_length = + planner_param_.occlusion.occlusion_attention_area_length; + const bool consider_wrong_direction_vehicle = + planner_param_.collision_detection.consider_wrong_direction_vehicle; + + // retrieve a stopline associated with a traffic light + bool has_traffic_light = false; + if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); + tl_reg_elems.size() != 0) { + const auto tl_reg_elem = tl_reg_elems.front(); + const auto stopline_opt = tl_reg_elem->stopLine(); + if (!!stopline_opt) has_traffic_light = true; + } + + // for low priority lane + // If ego_lane has right of way (i.e. is high priority), + // ignore yieldLanelets (i.e. low priority lanes) + lanelet::ConstLanelets yield_lanelets{}; + const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); + for (const auto & right_of_way : right_of_ways) { + if (lanelet::utils::contains(right_of_way->rightOfWayLanelets(), assigned_lanelet)) { + for (const auto & yield_lanelet : right_of_way->yieldLanelets()) { + yield_lanelets.push_back(yield_lanelet); + for (const auto & previous_lanelet : routing_graph_ptr->previous(yield_lanelet)) { + yield_lanelets.push_back(previous_lanelet); + } + } + } + } + + // get all following lanes of previous lane + lanelet::ConstLanelets ego_lanelets = lanelets_on_path; + for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { + ego_lanelets.push_back(previous_lanelet); + for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { + if (lanelet::utils::contains(ego_lanelets, following_lanelet)) { + continue; + } + ego_lanelets.push_back(following_lanelet); + } + } + + // get conflicting lanes on assigned lanelet + const auto & conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); + std::vector adjacent_followings; + + for (const auto & conflicting_lanelet : conflicting_lanelets) { + for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + } + + // final objective lanelets + lanelet::ConstLanelets detection_lanelets; + lanelet::ConstLanelets conflicting_ex_ego_lanelets; + // conflicting lanes is necessary to get stopline for stuck vehicle + for (auto && conflicting_lanelet : conflicting_lanelets) { + if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) + conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); + } + + // exclude yield lanelets and ego lanelets from detection_lanelets + if (turn_direction_ == std::string("straight") && has_traffic_light) { + // if assigned lanelet is "straight" with traffic light, detection area is not necessary + } else { + if (consider_wrong_direction_vehicle) { + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + for (const auto & adjacent_following : adjacent_followings) { + detection_lanelets.push_back(adjacent_following); + } + } else { + // otherwise we need to know the priority from RightOfWay + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if ( + lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || + lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + } + } + + // get possible lanelet path that reaches conflicting_lane longer than given length + lanelet::ConstLanelets detection_and_preceding_lanelets; + { + const double length = detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(l); + } + } + } + } + + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; + { + const double length = occlusion_detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); + } + } + } + } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; + for (const auto & ll : occlusion_detection_and_preceding_lanelets) { + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + if (turn_direction == "left" || turn_direction == "right") { + continue; + } + occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); + } + + auto [attention_lanelets, original_attention_lanelet_sequences] = + util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); + + IntersectionLanelets result; + result.attention_ = std::move(attention_lanelets); + for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { + // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that + // back() exists. + std::optional stopline{std::nullopt}; + for (auto it = original_attention_lanelet_seq.rbegin(); + it != original_attention_lanelet_seq.rend(); ++it) { + const auto traffic_lights = it->regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + break; + } + if (stopline) break; + } + result.attention_stoplines_.push_back(stopline); + } + result.attention_non_preceding_ = std::move(detection_lanelets); + for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { + std::optional stopline = std::nullopt; + const auto & ll = result.attention_non_preceding_.at(i); + const auto traffic_lights = ll.regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + } + result.attention_non_preceding_stoplines_.push_back(stopline); + } + result.conflicting_ = std::move(conflicting_ex_ego_lanelets); + result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_); + // NOTE: occlusion_attention is not inverted here + // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and + // then trim part of them based on curvature threshold + result.occlusion_attention_ = + std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); + + // NOTE: to properly update(), each element in conflicting_/conflicting_area_, + // attention_non_preceding_/attention_non_preceding_area_ need to be matched + result.attention_area_ = getPolygon3dFromLanelets(result.attention_); + result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); + result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); + result.adjacent_area_ = getPolygon3dFromLanelets(result.adjacent_); + result.occlusion_attention_area_ = getPolygon3dFromLanelets(result.occlusion_attention_); + return result; +} + +std::optional IntersectionModule::generateIntersectionStopLines( + lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, + const lanelet::ConstLanelet & first_attention_lane, + const util::InterpolatedPathInfo & interpolated_path_info, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) +{ + const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; + const double stopline_margin = planner_param_.common.default_stopline_margin; + const double max_accel = planner_param_.common.max_accel; + const double max_jerk = planner_param_.common.max_jerk; + const double delay_response_time = planner_param_.common.delay_response_time; + const double peeking_offset = planner_param_.occlusion.peeking_offset; + + const auto first_attention_area = first_attention_lane.polygon3d(); + const auto first_attention_lane_centerline = first_attention_lane.centerline2d(); + const auto & path_ip = interpolated_path_info.path; + const double ds = interpolated_path_info.ds; + const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + + const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); + const int base2front_idx_dist = + std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / ds); + + // find the index of the first point whose vehicle footprint on it intersects with detection_area + const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + std::optional first_footprint_inside_detection_ip_opt = + getFirstPointInsidePolygonByFootprint( + first_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (!first_footprint_inside_detection_ip_opt) { + return std::nullopt; + } + const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); + + std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; + for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { + // NOTE: maybe consideration of braking dist is necessary + first_footprint_attention_centerline_ip_opt = i; + break; + } + } + if (!first_footprint_attention_centerline_ip_opt) { + return std::nullopt; + } + const size_t first_footprint_attention_centerline_ip = + first_footprint_attention_centerline_ip_opt.value(); + + // (1) default stop line position on interpolated path + bool default_stopline_valid = true; + int stop_idx_ip_int = -1; + if (const auto map_stop_idx_ip = + getStopLineIndexFromMap(interpolated_path_info, assigned_lanelet); + map_stop_idx_ip) { + stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; + } + if (stop_idx_ip_int < 0) { + stop_idx_ip_int = first_footprint_inside_detection_ip - stopline_margin_idx_dist; + } + if (stop_idx_ip_int < 0) { + default_stopline_valid = false; + } + const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; + + // (2) ego front stop line position on interpolated path + const geometry_msgs::msg::Pose & current_pose = planner_data_->current_odometry->pose; + const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( + path_ip.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + + // (3) occlusion peeking stop line position on interpolated path + int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); + bool occlusion_peeking_line_valid = true; + // NOTE: if footprints[0] is already inside the detection area, invalid + { + const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; + const auto path_footprint0 = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + if (bg::intersects( + path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { + occlusion_peeking_line_valid = false; + } + } + if (occlusion_peeking_line_valid) { + occlusion_peeking_line_ip_int = + first_footprint_inside_detection_ip + std::ceil(peeking_offset / ds); + } + + const auto occlusion_peeking_line_ip = static_cast( + std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + const auto first_attention_stopline_ip = first_footprint_inside_detection_ip; + const bool first_attention_stopline_valid = true; + + // (4) pass judge line position on interpolated path + const double velocity = planner_data_->current_velocity->twist.linear.x; + const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x; + const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( + velocity, acceleration, max_accel, max_jerk, delay_response_time); + int pass_judge_ip_int = + static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); + const auto pass_judge_line_ip = static_cast( + std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + // TODO(Mamoru Sobue): maybe braking dist should be considered + const auto occlusion_wo_tl_pass_judge_line_ip = + static_cast(first_footprint_attention_centerline_ip); + + // (5) stuck vehicle stop line + int stuck_stopline_ip_int = 0; + bool stuck_stopline_valid = true; + if (use_stuck_stopline) { + // NOTE: when ego vehicle is approaching detection area and already passed + // first_conflicting_area, this could be null. + const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); + if (!stuck_stopline_idx_ip_opt) { + stuck_stopline_valid = false; + stuck_stopline_ip_int = 0; + } else { + stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; + } + } else { + stuck_stopline_ip_int = + std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); + } + if (stuck_stopline_ip_int < 0) { + stuck_stopline_valid = false; + } + const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); + + struct IntersectionStopLinesTemp + { + size_t closest_idx{0}; + size_t stuck_stopline{0}; + size_t default_stopline{0}; + size_t first_attention_stopline{0}; + size_t occlusion_peeking_stopline{0}; + size_t pass_judge_line{0}; + size_t occlusion_wo_tl_pass_judge_line{0}; + }; + + IntersectionStopLinesTemp intersection_stoplines_temp; + std::list> stoplines = { + {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, + {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, + {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, + {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, + {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, + {&pass_judge_line_ip, &intersection_stoplines_temp.pass_judge_line}, + {&occlusion_wo_tl_pass_judge_line_ip, + &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; + stoplines.sort( + [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); + for (const auto & [stop_idx_ip, stop_idx] : stoplines) { + const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; + const auto insert_idx = util::insertPointIndex( + insert_point, original_path, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + if (!insert_idx) { + return std::nullopt; + } + *stop_idx = insert_idx.value(); + } + if ( + intersection_stoplines_temp.occlusion_peeking_stopline < + intersection_stoplines_temp.default_stopline) { + intersection_stoplines_temp.occlusion_peeking_stopline = + intersection_stoplines_temp.default_stopline; + } + + IntersectionStopLines intersection_stoplines; + intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; + if (stuck_stopline_valid) { + intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; + } + if (default_stopline_valid) { + intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; + } + if (first_attention_stopline_valid) { + intersection_stoplines.first_attention_stopline = + intersection_stoplines_temp.first_attention_stopline; + } + if (occlusion_peeking_line_valid) { + intersection_stoplines.occlusion_peeking_stopline = + intersection_stoplines_temp.occlusion_peeking_stopline; + } + intersection_stoplines.pass_judge_line = intersection_stoplines_temp.pass_judge_line; + intersection_stoplines.occlusion_wo_tl_pass_judge_line = + intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; + return intersection_stoplines; +} + +/** + * @brief Get stop point from map if exists + * @param stop_pose stop point defined on map + * @return true when the stop point is defined on map. + */ +std::optional IntersectionModule::getStopLineIndexFromMap( + const util::InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet) +{ + const auto & path = interpolated_path_info.path; + const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); + + const auto road_markings = + assigned_lanelet.regulatoryElementsAs(); + lanelet::ConstLineStrings3d stopline; + for (const auto & road_marking : road_markings) { + const std::string type = + road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); + if (type == lanelet::AttributeValueString::StopLine) { + stopline.push_back(road_marking->roadMarking()); + break; // only one stopline exists. + } + } + if (stopline.empty()) { + return std::nullopt; + } + + const auto p_start = stopline.front().front(); + const auto p_end = stopline.front().back(); + const LineString2d extended_stopline = + planning_utils::extendLine(p_start, p_end, planner_data_->stop_line_extend_length); + + for (size_t i = lane_interval.first; i < lane_interval.second; i++) { + const auto & p_front = path.points.at(i).point.pose.position; + const auto & p_back = path.points.at(i + 1).point.pose.position; + + const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; + std::vector collision_points; + bg::intersection(extended_stopline, path_segment, collision_points); + + if (collision_points.empty()) { + continue; + } + + return i; + } + + geometry_msgs::msg::Pose stop_point_from_map; + stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); + stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); + stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); + + return motion_utils::findFirstNearestIndexWithSoftConstraints( + path.points, stop_point_from_map, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); +} + +static lanelet::ConstLanelets getPrevLanelets( + const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) +{ + lanelet::ConstLanelets previous_lanelets; + for (const auto & ll : lanelets_on_path) { + if (associative_ids.find(ll.id()) != associative_ids.end()) { + return previous_lanelets; + } + previous_lanelets.push_back(ll); + } + return previous_lanelets; +} + +// end inclusive +static lanelet::ConstLanelet generatePathLanelet( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, + const double interval) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + size_t prev_idx = start_idx; + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto & p = path.points.at(i).point.pose; + const auto & p_prev = path.points.at(prev_idx).point.pose; + if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { + continue; + } + prev_idx = i; + const double yaw = tf2::getYaw(p.orientation); + const double x = p.position.x; + const double y = p.position.y; + // NOTE: maybe this is opposite + const double left_x = x + width / 2 * std::sin(yaw); + const double left_y = y - width / 2 * std::cos(yaw); + const double right_x = x - width / 2 * std::sin(yaw); + const double right_y = y + width / 2 * std::cos(yaw); + lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); + rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); + } + lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); + lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); + + return lanelet::Lanelet(lanelet::InvalId, left, right); +} + +static std::optional> +getFirstPointInsidePolygons( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::pair lane_interval, + const std::vector & polygons, const bool search_forward = true) +{ + if (search_forward) { + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + } + } else { + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + if (i == 0) { + break; + } + } + } + return std::nullopt; +} + +std::optional IntersectionModule::generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx) { + const double width = planner_data_->vehicle_info_.vehicle_width_m; + static constexpr double path_lanelet_interval = 1.5; + + const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; + if (!assigned_lane_interval_opt) { + return std::nullopt; + } + const auto assigned_lane_interval = assigned_lane_interval_opt.value(); + const auto & path = interpolated_path_info.path; + + PathLanelets path_lanelets; + // prev + path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids_); + path_lanelets.all = path_lanelets.prev; + + // entry2ego if exist + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; + if (closest_idx > assigned_lane_start) { + path_lanelets.all.push_back( + generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); + } + + // ego_or_entry2exit + const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); + path_lanelets.ego_or_entry2exit = + generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); + + // next + if (assigned_lane_end < path.points.size() - 1) { + const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); + const auto next_lane_interval_opt = util::findLaneIdsInterval(path, {next_id}); + if (next_lane_interval_opt) { + const auto [next_start, next_end] = next_lane_interval_opt.value(); + path_lanelets.next = + generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.next.value()); + } + } + + const auto first_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) + : util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); + const auto last_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) + : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); + if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { + const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); + const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; + lanelet::ConstLanelet conflicting_interval = generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, + path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); + if (last_inside_conflicting_idx < assigned_lane_end) { + lanelet::ConstLanelet remaining_interval = generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); + } + } + return path_lanelets; +} + +bool IntersectionModule::checkStuckVehicleInIntersection( + const PathLanelets & path_lanelets, DebugData * debug_data) +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getLaneletLength3d; + using lanelet::utils::getPolygonFromArcLength; + using lanelet::utils::to2D; + const bool stuck_detection_direction = [&]() { return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || @@ -1397,22 +2088,146 @@ bool IntersectionModule::checkStuckVehicle( return false; } - const auto & objects_ptr = planner_data->predicted_objects; + const auto & objects_ptr = planner_data_->predicted_objects; // considering lane change in the intersection, these lanelets are generated from the path - const auto stuck_vehicle_detect_area = util::generateStuckVehicleDetectAreaPolygon( - path_lanelets, planner_param_.stuck_vehicle.stuck_vehicle_detect_dist); + const double stuck_vehicle_detect_dist = planner_param_.stuck_vehicle.stuck_vehicle_detect_dist; + Polygon2d stuck_vehicle_detect_area{}; + if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { + return false; + } + + double target_polygon_length = + getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); + lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; + if (path_lanelets.next) { + targets.push_back(path_lanelets.next.value()); + const double next_arc_length = + std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); + target_polygon_length += next_arc_length; + } + const auto target_polygon = + to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); + + if (target_polygon.empty()) { + return false; + } + + for (const auto & p : target_polygon) { + stuck_vehicle_detect_area.outer().emplace_back(p.x(), p.y()); + } + + stuck_vehicle_detect_area.outer().emplace_back(stuck_vehicle_detect_area.outer().front()); + bg::correct(stuck_vehicle_detect_area); + debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - return util::checkStuckVehicleInIntersection( - objects_ptr, stuck_vehicle_detect_area, - planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, &debug_data_); + for (const auto & object : objects_ptr->objects) { + if (!isTargetStuckVehicleType(object)) { + continue; // not target vehicle type + } + const auto obj_v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (obj_v_norm > planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold) { + continue; // not stop vehicle + } + + // check if the footprint is in the stuck detect area + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); + if (is_in_stuck_area && debug_data) { + debug_data->stuck_targets.objects.push_back(object); + return true; + } + } + return false; } -bool IntersectionModule::checkYieldStuckVehicle( - const util::TargetObjects & target_objects, - const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets) +static lanelet::LineString3d getLineStringFromArcLength( + const lanelet::ConstLineString3d & linestring, const double s1, const double s2) +{ + lanelet::Points3d points; + double accumulated_length = 0; + size_t start_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s1) { + start_index = i; + break; + } + accumulated_length += length; + } + if (start_index < linestring.size() - 1) { + const auto & p1 = linestring[start_index]; + const auto & p2 = linestring[start_index + 1]; + const double residue = s1 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto start_basic_point = p1.basicPoint() + residue * direction_vector; + const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); + points.push_back(start_point); + } + + accumulated_length = 0; + size_t end_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s2) { + end_index = i; + break; + } + accumulated_length += length; + } + + for (size_t i = start_index + 1; i < end_index; i++) { + const auto p = lanelet::Point3d(linestring[i]); + points.push_back(p); + } + if (end_index < linestring.size() - 1) { + const auto & p1 = linestring[end_index]; + const auto & p2 = linestring[end_index + 1]; + const double residue = s2 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto end_basic_point = p1.basicPoint() + residue * direction_vector; + const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); + points.push_back(end_point); + } + return lanelet::LineString3d{lanelet::InvalId, points}; +} + +static lanelet::ConstLanelet createLaneletFromArcLength( + const lanelet::ConstLanelet & lanelet, const double s1, const double s2) +{ + const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); + // make sure that s1, and s2 are between [0, lane_length] + const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); + const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); + + const auto ratio_s1 = s1_saturated / total_length; + const auto ratio_s2 = s2_saturated / total_length; + + const auto s1_left = + static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s2_left = + static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s1_right = + static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); + const auto s2_right = + static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); + + const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); + const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); + + return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); +} + +bool IntersectionModule::checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data) { const bool yield_stuck_detection_direction = [&]() { return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) || @@ -1423,20 +2238,74 @@ bool IntersectionModule::checkYieldStuckVehicle( return false; } - return util::checkYieldStuckVehicleInIntersection( - target_objects, interpolated_path_info, attention_lanelets, turn_direction_, - planner_data_->vehicle_info_.vehicle_width_m, - planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, - planner_param_.yield_stuck.distance_threshold, &debug_data_); + const double width = planner_data_->vehicle_info_.vehicle_width_m; + const double stuck_vehicle_vel_thr = + planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold; + const double yield_stuck_distance_thr = planner_param_.yield_stuck.distance_threshold; + + LineString2d sparse_intersection_path; + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + for (unsigned i = start; i < end; ++i) { + const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; + const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); + if (turn_direction_ == "right") { + const double right_x = point.x - width / 2 * std::sin(yaw); + const double right_y = point.y + width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(right_x, right_y); + } else if (turn_direction_ == "left") { + const double left_x = point.x + width / 2 * std::sin(yaw); + const double left_y = point.y - width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(left_x, left_y); + } else { + // straight + sparse_intersection_path.emplace_back(point.x, point.y); + } + } + lanelet::ConstLanelets yield_stuck_detect_lanelets; + for (const auto & attention_lanelet : attention_lanelets) { + const auto centerline = attention_lanelet.centerline2d().basicLineString(); + std::vector intersects; + bg::intersection(sparse_intersection_path, centerline, intersects); + if (intersects.empty()) { + continue; + } + const auto intersect = intersects.front(); + const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( + centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); + const double yield_stuck_start = + std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); + const double yield_stuck_end = intersect_arc_coords.length; + yield_stuck_detect_lanelets.push_back( + createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); + } + debug_data->yield_stuck_detect_area = getPolygon3dFromLanelets(yield_stuck_detect_lanelets); + for (const auto & object : target_objects.all_attention_objects) { + const auto obj_v_norm = std::hypot( + object.object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.object.kinematics.initial_twist_with_covariance.twist.linear.y); + + if (obj_v_norm > stuck_vehicle_vel_thr) { + continue; + } + for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { + const bool is_in_lanelet = lanelet::utils::isInLanelet( + object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); + if (is_in_lanelet) { + debug_data->yield_stuck_targets.objects.push_back(object.object); + return true; + } + } + } + return false; } -util::TargetObjects IntersectionModule::generateTargetObjects( - const util::IntersectionLanelets & intersection_lanelets, +TargetObjects IntersectionModule::generateTargetObjects( + const IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const { const auto & objects_ptr = planner_data_->predicted_objects; // extract target objects - util::TargetObjects target_objects; + TargetObjects target_objects; target_objects.header = objects_ptr->header; const auto & attention_lanelets = intersection_lanelets.attention(); const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); @@ -1449,10 +2318,8 @@ util::TargetObjects IntersectionModule::generateTargetObjects( // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto belong_adjacent_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, false); + const auto belong_adjacent_lanelet_id = + checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); if (belong_adjacent_lanelet_id) { continue; } @@ -1466,33 +2333,28 @@ util::TargetObjects IntersectionModule::generateTargetObjects( const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); const auto intersection_area_2d = intersection_area.value(); - const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, attention_lanelets, planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, is_parked_vehicle); + const auto belong_attention_lanelet_id = + checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); if (belong_attention_lanelet_id) { const auto id = belong_attention_lanelet_id.value(); - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = attention_lanelets.at(id); target_object.stopline = attention_lanelet_stoplines.at(id); container.push_back(target_object); } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = std::nullopt; target_object.stopline = std::nullopt; target_objects.intersection_area_objects.push_back(target_object); } - } else if (const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, attention_lanelets, - planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, is_parked_vehicle); + } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( + object_direction, attention_lanelets, is_parked_vehicle); belong_attention_lanelet_id.has_value()) { // intersection_area is not available, use detection_area_with_margin as before const auto id = belong_attention_lanelet_id.value(); - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = attention_lanelets.at(id); target_object.stopline = attention_lanelet_stoplines.at(id); @@ -1512,10 +2374,10 @@ util::TargetObjects IntersectionModule::generateTargetObjects( } bool IntersectionModule::checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, - const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, + const PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const TrafficPrioritizedLevel & traffic_prioritized_level) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1523,14 +2385,8 @@ bool IntersectionModule::checkCollision( // check collision between target_objects predicted path and ego lane // cut the predicted path at passing_time tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; - const auto time_distance_array = util::calcIntersectionPassingTime( - path, planner_data_, lane_id_, associative_ids_, closest_idx, - last_intersection_stopline_candidate_idx, time_delay, - planner_param_.collision_detection.velocity_profile.default_velocity, - planner_param_.collision_detection.velocity_profile.minimum_default_velocity, - planner_param_.collision_detection.velocity_profile.use_upstream, - planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity, - &ego_ttc_time_array); + const auto time_distance_array = calcIntersectionPassingTime( + path, closest_idx, last_intersection_stopline_candidate_idx, time_delay, &ego_ttc_time_array); if ( std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != @@ -1540,7 +2396,7 @@ bool IntersectionModule::checkCollision( } const double passing_time = time_distance_array.back().first; - util::cutPredictPathWithDuration(target_objects, clock_, passing_time); + cutPredictPathWithDuration(target_objects, passing_time); const auto & concat_lanelets = path_lanelets.all; const auto closest_arc_coords = getArcCoordinates( @@ -1551,12 +2407,12 @@ bool IntersectionModule::checkCollision( // change TTC margin based on ego traffic light color const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { - if (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { return std::make_pair( planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); } - if (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { return std::make_pair( planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); @@ -1565,7 +2421,7 @@ bool IntersectionModule::checkCollision( planner_param_.collision_detection.not_prioritized.collision_start_margin_time, planner_param_.collision_detection.not_prioritized.collision_end_margin_time); }(); - const auto expectedToStopBeforeStopLine = [&](const util::TargetObject & target_object) { + const auto expectedToStopBeforeStopLine = [&](const TargetObject & target_object) { if (!target_object.dist_to_stopline) { return false; } @@ -1580,7 +2436,7 @@ bool IntersectionModule::checkCollision( .object_expected_deceleration)); return dist_to_stopline > braking_distance; }; - const auto isTolerableOvershoot = [&](const util::TargetObject & target_object) { + const auto isTolerableOvershoot = [&](const TargetObject & target_object) { if ( !target_object.attention_lanelet || !target_object.dist_to_stopline || !target_object.stopline) { @@ -1639,14 +2495,14 @@ bool IntersectionModule::checkCollision( // If the vehicle is expected to stop before their stopline, ignore const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); if ( - traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && expected_to_stop_before_stopline) { debug_data_.amber_ignore_targets.objects.push_back(object); continue; } const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); if ( - traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED && + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && !expected_to_stop_before_stopline && is_tolerable_overshoot) { debug_data_.red_overshoot_ignore_targets.objects.push_back(object); continue; @@ -1766,16 +2622,288 @@ bool IntersectionModule::checkCollision( return collision_detected; } +std::optional IntersectionModule::checkAngleForTargetLanelets( + + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const bool is_parked_vehicle) const +{ + const double detection_area_angle_thr = planner_param_.common.attention_area_angle_threshold; + const bool consider_wrong_direction_vehicle = + planner_param_.common.attention_area_angle_threshold; + const double dist_margin = planner_param_.common.attention_area_margin; + + for (unsigned i = 0; i < target_lanelets.size(); ++i) { + const auto & ll = target_lanelets.at(i); + if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { + continue; + } + const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); + const double pose_angle = tf2::getYaw(pose.orientation); + const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); + if (consider_wrong_direction_vehicle) { + if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + } else { + if (std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is + // positive + if ( + is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return std::make_optional(i); + } + } + } + return std::nullopt; +} + +void IntersectionModule::cutPredictPathWithDuration( + TargetObjects * target_objects, const double time_thr) +{ + const rclcpp::Time current_time = clock_->now(); + for (auto & target_object : target_objects->all_attention_objects) { // each objects + for (auto & predicted_path : + target_object.object.kinematics.predicted_paths) { // each predicted paths + const auto origin_path = predicted_path; + predicted_path.path.clear(); + + for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points + const auto & predicted_pose = origin_path.path.at(k); + const auto predicted_time = + rclcpp::Time(target_objects->header.stamp) + + rclcpp::Duration(origin_path.time_step) * static_cast(k); + if ((predicted_time - current_time).seconds() < time_thr) { + predicted_path.path.push_back(predicted_pose); + } + } + } + } +} + +TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) +{ + const double intersection_velocity = + planner_param_.collision_detection.velocity_profile.default_velocity; + const double minimum_ego_velocity = + planner_param_.collision_detection.velocity_profile.minimum_default_velocity; + const bool use_upstream_velocity = + planner_param_.collision_detection.velocity_profile.use_upstream; + const double minimum_upstream_velocity = + planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity; + const double current_velocity = planner_data_->current_velocity->twist.linear.x; + + int assigned_lane_found = false; + + // crop intersection part of the path, and set the reference velocity to intersection_velocity + // for ego's ttc + PathWithLaneId reference_path; + std::optional upstream_stopline{std::nullopt}; + for (size_t i = 0; i < path.points.size() - 1; ++i) { + auto reference_point = path.points.at(i); + // assume backward velocity is current ego velocity + if (i < closest_idx) { + reference_point.point.longitudinal_velocity_mps = current_velocity; + } + if ( + i > last_intersection_stopline_candidate_idx && + std::fabs(reference_point.point.longitudinal_velocity_mps) < + std::numeric_limits::epsilon() && + !upstream_stopline) { + upstream_stopline = i; + } + if (!use_upstream_velocity) { + reference_point.point.longitudinal_velocity_mps = intersection_velocity; + } + reference_path.points.push_back(reference_point); + bool has_objective_lane_id = util::hasLaneIds(path.points.at(i), associative_ids_); + if (assigned_lane_found && !has_objective_lane_id) { + break; + } + assigned_lane_found = has_objective_lane_id; + } + if (!assigned_lane_found) { + return {{0.0, 0.0}}; // has already passed the intersection. + } + + std::vector> original_path_xy; + for (size_t i = 0; i < reference_path.points.size(); ++i) { + const auto & p = reference_path.points.at(i).point.pose.position; + original_path_xy.emplace_back(p.x, p.y); + } + + // apply smoother to reference velocity + PathWithLaneId smoothed_reference_path = reference_path; + if (!smoothPath(reference_path, smoothed_reference_path, planner_data_)) { + smoothed_reference_path = reference_path; + } + + // calculate when ego is going to reach each (interpolated) points on the path + TimeDistanceArray time_distance_array{}; + double dist_sum = 0.0; + double passing_time = time_delay; + time_distance_array.emplace_back(passing_time, dist_sum); + + // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so + // `last_intersection_stopline_candidate_idx` makes no sense + const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, path.points.at(closest_idx).point.pose, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + + const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { + if (upstream_stopline) { + const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, upstream_stopline_point, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + } else { + return std::nullopt; + } + }(); + const bool has_upstream_stopline = upstream_stopline_idx_opt.has_value(); + const size_t upstream_stopline_ind = upstream_stopline_idx_opt.value_or(0); + + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { + const auto & p1 = smoothed_reference_path.points.at(i); + const auto & p2 = smoothed_reference_path.points.at(i + 1); + + const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); + dist_sum += dist; + + // use average velocity between p1 and p2 + const double average_velocity = + (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; + const double passing_velocity = [=]() { + if (use_upstream_velocity) { + if (has_upstream_stopline && i > upstream_stopline_ind) { + return minimum_upstream_velocity; + } + return std::max(average_velocity, minimum_ego_velocity); + } else { + return std::max(average_velocity, minimum_ego_velocity); + } + }(); + passing_time += (dist / passing_velocity); + + time_distance_array.emplace_back(passing_time, dist_sum); + } + debug_ttc_array->layout.dim.resize(3); + debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; + debug_ttc_array->layout.dim.at(0).size = 5; + debug_ttc_array->layout.dim.at(1).label = "values"; + debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); + debug_ttc_array->data.reserve( + time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); + for (unsigned i = 0; i < time_distance_array.size(); ++i) { + debug_ttc_array->data.push_back(lane_id_); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(t); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(d); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.x); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.y); + } + return time_distance_array; +} + +static double getHighestCurvature(const lanelet::ConstLineString3d & centerline) +{ + std::vector points; + for (auto point = centerline.begin(); point != centerline.end(); point++) { + points.push_back(*point); + } + + SplineInterpolationPoints2d interpolation(points); + const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); + std::vector curvatures_positive; + for (const auto & curvature : curvatures) { + curvatures_positive.push_back(std::fabs(curvature)); + } + return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); +} + +std::vector IntersectionModule::generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets_all, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) +{ + const double curvature_threshold = + planner_param_.occlusion.attention_lane_crop_curvature_threshold; + const double curvature_calculation_ds = + planner_param_.occlusion.attention_lane_curvature_calculation_ds; + + using lanelet::utils::getCenterlineWithOffset; + + // (0) remove left/right lanelet + lanelet::ConstLanelets detection_lanelets; + for (const auto & detection_lanelet : detection_lanelets_all) { + // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet + const auto fine_centerline = + lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); + const double highest_curvature = getHighestCurvature(fine_centerline); + if (highest_curvature > curvature_threshold) { + continue; + } + detection_lanelets.push_back(detection_lanelet); + } + + // (1) tsort detection_lanelets + const auto [merged_detection_lanelets, originals] = + util::mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); + + // (2) merge each branch to one lanelet + // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here + std::vector> merged_lanelet_with_area; + for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { + const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); + const auto & original = originals.at(i); + double area = 0; + for (const auto & partition : original) { + area += bg::area(partition.polygon2d().basicPolygon()); + } + merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); + } + + // (3) discretize each merged lanelet + std::vector detection_divisions; + for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { + const double length = bg::length(merged_lanelet.centerline()); + const double width = area / length; + for (int i = 0; i < static_cast(width / resolution); ++i) { + const double offset = resolution * i - width / 2; + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); + } + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); + } + return detection_divisions; +} + IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( - const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, - const double occlusion_dist_thr) + const TargetObjects & target_objects) { + const auto & occ_grid = *planner_data_->occupancy_grid; + const auto & current_pose = planner_data_->current_odometry->pose; + const double occlusion_dist_thr = planner_param_.occlusion.occlusion_required_clearance_distance; + const auto & path_ip = interpolated_path_info.path; const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); @@ -2094,6 +3222,76 @@ IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( return OcclusionType::STATICALLY_OCCLUDED; } +static std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + + for (size_t i = start; i <= lane_end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (size_t j = 0; j < polygons.size(); ++j) { + const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); + const bool is_in_polygon = bg::intersects(area_2d, path_footprint); + if (is_in_polygon) { + return std::make_optional>(i, j); + } + } + } + return std::nullopt; +} + +void IntersectionLanelets::update( + const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + is_prioritized_ = is_prioritized; + // find the first conflicting/detection area polygon intersecting the path + if (!first_conflicting_area_) { + auto first = getFirstPointInsidePolygonsByFootprint( + conflicting_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_conflicting_lane_ = conflicting_.at(first.value().second); + first_conflicting_area_ = conflicting_area_.at(first.value().second); + } + } + if (!first_attention_area_) { + auto first = getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_attention_lane_ = attention_non_preceding_.at(first.value().second); + first_attention_area_ = attention_non_preceding_area_.at(first.value().second); + } + } +} + +void TargetObject::calc_dist_to_stopline() +{ + if (!attention_lanelet || !stopline) { + return; + } + const auto attention_lanelet_val = attention_lanelet.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); + const auto stopline_val = stopline.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; + stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; + stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); + dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); +} + /* bool IntersectionModule::checkFrontVehicleDeceleration( lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 2f8c296a348b9..6a350e66706e6 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -42,6 +43,259 @@ namespace behavior_velocity_planner using TimeDistanceArray = std::vector>; +struct DebugData +{ + std::optional collision_stop_wall_pose{std::nullopt}; + std::optional occlusion_stop_wall_pose{std::nullopt}; + std::optional occlusion_first_stop_wall_pose{std::nullopt}; + std::optional pass_judge_wall_pose{std::nullopt}; + std::optional> attention_area{std::nullopt}; + std::optional> occlusion_attention_area{std::nullopt}; + std::optional ego_lane{std::nullopt}; + std::optional> adjacent_area{std::nullopt}; + std::optional stuck_vehicle_detect_area{std::nullopt}; + std::optional> yield_stuck_detect_area{std::nullopt}; + std::optional candidate_collision_ego_lane_polygon{std::nullopt}; + std::vector candidate_collision_object_polygons; + autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; + std::vector occlusion_polygons; + std::optional> + nearest_occlusion_projection{std::nullopt}; + autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; + std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; +}; + +/** + * @struct + * @brief see the document for more details of IntersectionLanelets + */ +struct IntersectionLanelets +{ +public: + /** + * update conflicting lanelets and traffic priority information + */ + void update( + const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + + const lanelet::ConstLanelets & attention() const + { + return is_prioritized_ ? attention_non_preceding_ : attention_; + } + const std::vector> & attention_stoplines() const + { + return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; + } + const lanelet::ConstLanelets & conflicting() const { return conflicting_; } + const lanelet::ConstLanelets & adjacent() const { return adjacent_; } + const lanelet::ConstLanelets & occlusion_attention() const + { + return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; + } + const lanelet::ConstLanelets & attention_non_preceding() const + { + return attention_non_preceding_; + } + const std::vector & attention_area() const + { + return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; + } + const std::vector & conflicting_area() const + { + return conflicting_area_; + } + const std::vector & adjacent_area() const { return adjacent_area_; } + const std::vector & occlusion_attention_area() const + { + return occlusion_attention_area_; + } + const std::optional & first_conflicting_lane() const + { + return first_conflicting_lane_; + } + const std::optional & first_conflicting_area() const + { + return first_conflicting_area_; + } + const std::optional & first_attention_lane() const + { + return first_attention_lane_; + } + const std::optional & first_attention_area() const + { + return first_attention_area_; + } + + /** + * the set of attention lanelets which is topologically merged + */ + lanelet::ConstLanelets attention_; + std::vector attention_area_; + + /** + * the stop lines for each attention_ lanelets + */ + std::vector> attention_stoplines_; + + /** + * the conflicting part of attention lanelets + */ + lanelet::ConstLanelets attention_non_preceding_; + std::vector attention_non_preceding_area_; + + /** + * the stop lines for each attention_non_preceding_ + */ + std::vector> attention_non_preceding_stoplines_; + + /** + * the conflicting lanelets of the objective intersection lanelet + */ + lanelet::ConstLanelets conflicting_; + std::vector conflicting_area_; + + /** + * + */ + lanelet::ConstLanelets adjacent_; + std::vector adjacent_area_; + + /** + * the set of attention lanelets for occlusion detection which is topologically merged + */ + lanelet::ConstLanelets occlusion_attention_; + std::vector occlusion_attention_area_; + + /** + * the vector of sum of each occlusion_attention lanelet + */ + std::vector occlusion_attention_size_; + + /** + * the attention lanelet which ego path points intersect for the first time + */ + std::optional first_conflicting_lane_{std::nullopt}; + std::optional first_conflicting_area_{std::nullopt}; + + /** + * the attention lanelet which ego path points intersect for the first time + */ + std::optional first_attention_lane_{std::nullopt}; + std::optional first_attention_area_{std::nullopt}; + + /** + * flag if the intersection is prioritized by the traffic light + */ + bool is_prioritized_{false}; +}; + +/** + * @struct + * @brief see the document for more details of IntersectionStopLines + */ +struct IntersectionStopLines +{ + size_t closest_idx{0}; + + /** + * stuck_stopline is null if ego path does not intersect with first_conflicting_area + */ + std::optional stuck_stopline{std::nullopt}; + + /** + * default_stopline is null if it is calculated negative from first_attention_stopline + */ + std::optional default_stopline{std::nullopt}; + + /** + * first_attention_stopline is null if ego footprint along the path does not intersect with + * attention area. if path[0] satisfies the condition, it is 0 + */ + std::optional first_attention_stopline{std::nullopt}; + + /** + * occlusion_peeking_stopline is null if path[0] is already inside the attention area + */ + std::optional occlusion_peeking_stopline{std::nullopt}; + + /** + * pass_judge_line is before first_attention_stop_line by the braking distance. if its value is + * calculated negative, it is 0 + */ + size_t pass_judge_line{0}; + + /** + * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with + * the centerline of the first_attention_lane + */ + size_t occlusion_wo_tl_pass_judge_line{0}; +}; + +/** + * @struct + * @brief see the document for more details of PathLanelets + */ +struct PathLanelets +{ + lanelet::ConstLanelets prev; + // lanelet::ConstLanelet entry2ego; this is included in `all` if exists + lanelet::ConstLanelet + ego_or_entry2exit; // this is `assigned lane` part of the path(not from + // ego) if ego is before the intersection, otherwise from ego to exit + std::optional next = + std::nullopt; // this is nullopt is the goal is inside intersection + lanelet::ConstLanelets all; + lanelet::ConstLanelets + conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with + // conflicting lanelets plus the next lane part of the + // path +}; + +/** + * @struct + * @brief see the document for more details of IntersectionStopLines + */ +struct TargetObject +{ + autoware_auto_perception_msgs::msg::PredictedObject object; + std::optional attention_lanelet{std::nullopt}; + std::optional stopline{std::nullopt}; + std::optional dist_to_stopline{std::nullopt}; + void calc_dist_to_stopline(); +}; + +/** + * @struct + * @brief categorize TargetObjects + */ +struct TargetObjects +{ + std_msgs::msg::Header header; + std::vector attention_objects; + std::vector parked_attention_objects; + std::vector intersection_area_objects; + std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy +}; + +/** + * @struct + * @brief categorize traffic light priority + */ +enum class TrafficPrioritizedLevel { + //! The target lane's traffic signal is red or the ego's traffic signal has an arrow. + FULLY_PRIORITIZED = 0, + //! The target lane's traffic signal is amber + PARTIALLY_PRIORITIZED, + //! The target lane's traffic signal is green + NOT_PRIORITIZED +}; + class IntersectionModule : public SceneModuleInterface { public: @@ -159,33 +413,57 @@ class IntersectionModule : public SceneModuleInterface }; enum OcclusionType { + //! occlusion is not detected NOT_OCCLUDED, + //! occlusion is not caused by dynamic objects STATICALLY_OCCLUDED, + //! occlusion is caused by dynamic objects DYNAMICALLY_OCCLUDED, - RTC_OCCLUDED, // actual occlusion does not exist, only disapproved by RTC + //! actual occlusion does not exist, only disapproved by RTC + RTC_OCCLUDED, }; + /** + * @struct + * @brief Internal error or ego already passed pass_judge_line + */ struct Indecisive { std::string error; }; + /** + * @struct + * @brief detected stuck vehicle + */ struct StuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; std::optional occlusion_stopline_idx{std::nullopt}; }; + /** + * @struct + * @brief yielded by vehicle on the attention area + */ struct YieldStuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; }; + /** + * @struct + * @brief only collision is detected + */ struct NonOccludedCollisionStop { size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; }; + /** + * @struct + * @brief occlusion is detected so ego needs to stop at the default stop line position + */ struct FirstWaitBeforeOcclusion { bool is_actually_occlusion_cleared{false}; @@ -193,23 +471,29 @@ class IntersectionModule : public SceneModuleInterface size_t first_stopline_idx{0}; size_t occlusion_stopline_idx{0}; }; - // A state peeking to occlusion limit line in the presence of traffic light + /** + * @struct + * @brief ego is approaching the boundary of attention area in the presence of traffic light + */ struct PeekingTowardOcclusion { - // NOTE: if intersection_occlusion is disapproved externally through RTC, - // it indicates "is_forcefully_occluded" + //! if intersection_occlusion is disapproved externally through RTC, it indicates + //! "is_forcefully_occluded" bool is_actually_occlusion_cleared{false}; bool temporal_stop_before_attention_required{false}; size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t first_attention_stopline_idx{0}; size_t occlusion_stopline_idx{0}; - // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) - // if valid, it contains the remaining time to release the static occlusion stuck and shows up - // intersection_occlusion(x.y) + //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it + //! contains the remaining time to release the static occlusion stuck and shows up + //! intersection_occlusion(x.y) std::optional static_occlusion_timeout{std::nullopt}; }; - // A state detecting both collision and occlusion in the presence of traffic light + /** + * @struct + * @brief both collision and occlusion are detected in the presence of traffic light + */ struct OccludedCollisionStop { bool is_actually_occlusion_cleared{false}; @@ -218,10 +502,14 @@ class IntersectionModule : public SceneModuleInterface size_t collision_stopline_idx{0}; size_t first_attention_stopline_idx{0}; size_t occlusion_stopline_idx{0}; - // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) - // if valid, it contains the remaining time to release the static occlusion stuck + //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it + //! contains the remaining time to release the static occlusion stuck std::optional static_occlusion_timeout{std::nullopt}; }; + /** + * @struct + * @brief at least occlusion is detected in the absence of traffic light + */ struct OccludedAbsenceTrafficLight { bool is_actually_occlusion_cleared{false}; @@ -231,13 +519,20 @@ class IntersectionModule : public SceneModuleInterface size_t first_attention_area_stopline_idx{0}; size_t peeking_limit_line_idx{0}; }; + /** + * @struct + * @brief both collision and occlusion are not detected + */ struct Safe { - // NOTE: if RTC is disapproved status, default stop lines are still needed. size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; }; + /** + * @struct + * @brief traffic light is red or arrow signal + */ struct FullyPrioritized { bool collision_detected{false}; @@ -246,16 +541,16 @@ class IntersectionModule : public SceneModuleInterface size_t occlusion_stopline_idx{0}; }; using DecisionResult = std::variant< - Indecisive, // internal process error, or over the pass judge line - StuckStop, // detected stuck vehicle - YieldStuckStop, // detected yield stuck vehicle - NonOccludedCollisionStop, // detected collision while FOV is clear - FirstWaitBeforeOcclusion, // stop for a while before peeking to occlusion - PeekingTowardOcclusion, // peeking into occlusion while collision is not detected - OccludedCollisionStop, // occlusion and collision are both detected - OccludedAbsenceTrafficLight, // occlusion is detected in the absence of traffic light - Safe, // judge as safe - FullyPrioritized // only detect vehicles violating traffic rules + Indecisive, //! internal process error, or over the pass judge line + StuckStop, //! detected stuck vehicle + YieldStuckStop, //! detected yield stuck vehicle + NonOccludedCollisionStop, //! detected collision while FOV is clear + FirstWaitBeforeOcclusion, //! stop for a while before peeking to occlusion + PeekingTowardOcclusion, //! peeking into occlusion while collision is not detected + OccludedCollisionStop, //! occlusion and collision are both detected + OccludedAbsenceTrafficLight, //! occlusion is detected in the absence of traffic light + Safe, //! judge as safe + FullyPrioritized //! only detect vehicles violating traffic rules >; IntersectionModule( @@ -265,10 +560,6 @@ class IntersectionModule : public SceneModuleInterface const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); - /** - * @brief plan go-stop velocity at traffic crossing with collision check between reference path - * and object predicted path - */ bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; @@ -289,28 +580,27 @@ class IntersectionModule : public SceneModuleInterface const std::string turn_direction_; const bool has_traffic_light_; + // Parameter + PlannerParam planner_param_; + + std::optional intersection_lanelets_{std::nullopt}; + + // for pass judge decision bool is_go_out_{false}; bool is_permanent_go_{false}; DecisionResult prev_decision_result_{Indecisive{""}}; OcclusionType prev_occlusion_status_; - // Parameter - PlannerParam planner_param_; - - std::optional intersection_lanelets_{std::nullopt}; - // for occlusion detection const bool enable_occlusion_detection_; std::optional> occlusion_attention_divisions_{ - std::nullopt}; - StateMachine collision_state_machine_; //! for stable collision checking - StateMachine before_creep_state_machine_; //! for two phase stop - StateMachine occlusion_stop_state_machine_; + std::nullopt}; //! for caching discretized occlusion detection lanelets + StateMachine collision_state_machine_; //! for stable collision checking + StateMachine before_creep_state_machine_; //! for two phase stop + StateMachine occlusion_stop_state_machine_; //! for stable occlusion detection StateMachine temporal_stop_before_attention_state_machine_; StateMachine static_occlusion_timeout_state_machine_; - // for pseudo-collision detection when ego just entered intersection on green light and upcoming - // vehicles are very slow std::optional initial_green_light_observed_time_{std::nullopt}; // for RTC @@ -318,43 +608,135 @@ class IntersectionModule : public SceneModuleInterface bool occlusion_safety_{true}; double occlusion_stop_distance_{0.0}; bool occlusion_activated_{true}; - // for first stop in two-phase stop bool occlusion_first_stop_required_{false}; + /** + * @fn + * @brief set all RTC variable to safe and -inf + */ void initializeRTCStatus(); + /** + * @fn + * @brief analyze traffic_light/occupancy/objects context and return DecisionResult + */ + DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + /** + * @fn + * @brief set RTC value according to calculated DecisionResult + */ void prepareRTCStatus( const DecisionResult &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path); - DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + /** + * @fn + * @brief find TrafficPrioritizedLevel + */ + TrafficPrioritizedLevel getTrafficPrioritizedLevel( + lanelet::ConstLanelet lane, const std::map & tl_infos); + + /** + * @fn + * @brief generate IntersectionLanelets + */ + IntersectionLanelets getObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path); - bool checkStuckVehicle( - const std::shared_ptr & planner_data, - const util::PathLanelets & path_lanelets); + /** + * @fn + * @brief generate IntersectionStopLines + */ + std::optional generateIntersectionStopLines( + lanelet::ConstLanelet assigned_lanelet, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const lanelet::ConstLanelet & first_attention_lane, + const util::InterpolatedPathInfo & interpolated_path_info, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - bool checkYieldStuckVehicle( - const util::TargetObjects & target_objects, + /** + * @fn + * @brief find the associated stopline road marking of assigned lanelet + */ + std::optional getStopLineIndexFromMap( const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets); + lanelet::ConstLanelet assigned_lanelet); - util::TargetObjects generateTargetObjects( - const util::IntersectionLanelets & intersection_lanelets, + /** + * @fn + * @brief generate PathLanelets + */ + std::optional generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx); + + /** + * @fn + * @brief check stuck + */ + bool checkStuckVehicleInIntersection(const PathLanelets & path_lanelets, DebugData * debug_data); + + /** + * @fn + * @brief check yield stuck + */ + bool checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data); + + /** + * @fn + * @brief categorize target objects + */ + TargetObjects generateTargetObjects( + const IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const; + /** + * @fn + * @brief check collision + */ bool checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, - const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, + const PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const TrafficPrioritizedLevel & traffic_prioritized_level); + + std::optional checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const bool is_parked_vehicle) const; + + void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr); + /** + * @fn + * @brief calculate ego vehicle profile along the path inside the intersection as the sequence of + * (time of arrival, traveled distance) from current ego position + */ + TimeDistanceArray calcIntersectionPassingTime( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); + + std::vector generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + + /** + * @fn + * @brief check occlusion status + */ OcclusionType getOcclusionStatus( - const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, - const double occlusion_dist_thr); + const TargetObjects & target_objects); /* bool IntersectionModule::checkFrontVehicleDeceleration( @@ -364,7 +746,7 @@ class IntersectionModule : public SceneModuleInterface const double assumed_front_car_decel); */ - util::DebugData debug_data_; + DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr ego_ttc_pub_; rclcpp::Publisher::SharedPtr object_ttc_pub_; diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index b373f2cbc1c8a..f56d0a4adcff7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -16,17 +16,16 @@ #include "util.hpp" -#include #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -50,6 +49,32 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( state_machine_.setState(StateMachine::State::STOP); } +static std::optional getFirstConflictingLanelet( + const lanelet::ConstLanelets & conflicting_lanelets, + const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + + for (size_t i = start; i <= end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (const auto & conflicting_lanelet : conflicting_lanelets) { + const auto polygon_2d = conflicting_lanelet.polygon2d().basicPolygon(); + const bool intersects = bg::intersects(polygon_2d, path_footprint); + if (intersects) { + return std::make_optional(conflicting_lanelet); + } + } + } + return std::nullopt; +} + bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { debug_data_ = DebugData(); @@ -83,41 +108,36 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR return false; } - /* get detection area */ - if (!intersection_lanelets_) { - const auto & assigned_lanelet = - planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); - const auto lanelets_on_path = planning_utils::getLaneletsOnPath( - *path, lanelet_map_ptr, planner_data_->current_odometry->pose); - intersection_lanelets_ = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, - planner_param_.attention_area_length, planner_param_.occlusion_attention_area_length, - planner_param_.consider_wrong_direction_vehicle); - } - auto & intersection_lanelets = intersection_lanelets_.value(); const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - intersection_lanelets.update( - false, interpolated_path_info, local_footprint, - planner_data_->vehicle_info_.max_longitudinal_offset_m); - const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area(); - if (!first_conflicting_area) { + if (!first_conflicting_lanelet_) { + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + const auto conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); + first_conflicting_lanelet_ = getFirstConflictingLanelet( + conflicting_lanelets, interpolated_path_info, local_footprint, + planner_data_->vehicle_info_.max_longitudinal_offset_m); + } + if (!first_conflicting_lanelet_) { return false; } + const auto first_conflicting_lanelet = first_conflicting_lanelet_.value(); - /* set stop-line and stop-judgement-line for base_link */ - const auto stopline_idx_opt = util::generateStuckStopLine( - first_conflicting_area.value(), planner_data_, interpolated_path_info, - planner_param_.stopline_margin, false, path); - if (!stopline_idx_opt.has_value()) { - RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); + const auto first_conflicting_idx_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_lanelet.polygon3d(), interpolated_path_info, local_footprint, + planner_data_->vehicle_info_.max_longitudinal_offset_m); + if (!first_conflicting_idx_opt) { return false; } + const auto stopline_idx_ip = first_conflicting_idx_opt.value(); - const size_t stopline_idx = stopline_idx_opt.value(); - if (stopline_idx == 0) { - RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); + const auto stopline_idx_opt = util::insertPointIndex( + interpolated_path_info.path.points.at(stopline_idx_ip).point.pose, path, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + if (!stopline_idx_opt) { + RCLCPP_DEBUG(logger_, "failed to insert stopline, ignore planning."); return true; } + const auto stopline_idx = stopline_idx_opt.value(); debug_data_.virtual_wall_pose = planning_utils::getAheadPose( stopline_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); @@ -195,4 +215,5 @@ MergeFromPrivateRoadModule::extractPathNearExitOfPrivateRoad( std::reverse(private_path.points.begin(), private_path.points.end()); return private_path; } + } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index fab0303640700..ab368e6b3106b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -15,10 +15,7 @@ #ifndef SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ #define SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ -#include "scene_intersection.hpp" - #include -#include #include #include @@ -27,9 +24,6 @@ #include #include -#include -#include - #include #include #include @@ -89,7 +83,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface // Parameter PlannerParam planner_param_; - std::optional intersection_lanelets_; + std::optional first_conflicting_lanelet_; StateMachine state_machine_; //! for state diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index e491d2ce7c5ce..6afc2d61d77f2 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -18,15 +18,10 @@ #include #include -#include #include -#include -#include -#include #include #include #include -#include #include #include @@ -38,29 +33,12 @@ #include #include #include -#include #include #include #include -#include #include #include -namespace tier4_autoware_utils -{ - -template <> -inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) -{ - geometry_msgs::msg::Point point; - point.x = p.x(); - point.y = p.y(); - point.z = p.z(); - return point; -} - -} // namespace tier4_autoware_utils - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -84,7 +62,7 @@ static std::optional getDuplicatedPointIdx( return std::nullopt; } -static std::optional insertPointIndex( +std::optional insertPointIndex( const geometry_msgs::msg::Pose & in_pose, autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) @@ -156,66 +134,7 @@ std::optional> findLaneIdsInterval( return found ? std::make_optional(std::make_pair(start, end)) : std::nullopt; } -/** - * @brief Get stop point from map if exists - * @param stop_pose stop point defined on map - * @return true when the stop point is defined on map. - */ -static std::optional getStopLineIndexFromMap( - const InterpolatedPathInfo & interpolated_path_info, - const std::shared_ptr & planner_data) -{ - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - - lanelet::ConstLanelet lanelet = - planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get( - interpolated_path_info.lane_id); - const auto road_markings = lanelet.regulatoryElementsAs(); - lanelet::ConstLineStrings3d stopline; - for (const auto & road_marking : road_markings) { - const std::string type = - road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); - if (type == lanelet::AttributeValueString::StopLine) { - stopline.push_back(road_marking->roadMarking()); - break; // only one stopline exists. - } - } - if (stopline.empty()) { - return std::nullopt; - } - - const auto p_start = stopline.front().front(); - const auto p_end = stopline.front().back(); - const LineString2d extended_stopline = - planning_utils::extendLine(p_start, p_end, planner_data->stop_line_extend_length); - - for (size_t i = lane_interval.first; i < lane_interval.second; i++) { - const auto & p_front = path.points.at(i).point.pose.position; - const auto & p_back = path.points.at(i + 1).point.pose.position; - - const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; - std::vector collision_points; - bg::intersection(extended_stopline, path_segment, collision_points); - - if (collision_points.empty()) { - continue; - } - - return i; - } - - geometry_msgs::msg::Pose stop_point_from_map; - stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); - stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); - stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); - - return motion_utils::findFirstNearestIndexWithSoftConstraints( - path.points, stop_point_from_map, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); -} - -static std::optional getFirstPointInsidePolygonByFootprint( +std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { @@ -236,222 +155,6 @@ static std::optional getFirstPointInsidePolygonByFootprint( return std::nullopt; } -static std::optional> -getFirstPointInsidePolygonsByFootprint( - const std::vector & polygons, - const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) -{ - const auto & path_ip = interpolated_path_info.path; - const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); - const size_t start = - static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - - for (size_t i = start; i <= lane_end; ++i) { - const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = - tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); - for (size_t j = 0; j < polygons.size(); ++j) { - const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); - const bool is_in_polygon = bg::intersects(area_2d, path_footprint); - if (is_in_polygon) { - return std::make_optional>(i, j); - } - } - } - return std::nullopt; -} - -std::optional generateIntersectionStopLines( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_attention_area, - const lanelet::ConstLineString2d & first_attention_lane_centerline, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stopline_margin, const double max_accel, const double max_jerk, - const double delay_response_time, const double peeking_offset, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) -{ - const auto & path_ip = interpolated_path_info.path; - const double ds = interpolated_path_info.ds; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - const double baselink2front = planner_data->vehicle_info_.max_longitudinal_offset_m; - - const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); - const int base2front_idx_dist = - std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); - - // find the index of the first point whose vehicle footprint on it intersects with detection_area - const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); - std::optional first_footprint_inside_detection_ip_opt = - getFirstPointInsidePolygonByFootprint( - first_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (!first_footprint_inside_detection_ip_opt) { - return std::nullopt; - } - const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); - - std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; - for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { - // NOTE: maybe consideration of braking dist is necessary - first_footprint_attention_centerline_ip_opt = i; - break; - } - } - if (!first_footprint_attention_centerline_ip_opt) { - return std::nullopt; - } - const size_t first_footprint_attention_centerline_ip = - first_footprint_attention_centerline_ip_opt.value(); - - // (1) default stop line position on interpolated path - bool default_stopline_valid = true; - int stop_idx_ip_int = -1; - if (const auto map_stop_idx_ip = getStopLineIndexFromMap(interpolated_path_info, planner_data); - map_stop_idx_ip) { - stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; - } - if (stop_idx_ip_int < 0) { - stop_idx_ip_int = first_footprint_inside_detection_ip - stopline_margin_idx_dist; - } - if (stop_idx_ip_int < 0) { - default_stopline_valid = false; - } - const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; - - // (2) ego front stop line position on interpolated path - const geometry_msgs::msg::Pose & current_pose = planner_data->current_odometry->pose; - const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( - path_ip.points, current_pose, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); - - // (3) occlusion peeking stop line position on interpolated path - int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); - bool occlusion_peeking_line_valid = true; - // NOTE: if footprints[0] is already inside the detection area, invalid - { - const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; - const auto path_footprint0 = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); - if (bg::intersects( - path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { - occlusion_peeking_line_valid = false; - } - } - if (occlusion_peeking_line_valid) { - occlusion_peeking_line_ip_int = - first_footprint_inside_detection_ip + std::ceil(peeking_offset / ds); - } - - const auto occlusion_peeking_line_ip = static_cast( - std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - const auto first_attention_stopline_ip = first_footprint_inside_detection_ip; - const bool first_attention_stopline_valid = true; - - // (4) pass judge line position on interpolated path - const double velocity = planner_data->current_velocity->twist.linear.x; - const double acceleration = planner_data->current_acceleration->accel.accel.linear.x; - const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( - velocity, acceleration, max_accel, max_jerk, delay_response_time); - int pass_judge_ip_int = - static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); - const auto pass_judge_line_ip = static_cast( - std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - // TODO(Mamoru Sobue): maybe braking dist should be considered - const auto occlusion_wo_tl_pass_judge_line_ip = - static_cast(first_footprint_attention_centerline_ip); - - // (5) stuck vehicle stop line - int stuck_stopline_ip_int = 0; - bool stuck_stopline_valid = true; - if (use_stuck_stopline) { - // NOTE: when ego vehicle is approaching detection area and already passed - // first_conflicting_area, this could be null. - const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( - first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); - if (!stuck_stopline_idx_ip_opt) { - stuck_stopline_valid = false; - stuck_stopline_ip_int = 0; - } else { - stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; - } - } else { - stuck_stopline_ip_int = - std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); - } - if (stuck_stopline_ip_int < 0) { - stuck_stopline_valid = false; - } - const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); - - struct IntersectionStopLinesTemp - { - size_t closest_idx{0}; - size_t stuck_stopline{0}; - size_t default_stopline{0}; - size_t first_attention_stopline{0}; - size_t occlusion_peeking_stopline{0}; - size_t pass_judge_line{0}; - size_t occlusion_wo_tl_pass_judge_line{0}; - }; - - IntersectionStopLinesTemp intersection_stoplines_temp; - std::list> stoplines = { - {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, - {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, - {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, - {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, - {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, - {&pass_judge_line_ip, &intersection_stoplines_temp.pass_judge_line}, - {&occlusion_wo_tl_pass_judge_line_ip, - &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; - stoplines.sort( - [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); - for (const auto & [stop_idx_ip, stop_idx] : stoplines) { - const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; - const auto insert_idx = insertPointIndex( - insert_point, original_path, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); - if (!insert_idx) { - return std::nullopt; - } - *stop_idx = insert_idx.value(); - } - if ( - intersection_stoplines_temp.occlusion_peeking_stopline < - intersection_stoplines_temp.default_stopline) { - intersection_stoplines_temp.occlusion_peeking_stopline = - intersection_stoplines_temp.default_stopline; - } - - IntersectionStopLines intersection_stoplines; - intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; - if (stuck_stopline_valid) { - intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; - } - if (default_stopline_valid) { - intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; - } - if (first_attention_stopline_valid) { - intersection_stoplines.first_attention_stopline = - intersection_stoplines_temp.first_attention_stopline; - } - if (occlusion_peeking_line_valid) { - intersection_stoplines.occlusion_peeking_stopline = - intersection_stoplines_temp.occlusion_peeking_stopline; - } - intersection_stoplines.pass_judge_line = intersection_stoplines_temp.pass_judge_line; - intersection_stoplines.occlusion_wo_tl_pass_judge_line = - intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; - return intersection_stoplines; -} - std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, @@ -490,105 +193,7 @@ std::optional getFirstPointInsidePolygon( return std::nullopt; } -static std::optional> -getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, - const std::vector & polygons, const bool search_forward = true) -{ - if (search_forward) { - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; - const auto & p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - } - } else { - for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { - bool is_in_lanelet = false; - const auto & p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - if (i == 0) { - break; - } - } - } - return std::nullopt; -} - -std::optional generateStuckStopLine( - const lanelet::CompoundPolygon3d & conflicting_area, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const double stopline_margin, - const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) -{ - const auto & path_ip = interpolated_path_info.path; - const double ds = interpolated_path_info.ds; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - const auto lane_interval_ip_start = std::get<0>(lane_interval_ip); - size_t stuck_stopline_idx_ip = 0; - if (use_stuck_stopline) { - stuck_stopline_idx_ip = lane_interval_ip_start; - } else { - const auto stuck_stopline_idx_ip_opt = - getFirstPointInsidePolygon(path_ip, lane_interval_ip, conflicting_area); - if (!stuck_stopline_idx_ip_opt) { - return std::nullopt; - } - stuck_stopline_idx_ip = stuck_stopline_idx_ip_opt.value(); - } - - const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); - const int base2front_idx_dist = - std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); - const size_t insert_idx_ip = static_cast(std::max( - static_cast(stuck_stopline_idx_ip) - 1 - stopline_margin_idx_dist - base2front_idx_dist, - 0)); - const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose; - return insertPointIndex( - insert_point, original_path, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); -} - -static std::vector getPolygon3dFromLanelets( - const lanelet::ConstLanelets & ll_vec) -{ - std::vector polys; - for (auto && ll : ll_vec) { - polys.push_back(ll.polygon3d()); - } - return polys; -} - -static std::string getTurnDirection(lanelet::ConstLanelet lane) -{ - return lane.attributeOr("turn_direction", "else"); -} - -/** - * @param pair lanelets and the vector of original lanelets in topological order (not reversed as - *in generateDetectionLaneDivisions()) - **/ -static std::pair> +std::pair> mergeLaneletsByTopologicalSort( const lanelet::ConstLanelets & lanelets, const lanelet::routing::RoutingGraphPtr routing_graph_ptr) @@ -683,207 +288,9 @@ mergeLaneletsByTopologicalSort( return {merged, originals}; } -IntersectionLanelets getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, - const std::set & associative_ids, const double detection_area_length, - const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle) -{ - const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); - - // retrieve a stopline associated with a traffic light - bool has_traffic_light = false; - if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); - tl_reg_elems.size() != 0) { - const auto tl_reg_elem = tl_reg_elems.front(); - const auto stopline_opt = tl_reg_elem->stopLine(); - if (!!stopline_opt) has_traffic_light = true; - } - - // for low priority lane - // If ego_lane has right of way (i.e. is high priority), - // ignore yieldLanelets (i.e. low priority lanes) - lanelet::ConstLanelets yield_lanelets{}; - const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); - for (const auto & right_of_way : right_of_ways) { - if (lanelet::utils::contains(right_of_way->rightOfWayLanelets(), assigned_lanelet)) { - for (const auto & yield_lanelet : right_of_way->yieldLanelets()) { - yield_lanelets.push_back(yield_lanelet); - for (const auto & previous_lanelet : routing_graph_ptr->previous(yield_lanelet)) { - yield_lanelets.push_back(previous_lanelet); - } - } - } - } - - // get all following lanes of previous lane - lanelet::ConstLanelets ego_lanelets = lanelets_on_path; - for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { - ego_lanelets.push_back(previous_lanelet); - for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { - if (lanelet::utils::contains(ego_lanelets, following_lanelet)) { - continue; - } - ego_lanelets.push_back(following_lanelet); - } - } - - // get conflicting lanes on assigned lanelet - const auto & conflicting_lanelets = - lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); - std::vector adjacent_followings; - - for (const auto & conflicting_lanelet : conflicting_lanelets) { - for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - } - - // final objective lanelets - lanelet::ConstLanelets detection_lanelets; - lanelet::ConstLanelets conflicting_ex_ego_lanelets; - // conflicting lanes is necessary to get stopline for stuck vehicle - for (auto && conflicting_lanelet : conflicting_lanelets) { - if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) - conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); - } - - // exclude yield lanelets and ego lanelets from detection_lanelets - if (turn_direction == std::string("straight") && has_traffic_light) { - // if assigned lanelet is "straight" with traffic light, detection area is not necessary - } else { - if (consider_wrong_direction_vehicle) { - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - for (const auto & adjacent_following : adjacent_followings) { - detection_lanelets.push_back(adjacent_following); - } - } else { - // otherwise we need to know the priority from RightOfWay - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if ( - lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || - lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - } - } - - // get possible lanelet path that reaches conflicting_lane longer than given length - lanelet::ConstLanelets detection_and_preceding_lanelets; - { - const double length = detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(ll); - // get preceding lanelets without ego_lanelets - // to prevent the detection area from including the ego lanes and its' preceding lanes. - const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( - routing_graph_ptr, ll, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(l); - } - } - } - } - - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; - { - const double length = occlusion_detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll); - // get preceding lanelets without ego_lanelets - // to prevent the detection area from including the ego lanes and its' preceding lanes. - const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( - routing_graph_ptr, ll, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); - } - } - } - } - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; - for (const auto & ll : occlusion_detection_and_preceding_lanelets) { - const auto turn_direction = getTurnDirection(ll); - if (turn_direction == "left" || turn_direction == "right") { - continue; - } - occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); - } - - auto [attention_lanelets, original_attention_lanelet_sequences] = - mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); - - IntersectionLanelets result; - result.attention_ = std::move(attention_lanelets); - for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { - // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that - // back() exists. - std::optional stopline{std::nullopt}; - for (auto it = original_attention_lanelet_seq.rbegin(); - it != original_attention_lanelet_seq.rend(); ++it) { - const auto traffic_lights = it->regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - break; - } - if (stopline) break; - } - result.attention_stoplines_.push_back(stopline); - } - result.attention_non_preceding_ = std::move(detection_lanelets); - for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { - std::optional stopline = std::nullopt; - const auto & ll = result.attention_non_preceding_.at(i); - const auto traffic_lights = ll.regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - } - result.attention_non_preceding_stoplines_.push_back(stopline); - } - result.conflicting_ = std::move(conflicting_ex_ego_lanelets); - result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids); - // NOTE: occlusion_attention is not inverted here - // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and - // then trim part of them based on curvature threshold - result.occlusion_attention_ = - std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); - - // NOTE: to properly update(), each element in conflicting_/conflicting_area_, - // attention_non_preceding_/attention_non_preceding_area_ need to be matched - result.attention_area_ = getPolygon3dFromLanelets(result.attention_); - result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); - result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); - result.adjacent_area_ = getPolygon3dFromLanelets(result.adjacent_); - result.occlusion_attention_area_ = getPolygon3dFromLanelets(result.occlusion_attention_); - return result; -} - bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; @@ -893,8 +300,8 @@ bool isOverTargetIndex( } bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; @@ -903,81 +310,13 @@ bool isBeforeTargetIndex( return static_cast(target_idx > closest_idx); } -/* -static std::vector getAllAdjacentLanelets( - const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane) -{ - std::set results; - - results.insert(lane.id()); - - auto it = routing_graph->adjacentRight(lane); - // take all lane on the right side - while (!!it) { - results.insert(it.get().id()); - it = routing_graph->adjacentRight(it.get()); - } - // take all lane on the left side - it = routing_graph->adjacentLeft(lane); - while (!!it) { - results.insert(it.get().id()); - it = routing_graph->adjacentLeft(it.get()); - - } - return std::vector(results.begin(), results.end()); -} -*/ - -/* -lanelet::ConstLanelets extendedAdjacentDirectionLanes( - lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, - lanelet::ConstLanelet lane) -{ - // some of the intersections are not well-formed, and "adjacent" turning - // lanelets are not sharing the LineStrings - const std::string turn_direction = getTurnDirection(lane); - if (turn_direction != "left" && turn_direction != "right" && turn_direction != "straight") - return {}; - - std::set previous_lanelet_ids; - for (auto && previous_lanelet : routing_graph->previous(lane)) { - previous_lanelet_ids.insert(previous_lanelet.id()); - } - - std::set besides_previous_lanelet_ids; - for (auto && previous_lanelet_id : previous_lanelet_ids) { - lanelet::ConstLanelet previous_lanelet = map->laneletLayer.get(previous_lanelet_id); - for (auto && beside_lanelet : getAllAdjacentLanelets(routing_graph, previous_lanelet)) { - besides_previous_lanelet_ids.insert(beside_lanelet); - } - } - - std::set following_turning_lanelets; - following_turning_lanelets.insert(lane.id()); - for (auto && besides_previous_lanelet_id : besides_previous_lanelet_ids) { - lanelet::ConstLanelet besides_previous_lanelet = - map->laneletLayer.get(besides_previous_lanelet_id); - for (auto && following_lanelet : routing_graph->following(besides_previous_lanelet)) { - // if this has {"turn_direction", "${turn_direction}"}, take this - if (getTurnDirection(following_lanelet) == turn_direction) - following_turning_lanelets.insert(following_lanelet.id()); - } - } - lanelet::ConstLanelets ret{}; - for (auto && id : following_turning_lanelets) { - ret.push_back(map->laneletLayer.get(id)); - } - return ret; -} -*/ - -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) { const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); if (area_id_str == "else") return std::nullopt; - const int area_id = std::atoi(area_id_str.c_str()); + const lanelet::Id area_id = std::atoi(area_id_str.c_str()); const auto poly_3d = lanelet_map_ptr->polygonLayer.get(area_id); Polygon2d poly{}; for (const auto & p : poly_3d) poly.outer().emplace_back(p.x(), p.y()); @@ -994,111 +333,6 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -TrafficPrioritizedLevel getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos) -{ - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - - std::optional tl_id = std::nullopt; - for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { - tl_id = tl_reg_elem->id(); - break; - } - if (!tl_id) { - // this lane has no traffic light - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto tl_info_it = tl_infos.find(tl_id.value()); - if (tl_info_it == tl_infos.end()) { - // the info of this traffic light is not available - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto & tl_info = tl_info_it->second; - bool has_amber_signal{false}; - for (auto && tl_light : tl_info.signal.elements) { - if (tl_light.color == TrafficSignalElement::AMBER) { - has_amber_signal = true; - } - if (tl_light.color == TrafficSignalElement::RED) { - // NOTE: Return here since the red signal has the highest priority. - return TrafficPrioritizedLevel::FULLY_PRIORITIZED; - } - } - if (has_amber_signal) { - return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; - } - return TrafficPrioritizedLevel::NOT_PRIORITIZED; -} - -double getHighestCurvature(const lanelet::ConstLineString3d & centerline) -{ - std::vector points; - for (auto point = centerline.begin(); point != centerline.end(); point++) { - points.push_back(*point); - } - - SplineInterpolationPoints2d interpolation(points); - const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); - std::vector curvatures_positive; - for (const auto & curvature : curvatures) { - curvatures_positive.push_back(std::fabs(curvature)); - } - return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); -} - -std::vector generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets_all, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, - const double curvature_threshold, const double curvature_calculation_ds) -{ - using lanelet::utils::getCenterlineWithOffset; - - // (0) remove left/right lanelet - lanelet::ConstLanelets detection_lanelets; - for (const auto & detection_lanelet : detection_lanelets_all) { - // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet - const auto fine_centerline = - lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); - const double highest_curvature = getHighestCurvature(fine_centerline); - if (highest_curvature > curvature_threshold) { - continue; - } - detection_lanelets.push_back(detection_lanelet); - } - - // (1) tsort detection_lanelets - const auto [merged_detection_lanelets, originals] = - mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); - - // (2) merge each branch to one lanelet - // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here - std::vector> merged_lanelet_with_area; - for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { - const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); - const auto & original = originals.at(i); - double area = 0; - for (const auto & partition : original) { - area += bg::area(partition.polygon2d().basicPolygon()); - } - merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); - } - - // (3) discretize each merged lanelet - std::vector detection_divisions; - for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { - const double length = bg::length(merged_lanelet.centerline()); - const double width = area / length; - for (int i = 0; i < static_cast(width / resolution); ++i) { - const double offset = resolution * i - width / 2; - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); - } - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); - } - return detection_divisions; -} - std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, @@ -1116,7 +350,6 @@ std::optional generateInterpolatedPath( return interpolated_path_info; } -// from here geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state) { @@ -1134,609 +367,5 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( return obj_pose; } -static bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) -{ - if ( - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { - return true; - } - return false; -} - -bool checkStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, - DebugData * debug_data) -{ - for (const auto & object : objects_ptr->objects) { - if (!isTargetStuckVehicleType(object)) { - continue; // not target vehicle type - } - const auto obj_v_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); - if (obj_v_norm > stuck_vehicle_vel_thr) { - continue; // not stop vehicle - } - - // check if the footprint is in the stuck detect area - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); - const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); - if (is_in_stuck_area && debug_data) { - debug_data->stuck_targets.objects.push_back(object); - return true; - } - } - return false; -} - -static lanelet::LineString3d getLineStringFromArcLength( - const lanelet::ConstLineString3d & linestring, const double s1, const double s2) -{ - lanelet::Points3d points; - double accumulated_length = 0; - size_t start_index = linestring.size(); - for (size_t i = 0; i < linestring.size() - 1; i++) { - const auto & p1 = linestring[i]; - const auto & p2 = linestring[i + 1]; - const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); - if (accumulated_length + length > s1) { - start_index = i; - break; - } - accumulated_length += length; - } - if (start_index < linestring.size() - 1) { - const auto & p1 = linestring[start_index]; - const auto & p2 = linestring[start_index + 1]; - const double residue = s1 - accumulated_length; - const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); - const auto start_basic_point = p1.basicPoint() + residue * direction_vector; - const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); - points.push_back(start_point); - } - - accumulated_length = 0; - size_t end_index = linestring.size(); - for (size_t i = 0; i < linestring.size() - 1; i++) { - const auto & p1 = linestring[i]; - const auto & p2 = linestring[i + 1]; - const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); - if (accumulated_length + length > s2) { - end_index = i; - break; - } - accumulated_length += length; - } - - for (size_t i = start_index + 1; i < end_index; i++) { - const auto p = lanelet::Point3d(linestring[i]); - points.push_back(p); - } - if (end_index < linestring.size() - 1) { - const auto & p1 = linestring[end_index]; - const auto & p2 = linestring[end_index + 1]; - const double residue = s2 - accumulated_length; - const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); - const auto end_basic_point = p1.basicPoint() + residue * direction_vector; - const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); - points.push_back(end_point); - } - return lanelet::LineString3d{lanelet::InvalId, points}; -} - -static lanelet::ConstLanelet createLaneletFromArcLength( - const lanelet::ConstLanelet & lanelet, const double s1, const double s2) -{ - const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); - // make sure that s1, and s2 are between [0, lane_length] - const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); - const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); - - const auto ratio_s1 = s1_saturated / total_length; - const auto ratio_s2 = s2_saturated / total_length; - - const auto s1_left = - static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); - const auto s2_left = - static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); - const auto s1_right = - static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); - const auto s2_right = - static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); - - const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); - const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); - - return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); -} - -bool checkYieldStuckVehicleInIntersection( - const util::TargetObjects & target_objects, - const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets, const std::string & turn_direction, - const double width, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, - DebugData * debug_data) -{ - LineString2d sparse_intersection_path; - const auto [start, end] = interpolated_path_info.lane_id_interval.value(); - for (unsigned i = start; i < end; ++i) { - const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; - const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); - if (turn_direction == "right") { - const double right_x = point.x - width / 2 * std::sin(yaw); - const double right_y = point.y + width / 2 * std::cos(yaw); - sparse_intersection_path.emplace_back(right_x, right_y); - } else if (turn_direction == "left") { - const double left_x = point.x + width / 2 * std::sin(yaw); - const double left_y = point.y - width / 2 * std::cos(yaw); - sparse_intersection_path.emplace_back(left_x, left_y); - } else { - // straight - sparse_intersection_path.emplace_back(point.x, point.y); - } - } - lanelet::ConstLanelets yield_stuck_detect_lanelets; - for (const auto & attention_lanelet : attention_lanelets) { - const auto centerline = attention_lanelet.centerline2d().basicLineString(); - std::vector intersects; - bg::intersection(sparse_intersection_path, centerline, intersects); - if (intersects.empty()) { - continue; - } - const auto intersect = intersects.front(); - const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( - centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); - const double yield_stuck_start = - std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); - const double yield_stuck_end = intersect_arc_coords.length; - yield_stuck_detect_lanelets.push_back( - createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); - } - debug_data->yield_stuck_detect_area = getPolygon3dFromLanelets(yield_stuck_detect_lanelets); - for (const auto & object : target_objects.all_attention_objects) { - const auto obj_v_norm = std::hypot( - object.object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.object.kinematics.initial_twist_with_covariance.twist.linear.y); - - if (obj_v_norm > stuck_vehicle_vel_thr) { - continue; - } - for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { - const bool is_in_lanelet = lanelet::utils::isInLanelet( - object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); - if (is_in_lanelet) { - debug_data->yield_stuck_targets.objects.push_back(object.object); - return true; - } - } - } - return false; -} - -Polygon2d generateStuckVehicleDetectAreaPolygon( - const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist) -{ - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getLaneletLength3d; - using lanelet::utils::getPolygonFromArcLength; - using lanelet::utils::to2D; - - Polygon2d polygon{}; - if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { - return polygon; - } - - double target_polygon_length = - getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); - lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; - if (path_lanelets.next) { - targets.push_back(path_lanelets.next.value()); - const double next_arc_length = - std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); - target_polygon_length += next_arc_length; - } - const auto target_polygon = - to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); - - if (target_polygon.empty()) { - return polygon; - } - - for (const auto & p : target_polygon) { - polygon.outer().emplace_back(p.x(), p.y()); - } - - polygon.outer().emplace_back(polygon.outer().front()); - bg::correct(polygon); - - return polygon; -} - -std::optional checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double dist_margin, const bool is_parked_vehicle) -{ - for (unsigned i = 0; i < target_lanelets.size(); ++i) { - const auto & ll = target_lanelets.at(i); - if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { - continue; - } - const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); - const double pose_angle = tf2::getYaw(pose.orientation); - const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); - if (consider_wrong_direction_vehicle) { - if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - } else { - if (std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is - // positive - if ( - is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || - (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { - return std::make_optional(i); - } - } - } - return std::nullopt; -} - -void cutPredictPathWithDuration( - util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, const double time_thr) -{ - const rclcpp::Time current_time = clock->now(); - for (auto & target_object : target_objects->all_attention_objects) { // each objects - for (auto & predicted_path : - target_object.object.kinematics.predicted_paths) { // each predicted paths - const auto origin_path = predicted_path; - predicted_path.path.clear(); - - for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points - const auto & predicted_pose = origin_path.path.at(k); - const auto predicted_time = - rclcpp::Time(target_objects->header.stamp) + - rclcpp::Duration(origin_path.time_step) * static_cast(k); - if ((predicted_time - current_time).seconds() < time_thr) { - predicted_path.path.push_back(predicted_pose); - } - } - } - } -} - -TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const lanelet::Id lane_id, - const std::set & associative_ids, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) -{ - const double current_velocity = planner_data->current_velocity->twist.linear.x; - - int assigned_lane_found = false; - - // crop intersection part of the path, and set the reference velocity to intersection_velocity - // for ego's ttc - PathWithLaneId reference_path; - std::optional upstream_stopline{std::nullopt}; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - auto reference_point = path.points.at(i); - // assume backward velocity is current ego velocity - if (i < closest_idx) { - reference_point.point.longitudinal_velocity_mps = current_velocity; - } - if ( - i > last_intersection_stopline_candidate_idx && - std::fabs(reference_point.point.longitudinal_velocity_mps) < - std::numeric_limits::epsilon() && - !upstream_stopline) { - upstream_stopline = i; - } - if (!use_upstream_velocity) { - reference_point.point.longitudinal_velocity_mps = intersection_velocity; - } - reference_path.points.push_back(reference_point); - bool has_objective_lane_id = hasLaneIds(path.points.at(i), associative_ids); - if (assigned_lane_found && !has_objective_lane_id) { - break; - } - assigned_lane_found = has_objective_lane_id; - } - if (!assigned_lane_found) { - return {{0.0, 0.0}}; // has already passed the intersection. - } - - std::vector> original_path_xy; - for (size_t i = 0; i < reference_path.points.size(); ++i) { - const auto & p = reference_path.points.at(i).point.pose.position; - original_path_xy.emplace_back(p.x, p.y); - } - - // apply smoother to reference velocity - PathWithLaneId smoothed_reference_path = reference_path; - if (!smoothPath(reference_path, smoothed_reference_path, planner_data)) { - smoothed_reference_path = reference_path; - } - - // calculate when ego is going to reach each (interpolated) points on the path - TimeDistanceArray time_distance_array{}; - double dist_sum = 0.0; - double passing_time = time_delay; - time_distance_array.emplace_back(passing_time, dist_sum); - - // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so - // `last_intersection_stopline_candidate_idx` makes no sense - const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, path.points.at(closest_idx).point.pose, - planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); - - const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { - if (upstream_stopline) { - const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; - return motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, upstream_stopline_point, - planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); - } else { - return std::nullopt; - } - }(); - const bool has_upstream_stopline = upstream_stopline_idx_opt.has_value(); - const size_t upstream_stopline_ind = upstream_stopline_idx_opt.value_or(0); - - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { - const auto & p1 = smoothed_reference_path.points.at(i); - const auto & p2 = smoothed_reference_path.points.at(i + 1); - - const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); - dist_sum += dist; - - // use average velocity between p1 and p2 - const double average_velocity = - (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; - const double passing_velocity = [=]() { - if (use_upstream_velocity) { - if (has_upstream_stopline && i > upstream_stopline_ind) { - return minimum_upstream_velocity; - } - return std::max(average_velocity, minimum_ego_velocity); - } else { - return std::max(average_velocity, minimum_ego_velocity); - } - }(); - passing_time += (dist / passing_velocity); - - time_distance_array.emplace_back(passing_time, dist_sum); - } - debug_ttc_array->layout.dim.resize(3); - debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; - debug_ttc_array->layout.dim.at(0).size = 5; - debug_ttc_array->layout.dim.at(1).label = "values"; - debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); - debug_ttc_array->data.reserve( - time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); - for (unsigned i = 0; i < time_distance_array.size(); ++i) { - debug_ttc_array->data.push_back(lane_id); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(t); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(d); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.x); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.y); - } - return time_distance_array; -} - -double calcDistanceUntilIntersectionLanelet( - const lanelet::ConstLanelet & assigned_lanelet, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx) -{ - const auto lane_id = assigned_lanelet.id(); - const auto intersection_first_itr = - std::find_if(path.points.cbegin(), path.points.cend(), [&](const auto & p) { - return std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end(); - }); - if ( - intersection_first_itr == path.points.begin() || intersection_first_itr == path.points.end()) { - return 0.0; - } - const auto dst_idx = std::distance(path.points.begin(), intersection_first_itr) - 1; - - if (closest_idx > static_cast(dst_idx)) { - return 0.0; - } - - double distance = std::abs(motion_utils::calcSignedArcLength(path.points, closest_idx, dst_idx)); - const auto & lane_first_point = assigned_lanelet.centerline2d().front(); - distance += std::hypot( - path.points.at(dst_idx).point.pose.position.x - lane_first_point.x(), - path.points.at(dst_idx).point.pose.position.y - lane_first_point.y()); - return distance; -} - -void IntersectionLanelets::update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) -{ - is_prioritized_ = is_prioritized; - // find the first conflicting/detection area polygon intersecting the path - if (!first_conflicting_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( - conflicting_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_conflicting_lane_ = conflicting_.at(first.value().second); - first_conflicting_area_ = conflicting_area_.at(first.value().second); - } - } - if (!first_attention_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( - attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_attention_lane_ = attention_non_preceding_.at(first.value().second); - first_attention_area_ = attention_non_preceding_area_.at(first.value().second); - } - } -} - -static lanelet::ConstLanelets getPrevLanelets( - const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) -{ - lanelet::ConstLanelets previous_lanelets; - for (const auto & ll : lanelets_on_path) { - if (associative_ids.find(ll.id()) != associative_ids.end()) { - return previous_lanelets; - } - previous_lanelets.push_back(ll); - } - return previous_lanelets; -} - -// end inclusive -lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, - const double interval) -{ - lanelet::Points3d lefts; - lanelet::Points3d rights; - size_t prev_idx = start_idx; - for (size_t i = start_idx; i <= end_idx; ++i) { - const auto & p = path.points.at(i).point.pose; - const auto & p_prev = path.points.at(prev_idx).point.pose; - if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { - continue; - } - prev_idx = i; - const double yaw = tf2::getYaw(p.orientation); - const double x = p.position.x; - const double y = p.position.y; - // NOTE: maybe this is opposite - const double left_x = x + width / 2 * std::sin(yaw); - const double left_y = y - width / 2 * std::cos(yaw); - const double right_x = x - width / 2 * std::sin(yaw); - const double right_y = y + width / 2 * std::cos(yaw); - lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); - rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); - } - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); - - return lanelet::Lanelet(lanelet::InvalId, left, right); -} - -std::optional generatePathLanelets( - const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::set & associative_ids, - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, - const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx, - const double width) -{ - static constexpr double path_lanelet_interval = 1.5; - - const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; - if (!assigned_lane_interval_opt) { - return std::nullopt; - } - const auto assigned_lane_interval = assigned_lane_interval_opt.value(); - const auto & path = interpolated_path_info.path; - - PathLanelets path_lanelets; - // prev - path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids); - path_lanelets.all = path_lanelets.prev; - - // entry2ego if exist - const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; - if (closest_idx > assigned_lane_start) { - path_lanelets.all.push_back( - generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); - } - - // ego_or_entry2exit - const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); - path_lanelets.ego_or_entry2exit = - generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); - - // next - if (assigned_lane_end < path.points.size() - 1) { - const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); - const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); - if (next_lane_interval_opt) { - const auto [next_start, next_end] = next_lane_interval_opt.value(); - path_lanelets.next = - generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.next.value()); - } - } - - const auto first_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) - : getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); - const auto last_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) - : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); - if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { - const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); - const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; - lanelet::ConstLanelet conflicting_interval = generatePathLanelet( - path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, - path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); - if (last_inside_conflicting_idx < assigned_lane_end) { - lanelet::ConstLanelet remaining_interval = generatePathLanelet( - path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); - } - } - return path_lanelets; -} - -void TargetObject::calc_dist_to_stopline() -{ - if (!attention_lanelet || !stopline) { - return; - } - const auto attention_lanelet_val = attention_lanelet.value(); - const auto object_arc_coords = lanelet::utils::getArcCoordinates( - {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); - const auto stopline_val = stopline.value(); - geometry_msgs::msg::Pose stopline_center; - stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; - stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; - stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; - const auto stopline_arc_coords = - lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); - dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); -} - } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 33e511d911b82..86a34d1e95114 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -15,12 +15,12 @@ #ifndef UTIL_HPP_ #define UTIL_HPP_ -#include "scene_intersection.hpp" #include "util_type.hpp" #include -#include +#include +#include #include #include @@ -35,88 +35,82 @@ namespace behavior_velocity_planner { namespace util { -std::optional insertPoint( + +/** + * @fn + * @brief insert a new pose to the path and return its index + * @return if insertion was successful return the inserted point index + */ +std::optional insertPointIndex( const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path); + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); +/** + * @fn + * @brief check if a PathPointWithLaneId contains any of the given lane ids + */ bool hasLaneIds( const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids); -std::optional> findLaneIdsInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @brief get objective polygons for detection area + * @fn + * @brief find the first contiguous interval of the path points that contains the specified lane ids + * @return if no interval is found, return null. if the interval [start, end] (inclusive range) is + * found, returns the pair (start-1, end) */ -IntersectionLanelets getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, - const std::set & associative_ids, const double detection_area_length, - const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle); +std::optional> findLaneIdsInterval( + const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @brief Generate a stop line for stuck vehicle - * @param conflicting_areas used to generate stop line for stuck vehicle - * @param original_path ego-car lane - * @param target_path target lane to insert stop point (part of ego-car lane or same to ego-car - * lane) - " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane + * @fn + * @brief return the index of the first point which is inside the given polygon + * @param[in] lane_interval the interval of the path points on the intersection + * @param[in] search_forward flag for search direction */ -std::optional generateStuckStopLine( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const double stopline_margin, - const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - -std::optional generateIntersectionStopLines( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_attention_area, - const lanelet::ConstLineString2d & first_attention_lane_centerline, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stopline_margin, const double max_accel, const double max_jerk, - const double delay_response_time, const double peeking_offset, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward = true); /** + * @fn * @brief check if ego is over the target_idx. If the index is same, compare the exact pose - * @param path path - * @param closest_idx ego's closest index on the path - * @param current_pose ego's exact pose + * @param[in] path path + * @param[in] closest_idx ego's closest index on the path + * @param[in] current_pose ego's exact pose * @return true if ego is over the target_idx */ bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); +/** + * @fn + * @brief check if ego is before the target_idx. If the index is same, compare the exact pose + * @param[in] path path + * @param[in] closest_idx ego's closest index on the path + * @param[in] current_pose ego's exact pose + * @return true if ego is over the target_idx + */ bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx); - -/* -lanelet::ConstLanelets extendedAdjacentDirectionLanes( -lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, -lanelet::ConstLanelet lane); -*/ + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); +/** + * @fn + * @brief check if the given lane has related traffic light + */ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); -TrafficPrioritizedLevel getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos); - -std::vector generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, - const double curvature_threshold, const double curvature_calculation_ds); - +/** + * @fn + * @brief interpolate PathWithLaneId + */ std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, @@ -125,56 +119,28 @@ std::optional generateInterpolatedPath( geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); -bool checkStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, - DebugData * debug_data); - -bool checkYieldStuckVehicleInIntersection( - const util::TargetObjects & target_objects, - const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets, const std::string & turn_direction, - const double width, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, - DebugData * debug_data); - -Polygon2d generateStuckVehicleDetectAreaPolygon( - const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); - -std::optional checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double dist_margin, const bool is_parked_vehicle); - -void cutPredictPathWithDuration( - util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, - const double time_thr); +/** + * @fn + * @brief this function sorts the set of lanelets topologically using topological sort and merges + * the lanelets from each root to each end. each branch is merged and returned with the original + * lanelets + * @param[in] lanelets the set of lanelets + * @param[in] routing_graph_ptr the routing graph + * @return the pair of merged lanelets and their corresponding original lanelets + */ +std::pair> +mergeLaneletsByTopologicalSort( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr); -TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const lanelet::Id lane_id, - const std::set & associative_ids, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); - -double calcDistanceUntilIntersectionLanelet( - const lanelet::ConstLanelet & assigned_lanelet, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx); - -lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, - const double interval); - -std::optional generatePathLanelets( - const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::set & associative_ids, - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, - const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx, - const double width); +/** + * @fn + * @brief find the index of the first point where vehicle footprint intersects with the given + * polygon + */ +std::optional getFirstPointInsidePolygonByFootprint( + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index d05031bf19436..763d829dacf9b 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -34,178 +34,24 @@ namespace behavior_velocity_planner::util { -struct DebugData -{ - std::optional collision_stop_wall_pose{std::nullopt}; - std::optional occlusion_stop_wall_pose{std::nullopt}; - std::optional occlusion_first_stop_wall_pose{std::nullopt}; - std::optional pass_judge_wall_pose{std::nullopt}; - std::optional> attention_area{std::nullopt}; - std::optional> occlusion_attention_area{std::nullopt}; - std::optional ego_lane{std::nullopt}; - std::optional> adjacent_area{std::nullopt}; - std::optional stuck_vehicle_detect_area{std::nullopt}; - std::optional> yield_stuck_detect_area{std::nullopt}; - std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - std::vector candidate_collision_object_polygons; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; - autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; - std::vector occlusion_polygons; - std::optional> - nearest_occlusion_projection{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; - std::optional absence_traffic_light_creep_wall{std::nullopt}; - std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; -}; - +/** + * @struct + * @brief wrapper class of interpolated path with lane id + */ struct InterpolatedPathInfo { + /** the interpolated path */ autoware_auto_planning_msgs::msg::PathWithLaneId path; + /** discretization interval of interpolation */ double ds{0.0}; + /** the intersection lanelet id */ lanelet::Id lane_id{0}; + /** the associative lane ids of lane_id */ std::set associative_lane_ids{}; + /** the range of indices for the path points with associative lane id */ std::optional> lane_id_interval{std::nullopt}; }; -struct IntersectionLanelets -{ -public: - void update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); - const lanelet::ConstLanelets & attention() const - { - return is_prioritized_ ? attention_non_preceding_ : attention_; - } - const std::vector> & attention_stoplines() const - { - return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; - } - const lanelet::ConstLanelets & conflicting() const { return conflicting_; } - const lanelet::ConstLanelets & adjacent() const { return adjacent_; } - const lanelet::ConstLanelets & occlusion_attention() const - { - return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; - } - const std::vector & attention_area() const - { - return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; - } - const std::vector & conflicting_area() const - { - return conflicting_area_; - } - const std::vector & adjacent_area() const { return adjacent_area_; } - const std::vector & occlusion_attention_area() const - { - return occlusion_attention_area_; - } - const std::optional & first_conflicting_lane() const - { - return first_conflicting_lane_; - } - const std::optional & first_conflicting_area() const - { - return first_conflicting_area_; - } - const std::optional & first_attention_lane() const - { - return first_attention_lane_; - } - const std::optional & first_attention_area() const - { - return first_attention_area_; - } - - lanelet::ConstLanelets attention_; // topologically merged lanelets - std::vector> - attention_stoplines_; // the stop lines for each attention_ lanelets - lanelet::ConstLanelets attention_non_preceding_; - std::vector> - attention_non_preceding_stoplines_; // the stop lines for each attention_non_preceding_ - // lanelets - lanelet::ConstLanelets conflicting_; - lanelet::ConstLanelets adjacent_; - lanelet::ConstLanelets occlusion_attention_; // topologically merged lanelets - std::vector occlusion_attention_size_; // the area() of each occlusion attention lanelets - std::vector attention_area_; // topologically merged lanelets - std::vector attention_non_preceding_area_; - std::vector conflicting_area_; - std::vector adjacent_area_; - std::vector - occlusion_attention_area_; // topologically merged lanelets - // the first area intersecting with the path - // even if lane change/re-routing happened on the intersection, these areas area are supposed to - // be invariant under the 'associative' lanes. - std::optional first_conflicting_lane_{std::nullopt}; - std::optional first_conflicting_area_{std::nullopt}; - std::optional first_attention_lane_{std::nullopt}; - std::optional first_attention_area_{std::nullopt}; - bool is_prioritized_ = false; -}; - -struct IntersectionStopLines -{ - // NOTE: for baselink - size_t closest_idx{0}; - // NOTE: null if path does not conflict with first_conflicting_area - std::optional stuck_stopline{std::nullopt}; - // NOTE: null if path is over map stopline OR its value is calculated negative - std::optional default_stopline{std::nullopt}; - // NOTE: null if the index is calculated negative - std::optional first_attention_stopline{std::nullopt}; - // NOTE: null if footprints do not change from outside to inside of detection area - std::optional occlusion_peeking_stopline{std::nullopt}; - // if the value is calculated negative, its value is 0 - size_t pass_judge_line{0}; - size_t occlusion_wo_tl_pass_judge_line{0}; -}; - -struct PathLanelets -{ - lanelet::ConstLanelets prev; - // lanelet::ConstLanelet entry2ego; this is included in `all` if exists - lanelet::ConstLanelet - ego_or_entry2exit; // this is `assigned lane` part of the path(not from - // ego) if ego is before the intersection, otherwise from ego to exit - std::optional next = - std::nullopt; // this is nullopt is the goal is inside intersection - lanelet::ConstLanelets all; - lanelet::ConstLanelets - conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with - // conflicting lanelets plus the next lane part of the - // path -}; - -struct TargetObject -{ - autoware_auto_perception_msgs::msg::PredictedObject object; - std::optional attention_lanelet{std::nullopt}; - std::optional stopline{std::nullopt}; - std::optional dist_to_stopline{std::nullopt}; - void calc_dist_to_stopline(); -}; - -struct TargetObjects -{ - std_msgs::msg::Header header; - std::vector attention_objects; - std::vector parked_attention_objects; - std::vector intersection_area_objects; - std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy -}; - -enum class TrafficPrioritizedLevel { - // The target lane's traffic signal is red or the ego's traffic signal has an arrow. - FULLY_PRIORITIZED = 0, - // The target lane's traffic signal is amber - PARTIALLY_PRIORITIZED, - // The target lane's traffic signal is green - NOT_PRIORITIZED -}; } // namespace behavior_velocity_planner::util #endif // UTIL_TYPE_HPP_ From 8f7030813f1dbedaaf5f45e890b6c50bec389ab1 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 5 Jan 2024 16:05:55 +0900 Subject: [PATCH 22/32] fix(static_drivable_area_expansion): fix drivable bound edge process (#6014) Signed-off-by: satoshi-ota --- .../static_drivable_area.cpp | 45 +++++++++++++++++-- 1 file changed, 41 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index ec147a7bb2771..1aa55e5f350b3 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -31,6 +31,42 @@ namespace { +template +size_t findNearestSegmentIndexFromLateralDistance( + const std::vector & points, const geometry_msgs::msg::Point & target_point) +{ + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::normalizeRadian; + + std::optional closest_idx{std::nullopt}; + double min_lateral_dist = std::numeric_limits::max(); + for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { + const double lon_dist = + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); + const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + const double lat_dist = [&]() { + if (lon_dist < 0.0) { + return calcDistance2d(points.at(seg_idx), target_point); + } + if (segment_length < lon_dist) { + return calcDistance2d(points.at(seg_idx + 1), target_point); + } + return std::abs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); + }(); + if (lat_dist < min_lateral_dist) { + closest_idx = seg_idx; + min_lateral_dist = lat_dist; + } + } + + if (closest_idx) { + return *closest_idx; + } + + return motion_utils::findNearestSegmentIndex(points, target_point); +} + template size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Pose & target_point, @@ -813,9 +849,9 @@ void generateDrivableArea( const auto right_start_point = calcLongitudinalOffsetStartPoint( right_bound, front_pose, front_right_start_idx, -front_length); const size_t left_start_idx = - motion_utils::findNearestSegmentIndex(left_bound, left_start_point); + findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point); const size_t right_start_idx = - motion_utils::findNearestSegmentIndex(right_bound, right_start_point); + findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); // Insert a start point path.left_bound.push_back(left_start_point); @@ -835,9 +871,10 @@ void generateDrivableArea( const auto right_goal_point = calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length); const size_t left_goal_idx = std::max( - goal_left_start_idx, motion_utils::findNearestSegmentIndex(left_bound, left_goal_point)); + goal_left_start_idx, findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point)); const size_t right_goal_idx = std::max( - goal_right_start_idx, motion_utils::findNearestSegmentIndex(right_bound, right_goal_point)); + goal_right_start_idx, + findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point)); // Insert middle points for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { From 3aca08b1e0709cf4a580380f9bd2e2f2a0208a7b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 5 Jan 2024 16:35:33 +0900 Subject: [PATCH 23/32] feat(path_smoother): add a plotter of calculation time (#6012) * feat(path_smoother): add a plotter of calculation time Signed-off-by: Takayuki Murooka * add x/y label Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * update cmake to install script Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../scripts/calculation_time_plotter.py | 2 + planning/path_smoother/CMakeLists.txt | 5 + .../scripts/calculation_time_plotter.py | 117 ++++++++++++++++++ 3 files changed, 124 insertions(+) create mode 100755 planning/path_smoother/scripts/calculation_time_plotter.py diff --git a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py b/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py index daf6beea730d3..1904b45fb8225 100755 --- a/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py +++ b/planning/obstacle_avoidance_planner/scripts/calculation_time_plotter.py @@ -49,6 +49,8 @@ def __init__(self, args): self.y_vec.append(deque()) plt.legend(loc="lower left") + plt.xlabel("step [-]") + plt.ylabel("calculation time [ms]") plt.show() def CallbackCalculationCost(self, msg): diff --git a/planning/path_smoother/CMakeLists.txt b/planning/path_smoother/CMakeLists.txt index b2fcf9076a7b3..b2f42181f9a0f 100644 --- a/planning/path_smoother/CMakeLists.txt +++ b/planning/path_smoother/CMakeLists.txt @@ -35,3 +35,8 @@ ament_auto_package( launch config ) + +install(PROGRAMS + scripts/calculation_time_plotter.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/planning/path_smoother/scripts/calculation_time_plotter.py b/planning/path_smoother/scripts/calculation_time_plotter.py new file mode 100755 index 0000000000000..3a18fc56f39bc --- /dev/null +++ b/planning/path_smoother/scripts/calculation_time_plotter.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 + +# Copyright 2024 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +from collections import deque + +import matplotlib.pyplot as plt +import rclpy +from rclpy.node import Node +from tier4_debug_msgs.msg import StringStamped + + +class CalculationCostAnalyzer(Node): + def __init__(self, args): + super().__init__("calculation_cost_analyzer") + + self.functions_name = args.functions.split(",") + self.depth = args.depth + + self.calculation_cost_hist = [] + self.sub_calculation_cost = self.create_subscription( + StringStamped, + "/planning/scenario_planning/lane_driving/motion_planning/elastic_band_smoother/debug/calculation_time", + self.CallbackCalculationCost, + 1, + ) + + plt.ion() + plt.figure() + + self.lines = [] + self.y_vec = [] + for f_idx, function_name in enumerate(self.functions_name): + (line,) = plt.plot([], [], label="{}".format(function_name)) + self.lines.append(line) + self.y_vec.append(deque()) + + plt.legend(loc="lower left") + plt.xlabel("step [-]") + plt.ylabel("calculation time [ms]") + plt.show() + + def CallbackCalculationCost(self, msg): + max_y = 0 + max_x = 0 + + for f_idx, function_name in enumerate(self.functions_name): + is_found = False + for line in msg.data.split("\n"): + if function_name in line: + y = float(line.split(":=")[1].split("[ms]")[0]) + self.y_vec[f_idx].append(y) + + is_found = True + break + + if not is_found: + self.y_vec[f_idx].append(None) + + if len(self.y_vec[f_idx]) > self.depth: + self.y_vec[f_idx].popleft() + + x_vec = list(range(len(self.y_vec[f_idx]))) + + valid_x_vec = [] + valid_y_vec = [] + for i in range(len(x_vec)): + if self.y_vec[f_idx][i] is not None: + valid_x_vec.append(x_vec[i]) + valid_y_vec.append(self.y_vec[f_idx][i]) + + self.lines[f_idx].set_xdata(valid_x_vec) + self.lines[f_idx].set_ydata(valid_y_vec) + + if len(valid_y_vec) > 0: + max_x = max(max_x, max(valid_x_vec)) + max_y = max(max_y, max(valid_y_vec)) + + plt.xlim(0, max_x) + plt.ylim(0.0, max_y) + + plt.draw() + plt.pause(0.01) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "-f", + "--functions", + type=str, + default="onPath,calcSmoothedTrajectory", + ) + parser.add_argument( + "-d", + "--depth", + type=float, + default=500, + ) + args = parser.parse_args() + + rclpy.init() + node = CalculationCostAnalyzer(args) + rclpy.spin(node) From d02c1652f50afdf803bf190bb301e6ac3f08cb08 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 5 Jan 2024 16:35:59 +0900 Subject: [PATCH 24/32] fix(obstacle_avoidance_planner): use update_bounds instead of update_lower/upper_bounds (#6011) Signed-off-by: Takayuki Murooka --- planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 765136b3cf6f6..47f5f59827267 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -1519,8 +1519,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( osqp_solver_ptr_->updateCscP(P_csc); osqp_solver_ptr_->updateQ(f); osqp_solver_ptr_->updateCscA(A_csc); - osqp_solver_ptr_->updateL(lower_bound); - osqp_solver_ptr_->updateU(upper_bound); + osqp_solver_ptr_->updateBounds(lower_bound, upper_bound); } else { RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "no warm start"); osqp_solver_ptr_ = std::make_unique( From 764482f927ecbdde9d790296ff99ef534561223a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 5 Jan 2024 16:36:18 +0900 Subject: [PATCH 25/32] chore(traffic_light_module): use RCLCPP_DEBUG for the debug printing (#6010) Signed-off-by: Takayuki Murooka --- planning/behavior_velocity_traffic_light_module/src/scene.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index d2d18d74a5c1a..35d44a22f6cf0 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -217,7 +217,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // Move to go out state if ego vehicle over deadline. constexpr double signed_deadline_length = -2.0; if (signed_arc_length_to_stop_point < signed_deadline_length) { - RCLCPP_INFO(logger_, "APPROACH -> GO_OUT"); + RCLCPP_DEBUG(logger_, "APPROACH -> GO_OUT"); state_ = State::GO_OUT; stop_signal_received_time_ptr_.reset(); return true; @@ -263,7 +263,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * constexpr double restart_length = 1.0; if (use_initialization_after_start) { if (signed_arc_length_to_stop_point > restart_length) { - RCLCPP_INFO(logger_, "GO_OUT(RESTART) -> APPROACH"); + RCLCPP_DEBUG(logger_, "GO_OUT(RESTART) -> APPROACH"); state_ = State::APPROACH; } } From 67080aa8abde2a942d1ee08d42a166cbc3f0cf72 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 5 Jan 2024 16:36:54 +0900 Subject: [PATCH 26/32] chore(map_height_fitter): set log level of debug printing to DEBUG (#5997) Signed-off-by: Takayuki Murooka --- map/map_height_fitter/src/map_height_fitter.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index 1eb1edd04a68a..a05c63d44ebce 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -101,11 +101,11 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) req->area.center_y = point.y; req->area.radius = 50; - RCLCPP_INFO(logger, "Send request to map_loader"); + RCLCPP_DEBUG(logger, "Send request to map_loader"); auto future = cli_map_->async_send_request(req); auto status = future.wait_for(std::chrono::seconds(1)); while (status != std::future_status::ready) { - RCLCPP_INFO(logger, "waiting response"); + RCLCPP_DEBUG(logger, "waiting response"); if (!rclcpp::ok()) { return false; } @@ -113,7 +113,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } const auto res = future.get(); - RCLCPP_INFO( + RCLCPP_DEBUG( logger, "Loaded partial pcd map from map_loader (grid size: %lu)", res->new_pointcloud_with_ids.size()); @@ -168,7 +168,7 @@ std::optional MapHeightFitter::Impl::fit(const Point & position, const st const auto logger = node_->get_logger(); tf2::Vector3 point(position.x, position.y, position.z); - RCLCPP_INFO(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); + RCLCPP_DEBUG(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); if (cli_map_) { if (!get_partial_point_cloud_map(position)) { @@ -193,7 +193,7 @@ std::optional MapHeightFitter::Impl::fit(const Point & position, const st return std::nullopt; } - RCLCPP_INFO(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); + RCLCPP_DEBUG(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ()); Point result; result.x = point.getX(); From 2ca9a6bf8677fce4e47b1a8c2fdb795f299affb2 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 5 Jan 2024 16:38:05 +0900 Subject: [PATCH 27/32] chore: set log level of debug printing in rviz plugin to DEBUG (#5996) Signed-off-by: Takayuki Murooka --- .../include/tier4_api_utils/rclcpp/client.hpp | 8 ++++---- .../src/tools/manual_controller.cpp | 4 ++-- .../tier4_state_rviz_plugin/src/autoware_state_panel.hpp | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp index aedbe4b34ea22..16ff5a011536c 100644 --- a/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp +++ b/common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp @@ -41,20 +41,20 @@ class Client const typename ServiceT::Request::SharedPtr & request, const std::chrono::nanoseconds & timeout = std::chrono::seconds(2)) { - RCLCPP_INFO(logger_, "client request"); + RCLCPP_DEBUG(logger_, "client request"); if (!client_->service_is_ready()) { - RCLCPP_INFO(logger_, "client available"); + RCLCPP_DEBUG(logger_, "client available"); return {response_error("Internal service is not available."), nullptr}; } auto future = client_->async_send_request(request); if (future.wait_for(timeout) != std::future_status::ready) { - RCLCPP_INFO(logger_, "client timeout"); + RCLCPP_DEBUG(logger_, "client timeout"); return {response_error("Internal service has timed out."), nullptr}; } - RCLCPP_INFO(logger_, "client response"); + RCLCPP_DEBUG(logger_, "client response"); return {response_success(), future.get()}; } diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp index 9610bf1e2b7f1..fdd270fcce2ef 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp @@ -263,9 +263,9 @@ void ManualController::onClickEnableButton() { auto req = std::make_shared(); req->engage = true; - RCLCPP_INFO(raw_node_->get_logger(), "client request"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); if (!client_engage_->service_is_ready()) { - RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); return; } client_engage_->async_send_request( diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index eba3e4eacd275..8f67a90215bd1 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -171,15 +171,15 @@ public Q_SLOTS: // NOLINT for Qt { auto req = std::make_shared(); - RCLCPP_INFO(raw_node_->get_logger(), "client request"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); if (!client->service_is_ready()) { - RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable"); + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); return; } client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { - RCLCPP_INFO( + RCLCPP_DEBUG( raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code, result.get()->status.message.c_str()); }); From ed21acf2870b6d1ea8f9c7b80ba9d6ba05193145 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 5 Jan 2024 16:38:26 +0900 Subject: [PATCH 28/32] chore(component_interface_utils): set log level of debug printing to DEBUG (#5995) Signed-off-by: Takayuki Murooka --- .../include/component_interface_utils/rclcpp/interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp index bc48e2a0294e0..97f46933f2fe6 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp @@ -52,7 +52,7 @@ struct NodeInterface {ServiceLog::CLIENT_RESPONSE, "client exit"}, {ServiceLog::ERROR_UNREADY, "client unready"}, {ServiceLog::ERROR_TIMEOUT, "client timeout"}}); - RCLCPP_INFO_STREAM(node->get_logger(), type_text.at(type) << ": " << name); + RCLCPP_DEBUG_STREAM(node->get_logger(), type_text.at(type) << ": " << name); ServiceLog msg; msg.stamp = node->now(); From 5de111fe21701facb3a501bcbf0094577ad30667 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 5 Jan 2024 16:38:45 +0900 Subject: [PATCH 29/32] feat(dynamic_avoidance): apply ego's lateral acc/jerk constraints to object polygon (#5953) * feat(dynamic_avoidance): apply ego's lateral acc/jerk constraints to object polygon Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance.param.yaml | 4 + .../dynamic_avoidance_module.hpp | 103 ++++++---- .../dynamic_avoidance_module.cpp | 178 +++++++++++++++--- .../dynamic_avoidance/manager.cpp | 8 + .../marker_utils/utils.hpp | 4 + .../src/marker_utils/utils.cpp | 16 ++ 6 files changed, 249 insertions(+), 64 deletions(-) diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 29fdca78e5bcb..371f7da2fadf5 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -59,6 +59,10 @@ max_time_for_object_lat_shift: 0.0 # [s] lpf_gain_for_lat_avoid_to_offset: 0.9 # [-] + max_ego_lat_acc: 0.3 # [m/ss] + max_ego_lat_jerk: 0.3 # [m/sss] + delay_time_ego_shift: 1.0 # [s] + # for same directional object overtaking_object: max_time_to_collision: 40.0 # [s] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 3ceff4244f2c8..50f5b61e15169 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -35,6 +35,25 @@ #include #include +namespace +{ +template +bool isInVector(const T & val, const std::vector & vec) +{ + return std::find(vec.begin(), vec.end(), val) != vec.end(); +} + +template +std::vector getAllKeys(const std::unordered_map & map) +{ + std::vector keys; + for (const auto & pair : map) { + keys.push_back(pair.first); + } + return keys; +} +} // namespace + namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::PredictedPath; @@ -97,6 +116,10 @@ struct DynamicAvoidanceParameters double max_time_for_lat_shift{0.0}; double lpf_gain_for_lat_avoid_to_offset{0.0}; + double max_ego_lat_acc{0.0}; + double max_ego_lat_jerk{0.0}; + double delay_time_ego_shift{0.0}; + double max_time_to_collision_overtaking_object{0.0}; double start_duration_to_avoid_overtaking_object{0.0}; double end_duration_to_avoid_overtaking_object{0.0}; @@ -113,6 +136,11 @@ struct TimeWhileCollision double time_to_end_collision; }; +struct LatFeasiblePaths +{ + std::vector left_path; + std::vector right_path; +}; class DynamicAvoidanceModule : public SceneModuleInterface { public: @@ -151,6 +179,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool is_collision_left{false}; bool should_be_avoided{false}; std::vector ref_path_points_for_obj_poly; + LatFeasiblePaths ego_lat_feasible_paths; void update( const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid, @@ -172,14 +201,18 @@ class DynamicAvoidanceModule : public SceneModuleInterface { } int max_count_{0}; - int min_count_{0}; + int min_count_{0}; // TODO(murooka): The sign needs to be opposite? void initialize() { current_uuids_.clear(); } void updateObject(const std::string & uuid, const DynamicAvoidanceObject & object) { // add/update object if (object_map_.count(uuid) != 0) { + const auto prev_object = object_map_.at(uuid); object_map_.at(uuid) = object; + // TODO(murooka) refactor this. Counter can be moved to DynamicObject, + // and TargetObjectsManager can be removed. + object_map_.at(uuid).ego_lat_feasible_paths = prev_object.ego_lat_feasible_paths; } else { object_map_.emplace(uuid, object); } @@ -195,14 +228,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface current_uuids_.push_back(uuid); } - void finalize() + void finalize(const LatFeasiblePaths & ego_lat_feasible_paths) { // decrease counter for not updated uuids std::vector not_updated_uuids; for (const auto & object : object_map_) { - if ( - std::find(current_uuids_.begin(), current_uuids_.end(), object.first) == - current_uuids_.end()) { + if (!isInVector(object.first, current_uuids_)) { not_updated_uuids.push_back(object.first); } } @@ -214,45 +245,46 @@ class DynamicAvoidanceModule : public SceneModuleInterface } } - // remove objects whose counter is lower than threshold - std::vector obsolete_uuids; + // update valid object uuids and its variable for (const auto & counter : counter_map_) { - if (counter.second < min_count_) { - obsolete_uuids.push_back(counter.first); + if (!isInVector(counter.first, valid_object_uuids_) && max_count_ <= counter.second) { + valid_object_uuids_.push_back(counter.first); + object_map_.at(counter.first).ego_lat_feasible_paths = ego_lat_feasible_paths; } } - for (const auto & obsolete_uuid : obsolete_uuids) { - counter_map_.erase(obsolete_uuid); - object_map_.erase(obsolete_uuid); + valid_object_uuids_.erase( + std::remove_if( + valid_object_uuids_.begin(), valid_object_uuids_.end(), + [&](const auto & uuid) { + return counter_map_.count(uuid) == 0 || counter_map_.at(uuid) < max_count_; + }), + valid_object_uuids_.end()); + + // remove objects whose counter is lower than threshold + const auto counter_map_keys = getAllKeys(counter_map_); + for (const auto & key : counter_map_keys) { + if (counter_map_.at(key) < min_count_) { + counter_map_.erase(key); + object_map_.erase(key); + } } } std::vector getValidObjects() const { - std::vector objects; - for (const auto & object : object_map_) { - if (counter_map_.count(object.first) != 0) { - if (max_count_ <= counter_map_.at(object.first)) { - objects.push_back(object.second); - } + std::vector valid_objects; + for (const auto & valid_object_uuid : valid_object_uuids_) { + if (object_map_.count(valid_object_uuid) == 0) { + std::cerr + << "[DynamicAvoidance] Internal calculation has an error when getting valid objects." + << std::endl; + continue; } + valid_objects.push_back(object_map_.at(valid_object_uuid)); } - return objects; - } - std::optional getValidObject(const std::string & uuid) const - { - // add/update object - if (counter_map_.count(uuid) == 0) { - return std::nullopt; - } - if (counter_map_.at(uuid) < max_count_) { - return std::nullopt; - } - if (object_map_.count(uuid) == 0) { - return std::nullopt; - } - return object_map_.at(uuid); + + return valid_objects; } - void updateObject( + void updateObjectVariables( const std::string & uuid, const MinMaxValue & lon_offset_to_avoid, const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left, const bool should_be_avoided, @@ -269,6 +301,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface // NOTE: positive is for meeting entry condition, and negative is for exiting. std::unordered_map counter_map_; std::unordered_map object_map_; + std::vector valid_object_uuids_{}; }; struct DecisionWithReason @@ -319,6 +352,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface bool isLabelTargetObstacle(const uint8_t label) const; void updateTargetObjects(); + LatFeasiblePaths generateLateralFeasiblePaths( + const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const; void updateRefPathBeforeLaneChange(const std::vector & ego_ref_path_points); bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 87a940d04e8d3..01635d7a6cb70 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -262,6 +262,35 @@ double calcDistanceToSegment( first_to_second * first_to_target.dot(first_to_second) / std::pow(first_to_second.norm(), 2); return (Eigen::Vector2d{p1.x, p1.y} - p2_nearest).norm(); } + +std::optional> intersectLines( + const std::vector & source_line, + const std::vector & target_line) +{ + for (int source_seg_idx = 0; source_seg_idx < static_cast(source_line.size()) - 1; + ++source_seg_idx) { + for (int target_seg_idx = 0; target_seg_idx < static_cast(target_line.size()) - 1; + ++target_seg_idx) { + const auto intersect_point = tier4_autoware_utils::intersect( + source_line.at(source_seg_idx).position, source_line.at(source_seg_idx + 1).position, + target_line.at(target_seg_idx), target_line.at(target_seg_idx + 1)); + if (intersect_point) { + return std::make_pair(source_seg_idx, *intersect_point); + } + } + } + return std::nullopt; +} + +std::vector convertToPoints( + const std::vector & poses) +{ + std::vector points; + for (const auto & pose : poses) { + points.push_back(pose.position); + } + return points; +} } // namespace DynamicAvoidanceModule::DynamicAvoidanceModule( @@ -310,6 +339,9 @@ bool DynamicAvoidanceModule::isExecutionReady() const void DynamicAvoidanceModule::updateData() { + info_marker_.markers.clear(); + debug_marker_.markers.clear(); + // calculate target objects updateTargetObjects(); @@ -329,9 +361,6 @@ bool DynamicAvoidanceModule::canTransitSuccessState() BehaviorModuleOutput DynamicAvoidanceModule::plan() { - info_marker_.markers.clear(); - debug_marker_.markers.clear(); - const auto & input_path = getPreviousModuleOutput().path; // create obstacles to avoid (= extract from the drivable area) @@ -425,6 +454,10 @@ void DynamicAvoidanceModule::updateTargetObjects() updateRefPathBeforeLaneChange(input_ref_path_points); + const auto & ego_pose = getEgoPose(); + const double ego_vel = getEgoSpeed(); + const auto ego_lat_feasible_paths = generateLateralFeasiblePaths(ego_pose, ego_vel); + // 1. Rough filtering of target objects target_objects_manager_.initialize(); for (const auto & predicted_object : predicted_objects) { @@ -509,7 +542,7 @@ void DynamicAvoidanceModule::updateTargetObjects() latest_time_inside_ego_path); target_objects_manager_.updateObject(obj_uuid, target_object); } - target_objects_manager_.finalize(); + target_objects_manager_.finalize(ego_lat_feasible_paths); // 2. Precise filtering of target objects and check if they should be avoided for (const auto & object : target_objects_manager_.getValidObjects()) { @@ -580,18 +613,24 @@ void DynamicAvoidanceModule::updateTargetObjects() parameters_->max_time_to_collision_overtaking_object < time_to_collision) || (object.vel <= 0 && parameters_->max_time_to_collision_oncoming_object < time_to_collision)) { + const auto time_to_collision_str = time_to_collision == std::numeric_limits::max() + ? "infinity" + : std::to_string(time_to_collision); RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is large.", - obj_uuid.c_str(), time_to_collision); + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%s) is large.", + obj_uuid.c_str(), time_to_collision_str.c_str()); continue; } if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { + const auto time_to_collision_str = time_to_collision == std::numeric_limits::max() + ? "infinity" + : std::to_string(time_to_collision); RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, - "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is a small " + "[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%s) is a small " "negative value.", - obj_uuid.c_str(), time_to_collision); + obj_uuid.c_str(), time_to_collision_str.c_str()); continue; } } @@ -611,7 +650,7 @@ void DynamicAvoidanceModule::updateTargetObjects() const double signed_dist_ego_to_obj = [&]() { const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path.points); const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( - input_path.points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); + input_path.points, ego_pose.position, ego_seg_idx, lat_lon_offset.nearest_idx); if (0 < lon_offset_ego_to_obj) { return std::max( 0.0, lon_offset_ego_to_obj - planner_data_->parameters.front_overhang + @@ -649,7 +688,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } const bool should_be_avoided = true; - target_objects_manager_.updateObject( + target_objects_manager_.updateObjectVariables( obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided, ref_path_points_for_obj_poly); } @@ -657,6 +696,42 @@ void DynamicAvoidanceModule::updateTargetObjects() prev_input_ref_path_points_ = input_ref_path_points; } +LatFeasiblePaths DynamicAvoidanceModule::generateLateralFeasiblePaths( + const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const +{ + const double lat_acc = parameters_->max_ego_lat_acc; + const double lat_jerk = parameters_->max_ego_lat_jerk; + const double delay_time = parameters_->delay_time_ego_shift; + + LatFeasiblePaths ego_lat_feasible_paths; + for (double t = 0; t < 10.0; t += 1.0) { + const double feasible_lat_offset = lat_acc * std::pow(std::max(t - delay_time, 0.0), 2) / 2.0 + + lat_jerk * std::pow(std::max(t - delay_time, 0.0), 3) / 6.0 - + planner_data_->parameters.vehicle_width / 2.0; + const double x = t * ego_vel; + const double y = feasible_lat_offset; + + const auto feasible_left_bound_point = + tier4_autoware_utils::calcOffsetPose(ego_pose, x, -y, 0.0).position; + ego_lat_feasible_paths.left_path.push_back(feasible_left_bound_point); + + const auto feasible_right_bound_point = + tier4_autoware_utils::calcOffsetPose(ego_pose, x, y, 0.0).position; + ego_lat_feasible_paths.right_path.push_back(feasible_right_bound_point); + } + + tier4_autoware_utils::appendMarkerArray( + marker_utils::createPointsMarkerArray( + ego_lat_feasible_paths.left_path, "ego_lat_feasible_left_path", 0, 0.6, 0.9, 0.9), + &debug_marker_); + tier4_autoware_utils::appendMarkerArray( + marker_utils::createPointsMarkerArray( + ego_lat_feasible_paths.right_path, "ego_lat_feasible_right_path", 0, 0.6, 0.9, 0.9), + &debug_marker_); + + return ego_lat_feasible_paths; +} + void DynamicAvoidanceModule::updateRefPathBeforeLaneChange( const std::vector & ego_ref_path_points) { @@ -1251,32 +1326,75 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( ? lon_bound_end_idx_opt.value() : static_cast(ref_path_points_for_obj_poly.size() - 1); - // create inner/outer bound points - std::vector obj_inner_bound_points; - std::vector obj_outer_bound_points; + // create inner bound points + std::vector obj_inner_bound_poses; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { - obj_inner_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( - ref_path_points_for_obj_poly.at(i).point.pose, 0.0, - object.lat_offset_to_avoid->min_value, 0.0) - .position); - obj_outer_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( - ref_path_points_for_obj_poly.at(i).point.pose, 0.0, - object.lat_offset_to_avoid->max_value, 0.0) - .position); + // NOTE: object.lat_offset_to_avoid->min_value is not the minimum value but the inner value. + obj_inner_bound_poses.push_back(tier4_autoware_utils::calcOffsetPose( + ref_path_points_for_obj_poly.at(i).point.pose, 0.0, object.lat_offset_to_avoid->min_value, + 0.0)); } - // create obj_polygon from inner/outer bound points - tier4_autoware_utils::Polygon2d obj_poly; - for (const auto & bound_point : obj_inner_bound_points) { - const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); - obj_poly.outer().push_back(obj_poly_point); + // calculate start index laterally feasible for ego to shift considering maximum lateral jerk and + // acceleration + const auto obj_inner_bound_start_idx = [&]() -> std::optional { + const auto & ego_lat_feasible_path = object.is_collision_left + ? object.ego_lat_feasible_paths.left_path + : object.ego_lat_feasible_paths.right_path; + const auto intersect_result = intersectLines(obj_inner_bound_poses, ego_lat_feasible_path); + + // Check if the object polygon intersects with the ego_lat_feasible_path. + if (intersect_result) { + const auto & [bound_seg_idx, intersect_point] = *intersect_result; + const double lon_offset = tier4_autoware_utils::calcDistance2d( + obj_inner_bound_poses.at(bound_seg_idx), intersect_point); + + const auto obj_inner_bound_start_idx_opt = + motion_utils::insertTargetPoint(bound_seg_idx, lon_offset, obj_inner_bound_poses); + if (obj_inner_bound_start_idx_opt) { + return *obj_inner_bound_start_idx_opt; + } + } + + // Check if the object polygon is fully outside the ego_lat_feasible_path. + const double obj_poly_lat_offset = motion_utils::calcLateralOffset( + ego_lat_feasible_path, obj_inner_bound_poses.front().position); + if ( + (!object.is_collision_left && 0 < obj_poly_lat_offset) || + (object.is_collision_left && obj_poly_lat_offset < 0)) { + return std::nullopt; + } + + return 0; + }(); + if (!obj_inner_bound_start_idx) { + return std::nullopt; } - std::reverse(obj_outer_bound_points.begin(), obj_outer_bound_points.end()); - for (const auto & bound_point : obj_outer_bound_points) { - const auto obj_poly_point = tier4_autoware_utils::Point2d(bound_point.x, bound_point.y); - obj_poly.outer().push_back(obj_poly_point); + + // calculate feasible inner/outer bound points + const auto feasible_obj_inner_bound_poses = std::vector( + obj_inner_bound_poses.begin() + *obj_inner_bound_start_idx, obj_inner_bound_poses.end()); + const auto feasible_obj_inner_bound_points = convertToPoints(feasible_obj_inner_bound_poses); + std::vector feasible_obj_outer_bound_points; + for (const auto & feasible_obj_inner_bound_pose : feasible_obj_inner_bound_poses) { + feasible_obj_outer_bound_points.push_back( + tier4_autoware_utils::calcOffsetPose( + feasible_obj_inner_bound_pose, 0.0, + object.lat_offset_to_avoid->max_value - object.lat_offset_to_avoid->min_value, 0.0) + .position); } + // create obj_polygon from inner/outer bound points + tier4_autoware_utils::Polygon2d obj_poly; + const auto add_points_to_obj_poly = [&](const auto & bound_points) { + for (const auto & bound_point : bound_points) { + obj_poly.outer().push_back(tier4_autoware_utils::Point2d(bound_point.x, bound_point.y)); + } + }; + add_points_to_obj_poly(feasible_obj_inner_bound_points); + std::reverse(feasible_obj_outer_bound_points.begin(), feasible_obj_outer_bound_points.end()); + add_points_to_obj_poly(feasible_obj_outer_bound_points); + boost::geometry::correct(obj_poly); return obj_poly; } diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index 73d263319c2d9..f39727246d722 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -117,6 +117,10 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) p.lpf_gain_for_lat_avoid_to_offset = node->declare_parameter(ns + "lpf_gain_for_lat_avoid_to_offset"); + p.max_ego_lat_acc = node->declare_parameter(ns + "max_ego_lat_acc"); + p.max_ego_lat_jerk = node->declare_parameter(ns + "max_ego_lat_jerk"); + p.delay_time_ego_shift = node->declare_parameter(ns + "delay_time_ego_shift"); + p.max_time_to_collision_overtaking_object = node->declare_parameter(ns + "overtaking_object.max_time_to_collision"); p.start_duration_to_avoid_overtaking_object = @@ -233,6 +237,10 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "lpf_gain_for_lat_avoid_to_offset", p->lpf_gain_for_lat_avoid_to_offset); + updateParam(parameters, ns + "max_ego_lat_acc", p->max_ego_lat_acc); + updateParam(parameters, ns + "max_ego_lat_jerk", p->max_ego_lat_jerk); + updateParam(parameters, ns + "delay_time_ego_shift", p->delay_time_ego_shift); + updateParam( parameters, ns + "overtaking_object.max_time_to_collision", p->max_time_to_collision_overtaking_object); 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 f34c0266ac081..6426f16a44259 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 @@ -66,6 +66,10 @@ 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 createPointsMarkerArray( + const std::vector points, 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/src/marker_utils/utils.cpp b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp index 7933c21305432..d97ea2e09a89d 100644 --- a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp @@ -74,7 +74,23 @@ MarkerArray createFootprintMarkerArray( MarkerArray marker_array; addFootprintMarker(marker, base_link_pose, vehicle_info); + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + for (const auto & point : points) { + marker.points.push_back(point); + } + MarkerArray msg; msg.markers.push_back(marker); return msg; } From 4784c3347f8454874adff44e204533a30ae2d926 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 7 Jan 2024 13:48:49 +0900 Subject: [PATCH 30/32] fix(motion_utils): fix the right bound assignment in resamplePath (#6020) Signed-off-by: Takayuki Murooka --- common/motion_utils/src/resample/resample.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 1566b493c82f8..834d07a87ea09 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -486,7 +486,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( autoware_auto_planning_msgs::msg::Path resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; - resampled_path.right_bound = resampled_path.right_bound; + resampled_path.right_bound = input_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { autoware_auto_planning_msgs::msg::PathPoint path_point; From eb09f6881bbfc9f3a78f4d6ca85526d673c661d9 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 7 Jan 2024 13:49:56 +0900 Subject: [PATCH 31/32] feat(motion_utils, etc): add header argument in convertToTrajectory (#6021) * feat(motion_utils, etc): add header argument in convertToTrajectory Signed-off-by: Takayuki Murooka * fix include Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../include/motion_utils/trajectory/conversion.hpp | 4 +++- common/motion_utils/src/trajectory/conversion.cpp | 4 +++- .../utils/trajectory_utils.hpp | 3 --- .../obstacle_avoidance_planner/src/mpt_optimizer.cpp | 9 +++++---- planning/obstacle_avoidance_planner/src/node.cpp | 7 ++++--- .../src/utils/trajectory_utils.cpp | 9 --------- planning/obstacle_cruise_planner/src/node.cpp | 11 +---------- .../include/path_smoother/utils/trajectory_utils.hpp | 3 --- planning/path_smoother/src/elastic_band.cpp | 5 +++-- planning/path_smoother/src/elastic_band_smoother.cpp | 5 +++-- planning/path_smoother/src/utils/trajectory_utils.cpp | 9 --------- .../src/path_to_trajectory.cpp | 11 +---------- .../include/path_sampler/utils/trajectory_utils.hpp | 3 --- .../sampling_based_planner/path_sampler/src/node.cpp | 6 +++--- .../path_sampler/src/utils/trajectory_utils.cpp | 9 --------- 15 files changed, 26 insertions(+), 72 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp index 804ae47ff0f4e..6e77c1ec4a186 100644 --- a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp @@ -17,6 +17,7 @@ #include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" #include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" +#include "std_msgs/msg/header.hpp" #include @@ -34,7 +35,8 @@ namespace motion_utils * points larger than the capacity. (Tier IV) */ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory); + const std::vector & trajectory, + const std_msgs::msg::Header & header = std_msgs::msg::Header{}); /** * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to diff --git a/common/motion_utils/src/trajectory/conversion.cpp b/common/motion_utils/src/trajectory/conversion.cpp index 97a0bcd06e8cc..f198003d84091 100644 --- a/common/motion_utils/src/trajectory/conversion.cpp +++ b/common/motion_utils/src/trajectory/conversion.cpp @@ -30,9 +30,11 @@ namespace motion_utils * points larger than the capacity. (Tier IV) */ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory) + const std::vector & trajectory, + const std_msgs::msg::Header & header) { autoware_auto_planning_msgs::msg::Trajectory output{}; + output.header = header; for (const auto & pt : trajectory) output.points.push_back(pt); return output; } diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp index efc75f4c90bba..99c01dcf66960 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp @@ -154,9 +154,6 @@ size_t findEgoSegmentIndex( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } -Trajectory createTrajectory( - const std_msgs::msg::Header & header, const std::vector & traj_points); - std::vector resampleTrajectoryPoints( const std::vector traj_points, const double interval); diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 47f5f59827267..ee974db9506f8 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -15,6 +15,7 @@ #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" @@ -1708,17 +1709,17 @@ void MPTOptimizer::publishDebugTrajectories( time_keeper_ptr_->tic(__func__); // reference points - const auto ref_traj = trajectory_utils::createTrajectory( - header, trajectory_utils::convertToTrajectoryPoints(ref_points)); + const auto ref_traj = motion_utils::convertToTrajectory( + trajectory_utils::convertToTrajectoryPoints(ref_points), header); debug_ref_traj_pub_->publish(ref_traj); // fixed reference points const auto fixed_traj_points = extractFixedPoints(ref_points); - const auto fixed_traj = trajectory_utils::createTrajectory(header, fixed_traj_points); + const auto fixed_traj = motion_utils::convertToTrajectory(fixed_traj_points, header); debug_fixed_traj_pub_->publish(fixed_traj); // mpt points - const auto mpt_traj = trajectory_utils::createTrajectory(header, mpt_traj_points); + const auto mpt_traj = motion_utils::convertToTrajectory(mpt_traj_points, header); debug_mpt_traj_pub_->publish(mpt_traj); time_keeper_ptr_->toc(__func__, " "); diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 6d7367af835b6..cc6b0710c75d5 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -16,6 +16,7 @@ #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/marker/marker_helper.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "obstacle_avoidance_planner/debug_marker.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" @@ -236,7 +237,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) "Backward path is NOT supported. Just converting path to trajectory"); const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); - const auto output_traj_msg = trajectory_utils::createTrajectory(path_ptr->header, traj_points); + const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); return; } @@ -268,7 +269,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = - trajectory_utils::createTrajectory(path_ptr->header, full_traj_points); + motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); } @@ -656,7 +657,7 @@ void ObstacleAvoidancePlanner::publishDebugData(const Header & header) const // publish trajectories const auto debug_extended_traj = - trajectory_utils::createTrajectory(header, debug_data_ptr_->extended_traj_points); + motion_utils::convertToTrajectory(debug_data_ptr_->extended_traj_points, header); debug_extended_traj_pub_->publish(debug_extended_traj); time_keeper_ptr_->toc(__func__, " "); diff --git a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp index b836cee1aeaa6..453d55bd7f770 100644 --- a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp @@ -117,15 +117,6 @@ geometry_msgs::msg::Point getNearestPosition( return points.back().pose.position; } -Trajectory createTrajectory( - const std_msgs::msg::Header & header, const std::vector & traj_points) -{ - auto traj = motion_utils::convertToTrajectory(traj_points); - traj.header = header; - - return traj; -} - std::vector resampleTrajectoryPoints( const std::vector traj_points, const double interval) { diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index ccfd731416475..ee1d97f2c6c01 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -171,15 +171,6 @@ std::vector extendTrajectoryPoints( return output_points; } -Trajectory createTrajectory( - const std::vector & traj_points, const std_msgs::msg::Header & header) -{ - auto traj = motion_utils::convertToTrajectory(traj_points); - traj.header = header; - - return traj; -} - std::vector getTargetObjectType(rclcpp::Node & node, const std::string & param_prefix) { std::unordered_map types_map{ @@ -525,7 +516,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms publishVelocityLimit(slow_down_vel_limit, "slow_down"); // 7. Publish trajectory - const auto output_traj = createTrajectory(slow_down_traj_points, msg->header); + const auto output_traj = motion_utils::convertToTrajectory(slow_down_traj_points, msg->header); trajectory_pub_->publish(output_traj); // 8. Publish debug data diff --git a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp b/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp index a8a97964f21a4..724c1a966781f 100644 --- a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp +++ b/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp @@ -113,9 +113,6 @@ size_t findEgoSegmentIndex( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } -Trajectory createTrajectory( - const std_msgs::msg::Header & header, const std::vector & traj_points); - Path create_path(Path path_msg, const std::vector & traj_points); std::vector resampleTrajectoryPoints( diff --git a/planning/path_smoother/src/elastic_band.cpp b/planning/path_smoother/src/elastic_band.cpp index ecf847d496f34..8e538a0d6507f 100644 --- a/planning/path_smoother/src/elastic_band.cpp +++ b/planning/path_smoother/src/elastic_band.cpp @@ -14,6 +14,7 @@ #include "path_smoother/elastic_band.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "path_smoother/type_alias.hpp" #include "path_smoother/utils/geometry_utils.hpp" @@ -262,7 +263,7 @@ std::vector EBPathSmoother::smoothTrajectory( // 8. publish eb trajectory const auto eb_traj = - trajectory_utils::createTrajectory(createHeader(clock_.now()), *eb_traj_points); + motion_utils::convertToTrajectory(*eb_traj_points, createHeader(clock_.now())); debug_eb_traj_pub_->publish(eb_traj); time_keeper_ptr_->toc(__func__, " "); @@ -389,7 +390,7 @@ void EBPathSmoother::updateConstraint( // publish fixed trajectory const auto eb_fixed_traj = - trajectory_utils::createTrajectory(createHeader(clock_.now()), debug_fixed_traj_points); + motion_utils::convertToTrajectory(debug_fixed_traj_points, createHeader(clock_.now())); debug_eb_fixed_traj_pub_->publish(eb_fixed_traj); time_keeper_ptr_->toc(__func__, " "); diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 3247bfd7d3390..8227882cbb61c 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -15,6 +15,7 @@ #include "path_smoother/elastic_band_smoother.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "path_smoother/utils/geometry_utils.hpp" #include "path_smoother/utils/trajectory_utils.hpp" #include "rclcpp/time.hpp" @@ -167,7 +168,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) "Backward path is NOT supported. Just converting path to trajectory"); const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); - const auto output_traj_msg = trajectory_utils::createTrajectory(path_ptr->header, traj_points); + const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); path_pub_->publish(*path_ptr); return; @@ -220,7 +221,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = - trajectory_utils::createTrajectory(path_ptr->header, full_traj_points); + motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); const auto output_path_msg = trajectory_utils::create_path(*path_ptr, full_traj_points); path_pub_->publish(output_path_msg); diff --git a/planning/path_smoother/src/utils/trajectory_utils.cpp b/planning/path_smoother/src/utils/trajectory_utils.cpp index 872fc65a9d456..6409c61700f34 100644 --- a/planning/path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/path_smoother/src/utils/trajectory_utils.cpp @@ -34,15 +34,6 @@ namespace path_smoother { namespace trajectory_utils { -Trajectory createTrajectory( - const std_msgs::msg::Header & header, const std::vector & traj_points) -{ - auto traj = motion_utils::convertToTrajectory(traj_points); - traj.header = header; - - return traj; -} - Path create_path(Path path_msg, const std::vector & traj_points) { path_msg.points.clear(); diff --git a/planning/planning_topic_converter/src/path_to_trajectory.cpp b/planning/planning_topic_converter/src/path_to_trajectory.cpp index f5df79121029b..757dcd0d9f632 100644 --- a/planning/planning_topic_converter/src/path_to_trajectory.cpp +++ b/planning/planning_topic_converter/src/path_to_trajectory.cpp @@ -40,15 +40,6 @@ std::vector convertToTrajectoryPoints(const std::vector & trajectory_points) -{ - auto trajectory = motion_utils::convertToTrajectory(trajectory_points); - trajectory.header = header; - - return trajectory; -} } // namespace PathToTrajectory::PathToTrajectory(const rclcpp::NodeOptions & options) @@ -59,7 +50,7 @@ PathToTrajectory::PathToTrajectory(const rclcpp::NodeOptions & options) void PathToTrajectory::process(const Path::ConstSharedPtr msg) { const auto trajectory_points = convertToTrajectoryPoints(msg->points); - const auto output = createTrajectory(msg->header, trajectory_points); + const auto output = motion_utils::convertToTrajectory(trajectory_points, msg->header); pub_->publish(output); } diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp index c0a3c1d917c25..d848e9b209d28 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp @@ -138,9 +138,6 @@ size_t findEgoSegmentIndex( points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold); } -Trajectory createTrajectory( - const std_msgs::msg::Header & header, const std::vector & traj_points); - std::vector resampleTrajectoryPoints( const std::vector traj_points, const double interval); diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/path_sampler/src/node.cpp index e61f635de67c4..748486edf9ff6 100644 --- a/planning/sampling_based_planner/path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/path_sampler/src/node.cpp @@ -16,6 +16,7 @@ #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/marker/marker_helper.hpp" +#include "motion_utils/trajectory/conversion.hpp" #include "path_sampler/path_generation.hpp" #include "path_sampler/prepare_inputs.hpp" #include "path_sampler/utils/geometry_utils.hpp" @@ -244,13 +245,12 @@ void PathSampler::onPath(const Path::SharedPtr path_ptr) if (!generated_traj_points.empty()) { auto full_traj_points = extendTrajectory(planner_data.traj_points, generated_traj_points); const auto output_traj_msg = - trajectory_utils::createTrajectory(path_ptr->header, full_traj_points); + motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); } else { auto stopping_traj = trajectory_utils::convertToTrajectoryPoints(planner_data.traj_points); for (auto & p : stopping_traj) p.longitudinal_velocity_mps = 0.0; - const auto output_traj_msg = - trajectory_utils::createTrajectory(path_ptr->header, stopping_traj); + const auto output_traj_msg = motion_utils::convertToTrajectory(stopping_traj, path_ptr->header); traj_pub_->publish(output_traj_msg); } diff --git a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp index 76fa90c6baa4b..ad516faa717ff 100644 --- a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp @@ -58,15 +58,6 @@ void compensateLastPose( } } -Trajectory createTrajectory( - const std_msgs::msg::Header & header, const std::vector & traj_points) -{ - auto traj = motion_utils::convertToTrajectory(traj_points); - traj.header = header; - - return traj; -} - std::vector resampleTrajectoryPoints( const std::vector traj_points, const double interval) { From f4ea42908e87ae43b958af6a23d9bdabfe0e7c1b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 7 Jan 2024 20:13:00 +0900 Subject: [PATCH 32/32] chore(mission_planner): use RCLCPP_DEBUG for debug printing (#6024) Signed-off-by: Takayuki Murooka --- .../mission_planner/src/lanelet2_plugins/default_planner.cpp | 2 +- .../mission_planner/src/mission_planner/mission_planner.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index ed364f0987f4f..5d1c0c68d8208 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -400,7 +400,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) log_ss << "x: " << point.position.x << " " << "y: " << point.position.y << std::endl; } - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( logger, "start planning route with check points: " << std::endl << log_ss.str()); diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index a1536a1bebfbf..9ee874928d7d1 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -144,7 +144,7 @@ void MissionPlanner::checkInitialization() } // All data is ready. Now API is available. - RCLCPP_INFO(get_logger(), "Route API is ready."); + RCLCPP_DEBUG(get_logger(), "Route API is ready."); change_state(RouteState::Message::UNSET); data_check_timer_->cancel(); // stop timer callback }