From 79d76cac890d9ab969285eca85feb10ca52edb39 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Tue, 26 Sep 2023 19:22:18 +0200 Subject: [PATCH 01/97] build(landmark_tf_caster): add missing libopencv-dev dependency (#5138) Signed-off-by: Esteve Fernandez --- .../landmark_tf_caster/CMakeLists.txt | 6 ------ .../landmark_based_localizer/landmark_tf_caster/package.xml | 1 + 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt b/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt index 47899d716e411..8d8cb546b6162 100644 --- a/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt +++ b/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt @@ -12,16 +12,10 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(OpenCV REQUIRED) - ament_auto_add_executable(landmark_tf_caster src/landmark_tf_caster_node.cpp src/landmark_tf_caster_core.cpp ) -target_include_directories(landmark_tf_caster - SYSTEM PUBLIC - ${OpenCV_INCLUDE_DIRS} -) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/landmark_based_localizer/landmark_tf_caster/package.xml b/localization/landmark_based_localizer/landmark_tf_caster/package.xml index 9740e26e2d04a..64d81f744ce47 100644 --- a/localization/landmark_based_localizer/landmark_tf_caster/package.xml +++ b/localization/landmark_based_localizer/landmark_tf_caster/package.xml @@ -14,6 +14,7 @@ autoware_auto_mapping_msgs geometry_msgs lanelet2_extension + libopencv-dev rclcpp tf2_eigen tf2_geometry_msgs From 11206eb5d03157a880b56f2afa4841b7cdf4fe77 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 27 Sep 2023 07:53:34 +0900 Subject: [PATCH 02/97] feat(dynamic_avoidance): add max obstacle vel (#5142) * feat(dynamic_avoidance): add max obstacle vel Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/dynamic_avoidance/dynamic_avoidance.param.yaml | 1 + .../dynamic_avoidance/dynamic_avoidance_module.hpp | 1 + .../dynamic_avoidance/dynamic_avoidance_module.cpp | 6 ++++-- .../src/scene_module/dynamic_avoidance/manager.cpp | 2 ++ 4 files changed, 8 insertions(+), 2 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 fe30397683494..127b70fbf7bcb 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 @@ -16,6 +16,7 @@ motorcycle: true pedestrian: false + max_obstacle_vel: 100.0 # [m/s] min_obstacle_vel: 0.0 # [m/s] successive_num_to_entry_dynamic_avoidance_condition: 5 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 b00818f2f79d1..ab8e3cfd70dff 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 @@ -56,6 +56,7 @@ struct DynamicAvoidanceParameters bool avoid_bicycle{false}; bool avoid_motorcycle{false}; bool avoid_pedestrian{false}; + double max_obstacle_vel{0.0}; double min_obstacle_vel{0.0}; int successive_num_to_entry_dynamic_avoidance_condition{0}; int successive_num_to_exit_dynamic_avoidance_condition{0}; 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 181b9fbfd5260..24c074bcb629b 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 @@ -411,10 +411,12 @@ void DynamicAvoidanceModule::updateTargetObjects() continue; } - // 1.b. check if velocity is large enough + // 1.b. check obstacle velocity const auto [obj_tangent_vel, obj_normal_vel] = projectObstacleVelocityToTrajectory(prev_module_path->points, predicted_object); - if (std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel) { + if ( + std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel || + parameters_->max_obstacle_vel < std::abs(obj_tangent_vel)) { continue; } 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 616194ea66ac7..6f624a9f6f017 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 @@ -47,6 +47,7 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( p.avoid_bicycle = node->declare_parameter(ns + "bicycle"); p.avoid_motorcycle = node->declare_parameter(ns + "motorcycle"); p.avoid_pedestrian = node->declare_parameter(ns + "pedestrian"); + p.max_obstacle_vel = node->declare_parameter(ns + "max_obstacle_vel"); p.min_obstacle_vel = node->declare_parameter(ns + "min_obstacle_vel"); p.successive_num_to_entry_dynamic_avoidance_condition = node->declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); @@ -136,6 +137,7 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam(parameters, ns + "motorcycle", p->avoid_motorcycle); updateParam(parameters, ns + "pedestrian", p->avoid_pedestrian); + updateParam(parameters, ns + "max_obstacle_vel", p->max_obstacle_vel); updateParam(parameters, ns + "min_obstacle_vel", p->min_obstacle_vel); updateParam( From a596ce2466be35b44a0a98cd73b622a7d22c3c6a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 27 Sep 2023 09:30:57 +0900 Subject: [PATCH 03/97] fix(behavior_path_planner): add empty guard to combinePath (#5147) Signed-off-by: kosuke55 --- .../utils/goal_planner/util.hpp | 2 -- .../utils/lane_change/utils.hpp | 2 -- .../utils/path_utils.hpp | 2 ++ .../utils/start_planner/util.hpp | 1 - .../src/scene_module/lane_change/normal.cpp | 2 +- .../src/utils/goal_planner/util.cpp | 10 --------- .../src/utils/lane_change/utils.cpp | 13 +----------- .../src/utils/path_utils.cpp | 21 +++++++++++++++++++ .../start_planner/freespace_pull_out.cpp | 3 +-- .../start_planner/geometric_pull_out.cpp | 4 ++-- .../utils/start_planner/shift_pull_out.cpp | 1 - .../src/utils/start_planner/util.cpp | 13 ------------ 12 files changed, 28 insertions(+), 46 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index b7a9248d569c1..396798ee18519 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -44,8 +44,6 @@ using geometry_msgs::msg::Twist; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -// TODO(sugahara) move to util -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index a4c2c2e695752..adb180065f476 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -88,8 +88,6 @@ std::vector> getSortedLaneIds( const RouteHandler & route_handler, const Pose & current_pose, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); - lanelet::ConstLanelets getTargetPreferredLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction & direction, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index e907df8c7a58a..b42502d9edec8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -102,6 +102,8 @@ PathWithLaneId calcCenterLinePath( const std::shared_ptr & planner_data, const Pose & ref_pose, const double longest_dist_to_shift_line, const std::optional & prev_module_path = std::nullopt); + +PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index 09fb9911f7386..f2dba684de991 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -44,7 +44,6 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::RouteHandler; -PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2); PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const Pose & backed_pose, const double velocity); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index d1de12a3bd5f9..44eb74ed90f76 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -125,7 +125,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() const auto found_extended_path = extendPath(); if (found_extended_path) { - *output.path = utils::lane_change::combineReferencePath(*output.path, *found_extended_path); + *output.path = utils::combinePath(*output.path, *found_extended_path); } extendOutputDrivableArea(output); output.reference_path = std::make_shared(getReferencePath()); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 4f24d1b682f3d..038ca76a7fffd 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -44,16 +44,6 @@ namespace behavior_path_planner { namespace goal_planner_utils { -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) -{ - PathWithLaneId path; - path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); - - // skip overlapping point - path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - - return path; -} lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index e4fbf41e5af00..01d6cf4acae88 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -180,17 +180,6 @@ std::vector getAccelerationValues( return sampled_values; } -PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) -{ - PathWithLaneId path; - path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); - - // skip overlapping point - path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - - return path; -} - lanelet::ConstLanelets getTargetPreferredLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction & direction, @@ -355,7 +344,7 @@ std::optional constructCandidatePath( return std::nullopt; } - candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path); + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); candidate_path.shifted_path = shifted_path; return std::optional{candidate_path}; diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index 7673c9a1de9b4..509b31ad3da2b 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -561,4 +561,25 @@ PathWithLaneId calcCenterLinePath( return centerline_path; } + +PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2) +{ + if (path1.points.empty()) { + return path2; + } + if (path2.points.empty()) { + return path1; + } + + PathWithLaneId path{}; + path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); + + // skip overlapping point + path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); + + PathWithLaneId filtered_path = path; + filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); + return filtered_path; +} + } // namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp index 8e5d6b7545c1d..945c386668dee 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -97,8 +97,7 @@ boost::optional FreespacePullOut::plan(const Pose & start_pose, con const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); last_path = utils::resamplePathWithSpline( - start_planner_utils::combineReferencePath(last_path, road_center_line_path), - parameters_.center_line_path_interval); + utils::combinePath(last_path, road_center_line_path), parameters_.center_line_path_interval); const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps; utils::correctDividedPathVelocity(partial_paths); diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 5472885359c31..bc071e2a2a288 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -26,7 +27,6 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; namespace behavior_path_planner { -using start_planner_utils::combineReferencePath; using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters) @@ -106,7 +106,7 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con std::make_pair(velocity, velocity * velocity / (2 * arc_length_on_second_arc_path))); } else { const auto partial_paths = planner_.getPaths(); - const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); + const auto combined_path = utils::combinePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); // Calculate the acceleration required to reach the forward parking velocity at the center of diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 2de72d781994a..b82157cc17e41 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -32,7 +32,6 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; namespace behavior_path_planner { -using start_planner_utils::combineReferencePath; using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( diff --git a/planning/behavior_path_planner/src/utils/start_planner/util.cpp b/planning/behavior_path_planner/src/utils/start_planner/util.cpp index f5b4c9b979ee3..40b100a1bc070 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/util.cpp @@ -38,19 +38,6 @@ namespace behavior_path_planner::start_planner_utils { -PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2) -{ - PathWithLaneId path; - path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); - - // skip overlapping point - path.points.insert(path.points.end(), next(path2.points.begin()), path2.points.end()); - - PathWithLaneId filtered_path = path; - filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); - return filtered_path; -} - PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & shoulder_lanes, const Pose & current_pose, const Pose & backed_pose, const double velocity) From 5715e44352511f3c06ef364bd0294aa987b39d51 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 27 Sep 2023 10:29:30 +0900 Subject: [PATCH 04/97] feat(behavior_path_planner): lane change collided polygon intersect lanes (#5135) * feat(behavior_path_planner): lane change collided polygon intersect lanes Signed-off-by: Muhammad Zulfaqar Azmi * use wrapper instead of std::optional Signed-off-by: Muhammad Zulfaqar Azmi * rearrange code Signed-off-by: Muhammad Zulfaqar Azmi * Revert "chore: add workaround for tf2 update (#5127)" This reverts commit 19ab508c3d590101d8fcb9ffca1248adebd895f3. * fix(autoware_auto_tf2): remove tf2 geometry function duplicated in tf2 geometry msgs Signed-off-by: wep21 * revet change for local build Signed-off-by: kyoichi-sugahara --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: wep21 Signed-off-by: kyoichi-sugahara Co-authored-by: wep21 Co-authored-by: kyoichi-sugahara --- .../utils/lane_change/utils.hpp | 3 ++ .../path_safety_checker/safety_check.hpp | 23 +++++++++++- .../src/scene_module/lane_change/normal.cpp | 35 +++++++++++++------ .../src/utils/lane_change/utils.cpp | 12 +++++++ .../path_safety_checker/safety_check.cpp | 24 ++++++++++--- 5 files changed, 81 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index adb180065f476..8a78d2665fa8f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -183,5 +183,8 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters); + +bool isCollidedPolygonsInLanelet( + const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index f9fbf70b2113b..9cc4afe22bdce 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -88,7 +88,6 @@ boost::optional calcInterpolatedPoseWithVelocity( boost::optional getInterpolatedPoseWithVelocityAndPolygonStamped( const std::vector & pred_path, const double current_time, const VehicleInfo & ego_info); - /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. @@ -111,6 +110,28 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, const double hysteresis_factor, CollisionCheckDebug & debug); +/** + * @brief Iterate the points in the ego and target's predicted path and + * perform safety check for each of the iterated points. + * @param planned_path The predicted path of the ego vehicle. + * @param predicted_ego_path Ego vehicle's predicted path + * @param ego_current_velocity Current velocity of the ego vehicle. + * @param target_object The predicted object to check collision with. + * @param target_object_path The predicted path of the target object. + * @param common_parameters The common parameters used in behavior path planner. + * @param front_object_deceleration The deceleration of the object in the front.(used in RSS) + * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS) + * @param debug The debug information for collision checking. + * @return a list of polygon when collision is expected. + */ +std::vector getCollidedPolygons( + const PathWithLaneId & planned_path, + const std::vector & predicted_ego_path, + const ExtendedPredictedObject & target_object, + const PredictedPathWithPolygon & target_object_path, + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + const double hysteresis_factor, CollisionCheckDebug & debug); + /** * @brief Check collision between ego path footprints with extra longitudinal stopping margin and * objects. diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 44eb74ed90f76..1bcd3e6b0cdcd 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -1454,18 +1454,31 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { - if (!utils::path_safety_checker::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, - current_debug_data.second)) { - path_safety_status.is_safe = false; - marker_utils::updateCollisionCheckDebugMap( - debug_data, current_debug_data, path_safety_status.is_safe); - const auto & obj_pose = obj.initial_pose.pose; - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); - path_safety_status.is_object_coming_from_rear |= - !utils::path_safety_checker::isTargetObjectFront( - path, current_pose, common_parameters.vehicle_info, obj_polygon); + const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, + current_debug_data.second); + + if (collided_polygons.empty()) { + continue; } + + const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet( + collided_polygons, lane_change_path.info.current_lanes); + const auto collision_in_target_lanes = utils::lane_change::isCollidedPolygonsInLanelet( + collided_polygons, lane_change_path.info.target_lanes); + + if (!collision_in_current_lanes && !collision_in_target_lanes) { + continue; + } + + path_safety_status.is_safe = false; + marker_utils::updateCollisionCheckDebugMap( + debug_data, current_debug_data, path_safety_status.is_safe); + const auto & obj_pose = obj.initial_pose.pose; + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); + path_safety_status.is_object_coming_from_rear |= + !utils::path_safety_checker::isTargetObjectFront( + path, current_pose, common_parameters.vehicle_info, obj_polygon); } marker_utils::updateCollisionCheckDebugMap( debug_data, current_debug_data, path_safety_status.is_safe); diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 01d6cf4acae88..0ae599a74e439 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -1092,4 +1092,16 @@ ExtendedPredictedObject transform( return extended_object; } + +bool isCollidedPolygonsInLanelet( + const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes) +{ + const auto lanes_polygon = createPolygon(lanes, 0.0, std::numeric_limits::max()); + + const auto is_in_lanes = [&](const auto & collided_polygon) { + return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon); + }; + + return std::any_of(collided_polygons.begin(), collided_polygons.end(), is_in_lanes); +} } // namespace behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 9fb7d0acdf432..4e671f9f4b599 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -258,6 +258,20 @@ boost::optional getInterpolatedPoseWithVeloci } bool checkCollision( + const PathWithLaneId & planned_path, + const std::vector & predicted_ego_path, + const ExtendedPredictedObject & target_object, + const PredictedPathWithPolygon & target_object_path, + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + const double hysteresis_factor, CollisionCheckDebug & debug) +{ + const auto collided_polygons = getCollidedPolygons( + planned_path, predicted_ego_path, target_object, target_object_path, common_parameters, + rss_parameters, hysteresis_factor, debug); + return collided_polygons.empty(); +} + +std::vector getCollidedPolygons( [[maybe_unused]] const PathWithLaneId & planned_path, const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, @@ -271,6 +285,8 @@ bool checkCollision( debug.current_obj_pose = target_object.initial_pose.pose; } + std::vector collided_polygons{}; + collided_polygons.reserve(target_object_path.path.size()); for (const auto & obj_pose_with_poly : target_object_path.path) { const auto & current_time = obj_pose_with_poly.time; @@ -302,7 +318,8 @@ bool checkCollision( // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { debug.unsafe_reason = "overlap_polygon"; - return false; + collided_polygons.push_back(obj_polygon); + continue; } // compute which one is at the front of the other @@ -341,13 +358,12 @@ bool checkCollision( // check overlap with extended polygon if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { debug.unsafe_reason = "overlap_extended_polygon"; - return false; + collided_polygons.push_back(obj_polygon); } } - return true; + return collided_polygons; } - bool checkCollisionWithExtraStoppingMargin( const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, const double base_to_front, const double base_to_rear, const double width, From 7fdda423eb363f05625f7b26ac67b322cb8473a5 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 27 Sep 2023 10:44:28 +0900 Subject: [PATCH 05/97] feat(motion_utils): add debug backtrace print (#5101) * feat(motion_utils): add debug backtrace print Signed-off-by: Takamasa Horibe * fix style Signed-off-by: Takamasa Horibe * move implementation to cpp Signed-off-by: Takamasa Horibe * fix style Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../motion_utils/trajectory/trajectory.hpp | 10 ++++ common/tier4_autoware_utils/CMakeLists.txt | 1 + .../tier4_autoware_utils/system/backtrace.hpp | 30 +---------- .../src/system/backtrace.cpp | 52 +++++++++++++++++++ 4 files changed, 64 insertions(+), 29 deletions(-) create mode 100644 common/tier4_autoware_utils/src/system/backtrace.cpp diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 94cf48f29c57a..dcdefe61e4000 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -18,6 +18,7 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/geometry/pose_deviation.hpp" #include "tier4_autoware_utils/math/constants.hpp" +#include "tier4_autoware_utils/system/backtrace.hpp" #include @@ -43,6 +44,7 @@ template void validateNonEmpty(const T & points) { if (points.empty()) { + tier4_autoware_utils::print_backtrace(); throw std::invalid_argument("Points is empty."); } } @@ -80,6 +82,7 @@ void validateNonSharpAngle( constexpr double epsilon = 1e-3; if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { + tier4_autoware_utils::print_backtrace(); throw std::invalid_argument("Sharp angle."); } } @@ -396,6 +399,7 @@ double calcLongitudinalOffsetToSegment( { if (seg_idx >= points.size() - 1) { const std::out_of_range e("Segment index is invalid."); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -418,6 +422,7 @@ double calcLongitudinalOffsetToSegment( if (seg_idx >= overlap_removed_points.size() - 1) { const std::runtime_error e("Same points are given."); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -574,6 +579,7 @@ double calcLateralOffset( if (overlap_removed_points.size() == 1) { const std::runtime_error e("Same points are given."); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -635,6 +641,7 @@ double calcLateralOffset( if (overlap_removed_points.size() == 1) { const std::runtime_error e("Same points are given."); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -1037,6 +1044,7 @@ boost::optional calcLongitudinalOffsetPoint( if (points.size() - 1 < src_idx) { const auto e = std::out_of_range("Invalid source index"); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -1161,6 +1169,7 @@ boost::optional calcLongitudinalOffsetPose( if (points.size() - 1 < src_idx) { const auto e = std::out_of_range("Invalid source index"); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } @@ -2375,6 +2384,7 @@ double calcYawDeviation( if (overlap_removed_points.size() <= 1) { const std::runtime_error e("points size is less than 2"); + tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw e; } diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt index 938a78692d9a1..f0a548754a10c 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/common/tier4_autoware_utils/CMakeLists.txt @@ -14,6 +14,7 @@ ament_auto_add_library(tier4_autoware_utils SHARED src/math/trigonometry.cpp src/ros/msg_operation.cpp src/ros/marker_helper.cpp + src/system/backtrace.cpp ) if(BUILD_TESTING) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp index b0b526e8c8b9c..c784d71ad5060 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp @@ -15,38 +15,10 @@ #ifndef TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ #define TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ -#include - -#include -#include -#include -#include - namespace tier4_autoware_utils { -inline void print_backtrace() -{ - constexpr size_t max_frames = 100; - void * addrlist[max_frames + 1]; - - int addrlen = backtrace(addrlist, sizeof(addrlist) / sizeof(void *)); - - if (addrlen == 0) { - return; - } - - char ** symbol_list = backtrace_symbols(addrlist, addrlen); - - std::stringstream ss; - ss << " ********** back trace **********" << std::endl; - for (int i = 1; i < addrlen; i++) { - ss << " @ " << symbol_list[i] << std::endl; - } - std::cerr << ss.str() << std::endl; - - free(symbol_list); -} +void print_backtrace(); } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/src/system/backtrace.cpp b/common/tier4_autoware_utils/src/system/backtrace.cpp new file mode 100644 index 0000000000000..343f200296cad --- /dev/null +++ b/common/tier4_autoware_utils/src/system/backtrace.cpp @@ -0,0 +1,52 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tier4_autoware_utils/system/backtrace.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include + +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ + +void print_backtrace() +{ + constexpr size_t max_frames = 100; + void * addrlist[max_frames + 1]; + + int addrlen = backtrace(addrlist, sizeof(addrlist) / sizeof(void *)); + + if (addrlen == 0) { + return; + } + + char ** symbol_list = backtrace_symbols(addrlist, addrlen); + + std::stringstream ss; + ss << "\n @ ********** back trace **********" << std::endl; + for (int i = 1; i < addrlen; i++) { + ss << " @ " << symbol_list[i] << std::endl; + } + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("tier4_autoware_utils"), ss.str()); + + free(symbol_list); +} + +} // namespace tier4_autoware_utils From bb89363a631164aefbebb6dbf8698d8149b703ed Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 27 Sep 2023 12:46:54 +0900 Subject: [PATCH 06/97] feat(logger_level_configure): add util to set logger level externally in runtime (#5131) * add logger_level_configure Signed-off-by: Takamasa Horibe * add usage Signed-off-by: Takamasa Horibe * fix spell Signed-off-by: Takamasa Horibe * fix from reviewer comment Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- common/tier4_autoware_utils/CMakeLists.txt | 1 + .../ros/logger_level_configure.hpp | 68 +++++++++++++++++++ common/tier4_autoware_utils/package.xml | 1 + .../src/ros/logger_level_configure.cpp | 61 +++++++++++++++++ 4 files changed, 131 insertions(+) create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp create mode 100644 common/tier4_autoware_utils/src/ros/logger_level_configure.cpp diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt index f0a548754a10c..9cb54e52362a5 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/common/tier4_autoware_utils/CMakeLists.txt @@ -14,6 +14,7 @@ ament_auto_add_library(tier4_autoware_utils SHARED src/math/trigonometry.cpp src/ros/msg_operation.cpp src/ros/marker_helper.cpp + src/ros/logger_level_configure.cpp src/system/backtrace.cpp ) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp new file mode 100644 index 0000000000000..5aee3a251dad2 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp @@ -0,0 +1,68 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// =============== Note =============== +// This is a util class implementation of the logger_config_component provided by ROS 2 +// https://github.com/ros2/demos/blob/humble/logging_demo/src/logger_config_component.cpp +// +// When ROS 2 officially supports the set_logger_level option in release version, this class can be +// removed. +// https://github.com/ros2/ros2/issues/1355 + +// =============== How to use =============== +// ___In your_node.hpp___ +// #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +// class YourNode : public rclcpp::Node { +// ... +// +// // Define logger_configure as a node class member variable +// std::unique_ptr logger_configure_; +// } +// +// ___In your_node.cpp___ +// YourNode::YourNode() { +// ... +// +// // Set up logger_configure +// logger_configure_ = std::make_unique(this); +// } + +#ifndef TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ + +#include "logging_demo/srv/config_logger.hpp" + +#include + +namespace tier4_autoware_utils +{ +class LoggerLevelConfigure +{ +private: + using ConfigLogger = logging_demo::srv::ConfigLogger; + +public: + explicit LoggerLevelConfigure(rclcpp::Node * node); + +private: + rclcpp::Logger ros_logger_; + rclcpp::Service::SharedPtr srv_config_logger_; + + void onLoggerConfigService( + const ConfigLogger::Request::SharedPtr request, + const ConfigLogger::Response::SharedPtr response); +}; + +} // namespace tier4_autoware_utils +#endif // TIER4_AUTOWARE_UTILS__ROS__LOGGER_LEVEL_CONFIGURE_HPP_ diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index 6a6b3812cd69b..701907929ecc0 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -20,6 +20,7 @@ diagnostic_msgs geometry_msgs libboost-dev + logging_demo pcl_conversions pcl_ros rclcpp diff --git a/common/tier4_autoware_utils/src/ros/logger_level_configure.cpp b/common/tier4_autoware_utils/src/ros/logger_level_configure.cpp new file mode 100644 index 0000000000000..d764299290d05 --- /dev/null +++ b/common/tier4_autoware_utils/src/ros/logger_level_configure.cpp @@ -0,0 +1,61 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" + +#include + +namespace tier4_autoware_utils +{ +LoggerLevelConfigure::LoggerLevelConfigure(rclcpp::Node * node) : ros_logger_(node->get_logger()) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + srv_config_logger_ = node->create_service( + "~/config_logger", std::bind(&LoggerLevelConfigure::onLoggerConfigService, this, _1, _2)); +} + +void LoggerLevelConfigure::onLoggerConfigService( + const ConfigLogger::Request::SharedPtr request, const ConfigLogger::Response::SharedPtr response) +{ + int logging_severity; + const auto ret_level = rcutils_logging_severity_level_from_string( + request->level.c_str(), rcl_get_default_allocator(), &logging_severity); + + if (ret_level != RCUTILS_RET_OK) { + response->success = false; + RCLCPP_WARN_STREAM( + ros_logger_, "Failed to change logger level for " + << request->logger_name + << " due to an invalid logging severity: " << request->level); + return; + } + + const auto ret_set = + rcutils_logging_set_logger_level(request->logger_name.c_str(), logging_severity); + + if (ret_set != RCUTILS_RET_OK) { + response->success = false; + RCLCPP_WARN_STREAM(ros_logger_, "Failed to set logger level for " << request->logger_name); + return; + } + + response->success = true; + RCLCPP_INFO_STREAM( + ros_logger_, "Logger level [" << request->level << "] is set for " << request->logger_name); + return; +} + +} // namespace tier4_autoware_utils From 3731fd08385cd391c30f35a44b9a94dedfc29ea6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Muhammed=20Yavuz=20K=C3=B6seo=C4=9Flu?= Date: Wed, 27 Sep 2023 07:16:55 +0300 Subject: [PATCH 07/97] feat(ndt_scan_matcher): adding exe time to diagnostics for checking runtime_monitor (#4916) * adding exe time to diagnostics * Update localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> * order of the parameters in constructor corrected * spell check * Update localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> * Update localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml Co-authored-by: Motz <83898149+Motsu-san@users.noreply.github.com> * Update localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp Co-authored-by: Motz <83898149+Motsu-san@users.noreply.github.com> * rebase branch to main * the actual execution time added to diagnostics message * execution_time parameter corrected * integration with header file * style(pre-commit): autofix * parameter naming --------- Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: Motz <83898149+Motsu-san@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/ndt_scan_matcher.param.yaml | 3 +++ .../ndt_scan_matcher/ndt_scan_matcher_core.hpp | 3 +++ .../src/ndt_scan_matcher_core.cpp | 18 ++++++++++++++++-- 3 files changed, 22 insertions(+), 2 deletions(-) diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index c8afa9ea1a916..ecd6c65fa11df 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -85,3 +85,6 @@ # If lidar_point.z - base_link.z <= this threshold , the point will be removed z_margin_for_ground_removal: 0.8 + + # The execution time which means probably NDT cannot matches scans properly + critical_upper_bound_exe_time_ms: 100 # [ms] diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index ed3b99019f7d9..011cda6ba3356 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -208,6 +208,9 @@ class NDTScanMatcher : public rclcpp::Node double z_margin_for_ground_removal_; bool use_dynamic_map_loading_; + + // The execution time which means probably NDT cannot matches scans properly + int critical_upper_bound_exe_time_ms_; }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 7bb2b79e88f33..1d1b829bd5262 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -99,8 +99,9 @@ NDTScanMatcher::NDTScanMatcher() output_pose_covariance_(), regularization_enabled_(declare_parameter("regularization_enabled")), estimate_scores_for_degrounded_scan_( - declare_parameter("estimate_scores_for_degrounded_scan")), - z_margin_for_ground_removal_(declare_parameter("z_margin_for_ground_removal")) + declare_parameter("estimate_scores_for_degrounded_scan", false)), + z_margin_for_ground_removal_(declare_parameter("z_margin_for_ground_removal", 0.8)), + critical_upper_bound_exe_time_ms_(100) { (*state_ptr_)["state"] = "Initializing"; is_activated_ = false; @@ -140,6 +141,11 @@ NDTScanMatcher::NDTScanMatcher() this->declare_parameter("converged_param_nearest_voxel_transformation_likelihood"); lidar_topic_timeout_sec_ = this->declare_parameter("lidar_topic_timeout_sec"); + critical_upper_bound_exe_time_ms_ = + this->declare_parameter("critical_upper_bound_exe_time_ms", critical_upper_bound_exe_time_ms_); + + initial_pose_timeout_sec_ = + this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_); initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); @@ -292,6 +298,13 @@ void NDTScanMatcher::timer_diagnostic() diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT score is unreliably low. "; } + if ( + state_ptr_->count("execution_time") && + std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += + "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; + } // Ignore local optimal solution if ( state_ptr_->count("is_local_optimal_solution_oscillation") && @@ -528,6 +541,7 @@ void NDTScanMatcher::callback_sensor_points( } else { (*state_ptr_)["is_local_optimal_solution_oscillation"] = "0"; } + (*state_ptr_)["execution_time"] = std::to_string(exe_time); } void NDTScanMatcher::transform_sensor_measurement( From 0373d21a7f7dcbd64e96ebc7a3b08275e391656a Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Wed, 27 Sep 2023 14:18:55 +0900 Subject: [PATCH 08/97] fix(autoware_auto_tf2): remove tf2 geometry function duplicated in tf2 geometry msgs (#5089) --- common/autoware_auto_geometry/CMakeLists.txt | 6 --- common/autoware_auto_tf2/CMakeLists.txt | 5 -- .../tf2_autoware_auto_msgs.hpp | 51 ------------------- planning/behavior_path_planner/CMakeLists.txt | 6 --- .../surround_obstacle_checker/CMakeLists.txt | 6 --- 5 files changed, 74 deletions(-) diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt index bf48e8fcaca3f..454e0e7ef044f 100644 --- a/common/autoware_auto_geometry/CMakeLists.txt +++ b/common/autoware_auto_geometry/CMakeLists.txt @@ -12,12 +12,6 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/bounding_box.cpp ) -if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) - target_compile_definitions(${PROJECT_NAME} PRIVATE - DEFINE_LEGACY_FUNCTION - ) -endif() - if(BUILD_TESTING) set(GEOMETRY_GTEST geometry_gtest) set(GEOMETRY_SRC test/src/test_geometry.cpp diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt index fe4cec1c0fc89..a8ae9ec2d962e 100755 --- a/common/autoware_auto_tf2/CMakeLists.txt +++ b/common/autoware_auto_tf2/CMakeLists.txt @@ -17,11 +17,6 @@ if(BUILD_TESTING) "tf2_geometry_msgs" "tf2_ros" ) - if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) - target_compile_definitions(test_tf2_autoware_auto_msgs PRIVATE - DEFINE_LEGACY_FUNCTION - ) - endif() endif() ament_auto_package() diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp index ee8d52fa402c0..b35eb6e93ce6e 100644 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp @@ -45,57 +45,6 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; namespace tf2 { -#ifdef DEFINE_LEGACY_FUNCTION -/*************/ -/** Point32 **/ -/*************/ - -/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Point32 type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The point to transform, as a Point32 message. - * \param t_out The transformed point, as a Point32 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - const KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z); - t_out.x = static_cast(v_out[0]); - t_out.y = static_cast(v_out[1]); - t_out.z = static_cast(v_out[2]); -} - -/*************/ -/** Polygon **/ -/*************/ - -/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Polygon type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The polygon to transform. - * \param t_out The transformed polygon. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const geometry_msgs::msg::Polygon & t_in, geometry_msgs::msg::Polygon & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - // Don't call the Point32 doTransform to avoid doing this conversion every time - const auto kdl_frame = gmTransformToKDL(transform); - // We don't use std::back_inserter to allow aliasing between t_in and t_out - t_out.points.resize(t_in.points.size()); - for (size_t i = 0; i < t_in.points.size(); ++i) { - const KDL::Vector v_out = - kdl_frame * KDL::Vector(t_in.points[i].x, t_in.points[i].y, t_in.points[i].z); - t_out.points[i].x = static_cast(v_out[0]); - t_out.points[i].y = static_cast(v_out[1]); - t_out.points[i].z = static_cast(v_out[2]); - } -} -#endif - /******************/ /** Quaternion32 **/ /******************/ diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 542d4daa0c676..f62e418371401 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -58,12 +58,6 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/marker_utils/lane_change/debug.cpp ) -if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) - target_compile_definitions(behavior_path_planner_node PRIVATE - DEFINE_LEGACY_FUNCTION - ) -endif() - target_include_directories(behavior_path_planner_node SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR} ) diff --git a/planning/surround_obstacle_checker/CMakeLists.txt b/planning/surround_obstacle_checker/CMakeLists.txt index 694bddf421486..0d7b636646783 100644 --- a/planning/surround_obstacle_checker/CMakeLists.txt +++ b/planning/surround_obstacle_checker/CMakeLists.txt @@ -18,12 +18,6 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/node.cpp ) -if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) - target_compile_definitions(${PROJECT_NAME} PRIVATE - DEFINE_LEGACY_FUNCTION - ) -endif() - target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} ) From 0f19d71c8013ecf210a670dc8bf4d42f9e5d23bb Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Wed, 27 Sep 2023 17:04:50 +0900 Subject: [PATCH 09/97] fix(ndt_scan_matcher): removed duplicate parameter declarations "initial_pose_timeout_sec_" (#5153) Removed duplicate parameter declarations "initial_pose_timeout_sec_" Signed-off-by: Shintaro SAKODA --- localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 1d1b829bd5262..44c763721e159 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -144,9 +144,6 @@ NDTScanMatcher::NDTScanMatcher() critical_upper_bound_exe_time_ms_ = this->declare_parameter("critical_upper_bound_exe_time_ms", critical_upper_bound_exe_time_ms_); - initial_pose_timeout_sec_ = - this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_); - initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); initial_pose_distance_tolerance_m_ = From 7149338b3de32122b8db394b193bce0a781a8865 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 27 Sep 2023 17:17:24 +0900 Subject: [PATCH 10/97] feat(planning/control modules): enable logging_level_configure (#5146) * add logger_level_configure Signed-off-by: Takamasa Horibe * add usage Signed-off-by: Takamasa Horibe * fix spell Signed-off-by: Takamasa Horibe * fix from reviewer comment Signed-off-by: Takamasa Horibe * feat(planning/control modules): enable logging_level_configure Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../include/trajectory_follower_node/controller_node.hpp | 3 +++ control/trajectory_follower_node/src/controller_node.cpp | 2 ++ control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 2 ++ control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp | 3 +++ .../behavior_path_planner/behavior_path_planner_node.hpp | 3 +++ .../behavior_path_planner/src/behavior_path_planner_node.cpp | 2 ++ planning/behavior_velocity_planner/src/node.cpp | 2 ++ planning/behavior_velocity_planner/src/node.hpp | 3 +++ .../include/freespace_planner/freespace_planner_node.hpp | 4 ++++ .../src/freespace_planner/freespace_planner_node.cpp | 2 ++ .../mission_planner/src/mission_planner/mission_planner.cpp | 2 ++ .../mission_planner/src/mission_planner/mission_planner.hpp | 3 +++ .../motion_velocity_smoother_node.hpp | 3 +++ .../src/motion_velocity_smoother_node.cpp | 2 ++ .../include/obstacle_avoidance_planner/node.hpp | 3 +++ planning/obstacle_avoidance_planner/src/node.cpp | 2 ++ .../include/obstacle_cruise_planner/node.hpp | 3 +++ planning/obstacle_cruise_planner/src/node.cpp | 2 ++ .../include/obstacle_stop_planner/node.hpp | 3 +++ planning/obstacle_stop_planner/src/node.cpp | 2 ++ .../obstacle_velocity_limiter_node.hpp | 3 +++ .../src/obstacle_velocity_limiter_node.cpp | 2 ++ .../include/path_smoother/elastic_band_smoother.hpp | 3 +++ planning/path_smoother/src/elastic_band_smoother.cpp | 2 ++ .../include/planning_validator/planning_validator.hpp | 3 +++ planning/planning_validator/src/planning_validator.cpp | 2 ++ .../include/surround_obstacle_checker/node.hpp | 3 +++ planning/surround_obstacle_checker/src/node.cpp | 2 ++ .../system_error_monitor/system_error_monitor_core.hpp | 5 +++++ .../system_error_monitor/src/system_error_monitor_core.cpp | 2 ++ .../accel_brake_map_calibrator_node.hpp | 3 +++ .../src/accel_brake_map_calibrator_node.cpp | 2 ++ .../include/raw_vehicle_cmd_converter/node.hpp | 3 +++ vehicle/raw_vehicle_cmd_converter/src/node.cpp | 2 ++ 34 files changed, 88 insertions(+) diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index 489709d1f5c4c..c108bec2671b3 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -19,6 +19,7 @@ #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "trajectory_follower_base/lateral_controller_base.hpp" #include "trajectory_follower_base/longitudinal_controller_base.hpp" #include "trajectory_follower_node/visibility_control.hpp" @@ -111,6 +112,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node void publishDebugMarker( const trajectory_follower::InputData & input_data, const trajectory_follower::LateralOutput & lat_out) const; + + std::unique_ptr logger_configure_; }; } // namespace trajectory_follower_node } // namespace autoware::motion::control diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 675ca9dffe3ae..e35848e9495af 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -85,6 +85,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control timer_control_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&Controller::callbackTimerControl, this)); } + + logger_configure_ = std::make_unique(this); } Controller::LateralControllerMode Controller::getLateralControllerMode( diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 2f2083f626f6b..e2ee0ec65eec0 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -233,6 +233,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&VehicleCmdGate::onTimer, this)); timer_pub_status_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&VehicleCmdGate::publishStatus, this)); + + logger_configure_ = std::make_unique(this); } bool VehicleCmdGate::isHeartbeatTimeout( diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 78d91e266c516..8f0a6aa14d08b 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -17,6 +17,7 @@ #include "adapi_pause_interface.hpp" #include "moderate_stop_interface.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_cmd_filter.hpp" #include @@ -235,6 +236,8 @@ class VehicleCmdGate : public rclcpp::Node // debug MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); + + std::unique_ptr logger_configure_; }; } // namespace vehicle_cmd_gate diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 0f51144c0b792..e8ad6c7e2f987 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -25,6 +25,7 @@ #include "behavior_path_planner/scene_module/side_shift/manager.hpp" #include "behavior_path_planner/scene_module/start_planner/manager.hpp" #include "behavior_path_planner/steering_factor_interface.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" #include @@ -221,6 +222,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node Path convertToPath( const std::shared_ptr & path_candidate_ptr, const bool is_ready, const std::shared_ptr & planner_data); + + std::unique_ptr logger_configure_; }; } // namespace behavior_path_planner 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 8302779313feb..f52e018f2eef7 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -258,6 +258,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&BehaviorPathPlannerNode::run, this)); } + + logger_configure_ = std::make_unique(this); } std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index f30043ad42588..ec28656d8f866 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -151,6 +151,8 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio for (const auto & name : declare_parameter>("launch_modules")) { planner_manager_.launchScenePlugin(*this, name); } + + logger_configure_ = std::make_unique(this); } void BehaviorVelocityPlannerNode::onLoadPlugin( diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index bdc280434ef41..ded1d08700a1d 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -16,6 +16,7 @@ #define NODE_HPP_ #include "planner_manager.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -126,6 +127,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node autoware_auto_planning_msgs::msg::Path generatePath( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); + + std::unique_ptr logger_configure_; }; } // namespace behavior_velocity_planner diff --git a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp index fde0767006e33..9c13f14d180d0 100644 --- a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp +++ b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp @@ -31,6 +31,8 @@ #ifndef FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ #define FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" + #include #include #include @@ -158,6 +160,8 @@ class FreespacePlannerNode : public rclcpp::Node void initializePlanningAlgorithm(); TransformStamped getTransform(const std::string & from, const std::string & to); + + std::unique_ptr logger_configure_; }; } // namespace freespace_planner diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index 025cc32d4ed24..6400ae6d135ba 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -285,6 +285,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&FreespacePlannerNode::onTimer, this)); } + + logger_configure_ = std::make_unique(this); } PlannerCommonParam FreespacePlannerNode::getPlannerCommonParam() diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 7e071e3b926b2..513a5b4c25a2e 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -126,6 +126,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) // otherwise the mission planner rejects the request for the API. data_check_timer_ = create_wall_timer( std::chrono::milliseconds(100), std::bind(&MissionPlanner::checkInitialization, this)); + + logger_configure_ = std::make_unique(this); } void MissionPlanner::checkInitialization() diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index f7af76f43bbb0..ea44d2643e186 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -16,6 +16,7 @@ #define MISSION_PLANNER__MISSION_PLANNER_HPP_ #include "arrival_checker.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -147,6 +148,8 @@ class MissionPlanner : public rclcpp::Node std::shared_ptr normal_route_{nullptr}; std::shared_ptr mrm_route_{nullptr}; + + std::unique_ptr logger_configure_; }; } // namespace mission_planner diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index bb49700464715..347d19f631588 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -30,6 +30,7 @@ #include "tf2_ros/transform_listener.h" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -263,6 +264,8 @@ class MotionVelocitySmootherNode : public rclcpp::Node bool isReverse(const TrajectoryPoints & points) const; void flipVelocity(TrajectoryPoints & points) const; void publishStopWatchTime(); + + std::unique_ptr logger_configure_; }; } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index a4ad118ffe012..75c84d3c482f6 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -99,6 +99,8 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions pub_velocity_limit_->publish(max_vel_msg); clock_ = get_clock(); + + logger_configure_ = std::make_unique(this); } void MotionVelocitySmootherNode::setupSmoother(const double wheelbase) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 14925a8facf88..d6f49afad4391 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -21,6 +21,7 @@ #include "obstacle_avoidance_planner/replan_checker.hpp" #include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -127,6 +128,8 @@ class ObstacleAvoidancePlanner : public rclcpp::Node private: double vehicle_stop_margin_outside_drivable_area_; + + std::unique_ptr logger_configure_; }; } // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index f911fa87bedeb..9cee933a1c9a7 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -140,6 +140,8 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // initialized. set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 5786a96f72c41..9b2de4c8f2de0 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -20,6 +20,7 @@ #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" #include "obstacle_cruise_planner/type_alias.hpp" #include "signal_processing/lowpass_filter_1d.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include @@ -259,6 +260,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // previous closest obstacle std::shared_ptr prev_closest_stop_obstacle_ptr_{nullptr}; + + std::unique_ptr logger_configure_; }; } // namespace motion_planning diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 8ef610cc9df10..b5e0718f20a49 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -438,6 +438,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & // set parameter callback set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); } ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlanningAlgorithmType( diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 4cb71c710a225..93a9736bcdb36 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -18,6 +18,7 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" #include "obstacle_stop_planner/debug_marker.hpp" #include "obstacle_stop_planner/planner_data.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -274,6 +275,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node return ret; } + + std::unique_ptr logger_configure_; }; } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index bec94335c4739..4e4b5d2f91ef3 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -242,6 +242,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod "~/input/expand_stop_range", 1, std::bind(&ObstacleStopPlannerNode::onExpandStopRange, this, std::placeholders::_1), createSubscriptionOptions(this)); + + logger_configure_ = std::make_unique(this); } void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index 8916137c84077..a2ff72d0da562 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -18,6 +18,7 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -101,6 +102,8 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node /// @brief validate the inputs of the node /// @return true if the inputs are valid bool validInputs(); + + std::unique_ptr logger_configure_; }; } // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index 123ed997d4445..f4601979b9b8a 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -79,6 +79,8 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio set_param_res_ = add_on_set_parameters_callback([this](const auto & params) { return onParameter(params); }); + + logger_configure_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ObstacleVelocityLimiterNode::onParameter( diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index 8bd73292a56de..a67983393681f 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -21,6 +21,7 @@ #include "path_smoother/replan_checker.hpp" #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -104,6 +105,8 @@ class ElasticBandSmoother : public rclcpp::Node std::vector extendTrajectory( const std::vector & traj_points, const std::vector & optimized_points) const; + + std::unique_ptr logger_configure_; }; } // namespace path_smoother diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 31fe23a47b0ca..cd314b7b141bf 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -95,6 +95,8 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option // set parameter callback set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ElasticBandSmoother::onParam, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index 321eb23aa7fd4..3b09ebe51ff6b 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -17,6 +17,7 @@ #include "planning_validator/debug_marker.hpp" #include "planning_validator/msg/planning_validator_status.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -117,6 +118,8 @@ class PlanningValidator : public rclcpp::Node Odometry::ConstSharedPtr current_kinematics_; std::shared_ptr debug_pose_publisher_; + + std::unique_ptr logger_configure_; }; } // namespace planning_validator diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index db4510c2e5d0d..13a63a7e7d4ed 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -49,6 +49,8 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) if (publish_diag_) { setupDiag(); } + + logger_configure_ = std::make_unique(this); } void PlanningValidator::setupParameters() diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index f31bb7ddee331..c40fef5b0c43e 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -16,6 +16,7 @@ #define SURROUND_OBSTACLE_CHECKER__NODE_HPP_ #include "surround_obstacle_checker/debug_marker.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include @@ -138,6 +139,8 @@ class SurroundObstacleCheckerNode : public rclcpp::Node // State Machine State state_ = State::PASS; std::shared_ptr last_obstacle_found_time_; + + std::unique_ptr logger_configure_; }; } // namespace surround_obstacle_checker diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index ab542bdfa73ed..1b981fcddb155 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -188,6 +188,8 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio p.state_clear_time = this->declare_parameter("state_clear_time"); p.publish_debug_footprints = this->declare_parameter("publish_debug_footprints"); p.debug_footprint_label = this->declare_parameter("debug_footprint_label"); + + logger_configure_ = std::make_unique(this); } vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index 75d80b56ea711..0bb95882700a5 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -15,6 +15,8 @@ #ifndef SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ #define SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" + #include #include @@ -29,6 +31,7 @@ #include #include +#include #include #include #include @@ -144,6 +147,8 @@ class AutowareErrorMonitor : public rclcpp::Node void updateTimeoutHazardStatus(); bool canAutoRecovery() const; bool isEmergencyHoldingRequired() const; + + std::unique_ptr logger_configure_; }; #endif // SYSTEM_ERROR_MONITOR__SYSTEM_ERROR_MONITOR_CORE_HPP_ diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 1bd7caa19a220..e74ac9c360164 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -274,6 +274,8 @@ AutowareErrorMonitor::AutowareErrorMonitor() const auto period_ns = rclcpp::Rate(params_.update_rate).period(); timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&AutowareErrorMonitor::onTimer, this)); + + logger_configure_ = std::make_unique(this); } void AutowareErrorMonitor::loadRequiredModules(const std::string & key) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 1d8ec80c46e17..3580f0546bbfb 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -22,6 +22,7 @@ #include "raw_vehicle_cmd_converter/brake_map.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" #include @@ -369,6 +370,8 @@ class AccelBrakeMapCalibrator : public rclcpp::Node COMMAND = 1, }; + std::unique_ptr logger_configure_; + public: explicit AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options); }; diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 1910631b11e53..ecfc312d2bdfd 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -232,6 +232,8 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod // timer initTimer(1.0 / update_hz_); initOutputCSVTimer(30.0); + + logger_configure_ = std::make_unique(this); } void AccelBrakeMapCalibrator::initOutputCSVTimer(double period_s) diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp index 27346e99b60fb..98693337b5c28 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp @@ -19,6 +19,7 @@ #include "raw_vehicle_cmd_converter/brake_map.hpp" #include "raw_vehicle_cmd_converter/pid.hpp" #include "raw_vehicle_cmd_converter/steer_map.hpp" +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include @@ -115,6 +116,8 @@ class RawVehicleCommandConverterNode : public rclcpp::Node // for debugging rclcpp::Publisher::SharedPtr debug_pub_steer_pid_; DebugValues debug_steer_; + + std::unique_ptr logger_configure_; }; } // namespace raw_vehicle_cmd_converter diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index e45439ecb4d41..0b0e14a845200 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -84,6 +84,8 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( "~/input/steering", 1, std::bind(&RawVehicleCommandConverterNode::onSteering, this, _1)); debug_pub_steer_pid_ = create_publisher( "/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1); + + logger_configure_ = std::make_unique(this); } void RawVehicleCommandConverterNode::publishActuationCmd() From 154da3dd0c8f14da051247e8ff885a18b59fc560 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 27 Sep 2023 17:39:07 +0900 Subject: [PATCH 11/97] fix(goal_planner): fix goal searcher dying in free space (#5151) Signed-off-by: kosuke55 --- .../src/scene_module/goal_planner/goal_planner_module.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 1f5a823386774..556138dcfb524 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -459,6 +459,7 @@ ModuleStatus GoalPlannerModule::updateState() bool GoalPlannerModule::planFreespacePath() { mutex_.lock(); + goal_searcher_->setPlannerData(planner_data_); goal_searcher_->update(goal_candidates_); const auto goal_candidates = goal_candidates_; debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); From c6701df78115db83c3671be4f314cc0569f90a6b Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 27 Sep 2023 17:40:06 +0900 Subject: [PATCH 12/97] perf(goal_planner): speed up goal candidates update (#5114) * perf(goal_planner): speed up goal candidates update Signed-off-by: kosuke55 typo Signed-off-by: kosuke55 * fix(goal_planner): fix goal searcher dying in free space Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../utils/goal_planner/goal_searcher.hpp | 2 +- .../src/utils/goal_planner/goal_searcher.cpp | 30 +++++++------------ 2 files changed, 12 insertions(+), 20 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index d564dcaa19e29..6115e3fcec31e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -41,7 +41,7 @@ class GoalSearcher : public GoalSearcherBase private: void createAreaPolygons(std::vector original_search_poses); - bool checkCollision(const Pose & pose) const; + bool checkCollision(const Pose & pose, const PredictedObjects & dynamic_objects) const; bool checkCollisionWithLongitudinalDistance( const Pose & ego_pose, const PredictedObjects & dynamic_objects) const; BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index c1d24f02ac140..5e55d712f4f25 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -183,25 +183,25 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) void GoalSearcher::update(GoalCandidates & goal_candidates) const { + const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + const auto [pull_over_lane_stop_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_over_lanes); + // update is_safe for (auto & goal_candidate : goal_candidates) { const Pose goal_pose = goal_candidate.goal_pose; // check collision with footprint - if (checkCollision(goal_pose)) { + if (checkCollision(goal_pose, pull_over_lane_stop_objects)) { goal_candidate.is_safe = false; continue; } // check longitudinal margin with pull over lane objects - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, - parameters_.forward_goal_search_length); - const auto [pull_over_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); - const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_over_lane_objects, parameters_.th_moving_object_velocity); constexpr bool filter_inside = true; const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( goal_pose, planner_data_->parameters.vehicle_width, pull_over_lane_stop_objects, @@ -215,7 +215,7 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const } } -bool GoalSearcher::checkCollision(const Pose & pose) const +bool GoalSearcher::checkCollision(const Pose & pose, const PredictedObjects & dynamic_objects) const { if (parameters_.use_occupancy_grid_for_goal_search) { const Pose pose_grid_coords = global2local(occupancy_grid_map_->getMap(), pose); @@ -228,16 +228,8 @@ bool GoalSearcher::checkCollision(const Pose & pose) const } if (parameters_.use_object_recognition) { - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, - parameters_.forward_goal_search_length); - const auto [pull_over_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); - const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_over_lane_objects, parameters_.th_moving_object_velocity); if (utils::checkCollisionBetweenFootprintAndObjects( - vehicle_footprint_, pose, pull_over_lane_stop_objects, + vehicle_footprint_, pose, dynamic_objects, parameters_.object_recognition_collision_check_margin)) { return true; } From c7d3d0ea9baca4f5b3a6f85f2ddefff190939a05 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 27 Sep 2023 17:42:22 +0900 Subject: [PATCH 13/97] fix(goal_planner): disable freespace planner when fixed goal (#5152) Signed-off-by: kosuke55 --- .../src/scene_module/goal_planner/goal_planner_module.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 556138dcfb524..cffde017ccbe1 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -217,6 +217,10 @@ void GoalPlannerModule::onFreespaceParkingTimer() if (!planner_data_->costmap) { return; } + // fixed goal planner do not use freespace planner + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + return; + } const bool is_new_costmap = (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; From 0cd4f625d255f36d5ff55c571292625cb0272af9 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 27 Sep 2023 17:45:29 +0900 Subject: [PATCH 14/97] fix(out_of_lane): lateral distance calculation for predicted path (#5145) * fix(out_of_lane): lateral distance calculation for predicted path Signed-off-by: Takamasa Horibe * remove overlap points in the object_time_to_range function Signed-off-by: Maxime CLEMENT --------- Signed-off-by: Takamasa Horibe Signed-off-by: Maxime CLEMENT Co-authored-by: Maxime CLEMENT --- .../src/decisions.cpp | 19 ++++++++++--------- .../src/filter_predicted_objects.hpp | 10 +++++++++- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 62114f5a95b00..da46c1355e735 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -78,17 +78,18 @@ std::optional> object_time_to_range( auto worst_exit_time = std::optional(); for (const auto & predicted_path : object.kinematics.predicted_paths) { + const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); const auto enter_point = geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); const auto enter_segment_idx = - motion_utils::findNearestSegmentIndex(predicted_path.path, enter_point); + motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); const auto enter_offset = motion_utils::calcLongitudinalOffsetToSegment( - predicted_path.path, enter_segment_idx, enter_point); - const auto enter_lat_dist = std::abs( - motion_utils::calcLateralOffset(predicted_path.path, enter_point, enter_segment_idx)); + unique_path_points, enter_segment_idx, enter_point); + const auto enter_lat_dist = + std::abs(motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx)); const auto enter_segment_length = tier4_autoware_utils::calcDistance2d( - predicted_path.path[enter_segment_idx], predicted_path.path[enter_segment_idx + 1]); + unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]); const auto enter_offset_ratio = enter_offset / enter_segment_length; const auto enter_time = static_cast(enter_segment_idx) * time_step + enter_offset_ratio * time_step; @@ -96,13 +97,13 @@ std::optional> object_time_to_range( const auto exit_point = geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); const auto exit_segment_idx = - motion_utils::findNearestSegmentIndex(predicted_path.path, exit_point); + motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); const auto exit_offset = motion_utils::calcLongitudinalOffsetToSegment( - predicted_path.path, exit_segment_idx, exit_point); + unique_path_points, exit_segment_idx, exit_point); const auto exit_lat_dist = - std::abs(motion_utils::calcLateralOffset(predicted_path.path, exit_point, exit_segment_idx)); + std::abs(motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); const auto exit_segment_length = tier4_autoware_utils::calcDistance2d( - predicted_path.path[exit_segment_idx], predicted_path.path[exit_segment_idx + 1]); + unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); const auto exit_time = static_cast(exit_segment_idx) * time_step + exit_offset_ratio * time_step; diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index 13bad0d047922..f2555f31a4d5e 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -23,6 +23,9 @@ namespace behavior_velocity_planner::out_of_lane { +using motion_utils::calcLateralOffset; +using tier4_autoware_utils::calcDistance2d; + /// @brief filter predicted objects and their predicted paths /// @param [in] objects predicted objects to filter /// @param [in] ego_data ego data @@ -44,8 +47,13 @@ autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( auto filtered_object = object; const auto is_invalid_predicted_path = [&](const auto & predicted_path) { const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; + const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); + + // Note: lateral offset can not be calculated for path with less than 2 unique points const auto lat_offset_to_current_ego = - std::abs(motion_utils::calcLateralOffset(predicted_path.path, ego_data.pose.position)); + unique_path_points.size() > 1 + ? std::abs(calcLateralOffset(unique_path_points, ego_data.pose.position)) + : calcDistance2d(unique_path_points.front(), ego_data.pose.position); const auto is_crossing_ego = lat_offset_to_current_ego <= object.shape.dimensions.y / 2.0 + std::max( From b98abd2954fef7e6e8379d7dd128d6edcb76e909 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 27 Sep 2023 20:34:04 +0900 Subject: [PATCH 15/97] chore(vechicle_info_util): update maintainer (#4835) Signed-off-by: yamato-ando Co-authored-by: yamato-ando --- vehicle/vehicle_info_util/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vehicle/vehicle_info_util/package.xml b/vehicle/vehicle_info_util/package.xml index 41cb10003a1de..139f8439d60c9 100644 --- a/vehicle/vehicle_info_util/package.xml +++ b/vehicle/vehicle_info_util/package.xml @@ -4,7 +4,6 @@ vehicle_info_util 0.1.0 The vehicle_info_util package - Yamato ANDO Taiki Tanaka @@ -12,6 +11,7 @@ Shumpei Wakabayashi Apache License 2.0 + Yamato ANDO ament_cmake_auto autoware_cmake From d27efb28be8e95c75291c64dba94825552577369 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 27 Sep 2023 22:28:12 +0900 Subject: [PATCH 16/97] docs(start_planner): add freespace pull out docs (#5161) Signed-off-by: kosuke55 --- ...avior_path_planner_start_planner_design.md | 30 +++++++++++++++++-- 1 file changed, 27 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md index 8322952a0be76..95a9697a1d471 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md @@ -8,11 +8,11 @@ This module is activated when a new route is received. Use cases are as follows - start smoothly from the current ego position to centerline. - - ![case1](../image/start_from_road_lane.drawio.svg) + ![case1](../image/start_from_road_lane.drawio.svg) - pull out from the side of the road lane to centerline. - - ![case2](../image/start_from_road_side.drawio.svg) + ![case2](../image/start_from_road_side.drawio.svg) - pull out from the shoulder lane to the road lane centerline. - - ![case3](../image/start_from_road_shoulder.drawio.svg) + ![case3](../image/start_from_road_shoulder.drawio.svg) ## Design @@ -155,3 +155,27 @@ If a safe path cannot be generated from the current position, search backwards f | backward_search_resolution | [m] | double | distance interval for searching backward pull out start point | 2.0 | | backward_path_update_duration | [s] | double | time interval for searching backward pull out start point. this prevents chattering between back driving and pull_out | 3.0 | | ignore_distance_from_lane_end | [m] | double | distance from end of pull out lanes for ignoring start candidates | 15.0 | + +### **freespace pull out** + +If the vehicle gets stuck with pull out along lanes, execute freespace pull out. +To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../../costmap_generator/README.md) to `false` and `enable_freespace_planner` to `true` + + + +#### Unimplemented parts / limitations for freespace pull out + +- When a short path is generated, the ego does can not drive with it. +- Complex cases take longer to generate or fail. +- The drivable area is not guaranteed to fit in the parking_lot. + +#### Parameters freespace parking + +| Name | Unit | Type | Description | Default value | +| :----------------------------- | :--- | :----- | :--------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_freespace_planner | [-] | bool | this flag activates a free space pullout that is executed when a vehicle is stuck due to obstacles in the lanes where the ego is located | true | +| end_pose_search_start_distance | [m] | double | distance from ego to the start point of the search for the end point in the freespace_pull_out driving lane | 20.0 | +| end_pose_search_end_distance | [m] | double | distance from ego to the end point of the search for the end point in the freespace_pull_out driving lane | 30.0 | +| end_pose_search_interval | [m] | bool | interval to search for the end point in the freespace_pull_out driving lane | 2.0 | + +See [freespace_planner](../../freespace_planner/README.md) for other parameters. From f2ffc668e79cc09a3a637c2b0db191ed39056866 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 27 Sep 2023 22:31:21 +0900 Subject: [PATCH 17/97] docs(goal_planner): move smooth goal connection to goal planner docs (#5159) Signed-off-by: kosuke55 --- planning/behavior_path_planner/README.md | 6 ------ .../docs/behavior_path_planner_goal_planner_design.md | 7 ++++++- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 07d949aae8a39..0c0c7c806c119 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -173,12 +173,6 @@ struct ShiftLine Click [here](./docs/behavior_path_planner_path_generation_design.md) for details. -### Smooth goal connection - -If the target path contains a goal, modify the points of the path so that the path and the goal are connected smoothly. This process will change the shape of the path by the distance of `refine_goal_search_radius_range` from the goal. Note that this logic depends on the interpolation algorithm that will be executed in a later module (at the moment it uses spline interpolation), so it needs to be updated in the future. - -![path_goal_refinement](./image/path_goal_refinement.drawio.svg) - ## References / External links This module depends on the external [BehaviorTreeCpp](https://github.com/BehaviorTree/BehaviorTree.CPP) library. diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index ee337555ceb58..54a0ced19de42 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -89,6 +89,10 @@ Either one is activated when all conditions are met. - Route is set with `allow_goal_modification=false` by default. - ego-vehicle is in the same lane as the goal. +If the target path contains a goal, modify the points of the path so that the path and the goal are connected smoothly. This process will change the shape of the path by the distance of `refine_goal_search_radius_range` from the goal. Note that this logic depends on the interpolation algorithm that will be executed in a later module (at the moment it uses spline interpolation), so it needs to be updated in the future. + +![path_goal_refinement](../image/path_goal_refinement.drawio.svg) + ### rough_goal_planner @@ -264,7 +268,8 @@ If the vehicle gets stuck with `lane_parking`, run `freespace_parking`. To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../../costmap_generator/README.md) to `false` and `enable_freespace_parking` to `true` ![pull_over_freespace_parking_flowchart](../image/pull_over_freespace_parking_flowchart.drawio.svg) -\*Series execution with `avoidance_module` in the flowchart is under development. + +Simultaneous execution with `avoidance_module` in the flowchart is under development. From 639d5c0098c4806764a7c9e25a377a01c3f8bc5f Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 28 Sep 2023 09:14:32 +0900 Subject: [PATCH 18/97] feat(goal_planner): prioritize goals before objects to avoid (#5137) sort path priority update docs typo Signed-off-by: kosuke55 --- .../goal_planner/goal_planner.param.yaml | 4 +- ...havior_path_planner_goal_planner_design.md | 18 +-- .../goal_planner/goal_planner_module.hpp | 3 + .../goal_planner/goal_planner_parameters.hpp | 5 +- .../utils/goal_planner/goal_searcher.hpp | 8 +- .../utils/goal_planner/goal_searcher_base.hpp | 15 +- .../utils/goal_planner/util.hpp | 3 + .../goal_planner/goal_planner_module.cpp | 62 ++++++--- .../src/scene_module/goal_planner/manager.cpp | 6 +- .../src/utils/goal_planner/goal_searcher.cpp | 130 +++++++++++++++--- .../src/utils/goal_planner/util.cpp | 44 ++++-- 11 files changed, 238 insertions(+), 60 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index babbdc60d1cf9..0ca028e053fbc 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -12,7 +12,7 @@ goal_priority: "minimum_weighted_distance" # "minimum_weighted_distance" or "minimum_longitudinal_distance" minimum_weighted_distance: lateral_weight: 40.0 - path_priority: "efficient_path" # "efficient_path" or "close_goal" + prioritize_goals_before_objects: true parking_policy: "left_side" # "left_side" or "right_side" forward_goal_search_length: 20.0 backward_goal_search_length: 20.0 @@ -47,6 +47,8 @@ decide_path_distance: 10.0 maximum_deceleration: 1.0 maximum_jerk: 1.0 + path_priority: "efficient_path" # "efficient_path" or "close_goal" + efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exclude freespace parking) # shift parking shift_parking: diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 54a0ced19de42..888d5d9ff9826 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -168,7 +168,7 @@ searched for in certain range of the shoulder lane. | Name | Unit | Type | Description | Default value | | :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | | goal_priority | [-] | string | In case `minimum_weighted_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_longitudinal_distance`, sort with weighted lateral distance against longitudinal distance. | `minimum_weighted_distance` | -| path_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | +| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | | forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | | backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | | goal_search_interval | [m] | double | distance interval for goal search | 2.0 | @@ -184,13 +184,15 @@ searched for in certain range of the shoulder lane. There are three path generation methods. The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane. -| Name | Unit | Type | Description | Default value | -| :------------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | -| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | -| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | -| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | -| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :----- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--------------------------------------- | +| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | +| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | +| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | +| path_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path which is set in `efficient_path_order`. In case `close_goal` use the closest goal to the original one. | efficient_path | +| efficient_path_order | [-] | string | efficient order of pull over planner along lanes　excluding freespace pull over | ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] | ### **shift parking** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index f6b2fa8666f2e..eb6ea5996cc6f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -267,6 +267,9 @@ class GoalPlannerModule : public SceneModuleInterface BehaviorModuleOutput planWithGoalModification(); BehaviorModuleOutput planWaitingApprovalWithGoalModification(); void selectSafePullOverPath(); + void sortPullOverPathCandidatesByGoalPriority( + std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates) const; // deal with pull over partial paths PathWithLaneId getCurrentPath() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index 22624bd1e0a80..2b189d583b924 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -50,9 +50,8 @@ struct GoalPlannerParameters // goal search std::string goal_priority; // "minimum_weighted_distance" or "minimum_longitudinal_distance" double minimum_weighted_distance_lateral_weight{0.0}; - std::string path_priority; // "efficient_path" or "close_goal" + bool prioritize_goals_before_objects{false}; ParkingPolicy parking_policy; // "left_side" or "right_side" - double forward_goal_search_length{0.0}; double backward_goal_search_length{0.0}; double goal_search_interval{0.0}; @@ -83,6 +82,8 @@ struct GoalPlannerParameters double decide_path_distance{0.0}; double maximum_deceleration{0.0}; double maximum_jerk{0.0}; + std::string path_priority; // "efficient_path" or "close_goal" + std::vector efficient_path_order{}; // shift path bool enable_shift_parking{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index 6115e3fcec31e..9d957ae9767c9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -36,14 +36,16 @@ class GoalSearcher : public GoalSearcherBase const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, const std::shared_ptr & occupancy_grid_map); - GoalCandidates search(const Pose & original_goal_pose) override; + GoalCandidates search() override; void update(GoalCandidates & goal_candidates) const override; private: + void countObjectsToAvoid( + GoalCandidates & goal_candidates, const PredictedObjects & objects) const; void createAreaPolygons(std::vector original_search_poses); - bool checkCollision(const Pose & pose, const PredictedObjects & dynamic_objects) const; + bool checkCollision(const Pose & pose, const PredictedObjects & objects) const; bool checkCollisionWithLongitudinalDistance( - const Pose & ego_pose, const PredictedObjects & dynamic_objects) const; + const Pose & ego_pose, const PredictedObjects & objects) const; BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const; BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const; bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp index cac1d37e342c2..7364ea91339e4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp @@ -37,6 +37,7 @@ struct GoalCandidate double lateral_offset{0.0}; size_t id{0}; bool is_safe{true}; + size_t num_objects_to_avoid{0}; }; using GoalCandidates = std::vector; @@ -46,19 +47,25 @@ class GoalSearcherBase explicit GoalSearcherBase(const GoalPlannerParameters & parameters) { parameters_ = parameters; } virtual ~GoalSearcherBase() = default; + void setReferenceGoal(const Pose & reference_goal_pose) + { + reference_goal_pose_ = reference_goal_pose; + } + void setPlannerData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } MultiPolygon2d getAreaPolygons() { return area_polygons_; } - virtual GoalCandidates search(const Pose & original_goal_pose) = 0; + virtual GoalCandidates search() = 0; virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } protected: - GoalPlannerParameters parameters_; - std::shared_ptr planner_data_; - MultiPolygon2d area_polygons_; + GoalPlannerParameters parameters_{}; + std::shared_ptr planner_data_{nullptr}; + Pose reference_goal_pose_{}; + MultiPolygon2d area_polygons_{}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 396798ee18519..1dc6cc411a31f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -64,6 +64,9 @@ MarkerArray createTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createGoalCandidatesMarkerArray( GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); +MarkerArray createNumObjectsToAvoidTextsMarkerArray( + const GoalCandidates & goal_candidates, std::string && ns, + const std_msgs::msg::ColorRGBA & color); } // namespace goal_planner_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index cffde017ccbe1..5d9058890f351 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -68,22 +68,17 @@ GoalPlannerModule::GoalPlannerModule( // planner when goal modification is not allowed fixed_goal_planner_ = std::make_unique(); - // set enabled planner - if (parameters_->enable_shift_parking) { - pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, occupancy_grid_map_)); - } - - // set geometric pull over - if (parameters_->enable_arc_forward_parking) { - constexpr bool is_forward = true; - pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); - } - if (parameters_->enable_arc_backward_parking) { - constexpr bool is_forward = false; - pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); + for (const std::string & planner_type : parameters_->efficient_path_order) { + if (planner_type == "SHIFT" && parameters_->enable_shift_parking) { + pull_over_planners_.push_back(std::make_shared( + node, *parameters, lane_departure_checker, occupancy_grid_map_)); + } else if (planner_type == "ARC_FORWARD" && parameters_->enable_arc_forward_parking) { + pull_over_planners_.push_back(std::make_shared( + node, *parameters, lane_departure_checker, occupancy_grid_map_, /*is_forward*/ true)); + } else if (planner_type == "ARC_BACKWARD" && parameters_->enable_arc_backward_parking) { + pull_over_planners_.push_back(std::make_shared( + node, *parameters, lane_departure_checker, occupancy_grid_map_, /*is_forward*/ false)); + } } if (pull_over_planners_.empty()) { @@ -551,7 +546,8 @@ void GoalPlannerModule::generateGoalCandidates() refined_goal_pose_ = calcRefinedGoal(goal_pose); if (goal_planner_utils::isAllowedGoalModification(route_handler)) { goal_searcher_->setPlannerData(planner_data_); - goal_candidates_ = goal_searcher_->search(refined_goal_pose_); + goal_searcher_->setReferenceGoal(refined_goal_pose_); + goal_candidates_ = goal_searcher_->search(); } else { GoalCandidate goal_candidate{}; goal_candidate.goal_pose = goal_pose; @@ -577,15 +573,47 @@ BehaviorModuleOutput GoalPlannerModule::plan() } } +void GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( + std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates) const +{ + // Create a map of goal_id to its index in goal_candidates + std::map goal_id_to_index; + for (size_t i = 0; i < goal_candidates.size(); ++i) { + goal_id_to_index[goal_candidates[i].id] = i; + } + + // Sort pull_over_path_candidates based on the order in goal_candidates + std::stable_sort( + pull_over_path_candidates.begin(), pull_over_path_candidates.end(), + [&goal_id_to_index](const auto & a, const auto & b) { + return goal_id_to_index[a.goal_id] < goal_id_to_index[b.goal_id]; + }); + + // Sort pull_over_path_candidates based on the order in efficient_path_order + if (parameters_->path_priority == "efficient_path") { + std::stable_sort( + pull_over_path_candidates.begin(), pull_over_path_candidates.end(), + [this](const auto & a, const auto & b) { + const auto & order = parameters_->efficient_path_order; + const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type)); + const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type)); + return a_pos < b_pos; + }); + } +} + void GoalPlannerModule::selectSafePullOverPath() { // select safe lane pull over path from candidates mutex_.lock(); goal_searcher_->setPlannerData(planner_data_); goal_searcher_->update(goal_candidates_); + sortPullOverPathCandidatesByGoalPriority(pull_over_path_candidates_, goal_candidates_); const auto pull_over_path_candidates = pull_over_path_candidates_; const auto goal_candidates = goal_candidates_; mutex_.unlock(); + status_.is_safe_static_objects = false; for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 79459d6efb202..41d0299bca427 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -48,7 +48,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.goal_priority = node->declare_parameter(ns + "goal_priority"); p.minimum_weighted_distance_lateral_weight = node->declare_parameter(ns + "minimum_weighted_distance.lateral_weight"); - p.path_priority = node->declare_parameter(ns + "path_priority"); + p.prioritize_goals_before_objects = + node->declare_parameter(ns + "prioritize_goals_before_objects"); p.forward_goal_search_length = node->declare_parameter(ns + "forward_goal_search_length"); p.backward_goal_search_length = @@ -112,6 +113,9 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.decide_path_distance = node->declare_parameter(ns + "decide_path_distance"); p.maximum_deceleration = node->declare_parameter(ns + "maximum_deceleration"); p.maximum_jerk = node->declare_parameter(ns + "maximum_jerk"); + p.path_priority = node->declare_parameter(ns + "path_priority"); + p.efficient_path_order = + node->declare_parameter>(ns + "efficient_path_order"); } // shift parking diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 5e55d712f4f25..11173154f6b3a 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -21,6 +21,7 @@ #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -41,8 +42,20 @@ using tier4_autoware_utils::inverseTransformPose; // Sort with smaller longitudinal distances taking precedence over smaller lateral distances. struct SortByLongitudinalDistance { + bool prioritize_goals_before_objects{false}; + explicit SortByLongitudinalDistance(bool prioritize_goals_before_objects) + : prioritize_goals_before_objects(prioritize_goals_before_objects) + { + } + bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept { + if (prioritize_goals_before_objects) { + if (a.num_objects_to_avoid != b.num_objects_to_avoid) { + return a.num_objects_to_avoid < b.num_objects_to_avoid; + } + } + const double diff = a.distance_from_original_goal - b.distance_from_original_goal; constexpr double eps = 0.01; // If the longitudinal distances are approximately equal, sort based on lateral offset. @@ -58,10 +71,21 @@ struct SortByLongitudinalDistance struct SortByWeightedDistance { double lateral_cost{0.0}; - explicit SortByWeightedDistance(double cost) : lateral_cost(cost) {} + bool prioritize_goals_before_objects{false}; + + SortByWeightedDistance(double cost, bool prioritize_goals_before_objects) + : lateral_cost(cost), prioritize_goals_before_objects(prioritize_goals_before_objects) + { + } bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept { + if (prioritize_goals_before_objects) { + if (a.num_objects_to_avoid != b.num_objects_to_avoid) { + return a.num_objects_to_avoid < b.num_objects_to_avoid; + } + } + return a.distance_from_original_goal + lateral_cost * a.lateral_offset < b.distance_from_original_goal + lateral_cost * b.lateral_offset; } @@ -77,7 +101,7 @@ GoalSearcher::GoalSearcher( { } -GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) +GoalCandidates GoalSearcher::search() { GoalCandidates goal_candidates{}; @@ -101,7 +125,7 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); const auto goal_arc_coords = - lanelet::utils::getArcCoordinates(pull_over_lanes, original_goal_pose); + lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_); const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); const double s_end = goal_arc_coords.length + forward_length; auto center_line_path = utils::resamplePathWithSpline( @@ -131,10 +155,11 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = -distance_from_bound.value() + sign * margin_from_boundary; + // original means non lateral offset poses const Pose original_search_pose = calcOffsetPose(center_pose, 0, offset_from_center_line, 0); const double longitudinal_distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength( - center_line_path.points, original_goal_pose.position, original_search_pose.position)); + center_line_path.points, reference_goal_pose_.position, original_search_pose.position)); original_search_poses.push_back(original_search_pose); // for createAreaPolygon Pose search_pose{}; // search goal_pose in lateral direction @@ -170,17 +195,75 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) } createAreaPolygons(original_search_poses); - if (parameters_.goal_priority == "minimum_weighted_distance") { - std::sort( - goal_candidates.begin(), goal_candidates.end(), - SortByWeightedDistance(parameters_.minimum_weighted_distance_lateral_weight)); - } else if (parameters_.goal_priority == "minimum_longitudinal_distance") { - std::sort(goal_candidates.begin(), goal_candidates.end(), SortByLongitudinalDistance()); - } + update(goal_candidates); return goal_candidates; } +void GoalSearcher::countObjectsToAvoid( + GoalCandidates & goal_candidates, const PredictedObjects & objects) const +{ + const auto & route_handler = planner_data_->route_handler; + const double forward_length = parameters_.forward_goal_search_length; + const double backward_length = parameters_.backward_goal_search_length; + + // calculate search start/end pose in pull over lanes + const auto [search_start_pose, search_end_pose] = std::invoke([&]() -> std::pair { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + const auto goal_arc_coords = + lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_); + const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); + const double s_end = goal_arc_coords.length + forward_length; + const auto center_line_path = utils::resamplePathWithSpline( + route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), + parameters_.goal_search_interval); + return std::make_pair( + center_line_path.points.front().point.pose, center_line_path.points.back().point.pose); + }); + + // generate current lane center line path to check collision with objects + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + const auto current_center_line_path = std::invoke([&]() -> PathWithLaneId { + const double s_start = + lanelet::utils::getArcCoordinates(current_lanes, search_start_pose).length; + const double s_end = lanelet::utils::getArcCoordinates(current_lanes, search_end_pose).length; + return utils::resamplePathWithSpline( + route_handler->getCenterLinePath(current_lanes, s_start, s_end), 1.0); + }); + + // reset num_objects_to_avoid + for (auto & goal_candidate : goal_candidates) { + goal_candidate.num_objects_to_avoid = 0; + } + + // count number of objects to avoid + for (const auto & object : objects.objects) { + for (const auto & p : current_center_line_path.points) { + const auto transformed_vehicle_footprint = + transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(p.point.pose)); + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + const double distance = boost::geometry::distance(obj_polygon, transformed_vehicle_footprint); + if (distance > parameters_.object_recognition_collision_check_margin) { + continue; + } + const Pose & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const double s_object = lanelet::utils::getArcCoordinates(current_lanes, object_pose).length; + for (auto & goal_candidate : goal_candidates) { + const Pose & goal_pose = goal_candidate.goal_pose; + const double s_goal = lanelet::utils::getArcCoordinates(current_lanes, goal_pose).length; + if (s_object < s_goal) { + goal_candidate.num_objects_to_avoid++; + } + } + break; + } + } +} + void GoalSearcher::update(GoalCandidates & goal_candidates) const { const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( @@ -191,6 +274,22 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const const auto [pull_over_lane_stop_objects, others] = utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_over_lanes); + if (parameters_.prioritize_goals_before_objects) { + countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); + } + + if (parameters_.goal_priority == "minimum_weighted_distance") { + std::sort( + goal_candidates.begin(), goal_candidates.end(), + SortByWeightedDistance( + parameters_.minimum_weighted_distance_lateral_weight, + parameters_.prioritize_goals_before_objects)); + } else if (parameters_.goal_priority == "minimum_longitudinal_distance") { + std::sort( + goal_candidates.begin(), goal_candidates.end(), + SortByLongitudinalDistance(parameters_.prioritize_goals_before_objects)); + } + // update is_safe for (auto & goal_candidate : goal_candidates) { const Pose goal_pose = goal_candidate.goal_pose; @@ -215,7 +314,7 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const } } -bool GoalSearcher::checkCollision(const Pose & pose, const PredictedObjects & dynamic_objects) const +bool GoalSearcher::checkCollision(const Pose & pose, const PredictedObjects & objects) const { if (parameters_.use_occupancy_grid_for_goal_search) { const Pose pose_grid_coords = global2local(occupancy_grid_map_->getMap(), pose); @@ -229,7 +328,7 @@ bool GoalSearcher::checkCollision(const Pose & pose, const PredictedObjects & dy if (parameters_.use_object_recognition) { if (utils::checkCollisionBetweenFootprintAndObjects( - vehicle_footprint_, pose, dynamic_objects, + vehicle_footprint_, pose, objects, parameters_.object_recognition_collision_check_margin)) { return true; } @@ -238,7 +337,7 @@ bool GoalSearcher::checkCollision(const Pose & pose, const PredictedObjects & dy } bool GoalSearcher::checkCollisionWithLongitudinalDistance( - const Pose & ego_pose, const PredictedObjects & dynamic_objects) const + const Pose & ego_pose, const PredictedObjects & objects) const { if ( parameters_.use_occupancy_grid_for_goal_search && @@ -274,8 +373,7 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( if ( utils::calcLongitudinalDistanceFromEgoToObjects( ego_pose, planner_data_->parameters.base_link2front, - planner_data_->parameters.base_link2rear, - dynamic_objects) < parameters_.longitudinal_margin) { + planner_data_->parameters.base_link2rear, objects) < parameters_.longitudinal_margin) { return true; } } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 038ca76a7fffd..ff26c7a3654c3 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -131,7 +131,7 @@ MarkerArray createPosesMarkerArray( return msg; } -MarkerArray createTextsMarkerArray( +MarkerArray createGoalPriorityTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color) { MarkerArray msg{}; @@ -150,24 +150,52 @@ MarkerArray createTextsMarkerArray( return msg; } +MarkerArray createNumObjectsToAvoidTextsMarkerArray( + const GoalCandidates & goal_candidates, std::string && ns, const std_msgs::msg::ColorRGBA & color) +{ + MarkerArray msg{}; + int32_t i = 0; + for (const auto & goal_candidate : goal_candidates) { + const Pose & pose = goal_candidate.goal_pose; + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.3, 0.3, 0.3), color); + marker.pose = calcOffsetPose(pose, -0.5, 0, 1.0); + marker.id = i; + marker.text = std::to_string(goal_candidate.num_objects_to_avoid); + msg.markers.push_back(marker); + i++; + } + + return msg; +} + MarkerArray createGoalCandidatesMarkerArray( GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color) { - // convert to pose vector + GoalCandidates safe_goal_candidates{}; + std::copy_if( + goal_candidates.begin(), goal_candidates.end(), std::back_inserter(safe_goal_candidates), + [](const auto & goal_candidate) { return goal_candidate.is_safe; }); + std::vector pose_vector{}; - for (const auto & goal_candidate : goal_candidates) { - if (goal_candidate.is_safe) { - pose_vector.push_back(goal_candidate.goal_pose); - } - } + std::transform( + safe_goal_candidates.begin(), safe_goal_candidates.end(), std::back_inserter(pose_vector), + [](const auto & goal_candidate) { return goal_candidate.goal_pose; }); auto marker_array = createPosesMarkerArray(pose_vector, "goal_candidates", color); for (const auto & text_marker : - createTextsMarkerArray( + createGoalPriorityTextsMarkerArray( pose_vector, "goal_candidates_priority", createMarkerColor(1.0, 1.0, 1.0, 0.999)) .markers) { marker_array.markers.push_back(text_marker); } + for (const auto & text_marker : createNumObjectsToAvoidTextsMarkerArray( + safe_goal_candidates, "goal_candidates_num_objects_to_avoid", + createMarkerColor(0.5, 0.5, 0.5, 0.999)) + .markers) { + marker_array.markers.push_back(text_marker); + } return marker_array; } From 4558485999f2e0f063d0b3b5362d5246e08b6a12 Mon Sep 17 00:00:00 2001 From: Ryoma Sakata <66205605+SKT-r@users.noreply.github.com> Date: Thu, 28 Sep 2023 09:52:27 +0900 Subject: [PATCH 19/97] refactor(localization_error_monitor): rework parameters (#5143) Signed-off-by: SKT-r Co-authored-by: SKT-r --- localization/localization_error_monitor/src/node.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/node.cpp index 75858c1f4be4d..307dc9445eb28 100644 --- a/localization/localization_error_monitor/src/node.cpp +++ b/localization/localization_error_monitor/src/node.cpp @@ -33,14 +33,14 @@ LocalizationErrorMonitor::LocalizationErrorMonitor() : Node("localization_error_monitor"), updater_(this) { - scale_ = this->declare_parameter("scale", 3.0); - error_ellipse_size_ = this->declare_parameter("error_ellipse_size", 1.0); - warn_ellipse_size_ = this->declare_parameter("warn_ellipse_size", 0.8); + scale_ = this->declare_parameter("scale"); + error_ellipse_size_ = this->declare_parameter("error_ellipse_size"); + warn_ellipse_size_ = this->declare_parameter("warn_ellipse_size"); error_ellipse_size_lateral_direction_ = - this->declare_parameter("error_ellipse_size_lateral_direction", 0.3); + this->declare_parameter("error_ellipse_size_lateral_direction"); warn_ellipse_size_lateral_direction_ = - this->declare_parameter("warn_ellipse_size_lateral_direction", 0.2); + this->declare_parameter("warn_ellipse_size_lateral_direction"); odom_sub_ = this->create_subscription( "input/odom", 1, std::bind(&LocalizationErrorMonitor::onOdom, this, std::placeholders::_1)); From f2263e1c00212a01fe8608de022904bbda8ca2d7 Mon Sep 17 00:00:00 2001 From: Ryoma Sakata <66205605+SKT-r@users.noreply.github.com> Date: Thu, 28 Sep 2023 10:03:14 +0900 Subject: [PATCH 20/97] refactor(gnss_poser): rework parameters (#5140) * refactor(gnss_poser): rework parameters Signed-off-by: SKT-r * remove unnecessary declaration from launch Signed-off-by: kminoda --------- Signed-off-by: SKT-r Signed-off-by: kminoda Co-authored-by: SKT-r Co-authored-by: kminoda --- sensing/gnss_poser/CMakeLists.txt | 1 + .../gnss_poser/config/gnss_poser.param.yaml | 9 +++++++++ .../gnss_poser/launch/gnss_poser.launch.xml | 20 +++---------------- sensing/gnss_poser/src/gnss_poser_core.cpp | 14 ++++++------- 4 files changed, 20 insertions(+), 24 deletions(-) create mode 100644 sensing/gnss_poser/config/gnss_poser.param.yaml diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt index 167f2c51337ce..775cde514945f 100644 --- a/sensing/gnss_poser/CMakeLists.txt +++ b/sensing/gnss_poser/CMakeLists.txt @@ -34,5 +34,6 @@ rclcpp_components_register_node(gnss_poser_node ) ament_auto_package(INSTALL_TO_SHARE + config launch ) diff --git a/sensing/gnss_poser/config/gnss_poser.param.yaml b/sensing/gnss_poser/config/gnss_poser.param.yaml new file mode 100644 index 0000000000000..30be98bba8cf1 --- /dev/null +++ b/sensing/gnss_poser/config/gnss_poser.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + base_frame: base_link + gnss_base_frame: gnss_base_link + gnss_frame: gnss + map_frame: map + buff_epoch: 1 + use_gnss_ins_orientation: true + gnss_pose_pub_method: 0 diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index 60e952c6845f4..d9b850e6995ae 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -1,5 +1,7 @@ + + @@ -7,15 +9,6 @@ - - - - - - - - - @@ -23,13 +16,6 @@ - - - - - - - - + diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 1ae8bce2ed7f2..599a017bffed7 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -30,14 +30,14 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) : rclcpp::Node("gnss_poser", node_options), tf2_listener_(tf2_buffer_), tf2_broadcaster_(*this), - base_frame_(declare_parameter("base_frame", "base_link")), - gnss_frame_(declare_parameter("gnss_frame", "gnss")), - gnss_base_frame_(declare_parameter("gnss_base_frame", "gnss_base_link")), - map_frame_(declare_parameter("map_frame", "map")), - use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation", true)), + base_frame_(declare_parameter("base_frame")), + gnss_frame_(declare_parameter("gnss_frame")), + gnss_base_frame_(declare_parameter("gnss_base_frame")), + map_frame_(declare_parameter("map_frame")), + use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation")), msg_gnss_ins_orientation_stamped_( std::make_shared()), - gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method", 0)) + gnss_pose_pub_method(declare_parameter("gnss_pose_pub_method")) { // Subscribe to map_projector_info topic const auto adaptor = component_interface_utils::NodeAdaptor(this); @@ -45,7 +45,7 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) sub_map_projector_info_, [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); }); - int buff_epoch = declare_parameter("buff_epoch", 1); + int buff_epoch = declare_parameter("buff_epoch"); position_buffer_.set_capacity(buff_epoch); nav_sat_fix_sub_ = create_subscription( From c552c3dedc82f61015a918e505375d0bd04cde6c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 28 Sep 2023 10:07:19 +0900 Subject: [PATCH 21/97] fix(drivable_area_expansion): segment index out of range (#5120) Signed-off-by: Takayuki Murooka --- .../utils/drivable_area_expansion/drivable_area_expansion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index d0673d8bb9ea8..60fb2ba2ff2e8 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -60,7 +60,7 @@ std::vector crop_and_resample( // crop const auto crop_seg_idx = motion_utils::findNearestSegmentIndex(points, crop_pose.position); const auto cropped_points = motion_utils::cropPoints( - points, crop_pose.position, crop_seg_idx + 1, + points, crop_pose.position, crop_seg_idx, planner_data->drivable_area_expansion_parameters.max_path_arc_length, 0.0); planner_data->drivable_area_expansion_prev_crop_pose = crop_pose; // resample From 32c2e67ce4be25475120ff7599381d53dac60dd0 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 28 Sep 2023 12:07:18 +0900 Subject: [PATCH 22/97] feat(time_synchronizer): organize twist input (#5160) * feat(time_synchronizer): organize twist input Signed-off-by: kminoda * fix Signed-off-by: kminoda * style(pre-commit): autofix --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../time_synchronizer_nodelet.hpp | 11 ++-- .../launch/preprocessor.launch.py | 5 +- .../time_synchronizer_nodelet.cpp | 54 +++++++++++++++---- 3 files changed, 57 insertions(+), 13 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index f30c02a97a6c0..8c3cb98ba266e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -68,9 +68,10 @@ #include #include -#include #include #include +#include +#include #include #include #include @@ -127,11 +128,14 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node /** \brief A vector of subscriber. */ std::vector::SharedPtr> filters_; - rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::TimerBase::SharedPtr timer_; diagnostic_updater::Updater updater_{this}; + const std::string input_twist_topic_type_; + /** \brief Output TF frame the concatenated points should be transformed to. */ std::string output_frame_; @@ -168,7 +172,8 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node void cloud_callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name); - void twist_callback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input); + void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input); + void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); void timer_callback(); void checkSyncStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index 0d65b45b1b3de..d82c23152aee4 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -58,7 +58,10 @@ def launch_setup(context, *args, **kwargs): package=pkg, plugin="pointcloud_preprocessor::PointCloudDataSynchronizerComponent", name="synchronizer_filter", - remappings=[("output", "points_raw/concatenated")], + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "points_raw/concatenated"), + ], parameters=[ { "input_topics": LaunchConfiguration("input_points_raw_list"), diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index a6bf64b835f61..5d261f42391e9 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -41,7 +41,8 @@ namespace pointcloud_preprocessor { PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( const rclcpp::NodeOptions & node_options) -: Node("point_cloud_time_synchronizer_component", node_options) +: Node("point_cloud_time_synchronizer_component", node_options), + input_twist_topic_type_(declare_parameter("input_twist_topic_type", "twist")) { // initialize debug tool { @@ -129,10 +130,21 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( } // Subscribe to the twist - auto twist_cb = - std::bind(&PointCloudDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); - sub_twist_ = this->create_subscription( - "/vehicle/status/velocity_status", rclcpp::QoS{100}, twist_cb); + if (input_twist_topic_type_ == "twist") { + auto twist_cb = std::bind( + &PointCloudDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); + sub_twist_ = this->create_subscription( + "~/input/twist", rclcpp::QoS{100}, twist_cb); + } else if (input_twist_topic_type_ == "odom") { + auto odom_cb = + std::bind(&PointCloudDataSynchronizerComponent::odom_callback, this, std::placeholders::_1); + sub_odom_ = this->create_subscription( + "~/input/odom", rclcpp::QoS{100}, odom_cb); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), "input_twist_topic_type is invalid: " << input_twist_topic_type_); + throw std::runtime_error("input_twist_topic_type is invalid: " + input_twist_topic_type_); + } } // Transformed Raw PointCloud2 Publisher @@ -474,7 +486,33 @@ void PointCloudDataSynchronizerComponent::timer_callback() } void PointCloudDataSynchronizerComponent::twist_callback( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input) + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input) +{ + // if rosbag restart, clear buffer + if (!twist_ptr_queue_.empty()) { + if (rclcpp::Time(twist_ptr_queue_.front()->header.stamp) > rclcpp::Time(input->header.stamp)) { + twist_ptr_queue_.clear(); + } + } + + // pop old data + while (!twist_ptr_queue_.empty()) { + if ( + rclcpp::Time(twist_ptr_queue_.front()->header.stamp) + rclcpp::Duration::from_seconds(1.0) > + rclcpp::Time(input->header.stamp)) { + break; + } + twist_ptr_queue_.pop_front(); + } + + auto twist_ptr = std::make_shared(); + twist_ptr->header.stamp = input->header.stamp; + twist_ptr->twist = input->twist.twist; + twist_ptr_queue_.push_back(twist_ptr); +} + +void PointCloudDataSynchronizerComponent::odom_callback( + const nav_msgs::msg::Odometry::ConstSharedPtr input) { // if rosbag restart, clear buffer if (!twist_ptr_queue_.empty()) { @@ -495,9 +533,7 @@ void PointCloudDataSynchronizerComponent::twist_callback( auto twist_ptr = std::make_shared(); twist_ptr->header.stamp = input->header.stamp; - twist_ptr->twist.linear.x = input->longitudinal_velocity; - twist_ptr->twist.linear.y = input->lateral_velocity; - twist_ptr->twist.angular.z = input->heading_rate; + twist_ptr->twist = input->twist.twist; twist_ptr_queue_.push_back(twist_ptr); } From 17984b455b9f327a0942b0084c89c1bceab7a959 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 28 Sep 2023 13:24:02 +0900 Subject: [PATCH 23/97] feat(intersection): use planned velocity from upstream modules (#5156) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 2 ++ .../package.xml | 1 + .../src/manager.cpp | 4 ++++ .../src/scene_intersection.cpp | 15 +++++++++----- .../src/scene_intersection.hpp | 5 ++++- .../src/util.cpp | 20 ++++++++++++------- .../src/util.hpp | 5 +++-- 7 files changed, 37 insertions(+), 15 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index a932254140878..4042adeb3623e 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -34,6 +34,8 @@ collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr + use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module + minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 879b26209a05e..9443295981679 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -9,6 +9,7 @@ Takayuki Murooka Tomoya Kimura Shumpei Wakabayashi + Kyoichi Sugahara Apache License 2.0 diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 28cc7a1ad2881..e02a1e4220030 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -103,6 +103,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node, ns + ".collision_detection.not_prioritized.collision_end_margin_time"); ip.collision_detection.keep_detection_vel_thr = getOrDeclareParameter(node, ns + ".collision_detection.keep_detection_vel_thr"); + ip.collision_detection.use_upstream_velocity = + getOrDeclareParameter(node, ns + ".collision_detection.use_upstream_velocity"); + ip.collision_detection.minimum_upstream_velocity = + getOrDeclareParameter(node, ns + ".collision_detection.minimum_upstream_velocity"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index fec7589f3c18f..ca1cbfdd0ccc7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -911,7 +911,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); const bool has_collision = checkCollision( - *path, target_objects, path_lanelets, closest_idx, time_delay, traffic_prioritized_level); + *path, target_objects, path_lanelets, closest_idx, + std::min(occlusion_stop_line_idx, path->points.size() - 1), time_delay, + traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -1097,7 +1099,8 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & objects, - const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, + const util::PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stop_line_candidate_idx, const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level) { using lanelet::utils::getArcCoordinates; @@ -1106,9 +1109,11 @@ bool IntersectionModule::checkCollision( // check collision between target_objects predicted path and ego lane // cut the predicted path at passing_time const auto time_distance_array = util::calcIntersectionPassingTime( - path, planner_data_, associative_ids_, closest_idx, time_delay, - planner_param_.common.intersection_velocity, - planner_param_.collision_detection.minimum_ego_predicted_velocity); + path, planner_data_, associative_ids_, closest_idx, last_intersection_stop_line_candidate_idx, + time_delay, planner_param_.common.intersection_velocity, + planner_param_.collision_detection.minimum_ego_predicted_velocity, + planner_param_.collision_detection.use_upstream_velocity, + planner_param_.collision_detection.minimum_upstream_velocity); const double passing_time = time_distance_array.back().first; auto target_objects = objects; util::cutPredictPathWithDuration(&target_objects, clock_, passing_time); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 89444bb71c947..550b23aa17606 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -95,6 +95,8 @@ class IntersectionModule : public SceneModuleInterface double collision_end_margin_time; //! end margin time to check collision } not_prioritized; double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr + bool use_upstream_velocity; + double minimum_upstream_velocity; } collision_detection; struct Occlusion { @@ -260,7 +262,8 @@ class IntersectionModule : public SceneModuleInterface bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, - const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, + const util::PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stop_line_candidate_idx, const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level); bool isOcclusionCleared( diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index a3bebecbd2bca..fff1d08223e5f 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1108,8 +1108,9 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::shared_ptr & planner_data, const std::set & associative_ids, - const int closest_idx, const double time_delay, const double intersection_velocity, - const double minimum_ego_velocity) + const size_t closest_idx, const size_t last_intersection_stop_line_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) { double dist_sum = 0.0; int assigned_lane_found = false; @@ -1119,7 +1120,9 @@ TimeDistanceArray calcIntersectionPassingTime( PathWithLaneId reference_path; for (size_t i = closest_idx; i < path.points.size(); ++i) { auto reference_point = path.points.at(i); - reference_point.point.longitudinal_velocity_mps = intersection_velocity; + 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) { @@ -1150,10 +1153,13 @@ TimeDistanceArray calcIntersectionPassingTime( // use average velocity between p1 and p2 const double average_velocity = (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; - passing_time += - (dist / std::max( - minimum_ego_velocity, - average_velocity)); // to avoid zero-division + const double minimum_ego_velocity_division = + (use_upstream_velocity && i > last_intersection_stop_line_candidate_idx) + ? minimum_upstream_velocity /* to avoid null division */ + : minimum_ego_velocity; + const double passing_velocity = + std::max(minimum_ego_velocity_division, average_velocity); + passing_time += (dist / passing_velocity); time_distance_array.emplace_back(passing_time, dist_sum); } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 72e96876f02c9..f02362e3f803d 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -147,8 +147,9 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::shared_ptr & planner_data, const std::set & associative_ids, - const int closest_idx, const double time_delay, const double intersection_velocity, - const double minimum_ego_velocity); + const size_t closest_idx, const size_t last_intersection_stop_line_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); double calcDistanceUntilIntersectionLanelet( const lanelet::ConstLanelet & assigned_lanelet, From c56423ddcc6420089952ccb24bc171f9c199620c Mon Sep 17 00:00:00 2001 From: TaikiYamada4 <129915538+TaikiYamada4@users.noreply.github.com> Date: Thu, 28 Sep 2023 16:41:41 +0900 Subject: [PATCH 24/97] refactor(ndt_scan_matcher): delete default parameters from ndt_scan_matcher_core.cpp (#5155) * Removed default paramters in the constructor in ndt_scan_matcher_core.cpp Extracted paramter map_frame to ndt_scan_matcher.param.yaml Signed-off-by: TaikiYamada4 * Updated README.md to match ndt_scan_matcher.param.yaml Fixed minor grammar mistakes. Signed-off-by: TaikiYamada4 * style(pre-commit): autofix * style(pre-commit): autofix * Fixed minor grammar mistakes Signed-off-by: TaikiYamada4 * Removed unnecessary value initialization for parameters that should be defined in ndt_scan_matcher.param.yaml Signed-off-by: TaikiYamada4 --------- Signed-off-by: TaikiYamada4 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ndt_scan_matcher/README.md | 34 +++++++++++-------- .../config/ndt_scan_matcher.param.yaml | 11 +++--- .../src/ndt_scan_matcher_core.cpp | 32 +++++++---------- 3 files changed, 40 insertions(+), 37 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index c5a14d94ebed3..a1b9339b9780a 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -54,19 +54,25 @@ One optional function is regularization. Please see the regularization chapter i ### Core Parameters -| Name | Type | Description | -| --------------------------------------- | ------ | ----------------------------------------------------------------------------------------------- | -| `base_frame` | string | Vehicle reference frame | -| `ndt_base_frame` | string | NDT reference frame | -| `input_sensor_points_queue_size` | int | Subscriber queue size | -| `trans_epsilon` | double | The maximum difference between two consecutive transformations in order to consider convergence | -| `step_size` | double | The newton line search maximum step length | -| `resolution` | double | The ND voxel grid resolution [m] | -| `max_iterations` | int | The number of iterations required to calculate alignment | -| `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | -| `converged_param_transform_probability` | double | Threshold for deciding whether to trust the estimation result | -| `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud | -| `num_threads` | int | Number of threads used for parallel computing | +| Name | Type | Description | +| --------------------------------------------------------- | ---------------------- | -------------------------------------------------------------------------------------------------- | +| `base_frame` | string | Vehicle reference frame | +| `ndt_base_frame` | string | NDT reference frame | +| `map_frame` | string | map frame | +| `input_sensor_points_queue_size` | int | Subscriber queue size | +| `trans_epsilon` | double | The max difference between two consecutive transformations to consider convergence | +| `step_size` | double | The newton line search maximum step length | +| `resolution` | double | The ND voxel grid resolution [m] | +| `max_iterations` | int | The number of iterations required to calculate alignment | +| `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | +| `converged_param_transform_probability` | double | TP threshold for deciding whether to trust the estimation result (when converged_param_type = 0) | +| `converged_param_nearest_voxel_transformation_likelihood` | double | NVTL threshold for deciding whether to trust the estimation result (when converged_param_type = 1) | +| `initial_estimate_particles_num` | int | The number of particles to estimate initial pose | +| `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud | +| `initial_pose_timeout_sec` | int | Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] | +| `initial_pose_distance_tolerance_m` | double | Tolerance of distance difference between two initial poses used for linear interpolation. [m] | +| `num_threads` | int | Number of threads used for parallel computing | +| `output_pose_covariance` | std::array | The covariance of output pose | (TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) @@ -238,7 +244,7 @@ Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample ### Abstract -This is a function that using de-grounded LiDAR scan estimate scan matching score.This score can more accurately reflect the current localization performance. +This is a function that uses de-grounded LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. [related issue](https://github.com/autowarefoundation/autoware.universe/issues/2044). ### Parameters diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index ecd6c65fa11df..718bc6ca3b3a3 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -9,6 +9,9 @@ # NDT reference frame ndt_base_frame: "ndt_base_link" + # map frame + map_frame: "map" + # Subscriber queue size input_sensor_points_queue_size: 1 @@ -53,7 +56,7 @@ num_threads: 4 # The covariance of output pose - # Do note that this covariance matrix is empirically derived + # Note that this covariance matrix is empirically derived output_pose_covariance: [ 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -67,7 +70,7 @@ # Regularization switch regularization_enabled: false - # regularization scale factor + # Regularization scale factor regularization_scale_factor: 0.01 # Dynamic map loading distance @@ -86,5 +89,5 @@ # If lidar_point.z - base_link.z <= this threshold , the point will be removed z_margin_for_ground_removal: 0.8 - # The execution time which means probably NDT cannot matches scans properly - critical_upper_bound_exe_time_ms: 100 # [ms] + # The execution time which means probably NDT cannot matches scans properly. [ms] + critical_upper_bound_exe_time_ms: 100 diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 44c763721e159..d9054d77d2256 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -84,24 +84,9 @@ NDTScanMatcher::NDTScanMatcher() tf2_broadcaster_(*this), ndt_ptr_(new NormalDistributionsTransform), state_ptr_(new std::map), - base_frame_("base_link"), - ndt_base_frame_("ndt_base_link"), - map_frame_("map"), - converged_param_type_(ConvergedParamType::TRANSFORM_PROBABILITY), - converged_param_transform_probability_(4.5), - converged_param_nearest_voxel_transformation_likelihood_(2.3), - initial_estimate_particles_num_(100), - lidar_topic_timeout_sec_(1.0), - initial_pose_timeout_sec_(1.0), - initial_pose_distance_tolerance_m_(10.0), - inversion_vector_threshold_(-0.9), - oscillation_threshold_(10), - output_pose_covariance_(), - regularization_enabled_(declare_parameter("regularization_enabled")), - estimate_scores_for_degrounded_scan_( - declare_parameter("estimate_scores_for_degrounded_scan", false)), - z_margin_for_ground_removal_(declare_parameter("z_margin_for_ground_removal", 0.8)), - critical_upper_bound_exe_time_ms_(100) + inversion_vector_threshold_(-0.9), // Not necessary to extract to ndt_scan_matcher.param.yaml + oscillation_threshold_(10), // Not necessary to extract to ndt_scan_matcher.param.yaml + regularization_enabled_(declare_parameter("regularization_enabled")) { (*state_ptr_)["state"] = "Initializing"; is_activated_ = false; @@ -116,6 +101,9 @@ NDTScanMatcher::NDTScanMatcher() ndt_base_frame_ = this->declare_parameter("ndt_base_frame"); RCLCPP_INFO(get_logger(), "ndt_base_frame_id: %s", ndt_base_frame_.c_str()); + map_frame_ = this->declare_parameter("map_frame"); + RCLCPP_INFO(get_logger(), "map_frame_id: %s", map_frame_.c_str()); + pclomp::NdtParams ndt_params{}; ndt_params.trans_epsilon = this->declare_parameter("trans_epsilon"); ndt_params.step_size = this->declare_parameter("step_size"); @@ -141,8 +129,9 @@ NDTScanMatcher::NDTScanMatcher() this->declare_parameter("converged_param_nearest_voxel_transformation_likelihood"); lidar_topic_timeout_sec_ = this->declare_parameter("lidar_topic_timeout_sec"); + critical_upper_bound_exe_time_ms_ = - this->declare_parameter("critical_upper_bound_exe_time_ms", critical_upper_bound_exe_time_ms_); + this->declare_parameter("critical_upper_bound_exe_time_ms"); initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); @@ -157,6 +146,11 @@ NDTScanMatcher::NDTScanMatcher() initial_estimate_particles_num_ = this->declare_parameter("initial_estimate_particles_num"); + estimate_scores_for_degrounded_scan_ = + this->declare_parameter("estimate_scores_for_degrounded_scan"); + + z_margin_for_ground_removal_ = this->declare_parameter("z_margin_for_ground_removal"); + rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group; initial_pose_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); From 7e9b4dedd71a88472233e9f6f20d16bd7e54baf2 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Thu, 28 Sep 2023 16:51:01 +0900 Subject: [PATCH 25/97] feat(pose_twist_estimator): automatically initialize pose only with gnss (#5115) --- .../pose_twist_estimator.launch.xml | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index abbb15c797d19..7733b4e3117a1 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -20,7 +20,9 @@ - + + + @@ -42,7 +44,9 @@ - + + + @@ -74,7 +78,9 @@ - + + + @@ -99,7 +105,9 @@ - + + + @@ -131,7 +139,9 @@ - + + + From 3eda01859adbbc4c72f859880a76374b2d023273 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Thu, 28 Sep 2023 08:31:21 +0000 Subject: [PATCH 26/97] chore: update CODEOWNERS (#5087) Signed-off-by: GitHub Co-authored-by: kminoda --- .github/CODEOWNERS | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 82f7197d87aed..f9983f0fccc3f 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -41,6 +41,7 @@ common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp common/tier4_debug_tools/** ryohsuke.mitsudome@tier4.jp common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp takamasa.horibe@tier4.jp +common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp yutaka.shimizu@tier4.jp common/tier4_screen_capture_rviz_plugin/** taiki.tanaka@tier4.jp @@ -84,9 +85,10 @@ launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp launch/tier4_system_launch/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp -localization/ar_tag_based_localizer/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp localization/ekf_localizer/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/ar_tag_based_localizer/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp +localization/landmark_based_localizer/landmark_tf_caster/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp localization/localization_error_monitor/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose2twist/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp @@ -150,7 +152,7 @@ planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4 planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_crosswalk_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_intersection_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @@ -160,6 +162,7 @@ planning/behavior_velocity_planner_common/** isamu.takagi@tier4.jp shumpei.wakab planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +planning/behavior_velocity_template_module/** daniel.sanchez@tier4.jp planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -215,6 +218,8 @@ system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp system/emergency_handler/** makoto.kurihara@tier4.jp system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp +system/system_diagnostic_graph/** isamu.takagi@tier4.jp +system/system_diagnostic_monitor/** isamu.takagi@tier4.jp system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp @@ -224,4 +229,4 @@ vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** taiki.tanaka@ti vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp -vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp yamato.ando@tier4.jp +vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp From a3e16df9acaa86c05792dd7d03d593758caffa81 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 28 Sep 2023 18:32:37 +0900 Subject: [PATCH 27/97] refactor(behavior_path_planner): remove last_approved_pose_ from start_planner (#5157) remove last_approved_pose_ from start_planner Signed-off-by: kyoichi-sugahara --- .../scene_module/start_planner/start_planner_module.hpp | 1 - .../src/scene_module/start_planner/start_planner_module.cpp | 4 +--- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index d4013fd97b316..d22322b63b080 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -139,7 +139,6 @@ class StartPlannerModule : public SceneModuleInterface std::unique_ptr last_route_received_time_; std::unique_ptr last_pull_out_start_update_time_; - std::unique_ptr last_approved_pose_; // generate freespace pull out paths in a separate thread std::unique_ptr freespace_planner_; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index c2df11b4bede6..a9d8278523f45 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -215,8 +215,6 @@ BehaviorModuleOutput StartPlannerModule::plan() clearWaitingApproval(); resetPathCandidate(); resetPathReference(); - // save current_pose when approved for start_point of turn_signal for backward driving - last_approved_pose_ = std::make_unique(planner_data_->self_odometry->pose.pose); } BehaviorModuleOutput output; @@ -886,7 +884,7 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const // turn on hazard light when backward driving if (!status_.back_finished) { turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; - const auto back_start_pose = isWaitingApproval() ? current_pose : *last_approved_pose_; + const auto back_start_pose = planner_data_->route_handler->getOriginalStartPose(); turn_signal.desired_start_point = back_start_pose; turn_signal.required_start_point = back_start_pose; // pull_out start_pose is same to backward driving end_pose From fe9ad68131936f96f231dbd3b3fb72746b10c51b Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 28 Sep 2023 18:41:19 +0900 Subject: [PATCH 28/97] refactor(localization_error_monitor): refactor diag (#4964) * refactor(localization_error_monitor): refactor diag Signed-off-by: yamato-ando * refactor Signed-off-by: yamato-ando * create diagnostics file Signed-off-by: yamato-ando * delete unnecessary lines Signed-off-by: yamato-ando * add test Signed-off-by: yamato-ando * add image file Signed-off-by: yamato-ando * add newline Signed-off-by: yamato-ando * fix message Signed-off-by: yamato-ando * remove unnecessary code Signed-off-by: yamato-ando * modify include file Signed-off-by: yamato-ando * style(pre-commit): autofix * Update localization/localization_error_monitor/src/diagnostics.cpp Co-authored-by: Kento Yabuuchi * add comment Signed-off-by: yamato-ando * rename diag componet Signed-off-by: yamato-ando * update diag aggregator param Signed-off-by: yamato-ando * update default param Signed-off-by: yamato-ando * refactor Signed-off-by: yamato-ando * add OK message Signed-off-by: yamato-ando * style(pre-commit): autofix * fix conflict Signed-off-by: yamato-ando --------- Signed-off-by: yamato-ando Co-authored-by: yamato-ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kento Yabuuchi --- .../localization_error_monitor/CMakeLists.txt | 21 +++ .../localization_error_monitor/README.md | 18 ++- .../localization_error_monitor.param.yaml | 2 +- .../diagnostics.hpp | 31 ++++ .../localization_error_monitor/node.hpp | 7 +- .../media/diagnostics.png | Bin 0 -> 22762 bytes .../localization_error_monitor/package.xml | 1 + .../src/diagnostics.cpp | 94 ++++++++++++ .../localization_error_monitor/src/node.cpp | 73 +++------ .../test/test_diagnostics.cpp | 140 ++++++++++++++++++ .../localization.param.yaml | 2 +- 11 files changed, 323 insertions(+), 66 deletions(-) create mode 100644 localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp create mode 100644 localization/localization_error_monitor/media/diagnostics.png create mode 100644 localization/localization_error_monitor/src/diagnostics.cpp create mode 100644 localization/localization_error_monitor/test/test_diagnostics.cpp diff --git a/localization/localization_error_monitor/CMakeLists.txt b/localization/localization_error_monitor/CMakeLists.txt index 258e942169556..3528367486350 100644 --- a/localization/localization_error_monitor/CMakeLists.txt +++ b/localization/localization_error_monitor/CMakeLists.txt @@ -4,10 +4,31 @@ project(localization_error_monitor) find_package(autoware_cmake REQUIRED) autoware_package() +ament_auto_add_library(localization_error_monitor_module SHARED + src/diagnostics.cpp +) + ament_auto_add_executable(localization_error_monitor src/main.cpp src/node.cpp ) +target_link_libraries(localization_error_monitor localization_error_monitor_module) + +if(BUILD_TESTING) + function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gtest(${test_name} ${filepath}) + target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + target_link_libraries(${test_name} localization_error_monitor_module) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) + endfunction() + + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + add_testcase(test/test_diagnostics.cpp) +endif() ament_auto_package(INSTALL_TO_SHARE config diff --git a/localization/localization_error_monitor/README.md b/localization/localization_error_monitor/README.md index bf005d6a00bbb..2ddf41e4eac70 100644 --- a/localization/localization_error_monitor/README.md +++ b/localization/localization_error_monitor/README.md @@ -2,6 +2,10 @@ ## Purpose +

+ +

+ localization_error_monitor is a package for diagnosing localization errors by monitoring uncertainty of the localization results. The package monitors the following two values: @@ -25,10 +29,10 @@ The package monitors the following two values: ## Parameters -| Name | Type | Description | -| -------------------------------------- | ------ | ------------------------------------------------------------------------------------------- | -| `scale` | double | scale factor for monitored values (default: 3.0) | -| `error_ellipse_size` | double | error threshold for long radius of confidence ellipse [m] (default: 1.0) | -| `warn_ellipse_size` | double | warning threshold for long radius of confidence ellipse [m] (default: 0.8) | -| `error_ellipse_size_lateral_direction` | double | error threshold for size of confidence ellipse along lateral direction [m] (default: 0.3) | -| `warn_ellipse_size_lateral_direction` | double | warning threshold for size of confidence ellipse along lateral direction [m] (default: 0.2) | +| Name | Type | Description | +| -------------------------------------- | ------ | -------------------------------------------------------------------------------------------- | +| `scale` | double | scale factor for monitored values (default: 3.0) | +| `error_ellipse_size` | double | error threshold for long radius of confidence ellipse [m] (default: 1.0) | +| `warn_ellipse_size` | double | warning threshold for long radius of confidence ellipse [m] (default: 0.8) | +| `error_ellipse_size_lateral_direction` | double | error threshold for size of confidence ellipse along lateral direction [m] (default: 0.3) | +| `warn_ellipse_size_lateral_direction` | double | warning threshold for size of confidence ellipse along lateral direction [m] (default: 0.25) | diff --git a/localization/localization_error_monitor/config/localization_error_monitor.param.yaml b/localization/localization_error_monitor/config/localization_error_monitor.param.yaml index 026daf0532d33..2aa28014ead41 100644 --- a/localization/localization_error_monitor/config/localization_error_monitor.param.yaml +++ b/localization/localization_error_monitor/config/localization_error_monitor.param.yaml @@ -4,4 +4,4 @@ error_ellipse_size: 1.0 warn_ellipse_size: 0.8 error_ellipse_size_lateral_direction: 0.3 - warn_ellipse_size_lateral_direction: 0.2 + warn_ellipse_size_lateral_direction: 0.25 diff --git a/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp b/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp new file mode 100644 index 0000000000000..4fe4e5a5aac14 --- /dev/null +++ b/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp @@ -0,0 +1,31 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LOCALIZATION_ERROR_MONITOR__DIAGNOSTICS_HPP_ +#define LOCALIZATION_ERROR_MONITOR__DIAGNOSTICS_HPP_ + +#include + +#include +#include + +diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( + const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size); +diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection( + const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size); + +diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( + const std::vector & stat_array); + +#endif // LOCALIZATION_ERROR_MONITOR__DIAGNOSTICS_HPP_ diff --git a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp b/localization/localization_error_monitor/include/localization_error_monitor/node.hpp index 83b2ddef9215a..7bdc87d5d47f2 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/node.hpp @@ -16,7 +16,6 @@ #define LOCALIZATION_ERROR_MONITOR__NODE_HPP_ #include -#include #include #include @@ -37,6 +36,7 @@ class LocalizationErrorMonitor : public rclcpp::Node private: rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Publisher::SharedPtr ellipse_marker_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; rclcpp::TimerBase::SharedPtr timer_; double scale_; @@ -45,16 +45,11 @@ class LocalizationErrorMonitor : public rclcpp::Node double error_ellipse_size_lateral_direction_; double warn_ellipse_size_lateral_direction_; Ellipse ellipse_; - diagnostic_updater::Updater updater_; - void checkLocalizationAccuracy(diagnostic_updater::DiagnosticStatusWrapper & stat); - void checkLocalizationAccuracyLateralDirection( - diagnostic_updater::DiagnosticStatusWrapper & stat); void onOdom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg); visualization_msgs::msg::Marker createEllipseMarker( const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom); double measureSizeEllipseAlongBodyFrame(const Eigen::Matrix2d & Pinv, double theta); - void onTimer(); public: LocalizationErrorMonitor(); diff --git a/localization/localization_error_monitor/media/diagnostics.png b/localization/localization_error_monitor/media/diagnostics.png new file mode 100644 index 0000000000000000000000000000000000000000..74aacb68a2b7cefcd199bc3ae8ee120e6b545e4d GIT binary patch literal 22762 zcmd4(W0WRM)GdlWWmk1omu=g&ZL`a^ZQHiZF59+kyURHB?!C_$-=A~u{dsfb$c)Hb zk&ziAVvUG7XRdI0Sur?hOlSZA04E_XtOx)AMf{|zAi;l*#vd24KPON}K?!BZpUn%> zDC{SX;UuE&q-1O2Yj_Dr34BmU+M(=yS@S!=J)sC>3i&=wN@VgR;9%5J_922+ zu0EXrGJnWHSJ#}ISQ-4FIBL!H?HvwJcq5nN@ySis=j{&HqSjV+!C#;PzjB1`Tuu3Z zQf`04`3WEc@DC+Hp#^?{5~vZ%|KuRsko^C%8%?Hs$Aib^esMh%`p;O5=QlYZ&t|LL zmJ9F4s-H{Ji3jR`nnNQ0bJeUdhs{apG1rfhi{| zSZ*D5AOqri-|wwfUwhIyuOuY!r^TC?)~`ipoxxplM4=F-ha=;R#$3}(<0yoYUfm>2IjKD+0eF` zcj%(2&cVJXDsKFMGgriVcN+L@cdnJ)%-cWygh!ODGN-}AN6`IKsxZyVxZ4EDa}7JS ztFC%p0Up%m6zlD+y?j+9J{%7faDy9#&QC&i`S@;YlC^F{PL~)P$kn33L{H_OLSUg@ z0_4btAf885q<07e=Z}2i4Z*JR^Z_4PdC7o{j z9m0!ez6p%KbdGNXQlq{zzLqCDaZ|ht^?_O8&Vp_bh62|E%w9kB*u7qHaR)aC*S03; zozH(UJFxoxk?guh>$XsW|BCT2v*wmBn*P4M@Q1+Y3l4+B3pY`TDD>;Vak5}}oCZ^9 zo=}g%TgE*4z|LD(?`L}ShSl@|JBxjOg_hZd3C_*@v4gA1`cx=c!=L23`=yb2z`d}| z2t8F9L@+x72d|20R<$KxW`Z!uo^iWquSi?n8-`jOzoQG0U5zGq@jP@nVqUzr8^l_s zK#Bgkg=0{yAasN6_q_fcrnA<;Care6;E5@o0Ge#8cgYMmgXk8PFqLCYb6G2Z2@5Z! zuibfv{8tw;y$%;_`P@Hg^(wM&PL00$YpBaAe_>;Qsffad!w?uf*7n8soNhr)$7FKF!6{V(qJk3o896cDr**T8*tUR@%7jVe}puZd_qx83%c`-?^Ri=vC z!VIoriR->ldZC@By!I+AX*tRJdV=G;%P8_MjUyL06_NzW%5`Sx%+^xYj6=0?*dZy zB51sk8lF-KoGa7*#eL}{Z+R70yQi$pIQ(a0HKh!B9}c1m91{yc8QmLQ6lOa7>_gj2 zv6L9Pbxp8xxv#H>=PPRLU_f1zL+IW(X<_&`iDq1#)1ImMGS-GT_EJBPuX`#xJpUzf_X5)p;YyJ{mag#y}LO`ZcQ(`p+(J zpon>wa@$VOkApJtw%%-wUmJTqsSl49<{z3h2u**RPku`B(UlVz}`3HD*~fj)3olkgdY;onpF_tE95)o zg{Hz(CnEuYWn<19TmK##6tSuSAuvBp;!Er6>!a58hDE>_UHl|c@1G02k?W8)eDa8S zi*|1IXwgkk7$~vKTAEF^s9nc?8{xEBqmR{Ny)7QD2PeyBGwM9y^tC+{Z5CsB%56#5 z(_*zdt1{-kk0XxmY`Bv9YIa6IxZLP>2cepi!083@oN`WSZ@{r`W_i7^PA5)p$%oNt zN@m}y2rr7SUzxdGELY-oK1W8Fy6ESRW|4|r9SJ2iZ=dR#P;+>={@m>Gh7wzPE*b9v z`If{VQ?9u>6qY%gX?rbl|7Kb_Fe~y84FOludU3>MjWfcO^LfU-asskqH6dWL(HPhP zSI*B|Ppn4WEC?r-QDgt&p}Df~8cWXnYk%<;jJ>d!i&+`*an~7N6mLqloR{gS5yN5O z_O$LD&Y4{J{l}CK4oAxMV6Zr|)p!AVTB9}dS=v3BE0!}E@BVmg0mIoNs}ejVubDNC zXIV^%l!A|ad})H!;p7Ws^WO?+3)JF#%a`;6%RkNwW*=Ovi$&_sv)s*AXDT+@?B1t* zRb@2Heh~ASwzft_-9x(OEHkaD-(=uiI%4w(0d6Yw&E1FeMI(@zixM`c;`?(q}%;B0Dob2+h0n+janvz`=XF z`rzgK;8%v@2ZMiJOsUW+gnD)vKyus8#1&({vf-+ls=lN|2c9)A z7Dr)rQcIV+34D7JO`}by{vJ7JcsVgotgYAYuNq2E-;I)rerYFbrqXKXigqt^M(2(U zbK+!9`o6r7hg)*at0+b3iEy8Qn#Q+WD7~5I`qb{-o={g!b#6C8)J_uec+yHt)I6Ll z8P?=bm~y&4Ka`TXK_ZwOQyGJFw41AcT>NJ4cG8`1yJ_UF==vV1HlImZz$WKj^?q1T zc!^GahpVf)kVwGq`)I?Pd0!^l>FWzzF$h8upyABVXat@`6CH`A6o4bVyY~LDSVCx_GD~#4svw+Uw8`aw9zV`lra>~{iHk=F?n&wswQc~m zdv*v&1T|S^+5dp{>7f4o0P$ z2VTemT6cGrPN%fpPya>o88Q3h9~BWLA+YQG%x+G4+&t6ylNJLw-Gj!EKw@zd` z#^K&4#G8tw``U>=B)sQi!k}Vv>jbv{=YXKcNmrR1T^kUwYPR{8z1FsIvv#_m&&v2K z;8snlHO>K{<8Q90$3LXm!+Rf-s~baHwKh>08UTOD7%2Zde_=e}SHnkVq-ua_C4*WG zHQ&-{m;8DHxTdwJCMibHSQZ5Nu690A`^xP&V_2b|_v;4-{JA(L&7=maKnj$2da2H^ zW2Am~=RJa711!Hs!eJs!D_aj1CIUyG=8p8>y(Und%yu*b&HG-%5JsH6slDE#W`Wpq zbK>ly{lF_E76hy43|u}ss{uhq5c`0e2P3D`W>WoDKL)SY_DM|VlbS#^u+b~YbrJVP z62=LAsrI3YjnsVXNQwcTp`@46WcD@~7^5jA2E2iCXwJ z<{jKhxh9;-F;AKr5^-KlP&`*@e!BNEF>>9*@_ulrElumR9$K_2NSd@8_lmHhP<0C9 zbz$mEq+~UC+QTR99!K*x>cnRS^_wU`+p<`sMN6FiU^(7K{=#51yE7Fk6x6f*#ib2( zCRWVFqu^RYw|sw)6qSkL2?&NbM{FC+Q!fB&nPehAY{;=HXCMw91gJ=_Z(K#pzz=J9GBhX?azpa3I`(vy9i9KeBTDw_$x zY0lg`KW2XnDEAW^OSomg>1)|S$hDH@^s|Og+|}*`^-6|zL#6^pSTL~KN{+iC7uDv* z^e^&LMgK52RHoJi6_E<`$lt@P&Oa&N*PW-; zd16TWzZ+zw-BQQ1*(Ta^khf3i@5Gqz6@e&D>dSn+kta@IAt2NasT9Gg+u;ZCN{N%M zn1G|VNYJNc1hTzo?d<7dfV7bKy%`=c=V57U-J&sLe1jRUd2jEiBepXS1d{pIpDjSi zBtA3dm-8HU^2^Ss``p~&d8rR!$q-!GHP?C@AMGy1C*C0e0B7^XiepHy#JXZR$WCwX z-TgbD6y8biftmOojWZnFxYc7 z98N%nWEVr71Jbe2Cg^8r^bbm5lRjVLC`nE^qWgsocTiTBf-Cp-}uU&q15CC_L56 z;khtK;jE~^!jVw8g`<;fzlLoreBej*!G`+>tp!|R{=`q@FU^15+PdS75i6+@qkY=zAS{4n6E z|1h7f?mc`fo6IGb0&8L_+0&KS5AP0V_OK6P=|M1XH39QMQh4b#lPjPpN1aGO51EyV7rdVNqbU9Mj zu4m_1CC$r}CX-Pkt4{(4>FoYxM?uugmC4bdf!<}cq+DTrs?bDqB9`Hh{ZE+mZuL-= z`uw=}*;F*$w4WIJq)NA0eN;s*dOO{Jsonm(Z?zP>=BSnEqgo9I^{y^ow79U)76X3s zB~H3$SVah{A*~c{37UjuS@w}qJp$)Mmkj|Ji`HIuh4Imae8zSLBNvxlYGcsmI$~0x zoQzJ#kyese=DEee>SBJwiJO$I(KQc^5mUYMRr zB~a-(&Y67uRa+_Rh(77F?RW7oYA?oKsHPL*v8Gt4xG>CS1eY!qVfWYcxD2QK;HS}U9JNg@P?jQ00M8r@bA`XwN4JzaC%Yo-?)$SLt z-0>zh-1#XaW|Lk~Pqy<}B)7p2wzgiH^^}s}?1H0ZA-G}%%Rc(-sM!&7i*wa}K3FAh z>2GhYhN>g$mS%WOpGWITcF^xJ4s7jTpnJ9^26=JzZ1<$|P`yrIjiOV-|EPN)S#mNW z>L&^bKU~bcG zYDI56uIfp5$gA|eC?d4Fma1R^@YM(V`3?HBv0NEbvha~yrF)BbD2`>K1~Tw`hQgg3 z9mz+w`m;xv5Mf(Uc;WH7&;VpzsI1q0WnXtjm!6$Yg;1`4qeLrh za~SJJ5PXhH!dhd4<9XsVsm9#GQb*Rn)Chs&i_r@Ig%-GNS`n&V9$?Tz$Ani8bJXBV z&Mc_&yeBLBeGbM|jfrVV_JT|q>EZn~*gVP7i<{G%^#v!txB#x14RW7q5wk}{OTg69X<4zKQI$5aA)9)ud5nP(jvpVngv;ZtUZ*5l2HUTK&MhccW=HCO1u zRrQD&4p#}DpA>Cu`U7L`RX18e#Uj_d^rj#oJKf(G=_Z`c8o}R;7oFXMsxC_@d%Vpm zK?&-l?C8)ZB)#ej$$?uC3(hZ8>k}ORRZ|EM?y&gnzAZk^0JFNOKuC;&10va=T`u_O zR54)gYBcW@oLHg$V^wWQFO^Eu$Iph~Q+FB9Ayx0VHrVH%j6_axb*5H8E%mmh03?sW9`?Q;8w}m9MCYF{o||hG zRhl(&|0^V{R$aQ-_#XwEgBPvf`Lx_wZG%Z6hel3izU43n8ES)297@-A?qJv;whQtP zSR1O2N`BaprK%;xKgBrU^M}k9j6_BvhWB4s$2(e);(X(HYwnAGwlF!2U2l%lyqo{e z(2`GhtYW`rR+1Q>i(FgQ5hxR6zYOyqRw`5%wAMW_KMao=`HK#B%n~>6S|{g2LZHz& zy?Al#tZoDX7mFs&IB%Ae{zaa_R;#o3HHT83%r8?6)oDQ}^S7iiCRvy~Yt2oS7>*ck z+V-ch`Quwles3Y?G3trcM8r#!(C=R;Dhn0JG=gDVEOYDK&o4JAq8ysO=RVjKgu<*U zm;Skp6Jj<3hc2e#*;-mm{mKw?^!XG09|}FxP(#K6kML(|f|={BMKG6EB#$wx_c8vH z!RA_JwwU701v;>?tN&x$?Pt;$0zw9huT*DiM(&gV8mxu&fk6UNBCZ|HEP$bo7cEqc zj{$#h!Prf{Z3HN}iDjrwE2s1qh5O7CMPhxX1nqIKruXWTAU67#C3}6ucB(xE)8b+~ zd_dq2rh6iY2K>e&2=}|2L=-MUN~nQeZyC=WKoE^;b-ShiW8CZH2%phTi7(+1yx&cE3erCLxa6ThT9_5RY<+3S4J!=4B_Wxz+q^{on^m_x(NLV@EbZ#3}$z%yNm7 zji}ngRv~xlKQ&>pkE#s$R)v7cdGX7D3`knQ%L^0Vlp9;PLGX^%dyXD|!j_l>E&^2% zczyoG;Q|*__611EM=vzj}7Om|DNc= z-pz}5@}&svi`~f2waWl~uA96mlr;Zj20|ZrDsUtg&n#&{$a_+n)R9>JZLyyWXS|PL z;yo!Rc{f80T?GPF5TJT+T2DTD9_oR;~lc-py}g&>Bzl>(J*6s2t=nh-P*@Zlla&2D7UWu`yOQp zWANPH8&m3q%)!P3N(Cx^$#0*{Z1ib4Zj>OS8HAL~5o$%XmcnY#iUHC4qIhPV?{pRC z`WYx~#Ih&*;&!F|Y11wAu&s&k9$_gqRi!Seo3?^PzW;JG{6*& zM7*hjH+=<`jiWG!hD2PM1GGVKs=Z2OGjN>RKfnQY#pz$EriaVt_ekIxg&0SB795FR zTvC&RLuqApili|B;#!K#{BSLtY?LW<7CE( z>I2DS-n1>%tuge|XKBV;%!a004FcAKv# z$~uVSvTbk0ob!|pYXVBuS4C@?rz4gjWzT&241o#@SN&HhubE0IWyPXZMC>`JKrr=8 z>vHnoEgTnpqaIPxs3`<*XDsL%t`CXeUioC!dkQFV(4|Ddx^EFgCf`nkJBUB*; z)OrR;E2uB=^@mYfsQ7j(1w6=3H<9wbN@jklc6|%AyUR1v3tc$=(EnMTE=E1Gnl1TR73)-}6A(nXFEJ zNb}Ai(GwXqWodsK^YVDBXSAm&r*@}ALPIP8_Wk9l@7CEDW~dh=Nb@rbLl0K^-fF=< z!|Lc)p%9o~8kjT#rr&#%bKX7q1J^7F6f6kWl}+^#p(0Z0lUTQR&v!&2LdkvzBbbkm zx7u@_cn(L#a8zjWV&zVp)2>XcCj#1CP<~!Hp zO8;&vRy4TTdj&B@Pw}s=aYaBjW*}OrAq_R?Dxa_zRE(hu%S@w3Qq0AP;pR)2opFW>16TxuDW^r}>(i;neDJ{GLE~ zPAxW2RlI0#a{%l+eT5|crgifV1r`5)sJ1LAlmCZZivrB67GzfgClDLEQ|Rybu*?I} zWaUH?f6=X8EY7aF6_^DQs5@`nF5XQ?QGhiQ3nVa;KW~Ah9ag@SC&1fmu@Y>}-xpu@ z{f(x~6ZbJ)HCvihZzF6qp_}B$29;v?f;4%oirjz2(Ko(|iSq!ddl3}<#x$(AwvtkY zz6%M|1&NM^pp4>$H9SX@{;K1QIC2e176qY7?LS4c@d;B~PgX&n>m>HNbgFZp4I9t) z>8~jxYqsFzL(5&H2nV)lIlkEbUu;=d?UdWST2rMWtWQnLZ23~11Drr$e$Suv+ZH#v zQMhGIs#a7VyaBdNp9+Q<9y-Pv+RN|^66pQ`UCgaA(*IivfDo+-@%~XfXK|^z>G_dj zIsDH2rF=$o6vG_NP5A zxNc z(?gG%=murrw=|`JAq5!}J*L z6l8JYB4RD?7F_^7z8t+{Sr`%+Lm^Uv?OR9l{cNg04xhnn;!Egs9(FfF+| z3@1Ns6!8C{f4!W;EY{-1UU`b;SK-p@?1ZCmAt|`_6FSkDL@vHlB99%U=H6!$LGtPI zC!SHw67c(SGL)m>gW7@k;>X+Zb_5aL|NS zT8Md=D4+zU1bh@=!=&t z4fUH$YgIC+9eE^&#dO$=c`hlV*ORU0Za>6_x&WBO`}cG#S$a$O)+S2O;V8W2q~!>v zqM}*dZobr7@qaWQ7^Jn%7b;02^X=@ceg&XHD*9-xs&b&@=BO7y0z?7=etqPK>-ngD zKJL=Aw0v0mcZzDUOz1g89Dx`KY|vc68_F@a~Z$ zA;gdfy#nqyE)YE7Y;$WSc75bzK^xwBZ^oTnt3EL{#)LzQOBLHQii0n3Y;ZKUQt@Iw z_+ml4{ryJ1%H6`BIi@R0TLjBkWnG^+S&6&8G41*6! z`~z@|M)B2KlFxeSjwSmC2aF{MyT0ukle-6XsFMWIOf1#2w0Eh9*U-L&XN$IxE)9pZ z5L&w#m3{wGR!-M=>EqebWF&4jtx zf&~;_{7ll0+Zja4ibvi1aaHr(@qf-ox9&nBph#Y@eDYq=$kQ&?3qebiO`t9jF00FH zPp=5)c$*nWy1#HRi;qDH6Q{}C)NVfkO1`yFwGz`#0WfQpuVTc}UV)C;b%Q{Hx3`;9 z$X@SFc0CUy3qQEkeq<~B}mP&WFU0dk~U{IYr&aY)TT{36C>S-)-_W(VKwoRv=>mA)y`FCu}!`DM+>~#Lu27 z$+s`~rnA}oUU0&Of*2~tqqLI|hgo)>9B>div)2pgcQ zU${ar-VVz7`9g8-wmHzSH&FipPIIZwfY0;kexqDybO1ieXFzCUof~+z9vK$_-+Md% zKyUU%`q6L-RA7V>fXOO|n2Z=Y;@rPWf|d_!;m;75S;()+KJ4=bF%4*YzE{7WEmn)e ztjE=|$pDN&3lIR&ZGVq+S|r3710y+{3c3YFgJ2b&gvi5Y*8P$Z%rfBS!NB3P@$i2< zs5f36NkiH}b}V%j@Si-V$L=pPxw^|#qLWVJ!64yDYGtW~$oNR8- zltyiOAO>K5Klj~(;aPFWEFi3ZnCKorgRourIeMuKsF@H6eb zFHo!mBvOpjQ=V~*czPb8e#zFl(`$)dCR2qa1F7gc!d|W*T5qt5N}$0c)I$rM{X!vr zbL`%(KGSyS60n2!yG~nhyaEu+jt=>Y{$yg3(G~hd)W~JO?=3Wvcu9-V-?$Ey+fH->Y`_=wwz)yQ zQ2ry=Bt&O#hT|%1+^Q+TVt3&NkGXjssasZ2AY?`+k%H*oD_0^0YJlY!;mQ_YWrXE1 zEjb7*Pn=u77>kLD?wzZIR=(yy!m&sZHH)WDqPk2uB3u}dMBt8+a+~hupr+Q-MM*&e zZHfIXaWCQjRx0->|dca@m>7cPkgPwfES=8jswD z_8=zUCAb(2axLHN&wR`P2?n+yk?a?PLXYtca(HXWTfTn^@fvr!uHHpddmafXe4sPd1LyFQ?@+c!t1nF zr{6A@kR#)3zI#RcIOa9CP7+abE#8%3z>KgqWiC#6sX84Seoqb zjLnKouzl&XIp9qA;>Rm3#TvIf{hgXsmCENAwhQs47EebpN^@X2G3C6b_9^Ah7811- z#Tosn);W|2%wM-(CcL1XY<}I_HQ%%+ zvh)zm$yN`vnKpLxNyynlI&r2EvJJiec*=5D)|O?T*66aRkE_hV>|z3`bUs)Nte@m` z%67VzgSWsI#p9w;!Q}ElyXoh%O*d=*n{23+YU1Mwmn_Lrjm>?kR|Q3u&0K2T-jxAT z@iNC?26!&w5C9P1GRk=dB`2p@^g%D%z z>I-4un#mYiik)5IpU0*-`HS6=d%T!~WiB}rhR&2eD^f>$DV*kirVONojE5UN4JE2G z%M&@;N^K`3rBKl2+8blx>y(K*+DRVsZ2klXEx!w%N;c^LcA&Z>kbl(_6|G$OQ|MR=fgNMm+6ASH)UZJhH z+(B@wMe~K~Q@JL&a!Y=md7H399)*}TSBe%Al^s&eCSA)Rl~QWX&4(ic!`|C$#Anqy zvl0gxpJNAO8ml|F5SnOjX^w_$?;xE21cOM?6H@e3*B8OpoQ_<$7%TpV2k>>Sb|4K} zbgftCqji@;F9)BXWlZ1KW0h7EdzU%Q^vKY`{(3aKf!onfNjHqn*scRU;k6H!S*?zI zyRMnKO9#!Ij$$3H5E~IIM0PX2hV5>1wXgosrzQE-0dUT2p3X?NUccazxCG~u!w2;b z4G`piP5!DUejmn~hL0s1qwD)cy4q1d&d_~Jng|^qN3%ZtyxgOAjh09;(Y0V~#kewP z+~bNhpxB?)WES8Z{gRcTj{L?>8LeowM-Mlax3TcZ@KZfQmp{C~OKJK`Ro2@Qf*E7~ zVAgaLIE&b1U#maJ-Bo9;g53ML1XRFnfsuCR9U>+iA0YR8s=s2$P=Uz?b)5}2W=mpb z?O@Nn>a_-hGRZu24NWn3twxoUXsw)=_xp;jN{E_alZmwWy^LRf_dH}P)m4+J=9 zCQTxVP-yuc6-Ol3pRWwtHyv=-T_=V=!!vDX0;eN{S#+h%^;XtNe{g7s;llD@IC|3N zr(1ozUDW1&5Lo~2XrqE0i^o|m;{9?QKT`QOF0x%P#x_`)KvT=ecxJE_ZUerI-f)M7y<86h+GHSP+VL)(c;@ zPGw3a)y-5-$MpMO%Eh@4pVvK!X{^;WgYA|@i>p(d_efo&MoA&C=g6l{jwgk zZ0lxabE{K~H(m&8&XGe?3l1_y!xi6Q%3$x)GBu^*`eL!YjLWYJgW)l`?PMRr-#u6I z!2&g^%9YipnA(AbE+i-2H1>#@7yRV^t&2F*g)n}|`P&X!ICa|5#3xRcrDa;`RedBm zETuMAX=~I6=aObkk>+M97}dA19oyJ(hZ>ovZLaj(MOr}wEVw03GE_HPz=|->Zb8aZ z(vKMP9}=li(!XdHkS_d}N}sQs#S@z?WXt4jFa zz|R#koEi;Wxg^(r_$fO(dS4&do&gOIfVi6NzKt@kj)~yd3zoRl3-Y#E>@7P{ovofp zGnVUVaYdYB!XCKGir^?I<&K+_tvVv+K<2^>o4h=Fvm?bQYTkq2$DA(TZ4I!oJmO1fX>TeCrByox2l((WSNfP<)n1}lVrdKH2kIM7d&}>3i9osOb9*PURP_()`4c7;CPSlcp4b%QP2I?s$rfjW_ zv5I7Wu101x6&OIV+T+ZpSmq~{En1e>NbcG18|7t*_oSm%=ygR|H}R6$pbxirE7#}G zGjeAYTL;X$rFw8)Px4YBfnRmc-p%ku$SSQ9^jp?>uCu{f6!=wsRxiAA&(fX}3J~@p z^lq=JfqW#j4xrzC$q^EVDGSd4OTrLq5KCS+&hKnMc)!WOq7$ta%!QYdcbn&aq$-(N zE+OAjn??9pS6mq&{1%_>aDdi?W;$<9y%JZ>B^*zMVq`k;1rZ_Vx1 zYR|nAvI625#NR*Pg`JK7K%IG1Iob5V$EzbdcO^?Fq_`v<^QSTH8;UA$%vIfo4>h6O z)ldq}d!4fpm|1*G$+EuD0%%VTGv3L|%lc@!hRpgSPxl%kgl6)H+WXB93)Hd7 z)yeD>HPm7|Y^b#?D=T9y&Fc%}dUHJ-S3o&y2W6?68$eP>xz4#@Pjrnsjk6kZ%hN%B z=PgDq?ONx!!GUftSU*Nox7!W`-GflSyAQ(MiWc~CdUdLo)@G%2oNcF?;gRM-gUN6g z9;R^*ltqjp8O!0{>X~ZiCdTe^l^y-`niO~7@@Inq1Yd6ePwy*$i2`X8^CRjv1A-kL zC;YQql!!<(Xey_S{Fr0__U{;Hhz0*gBFG&K7>&KCb{VP@cOjT>nUb=OumU3Ptwyf| z`PRM#8Ntmh@%3O zdEUhLdfvL2mcsSQE4sQc6g|}QH3`4NbAV^c^5Xawh)Yy|HaUhqvnu=tvZtl=a1p95 z2Xi|mG8X;sofMv6pCifqJ2IZxqVv+$O$S<^i^~VuD1^=&?on5}+g)e$d|~-W*|Os* zz+>>-U7@6Wpg4CYUTL(b_G%--9Ah5s0@$aeryv}_0&DdgFpSqbbJf3Vasi0__=2q9 zLo|&790_wv`qU%hY-UAlUQ43nNqFY zw>f4CgJes=4?R~I+o1xot#&VuV7>{j=$}iV^pS8$ki>a|w#4T7gJCfLU5ChHLHPlz zwk*p7$8l%u24=LxN&l_)`DZ|!yB>6_MM|!XnWy5MnL3q)--%sJ$2rC0_qm~}@4&`Z z^;t*RUL6t8CV`J+t2g-1h>!Vn6E>gK516gqdzukjV$_To@(?+i@%z#2og8*e5w&ob z)o;S_nulW#e0qmCqUAE^4UE}u-Y@dvyZ5}2g5%~krRzyQ*_k?R4Lrn{qtHAFc@2qm z|F%^Yz4MS6=~GL8axz%`MamR;>|^>LVRY*qw9myl&I6HgD@$6~5YbLxHfZmUqQ++) z#uq!n-*U?!twkU4>!pd?DS-nB*pE0}j@B3k^N!5)!~V$u8`9SU??=816-1qRPmNlL zmams8vFbhnjpF$o%1DPAF%@V$J}8Ve8%3@rhi7O~ib1~Vvu6fAc{=k)QwiaLW2p2jQLO*^&$s-aCVU-{SxqAhPu?B8AyJ;8nLP+H?39$G9EJKIG} zLRrX}LZ|1TOytlG1r6}xBlOKkQf!{hyL(6JMZ0^tkcOp<{FS03bTumINK)!yI1K)I z6WEInrlimB{tp8lhTV4(cBdD#q!{#I-9*^|VvT{V0r;r)z%uG+QAuTFBw{wVcC0$f zYeDU<9!#Cj$Nxm(Yk&I^ikPdeF5$mgTsi|(^mZh=6a)f+_GYma%i=Rzk zG3igvwqtt6q`;v`lqCV;!_k%saVWavTFYpXX?EsAR+nD*AQUGIu6?-Z_sV#~8UqrmYk-Vw5^_5DB#usG+Mr2cKhCul#46Z`vwwvh$1 z(egm?;N5%U-68<27^=Zjy%;{;{%96S7(Fob$vpY*+kh=D4*IR`sLCbFZ<(+BtXK^`xK~35XML-$%?okNN;4m8-R1924p*1gw z^or@h!x))?{IS@?(~`lbg$n6MB!w4cUO+7n#NSI%6WgYm+;{d7QJ9}Lh`tvr#n`Q&cvmNTiBKsLZ5OT zA*I{7{J)JrcF#9rLPEmHS|Pst9Nj>RolDV|Fo#+6TEcy0!mPl6-7I&rlEhWXMFr?$ui6^Uo+uJ zx0%%E@eKD9j#lykhk(e`3zocZysgYqm`On*uD}M?$T!hhro0v?{y$n2BSm)4W3`M4 z{iE>@URtLz-thPmY!ja1+&wv!t8xH&dDYpWkxVhDputQ9U$z0KV_NbMv(3k9GPJnU zc~eUH%%T(P9o1`mW8xk+}uQ{4J)Oj#`ufX*Oa2`vndb>q=a1UzKTYNtO#n8(yxS^ z>eS){=DVi?)<@NhnP9~maaT66^M5F>2S=0UIN&*$ItiF#+yl}mj8S3^nK6VPC)`{E zBcl^Niv@wKel))7W7Sx`raSjBi{Q@(F=E6rf7yGQ5pV?G94`v3AegXE&}Jq*81J6e z@z~E-NVI#b$y}{8hs!}MW~GcrDt^<=iYJ(dvQsNdZ6IxN5Pd+(iCpf@$NxJkbTOU? zex78i#ZG3c>S%gb}Xq;wALbGuko8oIg4!?#46{JQp$ z&FyoJ{&xmvRC3zl?P+zgO60ZUM|G>6hoA0X1N6QKTT5Ixd!P5|ihOF(Lw%1vBd-wPI?QDXy`X zXy)6ve*mfI&0GA{4oqHSas;%5NOm?BOpuCUsfH=jxytc?5)6B*JH{KwS-I-j>8VzP z&aqx8#fmjCUiRVTwln&^fl#!fNOd)pw-8msT%B`NLM|RTIoa)Ll$?5D9hdbYw4@}Q zwYC?AW;w(taWzYp@11S$VKId(TY#^LOG}XzE02E=Dt>$nGtilM zV!D(%kLv%7G;uYk^$6?Hg-WH!4aiFtP#sTD)Zw-g}q?*As9=tpD&0MJSPiwmG|bEu|#a%i7c z@Xj~U@@&}T;huBAE53%eWokPd^!Bq3`ga!}ll(bhuRq}u=kRe+_Kn`-zZB^txp;x`5{6d~6RWylL z#r++&?SHYG*7BR_?oS#Uu*FX>flLg9Hd3+}#;qfWcipcI}(5PMsfc`nLP7tKP19)_T`bPvM8BSXmFd8xNnx zZGakv>xyM_^&VC|LI6P1u@nOoj)Jd}kWE%fm^{}odZv($%tzBJiW}0;zQ%K3_8G8x z_j3_%<;C>qjy0PjFS?nRH2>~ss5^hu3``ot))`;2?8D)MCPgs%dF zXE(0QOnU-PHFHydGS81w1!v8V)kua1Moi=@1eZg%Ju7=?Icg0Pp38E7cPpjq-`=#A zvd_V2Wm=6rTpza_YlJ5rc~`TF43!j!uk;p8<~ecJcRV3jkRHtgOiN)mmLe~3$m}*& zJG4}>V!vv(p=9g0UI7)eg zoqm4{hX!@{t?=($06|wiVPUte)cQ*~KQh=Vx8!dI7mD}%~Wb_OMN_BTih`~ zhcy7l+gaRCYT!b#wF?sozll#4BkphPyEd*cW@QS^lEkiM&rejT9UX@;qtX`fP=aC& zA2WU*DZ>>c+;_Ti zP+i~j11*+X%l1^Db0sa+JuC1c;ro>+(JcLuIB$D)b}!_lmjvCafpV8u-8W zI0I}oyDu2SIA%uV@y`cN3mmgRPYAxGbNPLvWy&+-{PtFE3B@;Vy`ADjmhvu><;Jimq6FTVV|;mc@;0Q$=RRZfw$YjLDBj{?u#=f zI~=|H7CdU~9<@3a0$96H>{vwveqTFi=3+6qti)Ic(7#Ibl4hj@TG; zvMWB1U`|;2++;YT=65m9IPHyE{XKZ~!Nu99vQ{gt+852hPW4AuHrjz=2d4KGMP*cH zpFyh*ooN2=CETLzZB9oJx|d8Y%Z01`fP!vI1hMU@ap8MtA~ zal&ZdnVvw#9o89rG(yPTO}uASXw1f0A_gAqG=$3E@^#oBe}W%^$cf(Q`N9xeWKxr-?^t&gVWl)oW|L*vXt=k zBY|}&1ceLqO(c+lOv|Jt4xqPuF%3m$%U>wVY~f9FXIYD@S{+>NRo(C+gm-elT0hwJ z7uUv2L{bm|ckkQe@5@q}bJZP_sZPjPztB@LAG>4I8N_G#t#(y?OC>%T(ldW_xB29c z*i!8DD|3Dvdjm)E)0z4D9B5;Y$Z3y8gtXMHECY+Fm?4*Rlsa<>ZjSHoxDKLZ<*(L# zlHZu$3dB@BV}pg~Nb#<>%R@`EyACU{CN8&r&r@WeW%Uw;B@!2sA?GEm{37v38}#x| zWFbSozR)=Y@sJ4J`Nq~&UL75@y~CvpjClFSCXpy!9WHbx-}f%@b=(K<(q^ws2H+Yl z-M68j!{PAiyWyujo|#pNHp!t&p`FMKii!;Ec_qWX$mr8E7^4FlC>RRGazxi&KTpZX zoobq>MjEd??q6*9;<&x6X`1%1j!2Gfrtvq_TT@DXw{dG4da5w-6z&u=T#v@7GSgRE}Nc%1aq6}sIwt2VMG*P}n%(U$J z>TaCv#w<&6I~H7^I7h-)id-#ea)}dJ?dOkKS$ceOimX<-zX7j;+T1SJ)pwq(p0BX& zTn9JD+XXezlN6i8XH{*04DSk|FswAb6WMQE$0AsaI3Tg2K_Wu>_cdBLsM`<<(=lT@ zqW3SWa7a9HTYm3&EfCP`=z^jJ7K-3J<<0ZB?B=MC&t-p z+HT@Ci2r#i$tKL)jHB9SX|1_VdOqoKrl9V00pcpPv-hrKokQ7xMj8T&$ zF2m0ZyzNz{)S7KA1jBTCaT6!-UfFuLJ^|!%he%*`yEZ^ScGP1zaI-Nh@?`QXKpt|XVD zpJR9L;}vrr%{HK;mM2nU%t22ak9Gjl_DgWMI(Iw+cvYq#y3p+wz40pUwZ1I&LZ+m- z&ETv^KkEKpiVQ~5fD%-MQrRf3r zDGkF`zv0Vvy5&hC(^l?=aqsGSbw7&<5%{?@ zC1dTFxo{Rk7<$Cj>C2u;8+=&iOo~!gGyK=JEft2=I( zQ~#~=Z%IOTatc-o<9FIG^(iwHUSP&J;>gb*?LwpI{&FpjCqm*-7Lel28$cMxN!KZ|ZW~GAvqmZT{ek;&^)8v@ znk=mCg8pgLK{|Ruo(;CcMhBRT{nxvG*K2fzA@PI5OclJ0ZTj9&kKx#EADv9;II5c) zYkcnD#}oIzI3Vk^SIZX&V}P40Z~iDB@v3uK*xif}wWzq*vL;bBX?cY16H9tZ;~(;O zL6b(xCIbgROX^9rsjo*IZnh#=WcY~)eoX!0d8%I5Z1d)c!A3FVs_8D%NfbSD9tAI? z_Z`_Mj35j%hRg1@w+w$WWE5%QI>^3Vn+Mj4EaH%a3}xd2NRG{M_5doWM)m zC!HGf{qQ-XM@fQO|8FjK2f8JR1g*B5S0;0!j02d59#tF)@u>XOU;Luy0NjKY6fA_u zbf54Np-E#xuJ?wOZ zZiWDcA;rZu6{F8irPu?{?7sW0CHmjd>hb@6HeEJhL8OE+5^d$C+2lKSx>>3ekXkqR zk*!68e;g{lt=Y^+`^41ijhBDA`RuE5!ey(k+t%>_G~QB+bGziz>L7|ip>mbzSH^)^ zk1&+*Jh&e&K7A&9HXu3)$F>w;z~yyf!5yq3^Aq;NPDxPvoKSvb+j`w5g zvbu0k&`m_k!~VD#7@vk%Y|tX9C~rWu*Zz({D_&a(p_$xk1Ug>3v^~wRl;l3Xi#P53 zP}-vMW39vWIin4Voh-*jj1~9QGTm#=mVJW=lZ=hxmzAF-n<)dR0uhZT(jH6?PjI># z&)o%TBqO43M;npi%m(67+3?a>l-V1gEmtTPS@oo+l*$Eb7T^Li2l8aU6QtySpGQ zWh95D;R#tcT7;&IwI|qTRUR@eU;7YgpSg)Py&vk6?Rmo{dav9;wy;N@R3DwEOX#C!J4nYK^ z{wo4!B`AHhAOt;G`m$B=wB8Lf>9b$xj0ssmF|9jucI3$lc(S@6;Y4lszW554%WeV# zTlgT_Z^N(|@}Gt9{J#&C>eZ^p#qXGYLj%wf_oA2uXxCWW%krfedZ1&?jb(PZVyO(d zcSjhp7D&fpmu%#~)y_(O7U!q|nBSLKFn8}~qDwACCuSCVzY#t{E0BIeG41Sp@)RXr zeIS=FpHt+p-{kz$IOkVB=hjXx*b#xci_i1bvfX zysT0zdg+t2|8h!HisU3cOpp3UADfN=Sf~9lTj?EKLrE%z`!_<^lwX3x(XrS61fuYr zTuVij*#s!}gKKQ|$vhx`1Z4q*$U)htvhheW7-f22sc8W%>$$fB1lCbv(L1~&%XCPI z_#%y2t}(OoEr-NFyf`}O?f6)E6WG11@7ld^K_sOkgKG9H7xjfx?gmibWkh4{b^g2; zsgx{0l?wxC*^UQQq-zxBV0O=EuIW#bQ*y#%I`l3dm>L^pP=TaA@*E#Oc0XNfV0A#B zm@FE2)Pp2?p@ML3a4!tJY)gJ9pK*TtSi z_7M&NXnTV@Iw+@QsZHOK`8y^qxJ;*H_cm0as)R|b`IVhritHw|WwxW3iN){hLU=M7 zY`Yd|6%%t!etgI{>gT)V&N)x|k?C^t>eaaMnc;oQT-uRoMxTqlXBiX0n?xX<@6T|K zarL}ym#A(QXShSy{#O}$CQt?YxBBuiClJSnmYStWg?I+m$H}Y@0^YiN%Ra>Td*ZxJ zcMpdb6$?CCl-U}=XoJETQl)%h-gWwL)|pCHbKaaxMC0^M;v@UDW@Adqh4Gu-C*Sz4 z$UxDKi*b1_q348lgN^_a;mSni-6CFHF^e3t9grU?@0y*S!mgj6#++^Es2T5F=rT)+ zH%URY^9S&v>(i}yb2~XOO-i0R_(9O}Cvh(wqt%K?jONAFScZ?d?#F?QnZDzKnJ)Gj z=qwWUF%6uu!$979ps9ul`mp9?|NS0s4FVT8`O9W6OIr%s z4zka?sN~BeQ`eu*0jl$!SDblyY57Mxf4p|ShS2*6|NRq>qJ8SsVWr{1C-tkro2TDb zIF@l1ypb%DateJvLXv-N`luW`DK+V~sjZaFkuf}LZ@#_D{ouN2ID4u|JnxCb3X2LR z|Hk!8-Cu9iHKWO)2O;9L{B~#0ynNu-(%EP%6NLR|_bZHel71p5%k$T09yfIe#MQrL z!H$pu1CR7PkQC`M3h}B3p!ktsY}v#^<ax>P)aKRL@ckzx6)F?9p9|o&6EjpJF0e zQ~VG`tadf4Bac9*>BysUEbXlqj2+QtRIXY;T*;S8$(Xiar^VM36EGYjMOlgjhughUh*ezeUl7-;QLFxpg{Dkc zHvLoF9AV-#`pRuBFo*%~=`PAQn^|7OJ+!uPe4AjP`ap3W(Qm>J)7*;gdCLz`w@bcq zEda4k6l*AA>o~+frtE;~IF6%n$>qd{vcXDu6Xbu?lHhfZsWVY?nF@sCYR%x;GFz<= z5!qg8@uOdrpVi8jb7@3kTQ!#~&i?&;b2yVLh$48Q5e335o&3d#y+XM$_5V>^0R)$I zae0(Jf7nZ;U_mkL{DEB*8v8;5nED=0-`{g#SmYK53iL`K#^r1UF4hFACaolW=FTQ? zo6W3wXBmw0`Z?7-x%>y@RqSX=@2DBxeIFo`)nj$s zvOrlVH)b0_3-ubKmg349_)pMYUyn%VP~XmjrJ4)ZZK6@no269UKkQj^NY`X^GXEP| z_PP8BbI1Da9%{54chLRB&yk;UvI1jdAL#j-0OsfMHDSRiCeYw>N5hELetw9fI7s~6 znIYD@*SneY&EZ6Qi8#mEkMr8EmlBJSZN9u%3DFf$J61il7b@suLn$=X0*|!!;rSRx z=@p9R{Emk7a--j7?0nRsI3ELF?WM9Cl{GZ*^%|^`if2BozGW4Zfk4EH6Mm8NBV+0L z{MZs+IfYM1?j1bObJd6T2b8#}2g2rcW=D>KHGE=6}y4kGj~#gH;!wPlCe*UG4e5Jku+Hj-sGh$2zODKLw7o3Cr zMzv^P_4YrL(sHp^8D*e}m`af~?|GDNfj>Ygk9eFBP@IQ&evXV*?m12fL F{|}+<01E&B literal 0 HcmV?d00001 diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 4a186bdf95ed1..42f7c2de44ee9 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -22,6 +22,7 @@ tf2_geometry_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/localization_error_monitor/src/diagnostics.cpp b/localization/localization_error_monitor/src/diagnostics.cpp new file mode 100644 index 0000000000000..0c5a4a7800639 --- /dev/null +++ b/localization/localization_error_monitor/src/diagnostics.cpp @@ -0,0 +1,94 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( + const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "localization_accuracy"; + key_value.value = std::to_string(ellipse_size); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (ellipse_size >= warn_ellipse_size) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "ellipse size is too large"; + } + if (ellipse_size >= error_ellipse_size) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat.message = "ellipse size is over the expected range"; + } + + return stat; +} + +diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection( + const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "localization_accuracy_lateral_direction"; + key_value.value = std::to_string(ellipse_size); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (ellipse_size >= warn_ellipse_size) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "ellipse size along lateral direction is too large"; + } + if (ellipse_size >= error_ellipse_size) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat.message = "ellipse size along lateral direction is over the expected range"; + } + + return stat; +} + +// The highest level within the stat_array will be reflected in the merged_stat. +diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( + const std::vector & stat_array) +{ + diagnostic_msgs::msg::DiagnosticStatus merged_stat; + + for (const auto & stat : stat_array) { + if ((stat.level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!merged_stat.message.empty()) { + merged_stat.message += "; "; + } + merged_stat.message += stat.message; + } + if (stat.level > merged_stat.level) { + merged_stat.level = stat.level; + } + for (const auto & value : stat.values) { + merged_stat.values.push_back(value); + } + } + + if (merged_stat.level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + merged_stat.message = "OK"; + } + + return merged_stat; +} diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/node.cpp index 307dc9445eb28..43795d4036d42 100644 --- a/localization/localization_error_monitor/src/node.cpp +++ b/localization/localization_error_monitor/src/node.cpp @@ -14,6 +14,8 @@ #include "localization_error_monitor/node.hpp" +#include "localization_error_monitor/diagnostics.hpp" + #include #include @@ -30,8 +32,7 @@ #include #include -LocalizationErrorMonitor::LocalizationErrorMonitor() -: Node("localization_error_monitor"), updater_(this) +LocalizationErrorMonitor::LocalizationErrorMonitor() : Node("localization_error_monitor") { scale_ = this->declare_parameter("scale"); error_ellipse_size_ = this->declare_parameter("error_ellipse_size"); @@ -51,55 +52,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor() ellipse_marker_pub_ = this->create_publisher("debug/ellipse_marker", durable_qos); - updater_.setHardwareID("localization_error_monitor"); - updater_.add("localization_accuracy", this, &LocalizationErrorMonitor::checkLocalizationAccuracy); - updater_.add( - "localization_accuracy_lateral_direction", this, - &LocalizationErrorMonitor::checkLocalizationAccuracyLateralDirection); - - // Set timer - using std::chrono_literals::operator""ms; - timer_ = rclcpp::create_timer( - this, get_clock(), 100ms, std::bind(&LocalizationErrorMonitor::onTimer, this)); -} - -void LocalizationErrorMonitor::onTimer() -{ - updater_.force_update(); -} - -void LocalizationErrorMonitor::checkLocalizationAccuracy( - diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - stat.add("localization_accuracy", ellipse_.long_radius); - int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string diag_message = "ellipse size is within the expected range"; - if (warn_ellipse_size_ <= ellipse_.long_radius) { - diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_message = "ellipse size is too large"; - } - if (error_ellipse_size_ <= ellipse_.long_radius) { - diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - diag_message = "ellipse size is over the expected range"; - } - stat.summary(diag_level, diag_message); -} - -void LocalizationErrorMonitor::checkLocalizationAccuracyLateralDirection( - diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - stat.add("localization_accuracy_lateral_direction", ellipse_.size_lateral_direction); - int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string diag_message = "ellipse size along lateral direction is within the expected range"; - if (warn_ellipse_size_lateral_direction_ <= ellipse_.size_lateral_direction) { - diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - diag_message = "ellipse size along lateral direction is too large"; - } - if (error_ellipse_size_lateral_direction_ <= ellipse_.size_lateral_direction) { - diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - diag_message = "ellipse size along lateral direction is over the expected range"; - } - stat.summary(diag_level, diag_message); + diag_pub_ = this->create_publisher("/diagnostics", 10); } visualization_msgs::msg::Marker LocalizationErrorMonitor::createEllipseMarker( @@ -158,6 +111,24 @@ void LocalizationErrorMonitor::onOdom(nav_msgs::msg::Odometry::ConstSharedPtr in const auto ellipse_marker = createEllipseMarker(ellipse_, input_msg); ellipse_marker_pub_->publish(ellipse_marker); + + // diagnostics + std::vector diag_status_array; + diag_status_array.push_back( + checkLocalizationAccuracy(ellipse_.long_radius, warn_ellipse_size_, error_ellipse_size_)); + diag_status_array.push_back(checkLocalizationAccuracyLateralDirection( + ellipse_.size_lateral_direction, warn_ellipse_size_lateral_direction_, + error_ellipse_size_lateral_direction_)); + + diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; + diag_merged_status = mergeDiagnosticStatus(diag_status_array); + diag_merged_status.name = "localization: " + std::string(this->get_name()); + diag_merged_status.hardware_id = this->get_name(); + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_merged_status); + diag_pub_->publish(diag_msg); } double LocalizationErrorMonitor::measureSizeEllipseAlongBodyFrame( diff --git a/localization/localization_error_monitor/test/test_diagnostics.cpp b/localization/localization_error_monitor/test/test_diagnostics.cpp new file mode 100644 index 0000000000000..7b7e8aaab4ed9 --- /dev/null +++ b/localization/localization_error_monitor/test/test_diagnostics.cpp @@ -0,0 +1,140 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "localization_error_monitor/diagnostics.hpp" + +#include + +TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracy) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const double warn_ellipse_size = 0.8; + const double error_ellipse_size = 1.0; + + double ellipse_size = 0.0; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + ellipse_size = 0.7; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + ellipse_size = 0.8; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + ellipse_size = 0.9; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + ellipse_size = 1.0; + stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); +} + +TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracyLateralDirection) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const double warn_ellipse_size = 0.25; + const double error_ellipse_size = 0.3; + + double ellipse_size = 0.0; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + ellipse_size = 0.24; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + ellipse_size = 0.25; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + ellipse_size = 0.29; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + ellipse_size = 0.3; + stat = + checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); +} + +TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) +{ + diagnostic_msgs::msg::DiagnosticStatus merged_stat; + std::vector stat_array(2); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(1).message = "OK"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + EXPECT_EQ(merged_stat.message, "OK"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(1).message = "OK"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN0"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(1).message = "WARN1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(1).message = "WARN1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "ERROR1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(0).message = "ERROR0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1"); +} diff --git a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml index 2d0b2b00af343..f82f7d732f5dc 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml @@ -27,5 +27,5 @@ localization_accuracy: type: diagnostic_aggregator/GenericAnalyzer path: localization_accuracy - contains: [": localization_accuracy"] + contains: ["localization: localization_error_monitor"] timeout: 1.0 From 32d2b8437578de4abc35f417cbead557159105d0 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Thu, 28 Sep 2023 19:55:57 +0900 Subject: [PATCH 29/97] fix(tier4_perception_launch): add parameters for light weight radar fusion and fix launch order (#5166) * fix(tier4_perception_launch): add parameters for light weight radar fusion and fix launch order Signed-off-by: scepter914 * style(pre-commit): autofix * add far_object_merger_sync_queue_size param for package arg Signed-off-by: scepter914 --------- Signed-off-by: scepter914 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- ...ar_radar_fusion_based_detection.launch.xml | 98 ++++++++++--------- 1 file changed, 53 insertions(+), 45 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index df833296e3f66..38d141241c215 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -47,6 +47,8 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -114,6 +158,8 @@ + + @@ -142,54 +188,12 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -293,7 +297,7 @@ - + @@ -302,6 +306,9 @@ + + + @@ -328,7 +335,7 @@ - + @@ -403,5 +410,6 @@ + From 684a4ccc4b58975359a6ffcca1cbeb7e32e0706a Mon Sep 17 00:00:00 2001 From: Alexey Panferov <37497658+lexavtanke@users.noreply.github.com> Date: Thu, 28 Sep 2023 15:11:27 +0300 Subject: [PATCH 30/97] fix(traffic_light_fine_detector): add data_path arg and update default model path (#5118) * fix(traffic_light_fine_detector): add data_path arg and update default model path Signed-off-by: Alexey Panferov * fix(traffic_light_fine_detector): update README with data path arg Signed-off-by: Alexey Panferov * style(pre-commit): autofix --------- Signed-off-by: Alexey Panferov Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- perception/traffic_light_fine_detector/README.md | 13 +++++++------ .../launch/traffic_light_fine_detector.launch.xml | 5 +++-- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/perception/traffic_light_fine_detector/README.md b/perception/traffic_light_fine_detector/README.md index 1ed6debfeae91..dcc89c76387c6 100644 --- a/perception/traffic_light_fine_detector/README.md +++ b/perception/traffic_light_fine_detector/README.md @@ -50,12 +50,13 @@ Based on the camera image and the global ROI array detected by `map_based_detect ### Node Parameters -| Name | Type | Default Value | Description | -| -------------------------- | ------ | ------------- | ------------------------------------------------------------------ | -| `fine_detector_model_path` | string | "" | The onnx file name for yolo model | -| `fine_detector_label_path` | string | "" | The label file with label names for detected objects written on it | -| `fine_detector_precision` | string | "fp32" | The inference mode: "fp32", "fp16" | -| `approximate_sync` | bool | false | Flag for whether to ues approximate sync policy | +| Name | Type | Default Value | Description | +| -------------------------- | ------ | --------------------------- | ------------------------------------------------------------------ | +| `data_path` | string | "$(env HOME)/autoware_data" | packages data and artifacts directory path | +| `fine_detector_model_path` | string | "" | The onnx file name for yolo model | +| `fine_detector_label_path` | string | "" | The label file with label names for detected objects written on it | +| `fine_detector_precision` | string | "fp32" | The inference mode: "fp32", "fp16" | +| `approximate_sync` | bool | false | Flag for whether to ues approximate sync policy | ## Assumptions / Known limits diff --git a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml index 5ce7840c28fd6..6e32d3410c260 100644 --- a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml +++ b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml @@ -1,6 +1,7 @@ - - + + + From 376ca2d76793be6e3d06c7ca6f8172274e634f99 Mon Sep 17 00:00:00 2001 From: Alexey Panferov <37497658+lexavtanke@users.noreply.github.com> Date: Thu, 28 Sep 2023 15:13:18 +0300 Subject: [PATCH 31/97] fix(traffic_light_ssd_fine_detector): add arg data_path, update model file path (#5141) * fix(traffic_light_ssd_fine_detector): add arg data_path, update model file path Signed-off-by: Alexey Panferov * fix(traffic_light_ssd_fine_detector): update README with data_path arg Signed-off-by: Alexey Panferov * style(pre-commit): autofix --------- Signed-off-by: Alexey Panferov Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../traffic_light_ssd_fine_detector/README.md | 19 ++++++++++--------- ...traffic_light_ssd_fine_detector.launch.xml | 5 +++-- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/perception/traffic_light_ssd_fine_detector/README.md b/perception/traffic_light_ssd_fine_detector/README.md index 1dd05665709f5..4dbda8421d85d 100644 --- a/perception/traffic_light_ssd_fine_detector/README.md +++ b/perception/traffic_light_ssd_fine_detector/README.md @@ -122,15 +122,16 @@ Based on the camera image and the global ROI array detected by `map_based_detect ### Node Parameters -| Name | Type | Default Value | Description | -| ------------------ | ------ | ------------------------------ | -------------------------------------------------------------------- | -| `onnx_file` | string | "./data/mb2-ssd-lite-tlr.onnx" | The onnx file name for yolo model | -| `label_file` | string | "./data/voc_labels_tl.txt" | The label file with label names for detected objects written on it | -| `dnn_header_type` | string | "pytorch" | Name of DNN trained toolbox: "pytorch" or "mmdetection" | -| `mode` | string | "FP32" | The inference mode: "FP32", "FP16", "INT8" | -| `max_batch_size` | int | 8 | The size of the batch processed at one time by inference by TensorRT | -| `approximate_sync` | bool | false | Flag for whether to ues approximate sync policy | -| `build_only` | bool | false | shutdown node after TensorRT engine file is built | +| Name | Type | Default Value | Description | +| ------------------ | ------ | ------------------------------------------------------------------------ | -------------------------------------------------------------------- | +| `data_path` | string | "$(env HOME)/autoware_data" | packages data and artifacts directory path | +| `onnx_file` | string | "$(var data_path)/traffic_light_ssd_fine_detector/mb2-ssd-lite-tlr.onnx" | The onnx file name for yolo model | +| `label_file` | string | "$(var data_path)/traffic_light_ssd_fine_detector/voc_labels_tl.txt" | The label file with label names for detected objects written on it | +| `dnn_header_type` | string | "pytorch" | Name of DNN trained toolbox: "pytorch" or "mmdetection" | +| `mode` | string | "FP32" | The inference mode: "FP32", "FP16", "INT8" | +| `max_batch_size` | int | 8 | The size of the batch processed at one time by inference by TensorRT | +| `approximate_sync` | bool | false | Flag for whether to ues approximate sync policy | +| `build_only` | bool | false | shutdown node after TensorRT engine file is built | ## Assumptions / Known limits diff --git a/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml index a4d61b774652a..714c4d288b603 100644 --- a/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml +++ b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml @@ -1,6 +1,7 @@ - - + + + From d4ceaa6e5fe259c9fc4ad41a9761ffbf51decf0a Mon Sep 17 00:00:00 2001 From: Alexey Panferov <37497658+lexavtanke@users.noreply.github.com> Date: Thu, 28 Sep 2023 15:15:11 +0300 Subject: [PATCH 32/97] fix(traffic_light_classifier): add arg data_path, update model path (#5144) * fix(traffic_light_classfier): add arg data_path, update model path Signed-off-by: Alexey Panferov * fix(traffic_light_classifier): add arg data_path, update model path Signed-off-by: Alexey Panferov * fix(traffic_light_classifier): add arg data_path to README Signed-off-by: Alexey Panferov --------- Signed-off-by: Alexey Panferov --- perception/traffic_light_classifier/README.md | 1 + .../launch/traffic_light_classifier.launch.xml | 5 +++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md index 3d15af0cf7805..7df0c5466695b 100644 --- a/perception/traffic_light_classifier/README.md +++ b/perception/traffic_light_classifier/README.md @@ -55,6 +55,7 @@ These colors and shapes are assigned to the message as follows: | Name | Type | Description | | ----------------- | ---- | ------------------------------------------- | | `classifier_type` | int | if the value is `1`, cnn_classifier is used | +| `data_path` | str | packages data and artifacts directory path | ### Core Parameters 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 d4794443d95d9..10aa04cc585af 100644 --- a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml +++ b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -2,8 +2,9 @@ - - + + + From dea2b074400a7af8e66f0828fd8f3b44a4bbe585 Mon Sep 17 00:00:00 2001 From: Alexey Panferov <37497658+lexavtanke@users.noreply.github.com> Date: Thu, 28 Sep 2023 15:16:16 +0300 Subject: [PATCH 33/97] fix(tensorrt yolo): update model path (#5158) * fix(tensorrt_yolo): add arg data_path, update onnx_file Signed-off-by: Alexey Panferov * fix(tensorrt_yolo): update model downloading in README, add data_path Signed-off-by: Alexey Panferov --------- Signed-off-by: Alexey Panferov --- perception/tensorrt_yolo/README.md | 3 ++- perception/tensorrt_yolo/launch/tensorrt_yolo.launch.xml | 9 +++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/perception/tensorrt_yolo/README.md b/perception/tensorrt_yolo/README.md index afa9209c43bb2..58d4af0dfa83d 100644 --- a/perception/tensorrt_yolo/README.md +++ b/perception/tensorrt_yolo/README.md @@ -55,6 +55,7 @@ Jocher, G., et al. (2021). ultralytics/yolov5: v6.0 - YOLOv5n 'Nano' models, Rob | Name | Type | Default Value | Description | | ----------------------- | ------ | ------------- | ------------------------------------------------------------------ | +| `data_path` | string | "" | Packages data and artifacts directory path | | `onnx_file` | string | "" | The onnx file name for yolo model | | `engine_file` | string | "" | The tensorrt engine file name for yolo model | | `label_file` | string | "" | The label file with label names for detected objects written on it | @@ -71,7 +72,7 @@ This package includes multiple licenses. All YOLO ONNX models are converted from the officially trained model. If you need information about training datasets and conditions, please refer to the official repositories. -All models are downloaded automatically when building. When launching the node with a model for the first time, the model is automatically converted to TensorRT, although this may take some time. +All models are downloaded during env preparation by ansible (as mention in [installation](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/source-installation/)). It is also possible to download them manually, see [Manual downloading of artifacts](https://github.com/autowarefoundation/autoware/tree/main/ansible/roles/artifacts) . When launching the node with a model for the first time, the model is automatically converted to TensorRT, although this may take some time. ### YOLOv3 diff --git a/perception/tensorrt_yolo/launch/tensorrt_yolo.launch.xml b/perception/tensorrt_yolo/launch/tensorrt_yolo.launch.xml index a548939f2cebe..b2656de0ab72e 100755 --- a/perception/tensorrt_yolo/launch/tensorrt_yolo.launch.xml +++ b/perception/tensorrt_yolo/launch/tensorrt_yolo.launch.xml @@ -3,7 +3,8 @@ - + + @@ -11,11 +12,11 @@ - + - + - + From a48e80bc9cbaa0d04d59b263fbd8cd7cce148886 Mon Sep 17 00:00:00 2001 From: Alexey Panferov <37497658+lexavtanke@users.noreply.github.com> Date: Thu, 28 Sep 2023 15:17:48 +0300 Subject: [PATCH 34/97] fix(image_projection_based_fusion): add arg data_path, update model_path (#5163) Signed-off-by: Alexey Panferov --- .../launch/pointpainting_fusion.launch.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index e6275a2ee83c7..8e79e21db40e3 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -18,8 +18,9 @@ + - + From 22d595904e783b4b5fdd2f5790e3ff16791176a9 Mon Sep 17 00:00:00 2001 From: Alexey Panferov <37497658+lexavtanke@users.noreply.github.com> Date: Thu, 28 Sep 2023 15:52:00 +0300 Subject: [PATCH 35/97] fix(tensorrt_yolox): add data_path arg and update default model path (#5119) * fix(tensorrt_yolox): add data_path arg and update default model path Signed-off-by: Alexey Panferov * fix(tensorrt_yolox): update README Signed-off-by: Alexey Panferov --------- Signed-off-by: Alexey Panferov --- perception/tensorrt_yolox/README.md | 4 ++-- perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml | 3 ++- perception/tensorrt_yolox/launch/yolox_tiny.launch.xml | 3 ++- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/perception/tensorrt_yolox/README.md b/perception/tensorrt_yolox/README.md index 3c253a0b68489..ca407b1ff6811 100644 --- a/perception/tensorrt_yolox/README.md +++ b/perception/tensorrt_yolox/README.md @@ -71,7 +71,7 @@ those are labeled as `UNKNOWN`, while detected rectangles are drawn in the visua ## Onnx model -A sample model (named `yolox-tiny.onnx`) is downloaded automatically during the build process. +A sample model (named `yolox-tiny.onnx`) is downloaded by ansible script on env preparation stage, if not, please, follow [Manual downloading of artifacts](https://github.com/autowarefoundation/autoware/tree/main/ansible/roles/artifacts). To accelerate Non-maximum-suppression (NMS), which is one of the common post-process after object detection inference, `EfficientNMS_TRT` module is attached after the ordinal YOLOX (tiny) network. The `EfficientNMS_TRT` module contains fixed values for `score_threshold` and `nms_threshold` in it, @@ -146,7 +146,7 @@ Please refer [the official document](https://github.com/Megvii-BaseDetection/YOL ## Label file -A sample label file (named `label.txt`)is also downloaded automatically during the build process +A sample label file (named `label.txt`)is also downloaded automatically during env preparation process (**NOTE:** This file is incompatible with models that output labels for the COCO dataset (e.g., models from the official YOLOX repository)). This file represents the correspondence between class index (integer outputted from YOLOX network) and diff --git a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml index cb89f5829c65d..3f8d7897ab5d3 100644 --- a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml +++ b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -4,7 +4,8 @@ - + + diff --git a/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml b/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml index d8c67e39e0b8a..2f08031ea159f 100644 --- a/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml +++ b/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml @@ -3,7 +3,8 @@ - + + From aa90375c9ff67081c7968036b12c747e51ebb3f1 Mon Sep 17 00:00:00 2001 From: Alexey Panferov <37497658+lexavtanke@users.noreply.github.com> Date: Thu, 28 Sep 2023 16:11:12 +0300 Subject: [PATCH 36/97] fix(lidar_centerpoint): add arg data_path, update model path (#5162) Signed-off-by: Alexey Panferov --- .../centerpoint_vs_centerpoint-tiny.launch.xml | 5 +++-- .../lidar_centerpoint/launch/lidar_centerpoint.launch.xml | 3 ++- .../launch/single_inference_lidar_centerpoint.launch.xml | 3 ++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml index 13fd386238eda..b9c056cfb5686 100644 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml @@ -5,6 +5,7 @@ + @@ -21,7 +22,7 @@ - + @@ -35,7 +36,7 @@ - + diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml index d552cb702b980..a7f181ab78ac6 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -2,8 +2,9 @@ + - + diff --git a/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml index 0f6923d5e6414..491abfbad7764 100644 --- a/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml @@ -1,7 +1,8 @@ + - + From 952753f86b1408e011c178193604b7aa8d88f90c Mon Sep 17 00:00:00 2001 From: Alexey Panferov <37497658+lexavtanke@users.noreply.github.com> Date: Thu, 28 Sep 2023 16:30:47 +0300 Subject: [PATCH 37/97] feat(perception_launch): add data_path arg to perception launch (#5069) * feat(perception_launch): add var data_path to perception.launch Signed-off-by: Alexey Panferov * feat(perception_launch): update default center_point_model_path Signed-off-by: Alexey Panferov --------- Signed-off-by: Alexey Panferov --- .../detection/lidar_based_detection.launch.xml | 2 +- .../tier4_perception_launch/launch/perception.launch.xml | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 23e0297dc5e44..3b6b9ba652a24 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -10,7 +10,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 36d43bab74894..b0219376e9625 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -25,11 +25,12 @@ - + + @@ -78,11 +79,11 @@ - + Date: Thu, 28 Sep 2023 18:27:43 +0200 Subject: [PATCH 38/97] build(behavior_velocity_intersection_module): add missing grid_map_core dependency (#5176) Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_intersection_module/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 9443295981679..2fd733f167546 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -23,6 +23,7 @@ autoware_perception_msgs behavior_velocity_planner_common geometry_msgs + grid_map_core interpolation lanelet2_extension libopencv-dev From 1caa1b3eeff0190255edb6331753b9f7f752e151 Mon Sep 17 00:00:00 2001 From: Yusuke Mizoguchi <33048729+swiftfile@users.noreply.github.com> Date: Fri, 29 Sep 2023 01:46:49 +0900 Subject: [PATCH 39/97] feat(blockage_diag): dust detection (#3212) * implement about dust detection Signed-off-by: Yusuke Mizoguchi * chore: fix spell Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen * bug fix Signed-off-by: Shunsuke Miura * fix: avoid counter overflow Signed-off-by: badai-nguyen --------- Signed-off-by: Yusuke Mizoguchi Signed-off-by: badai-nguyen Signed-off-by: Shunsuke Miura Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Co-authored-by: badai-nguyen Co-authored-by: Shunsuke Miura --- .../docs/blockage_diag.md | 63 ++- .../blockage_diag/blockage_diag_nodelet.hpp | 31 +- .../launch/blockage_diag.launch.xml | 28 ++ .../blockage_diag/blockage_diag_nodelet.cpp | 389 ++++++++++++------ 4 files changed, 362 insertions(+), 149 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index 519bd831fd39c..32e5a5869109d 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -2,11 +2,13 @@ ## Purpose -To ensure the performance of LiDAR and safety for autonomous driving, the abnormal condition diagnostics feature is needed. -LiDAR blockage is abnormal condition of LiDAR when some unwanted objects stitch to and block the light pulses and return signal. +To ensure the performance of LiDAR and safety for autonomous driving, the abnormal condition diagnostics feature is +needed. +LiDAR blockage is abnormal condition of LiDAR when some unwanted objects stitch to and block the light pulses and return +signal. This node's purpose is to detect the existing of blockage on LiDAR and its related size and location. -## Inner-workings / Algorithms +## Inner-workings / Algorithms(Blockage detection) This node bases on the no-return region and its location to decide if it is a blockage. @@ -16,6 +18,14 @@ The logic is showed as below ![blockage_diag_flowchart](./image/blockage_diag_flowchart.drawio.svg) +## Inner-workings /Algorithms(Dust detection) + +About dust detection, morphological processing is implemented. +If the lidar's ray cannot be acquired due to dust in the lidar area where the point cloud is considered to return from +the ground, +black pixels appear as noise in the depth image. +The area of noise is found by erosion and dilation these black pixels. + ## Inputs / Outputs This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). @@ -28,30 +38,41 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Output -| Name | Type | Description | -| ---------------------------------------------------- | --------------------------------------- | -------------------------------------------------- | -| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | -| `~/output/blockage_diag/debug/ground_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | -| `~/output/blockage_diag/debug/sky_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | -| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | +| Name | Type | Description | +| :-------------------------------------------------------- | :-------------------------------------- | ------------------------------------------------------------------------------------------------ | +| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | +| `~/output/blockage_diag/debug/ground_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | +| `~/output/blockage_diag/debug/sky_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | +| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | +| `~/output/blockage_diag/debug/single_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of detected dusty area in latest single frame | +| `~/output/blockage_diag/debug/multi_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of continuous detected dusty area | +| `~/output/blockage_diag/debug/blockage_dust_merged_image` | `sensor_msgs::msg::Image` | The merged image of blockage detection(red) and multi frame dusty area detection(yellow) results | +| `~/output/blockage_diag/debug/ground_dust_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The ratio of dusty area divided by area where ray usually returns from the ground. | ## Parameters -| Name | Type | Description | -| -------------------------- | ------ | -------------------------------------------------- | -| `blockage_ratio_threshold` | float | The threshold of blockage area ratio | -| `blockage_count_threshold` | float | The threshold of number continuous blockage frames | -| `horizontal_ring_id` | int | The id of horizontal ring of the LiDAR | -| `angle_range` | vector | The effective range of LiDAR | -| `vertical_bins` | int | The LiDAR channel number | -| `model` | string | The LiDAR model | -| `buffering_frames` | uint | The number of buffering [range:1-200] | -| `buffering_interval` | uint | The interval of buffering | +| Name | Type | Description | +| ----------------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------- | +| `blockage_ratio_threshold` | float | The threshold of blockage area ratio.If the blockage value exceeds this threshold, the diagnostic state will be set to ERROR. | +| `blockage_count_threshold` | float | The threshold of number continuous blockage frames | +| `horizontal_ring_id` | int | The id of horizontal ring of the LiDAR | +| `angle_range` | vector | The effective range of LiDAR | +| `vertical_bins` | int | The LiDAR channel number | +| `model` | string | The LiDAR model | +| `blockage_buffering_frames` | int | The number of buffering about blockage detection [range:1-200] | +| `blockage_buffering_interval` | int | The interval of buffering about blockage detection | +| `dust_ratio_threshold` | float | The threshold of dusty area ratio | +| `dust_count_threshold` | int | The threshold of number continuous frames include dusty area | +| `dust_kernel_size` | int | The kernel size of morphology processing in dusty area detection | +| `dust_buffering_frames` | int | The number of buffering about dusty area detection [range:1-200] | +| `dust_buffering_interval` | int | The interval of buffering about dusty area detection | ## Assumptions / Known limits -1. Only Hesai Pandar40P and Hesai PandarQT were tested. For a new LiDAR, it is necessary to check order of channel id in vertical distribution manually and modify the code. -2. The current method is still limited for dust type of blockage when dust particles are sparsely distributed. +1. Only Hesai Pandar40P and Hesai PandarQT were tested. For a new LiDAR, it is necessary to check order of channel id in + vertical distribution manually and modify the code. +2. About dusty area detection, False positives occur when there are water puddles on the road surface due to rain, etc. + Also, the area of the ray to the sky is currently undetectable. ## (Optional) Error detection and handling diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp index 116dd2f832b93..66e14c418b52c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp @@ -53,27 +53,42 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); image_transport::Publisher lidar_depth_map_pub_; image_transport::Publisher blockage_mask_pub_; + image_transport::Publisher single_frame_dust_mask_pub; + image_transport::Publisher multi_frame_dust_mask_pub; + image_transport::Publisher blockage_dust_merged_pub; rclcpp::Publisher::SharedPtr ground_blockage_ratio_pub_; rclcpp::Publisher::SharedPtr sky_blockage_ratio_pub_; + rclcpp::Publisher::SharedPtr ground_dust_ratio_pub_; + rclcpp::Publisher::SharedPtr blockage_type_pub_; private: void onBlockageChecker(DiagnosticStatusWrapper & stat); + void dustChecker(DiagnosticStatusWrapper & stat); Updater updater_{this}; - uint vertical_bins_; + int vertical_bins_; std::vector angle_range_deg_; - uint horizontal_ring_id_ = 12; + int horizontal_ring_id_; float blockage_ratio_threshold_; + float dust_ratio_threshold_; float ground_blockage_ratio_ = -1.0f; float sky_blockage_ratio_ = -1.0f; + float ground_dust_ratio_ = -1.0f; std::vector ground_blockage_range_deg_ = {0.0f, 0.0f}; std::vector sky_blockage_range_deg_ = {0.0f, 0.0f}; - uint erode_kernel_ = 10; - uint ground_blockage_count_ = 0; - uint sky_blockage_count_ = 0; - uint blockage_count_threshold_; + int blockage_kernel_ = 10; + int blockage_frame_count_ = 0; + int ground_blockage_count_ = 0; + int sky_blockage_count_ = 0; + int blockage_count_threshold_; std::string lidar_model_; - uint buffering_frames_ = 100; - uint buffering_interval_ = 5; + int blockage_buffering_frames_; + int blockage_buffering_interval_; + int dust_kernel_size_; + int dust_buffering_frames_; + int dust_buffering_interval_; + int dust_buffering_frame_counter_ = 0; + int dust_count_threshold_; + int dust_frame_count_ = 0; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml index 47dc1e73eef12..8fa5796e2802d 100644 --- a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml +++ b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml @@ -2,8 +2,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index 3563bf2b6c408..06da91e61b3bd 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -19,6 +19,7 @@ #include #include +#include namespace pointcloud_preprocessor { @@ -30,34 +31,45 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options { { // initialize params: - horizontal_ring_id_ = static_cast(declare_parameter("horizontal_ring_id", 12)); - blockage_ratio_threshold_ = - static_cast(declare_parameter("blockage_ratio_threshold", 0.1)); - vertical_bins_ = static_cast(declare_parameter("vertical_bins", 40)); - angle_range_deg_ = declare_parameter("angle_range", std::vector{0.0, 360.0}); - lidar_model_ = static_cast(declare_parameter("model", "Pandar40P")); - blockage_count_threshold_ = - static_cast(declare_parameter("blockage_count_threshold", 50)); - buffering_frames_ = static_cast(declare_parameter("buffering_frames", 100)); - buffering_interval_ = static_cast(declare_parameter("buffering_interval", 5)); + horizontal_ring_id_ = declare_parameter("horizontal_ring_id"); + blockage_ratio_threshold_ = declare_parameter("blockage_ratio_threshold"); + vertical_bins_ = declare_parameter("vertical_bins"); + angle_range_deg_ = declare_parameter>("angle_range"); + lidar_model_ = declare_parameter("model"); + blockage_count_threshold_ = declare_parameter("blockage_count_threshold"); + blockage_buffering_frames_ = declare_parameter("blockage_buffering_frames"); + blockage_buffering_interval_ = declare_parameter("blockage_buffering_interval"); + dust_ratio_threshold_ = declare_parameter("dust_ratio_threshold"); + dust_count_threshold_ = declare_parameter("dust_count_threshold"); + dust_kernel_size_ = declare_parameter("dust_kernel_size"); + dust_buffering_frames_ = declare_parameter("dust_buffering_frames"); + dust_buffering_interval_ = declare_parameter("dust_buffering_interval"); } updater_.setHardwareID("blockage_diag"); updater_.add( std::string(this->get_namespace()) + ": blockage_validation", this, &BlockageDiagComponent::onBlockageChecker); + updater_.add( + std::string(this->get_namespace()) + ": dust_validation", this, + &BlockageDiagComponent::dustChecker); updater_.setPeriod(0.1); - + single_frame_dust_mask_pub = + image_transport::create_publisher(this, "blockage_diag/debug/single_frame_dust_mask_image"); + multi_frame_dust_mask_pub = + image_transport::create_publisher(this, "blockage_diag/debug/multi_frame_dust_mask_image"); lidar_depth_map_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/lidar_depth_map"); blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image"); - + blockage_dust_merged_pub = + image_transport::create_publisher(this, "blockage_diag/debug/blockage_dust_merged_image"); ground_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); sky_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/sky_blockage_ratio", rclcpp::SensorDataQoS()); - + ground_dust_ratio_pub_ = create_publisher( + "blockage_diag/debug/ground_dust_ratio", rclcpp::SensorDataQoS()); using std::placeholders::_1; set_param_res_ = this->add_on_set_parameters_callback( std::bind(&BlockageDiagComponent::paramCallback, this, _1)); @@ -105,19 +117,59 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat) stat.summary(level, msg); } +void BlockageDiagComponent::dustChecker(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + stat.add("ground_dust_ratio", std::to_string(ground_dust_ratio_)); + auto level = DiagnosticStatus::OK; + std::string msg; + if (ground_dust_ratio_ < 0.0f) { + level = DiagnosticStatus::STALE; + msg = "STALE"; + } else if ( + (ground_dust_ratio_ > dust_ratio_threshold_) && (dust_frame_count_ > dust_count_threshold_)) { + level = DiagnosticStatus::ERROR; + msg = "ERROR"; + } else if (ground_dust_ratio_ > 0.0f) { + level = DiagnosticStatus::WARN; + msg = "WARN"; + } else { + level = DiagnosticStatus::OK; + msg = "OK"; + } + + if (ground_dust_ratio_ > 0.0f) { + msg = msg + ": LIDAR ground dust"; + } + stat.summary(level, msg); +} + void BlockageDiagComponent::filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) { std::scoped_lock lock(mutex_); - if (indices) { - RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + int vertical_bins = vertical_bins_; + int ideal_horizontal_bins; + float distance_coefficient = 327.67f; + float horizontal_resolution_ = 0.4f; + if (lidar_model_ == "Pandar40P") { + distance_coefficient = 327.67f; + horizontal_resolution_ = 0.4f; + } else if (lidar_model_ == "PandarQT") { + distance_coefficient = 3276.75f; + horizontal_resolution_ = 0.6f; } - uint horizontal_bins = static_cast((angle_range_deg_[1] - angle_range_deg_[0])); - uint vertical_bins = vertical_bins_; + ideal_horizontal_bins = + static_cast((angle_range_deg_[1] - angle_range_deg_[0]) / horizontal_resolution_); pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); - cv::Mat lidar_depth_map(cv::Size(horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); - cv::Mat lidar_depth_map_8u(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + std::vector horizontal_bin_reference(ideal_horizontal_bins); + std::vector> each_ring_pointcloud(vertical_bins); + cv::Mat full_size_depth_map( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); + cv::Mat lidar_depth_map(cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); + cv::Mat lidar_depth_map_8u( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); if (pcl_input->points.empty()) { ground_blockage_ratio_ = 1.0f; sky_blockage_ratio_ = 1.0f; @@ -132,114 +184,186 @@ void BlockageDiagComponent::filter( sky_blockage_range_deg_[0] = angle_range_deg_[0]; sky_blockage_range_deg_[1] = angle_range_deg_[1]; } else { - for (const auto & p : pcl_input->points) { - if ((p.azimuth / 100.0 > angle_range_deg_[0]) && (p.azimuth / 100.0 < angle_range_deg_[1])) { - if (lidar_model_ == "Pandar40P") { - lidar_depth_map.at( - p.ring, static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) += - static_cast(6250.0 / p.distance); // make image clearly - lidar_depth_map_8u.at( - p.ring, static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) = 255; - } else if (lidar_model_ == "PandarQT") { - lidar_depth_map.at( - vertical_bins - p.ring - 1, - static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) += - static_cast(6250.0 / p.distance); - lidar_depth_map_8u.at( - vertical_bins - p.ring - 1, - static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) = 255; + for (int i = 0; i < ideal_horizontal_bins; ++i) { + horizontal_bin_reference.at(i) = angle_range_deg_[0] + i * horizontal_resolution_; + } + for (const auto p : pcl_input->points) { + for (int horizontal_bin = 0; + horizontal_bin < static_cast(horizontal_bin_reference.size()); horizontal_bin++) { + if ( + (p.azimuth / 100 > + (horizontal_bin_reference.at(horizontal_bin) - horizontal_resolution_ / 2)) && + (p.azimuth / 100 <= + (horizontal_bin_reference.at(horizontal_bin) + horizontal_resolution_ / 2))) { + if (lidar_model_ == "Pandar40P") { + full_size_depth_map.at(p.ring, horizontal_bin) = + UINT16_MAX - distance_coefficient * p.distance; + } else if (lidar_model_ == "PandarQT") { + full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin) = + UINT16_MAX - distance_coefficient * p.distance; + } } } } - lidar_depth_map.convertTo(lidar_depth_map, CV_8UC1, 1.0 / 100.0); - cv::Mat no_return_mask; - cv::inRange(lidar_depth_map_8u, 0, 1, no_return_mask); - - cv::Mat erosion_dst; - cv::Mat element = cv::getStructuringElement( - cv::MORPH_RECT, cv::Size(2 * erode_kernel_ + 1, 2 * erode_kernel_ + 1), - cv::Point(erode_kernel_, erode_kernel_)); - cv::erode(no_return_mask, erosion_dst, element); - cv::dilate(erosion_dst, no_return_mask, element); - - static boost::circular_buffer no_return_mask_buffer(buffering_frames_); - - cv::Mat no_return_mask_result(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - cv::Mat time_series_blockage_mask( - cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - cv::Mat no_return_mask_binarized( - cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + } + full_size_depth_map.convertTo(lidar_depth_map_8u, CV_8UC1, 1.0 / 300); + cv::Mat no_return_mask(cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::inRange(lidar_depth_map_8u, 0, 1, no_return_mask); + cv::Mat erosion_dst; + cv::Mat blockage_element = cv::getStructuringElement( + cv::MORPH_RECT, cv::Size(2 * blockage_kernel_ + 1, 2 * blockage_kernel_ + 1), + cv::Point(blockage_kernel_, blockage_kernel_)); + cv::erode(no_return_mask, erosion_dst, blockage_element); + cv::dilate(erosion_dst, no_return_mask, blockage_element); + static boost::circular_buffer no_return_mask_buffer(blockage_buffering_frames_); + cv::Mat time_series_blockage_result( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat time_series_blockage_mask( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat no_return_mask_binarized( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - static uint frame_count; - frame_count++; - if (buffering_interval_ != 0) { - no_return_mask_binarized = no_return_mask / 255; - if (frame_count == buffering_interval_) { - no_return_mask_buffer.push_back(no_return_mask_binarized); - frame_count = 0; - } - for (const auto & binary_mask : no_return_mask_buffer) { - time_series_blockage_mask += binary_mask; - } - cv::inRange( - time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(), - no_return_mask_result); + if (blockage_buffering_interval_ == 0) { + no_return_mask.copyTo(time_series_blockage_result); + } else { + no_return_mask_binarized = no_return_mask / 255; + if (blockage_frame_count_ >= blockage_buffering_interval_) { + no_return_mask_buffer.push_back(no_return_mask_binarized); + blockage_frame_count_ = 0; } else { - no_return_mask.copyTo(no_return_mask_result); + blockage_frame_count_++; } - cv::Mat ground_no_return_mask; - cv::Mat sky_no_return_mask; - no_return_mask_result(cv::Rect(0, 0, horizontal_bins, horizontal_ring_id_)) - .copyTo(sky_no_return_mask); - no_return_mask_result( - cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) - .copyTo(ground_no_return_mask); - ground_blockage_ratio_ = - static_cast(cv::countNonZero(ground_no_return_mask)) / - static_cast(horizontal_bins * (vertical_bins - horizontal_ring_id_)); - sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / - static_cast(horizontal_bins * horizontal_ring_id_); - - if (ground_blockage_ratio_ > blockage_ratio_threshold_) { - cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask); - ground_blockage_range_deg_[0] = - static_cast(ground_blockage_bb.x) + angle_range_deg_[0]; - ground_blockage_range_deg_[1] = - static_cast(ground_blockage_bb.x + ground_blockage_bb.width) + angle_range_deg_[0]; + for (const auto & binary_mask : no_return_mask_buffer) { + time_series_blockage_mask += binary_mask; + } + cv::inRange( + time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(), + time_series_blockage_result); + } + cv::Mat ground_no_return_mask; + cv::Mat sky_no_return_mask; + no_return_mask(cv::Rect(0, 0, ideal_horizontal_bins, horizontal_ring_id_)) + .copyTo(sky_no_return_mask); + no_return_mask( + cv::Rect(0, horizontal_ring_id_, ideal_horizontal_bins, vertical_bins - horizontal_ring_id_)) + .copyTo(ground_no_return_mask); + ground_blockage_ratio_ = + static_cast(cv::countNonZero(ground_no_return_mask)) / + static_cast(ideal_horizontal_bins * (vertical_bins - horizontal_ring_id_)); + sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / + static_cast(ideal_horizontal_bins * horizontal_ring_id_); - if (ground_blockage_count_ <= 2 * blockage_count_threshold_) { - ground_blockage_count_ += 1; - } - } else { - ground_blockage_count_ = 0; + if (ground_blockage_ratio_ > blockage_ratio_threshold_) { + cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask); + ground_blockage_range_deg_[0] = static_cast(ground_blockage_bb.x) + angle_range_deg_[0]; + ground_blockage_range_deg_[1] = + static_cast(ground_blockage_bb.x + ground_blockage_bb.width) + angle_range_deg_[0]; + if (ground_blockage_count_ <= 2 * blockage_count_threshold_) { + ground_blockage_count_ += 1; } + } else { + ground_blockage_count_ = 0; + } + if (sky_blockage_ratio_ > blockage_ratio_threshold_) { + cv::Rect sky_blockage_bx = cv::boundingRect(sky_no_return_mask); + sky_blockage_range_deg_[0] = static_cast(sky_blockage_bx.x) + angle_range_deg_[0]; + sky_blockage_range_deg_[1] = + static_cast(sky_blockage_bx.x + sky_blockage_bx.width) + angle_range_deg_[0]; + if (sky_blockage_count_ <= 2 * blockage_count_threshold_) { + sky_blockage_count_ += 1; + } + } else { + sky_blockage_count_ = 0; + } + // dust + cv::Mat ground_depth_map = lidar_depth_map_8u( + cv::Rect(0, horizontal_ring_id_, ideal_horizontal_bins, vertical_bins - horizontal_ring_id_)); + cv::Mat sky_blank(horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0)); + cv::Mat ground_blank( + vertical_bins - horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0)); + cv::Mat single_dust_img( + cv::Size(lidar_depth_map_8u.rows, lidar_depth_map_8u.cols), CV_8UC1, cv::Scalar(0)); + cv::Mat single_dust_ground_img = ground_depth_map.clone(); + cv::inRange(single_dust_ground_img, 0, 1, single_dust_ground_img); + cv::Mat dust_element = getStructuringElement( + cv::MORPH_RECT, cv::Size(2 * dust_kernel_size_ + 1, 2 * dust_kernel_size_ + 1), + cv::Point(-1, -1)); + cv::dilate(single_dust_ground_img, single_dust_ground_img, dust_element); + cv::erode(single_dust_ground_img, single_dust_ground_img, dust_element); + cv::inRange(single_dust_ground_img, 254, 255, single_dust_ground_img); + cv::Mat ground_mask(cv::Size(ideal_horizontal_bins, horizontal_ring_id_), CV_8UC1); + cv::vconcat(sky_blank, single_dust_ground_img, single_dust_img); + static boost::circular_buffer dust_mask_buffer(dust_buffering_frames_); + cv::Mat binarized_dust_mask_( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat multi_frame_dust_mask( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat multi_frame_ground_dust_result( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - if (sky_blockage_ratio_ > blockage_ratio_threshold_) { - cv::Rect sky_blockage_bx = cv::boundingRect(sky_no_return_mask); - sky_blockage_range_deg_[0] = static_cast(sky_blockage_bx.x) + angle_range_deg_[0]; - sky_blockage_range_deg_[1] = - static_cast(sky_blockage_bx.x + sky_blockage_bx.width) + angle_range_deg_[0]; - if (sky_blockage_count_ <= 2 * blockage_count_threshold_) { - sky_blockage_count_ += 1; - } + if (dust_buffering_interval_ == 0) { + single_dust_img.copyTo(multi_frame_ground_dust_result); + dust_buffering_frame_counter_ = 0; + } else { + binarized_dust_mask_ = single_dust_img / 255; + if (dust_buffering_frame_counter_ >= dust_buffering_interval_) { + dust_mask_buffer.push_back(binarized_dust_mask_); + dust_buffering_frame_counter_ = 0; } else { - sky_blockage_count_ = 0; + dust_buffering_frame_counter_++; } - - cv::Mat lidar_depth_colorized; - cv::applyColorMap(lidar_depth_map, lidar_depth_colorized, cv::COLORMAP_JET); - sensor_msgs::msg::Image::SharedPtr lidar_depth_msg = - cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", lidar_depth_colorized).toImageMsg(); - lidar_depth_msg->header = input->header; - lidar_depth_map_pub_.publish(lidar_depth_msg); - - cv::Mat blockage_mask_colorized; - cv::applyColorMap(no_return_mask_result, blockage_mask_colorized, cv::COLORMAP_JET); - sensor_msgs::msg::Image::SharedPtr blockage_mask_msg = - cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg(); - blockage_mask_msg->header = input->header; - blockage_mask_pub_.publish(blockage_mask_msg); + for (const auto & binarized_dust_mask : dust_mask_buffer) { + multi_frame_dust_mask += binarized_dust_mask; + } + cv::inRange( + multi_frame_dust_mask, dust_mask_buffer.size() - 1, dust_mask_buffer.size(), + multi_frame_ground_dust_result); + } + cv::Mat single_frame_ground_dust_colorized( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0)); + cv::applyColorMap(single_dust_img, single_frame_ground_dust_colorized, cv::COLORMAP_JET); + cv::Mat multi_frame_ground_dust_colorized; + cv::Mat blockage_dust_merged_img( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0)); + cv::Mat blockage_dust_merged_mask( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + blockage_dust_merged_img.setTo( + cv::Vec3b(0, 0, 255), time_series_blockage_result); // red:blockage + blockage_dust_merged_img.setTo( + cv::Vec3b(0, 255, 255), multi_frame_ground_dust_result); // yellow:dust + sensor_msgs::msg::Image::SharedPtr single_frame_dust_mask_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", single_frame_ground_dust_colorized) + .toImageMsg(); + single_frame_dust_mask_pub.publish(single_frame_dust_mask_msg); + sensor_msgs::msg::Image::SharedPtr multi_frame_dust_mask_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", multi_frame_ground_dust_colorized) + .toImageMsg(); + multi_frame_dust_mask_pub.publish(multi_frame_dust_mask_msg); + sensor_msgs::msg::Image::SharedPtr lidar_depth_map_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "mono16", full_size_depth_map).toImageMsg(); + lidar_depth_map_msg->header = input->header; + lidar_depth_map_pub_.publish(lidar_depth_map_msg); + cv::Mat blockage_mask_colorized; + cv::applyColorMap(time_series_blockage_result, blockage_mask_colorized, cv::COLORMAP_JET); + sensor_msgs::msg::Image::SharedPtr blockage_mask_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg(); + blockage_mask_msg->header = input->header; + blockage_mask_pub_.publish(blockage_mask_msg); + cv::Mat blockage_dust_merged_colorized( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0)); + blockage_dust_merged_img.copyTo(blockage_dust_merged_colorized); + sensor_msgs::msg::Image::SharedPtr blockage_dust_merged_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_dust_merged_colorized) + .toImageMsg(); + if (ground_dust_ratio_ > dust_ratio_threshold_) { + if (dust_frame_count_ < 2 * dust_count_threshold_) { + dust_frame_count_++; + } + } else { + dust_frame_count_ = 0; } + blockage_dust_merged_msg->header = input->header; + blockage_dust_merged_pub.publish(blockage_dust_merged_msg); tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; ground_blockage_ratio_msg.data = ground_blockage_ratio_; @@ -250,7 +374,14 @@ void BlockageDiagComponent::filter( sky_blockage_ratio_msg.data = sky_blockage_ratio_; sky_blockage_ratio_msg.stamp = now(); sky_blockage_ratio_pub_->publish(sky_blockage_ratio_msg); + tier4_debug_msgs::msg::Float32Stamped blockage_ratio_msg; + tier4_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg; + ground_dust_ratio_ = static_cast(cv::countNonZero(single_dust_ground_img)) / + (single_dust_ground_img.cols * single_dust_ground_img.rows); + ground_dust_ratio_msg.data = ground_dust_ratio_; + ground_dust_ratio_msg.stamp = now(); + ground_dust_ratio_pub_->publish(ground_dust_ratio_msg); pcl::toROSMsg(*pcl_input, output); output.header = input->header; } @@ -280,11 +411,29 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback( get_logger(), " Setting new angle_range to: [%f , %f].", angle_range_deg_[0], angle_range_deg_[1]); } - if (get_param(p, "buffering_frames", buffering_frames_)) { - RCLCPP_DEBUG(get_logger(), "Setting new buffering_frames to: %d.", buffering_frames_); + if (get_param(p, "blockage_buffering_frames", blockage_buffering_frames_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new blockage_buffering_frames_ to: %d.", blockage_buffering_frames_); + } + if (get_param(p, "blockage_buffering_interval", blockage_buffering_interval_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new blockage_buffering_interval_ to: %d.", + blockage_buffering_interval_); + } + if (get_param(p, "dust_kernel_size", dust_kernel_size_)) { + RCLCPP_DEBUG(get_logger(), "Setting new dust_kernel_size_ to: %d.", dust_kernel_size_); } - if (get_param(p, "buffering_interval", buffering_interval_)) { - RCLCPP_DEBUG(get_logger(), "Setting new buffering_interval to: %d.", buffering_interval_); + if (get_param(p, "dust_buffering_frames", dust_buffering_frames_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new dust_buffering_frames_ to: %d.", dust_buffering_frames_); + // note:NOT affects to actual variable. + // if you want change this param/variable, change the parameter called at launch this + // node(aip_launcher). + } + if (get_param(p, "dust_buffering_interval", dust_buffering_interval_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new dust_buffering_interval_ to: %d.", dust_buffering_interval_); + dust_buffering_frame_counter_ = 0; } rcl_interfaces::msg::SetParametersResult result; result.successful = true; From e32ed7252a4a8aa4ed171d8714ffa73fdbcab2fc Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 29 Sep 2023 13:01:10 +0900 Subject: [PATCH 40/97] refactor(localization_packages): remove unused in packages.xml files (#5171) Signed-off-by: yamato-ando Co-authored-by: yamato-ando --- localization/ekf_localizer/package.xml | 2 -- localization/gyro_odometer/package.xml | 2 -- .../ar_tag_based_localizer/package.xml | 2 -- .../landmark_based_localizer/landmark_tf_caster/package.xml | 2 -- localization/localization_error_monitor/package.xml | 2 +- localization/yabloc/yabloc_common/package.xml | 6 ------ localization/yabloc/yabloc_image_processing/package.xml | 4 ---- localization/yabloc/yabloc_particle_filter/package.xml | 2 -- localization/yabloc/yabloc_pose_initializer/package.xml | 4 ---- 9 files changed, 1 insertion(+), 25 deletions(-) diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 005bddd3eb22b..4101bae88b6e2 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -24,8 +24,6 @@ kalman_filter nav_msgs rclcpp - sensor_msgs - std_msgs std_srvs tf2 tf2_ros diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index cbcaadebddc2e..ad4a865a2014a 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -15,10 +15,8 @@ fmt geometry_msgs sensor_msgs - std_msgs tf2 tf2_geometry_msgs - tf2_ros tier4_autoware_utils rclcpp diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 2c35be181d2f0..216fa21bc951a 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -14,10 +14,8 @@ aruco cv_bridge diagnostic_msgs - geometry_msgs image_transport rclcpp - sensor_msgs tf2_eigen tf2_geometry_msgs tf2_ros diff --git a/localization/landmark_based_localizer/landmark_tf_caster/package.xml b/localization/landmark_based_localizer/landmark_tf_caster/package.xml index 64d81f744ce47..75f42b91f502a 100644 --- a/localization/landmark_based_localizer/landmark_tf_caster/package.xml +++ b/localization/landmark_based_localizer/landmark_tf_caster/package.xml @@ -12,9 +12,7 @@ autoware_cmake autoware_auto_mapping_msgs - geometry_msgs lanelet2_extension - libopencv-dev rclcpp tf2_eigen tf2_geometry_msgs diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 42f7c2de44ee9..4cc5599fd81a9 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -13,10 +13,10 @@ ament_cmake_auto autoware_cmake + eigen diagnostic_msgs diagnostic_updater - eigen nav_msgs tf2 tf2_geometry_msgs diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index 1c3ba4de307f6..f3f46952dec13 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -13,22 +13,16 @@ autoware_auto_mapping_msgs cv_bridge - geographiclib geometry_msgs lanelet2_core lanelet2_extension - lanelet2_io - libgoogle-glog-dev pcl_conversions - rcl_interfaces rclcpp - rclcpp_components sensor_msgs signal_processing sophus std_msgs tf2_ros - ublox_msgs visualization_msgs ament_lint_auto diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index 961ce574f7c7a..ffdcea25b4b6d 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -12,14 +12,10 @@ autoware_cmake cv_bridge - nav_msgs pcl_conversions rclcpp sensor_msgs - sophus std_msgs - tf2 - tf2_ros tier4_autoware_utils visualization_msgs yabloc_common diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index 6d11b12d9c922..aabb6009c2148 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -13,7 +13,6 @@ rosidl_default_generators geometry_msgs - libgoogle-glog-dev rclcpp sensor_msgs sophus @@ -22,7 +21,6 @@ tf2 tf2_geometry_msgs tf2_ros - tf2_sensor_msgs tier4_autoware_utils visualization_msgs yabloc_common diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index f6e8aa83bafc7..4e3ca1c1e61d1 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -15,11 +15,7 @@ geometry_msgs lanelet2_extension rclcpp - rclpy sensor_msgs - std_msgs - std_srvs - tf2 tier4_localization_msgs visualization_msgs yabloc_common From ec305031e8cfa50c4da58ebe6b4bde1e99bc5b3c Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 29 Sep 2023 15:55:21 +0900 Subject: [PATCH 41/97] refactor(behavior_path_planner): use updateData() (#5165) use updateData Signed-off-by: kyoichi-sugahara --- .../start_planner/start_planner_module.hpp | 6 ++- .../start_planner/start_planner_module.cpp | 47 ++++++++++++++----- 2 files changed, 39 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index d22322b63b080..9bbd1203ee6b0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -63,6 +63,9 @@ struct PullOutStatus bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects bool back_finished{false}; // if backward driving is not required, this is also set to true // todo: rename to clear variable name. + bool backward_driving_complete{ + false}; // after backward driving is complete, this is set to true (warning: this is set to + // false at next cycle after backward driving is complete) Pose pull_out_start_pose{}; PullOutStatus() {} @@ -87,12 +90,13 @@ class StartPlannerModule : public SceneModuleInterface bool isExecutionRequested() const override; bool isExecutionReady() const override; // TODO(someone): remove this, and use base class function - [[deprecated]] ModuleStatus updateState() override; + [[deprecated]] void updateCurrentState() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; void processOnEntry() override; void processOnExit() override; + void updateData() override; void setParameters(const std::shared_ptr & parameters) { diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index a9d8278523f45..819686c20ff54 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -96,6 +96,7 @@ void StartPlannerModule::onFreespacePlannerTimer() BehaviorModuleOutput StartPlannerModule::run() { + updateData(); if (!isActivated()) { return planWaitingApproval(); } @@ -120,6 +121,23 @@ void StartPlannerModule::processOnExit() debug_marker_.markers.clear(); } +void StartPlannerModule::updateData() +{ + if (isBackwardDrivingComplete()) { + updateStatusAfterBackwardDriving(); + } else { + status_.backward_driving_complete = false; + } + + const bool has_received_new_route = + !planner_data_->prev_route_id || + *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); + + if (has_received_new_route) { + status_ = PullOutStatus(); + } +} + bool StartPlannerModule::isExecutionRequested() const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; @@ -178,9 +196,16 @@ bool StartPlannerModule::isExecutionReady() const return true; } -ModuleStatus StartPlannerModule::updateState() +void StartPlannerModule::updateCurrentState() { - RCLCPP_DEBUG(getLogger(), "START_PLANNER updateState"); + RCLCPP_DEBUG(getLogger(), "START_PLANNER updateCurrentState"); + + const auto print = [this](const auto & from, const auto & to) { + RCLCPP_DEBUG(getLogger(), "[start_planner] Transit from %s to %s.", from.data(), to.data()); + }; + + const auto & from = current_state_; + // current_state_ = updateState(); if (isActivated() && !isWaitingApproval()) { current_state_ = ModuleStatus::RUNNING; @@ -188,16 +213,15 @@ ModuleStatus StartPlannerModule::updateState() current_state_ = ModuleStatus::IDLE; } + // TODO(someone): move to canTransitSuccessState if (hasFinishedPullOut()) { - return ModuleStatus::SUCCESS; + current_state_ = ModuleStatus::SUCCESS; } - - if (isBackwardDrivingComplete()) { - updateStatusAfterBackwardDriving(); - return ModuleStatus::SUCCESS; // for breaking loop + // TODO(someone): move to canTransitSuccessState + if (status_.backward_driving_complete) { + current_state_ = ModuleStatus::SUCCESS; // for breaking loop } - - return current_state_; + print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); } BehaviorModuleOutput StartPlannerModule::plan() @@ -614,10 +638,6 @@ void StartPlannerModule::updatePullOutStatus() !planner_data_->prev_route_id || *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); - if (has_received_new_route) { - status_ = PullOutStatus(); - } - // save pull out lanes which is generated using current pose before starting pull out // (before approval) status_.pull_out_lanes = start_planner_utils::getPullOutLanes( @@ -672,6 +692,7 @@ void StartPlannerModule::updatePullOutStatus() void StartPlannerModule::updateStatusAfterBackwardDriving() { status_.back_finished = true; + status_.backward_driving_complete = true; // request start_planner approval waitApproval(); // To enable approval of the forward path, the RTC status is removed. From ad69c2851b7b84e12c9f0c3b177fb6a9032bf284 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 29 Sep 2023 16:35:27 +0900 Subject: [PATCH 42/97] refactor(behavior path planner): simplify code by using lambda (#5124) simplify code by using lambda Signed-off-by: Daniel Sanchez --- .../src/behavior_path_planner_node.cpp | 61 +++++++------------ 1 file changed, 21 insertions(+), 40 deletions(-) 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 f52e018f2eef7..6e37bd0ec4b2c 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -134,43 +134,34 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const auto & p = planner_data_->parameters; planner_manager_ = std::make_shared(*this, p.verbose); - const auto register_and_create_publisher = [&](const auto & manager) { - const auto & module_name = manager->name(); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - module_name, create_publisher(path_candidate_name_space + module_name, 1)); - path_reference_publishers_.emplace( - module_name, create_publisher(path_reference_name_space + module_name, 1)); - }; + const auto register_and_create_publisher = + [&](const auto & manager, const bool create_publishers) { + const auto & module_name = manager->name(); + planner_manager_->registerSceneModuleManager(manager); + if (create_publishers) { + path_candidate_publishers_.emplace( + module_name, create_publisher(path_candidate_name_space + module_name, 1)); + path_reference_publishers_.emplace( + module_name, create_publisher(path_reference_name_space + module_name, 1)); + } + }; if (p.config_start_planner.enable_module) { auto manager = std::make_shared(this, "start_planner", p.config_start_planner); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "start_planner", create_publisher(path_candidate_name_space + "start_planner", 1)); - path_reference_publishers_.emplace( - "start_planner", create_publisher(path_reference_name_space + "start_planner", 1)); + register_and_create_publisher(manager, true); } if (p.config_goal_planner.enable_module) { auto manager = std::make_shared(this, "goal_planner", p.config_goal_planner); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "goal_planner", create_publisher(path_candidate_name_space + "goal_planner", 1)); - path_reference_publishers_.emplace( - "goal_planner", create_publisher(path_reference_name_space + "goal_planner", 1)); + register_and_create_publisher(manager, true); } if (p.config_side_shift.enable_module) { auto manager = std::make_shared(this, "side_shift", p.config_side_shift); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "side_shift", create_publisher(path_candidate_name_space + "side_shift", 1)); - path_reference_publishers_.emplace( - "side_shift", create_publisher(path_reference_name_space + "side_shift", 1)); + register_and_create_publisher(manager, true); } if (p.config_lane_change_left.enable_module) { @@ -178,7 +169,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_lane_change_left, route_handler::Direction::LEFT, LaneChangeModuleType::NORMAL); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_lane_change_right.enable_module) { @@ -186,7 +177,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_lane_change_right, route_handler::Direction::RIGHT, LaneChangeModuleType::NORMAL); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_ext_request_lane_change_right.enable_module) { @@ -194,7 +185,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_ext_request_lane_change_right, route_handler::Direction::RIGHT, LaneChangeModuleType::EXTERNAL_REQUEST); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_ext_request_lane_change_left.enable_module) { @@ -202,35 +193,25 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto manager = std::make_shared( this, module_topic, p.config_ext_request_lane_change_left, route_handler::Direction::LEFT, LaneChangeModuleType::EXTERNAL_REQUEST); - register_and_create_publisher(manager); + register_and_create_publisher(manager, true); } if (p.config_avoidance.enable_module) { auto manager = std::make_shared(this, "avoidance", p.config_avoidance); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "avoidance", create_publisher(path_candidate_name_space + "avoidance", 1)); - path_reference_publishers_.emplace( - "avoidance", create_publisher(path_reference_name_space + "avoidance", 1)); + register_and_create_publisher(manager, true); } if (p.config_avoidance_by_lc.enable_module) { auto manager = std::make_shared( this, "avoidance_by_lane_change", p.config_avoidance_by_lc); - planner_manager_->registerSceneModuleManager(manager); - path_candidate_publishers_.emplace( - "avoidance_by_lane_change", - create_publisher(path_candidate_name_space + "avoidance_by_lane_change", 1)); - path_reference_publishers_.emplace( - "avoidance_by_lane_change", - create_publisher(path_reference_name_space + "avoidance_by_lane_change", 1)); + register_and_create_publisher(manager, true); } if (p.config_dynamic_avoidance.enable_module) { auto manager = std::make_shared( this, "dynamic_avoidance", p.config_dynamic_avoidance); - planner_manager_->registerSceneModuleManager(manager); + register_and_create_publisher(manager, false); } } From e337576f8a22a015df9a21f21e4beb12bbdae2b7 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Fri, 29 Sep 2023 15:55:55 +0300 Subject: [PATCH 43/97] feat(predicted_path_checker): check predicted trajectory to avoid collisions planning can not handle (#2528) * feat(predicted_path_checker): check predicted trajectory to avoid collisions planning can not handle (#2528) Signed-off-by: Berkay Karaman * Added pkg to control.launch.py Signed-off-by: Berkay Karaman --------- Signed-off-by: Berkay Karaman --- control/predicted_path_checker/CMakeLists.txt | 37 ++ control/predicted_path_checker/README.md | 103 +++ .../config/predicted_path_checker.param.yaml | 22 + .../images/FlowChart.png | Bin 0 -> 64755 bytes .../images/Z_axis_filtering.png | Bin 0 -> 22632 bytes .../images/general-structure.png | Bin 0 -> 84656 bytes .../collision_checker.hpp | 128 ++++ .../predicted_path_checker/debug_marker.hpp | 92 +++ .../predicted_path_checker_node.hpp | 178 ++++++ .../include/predicted_path_checker/utils.hpp | 104 ++++ .../launch/predicted_path_checker.launch.xml | 21 + control/predicted_path_checker/package.xml | 47 ++ .../collision_checker.cpp | 231 +++++++ .../debug_marker.cpp | 329 ++++++++++ .../predicted_path_checker_node.cpp | 585 ++++++++++++++++++ .../src/predicted_path_checker_node/utils.cpp | 429 +++++++++++++ .../launch/control.launch.py | 32 + 17 files changed, 2338 insertions(+) create mode 100644 control/predicted_path_checker/CMakeLists.txt create mode 100644 control/predicted_path_checker/README.md create mode 100644 control/predicted_path_checker/config/predicted_path_checker.param.yaml create mode 100644 control/predicted_path_checker/images/FlowChart.png create mode 100644 control/predicted_path_checker/images/Z_axis_filtering.png create mode 100644 control/predicted_path_checker/images/general-structure.png create mode 100644 control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp create mode 100644 control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp create mode 100644 control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp create mode 100644 control/predicted_path_checker/include/predicted_path_checker/utils.hpp create mode 100755 control/predicted_path_checker/launch/predicted_path_checker.launch.xml create mode 100644 control/predicted_path_checker/package.xml create mode 100644 control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp create mode 100644 control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp create mode 100644 control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp create mode 100644 control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp diff --git a/control/predicted_path_checker/CMakeLists.txt b/control/predicted_path_checker/CMakeLists.txt new file mode 100644 index 0000000000000..cfbe95df99e74 --- /dev/null +++ b/control/predicted_path_checker/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.14) +project(predicted_path_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) + +include_directories( + include + SYSTEM + ${Eigen3_INCLUDE_DIRS} +) + +ament_auto_add_library(predicted_path_checker SHARED + src/predicted_path_checker_node/predicted_path_checker_node.cpp + src/predicted_path_checker_node/collision_checker.cpp + src/predicted_path_checker_node/utils.cpp + src/predicted_path_checker_node/debug_marker.cpp + +) + +rclcpp_components_register_node(predicted_path_checker + PLUGIN "autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode" + EXECUTABLE predicted_path_checker_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/predicted_path_checker/README.md b/control/predicted_path_checker/README.md new file mode 100644 index 0000000000000..24e4b91ef441b --- /dev/null +++ b/control/predicted_path_checker/README.md @@ -0,0 +1,103 @@ +# Predicted Path Checker + +## Purpose + +The Predicted Path Checker package is designed for autonomous vehicles to check the predicted path generated by control +modules. It handles potential collisions that the planning module might not be able to handle and that in the brake +distance. In case of collision in brake distance, the package will send a diagnostic message labeled "ERROR" to alert +the system to send emergency and in the case of collisions in outside reference trajectory, it sends pause request to +pause interface to make the vehicle stop. + +![general-structure.png](images%2Fgeneral-structure.png) + +## Algorithm + +The package algorithm evaluates the predicted trajectory against the reference trajectory and the predicted objects in +the environment. It checks for potential collisions and, if necessary, generates an appropriate response to avoid them ( +emergency or pause request). + +### Inner Algorithm + +![FlowChart.png](images%2FFlowChart.png) + +**cutTrajectory() ->** It cuts the predicted trajectory with input length. Length is calculated by multiplying the +velocity +of ego vehicle with "trajectory_check_time" parameter and "min_trajectory_length". + +**filterObstacles() ->** It filters the predicted objects in the environment. It filters the objects which are not in +front of the vehicle and far away from predicted trajectory. + +**checkTrajectoryForCollision() ->** It checks the predicted trajectory for collision with the predicted objects. It +calculates both polygon of trajectory points and predicted objects and checks intersection of both polygons. If there is +an intersection, it calculates the nearest collision point. It returns the nearest collision point of polygon and the +predicted object. It also checks predicted objects history which are intersect with the footprint before to avoid +unexpected behaviors. Predicted objects history stores the objects if it was detected below the "chattering_threshold" +seconds ago. + +If the "enable_z_axis_obstacle_filtering" parameter is set to true, it filters the predicted objects in the Z-axis by +using "z_axis_filtering_buffer". If the object does not intersect with the Z-axis, it is filtered out. + +![Z_axis_filtering.png](images%2FZ_axis_filtering.png) + +**calculateProjectedVelAndAcc() ->** It calculates the projected velocity and acceleration of the predicted object on +predicted trajectory's collision point's axes. + +**isInBrakeDistance() ->** It checks if the stop point is in brake distance. It gets relative velocity and +acceleration of ego vehicle with respect to the predicted object. It calculates the brake distance, if the point in +brake distance, it returns true. + +**isItDiscretePoint() ->** It checks if the stop point on predicted trajectory is discrete point or not. If it is not +discrete point, planning should handle the stop. + +**isThereStopPointOnRefTrajectory() ->** It checks if there is a stop point on reference trajectory. If there is a stop +point before the stop index, it returns true. Otherwise, it returns false, and node is going to call pause interface to +make the vehicle stop. + +## Inputs + +| Name | Type | Description | +| ------------------------------------- | ----------------------------------------------------- | --------------------------------------------------- | +| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/predicted_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment | +| `~/input/odometry` | `nav_msgs::msg::Odometry` | Odometry message of vehicle to get current velocity | +| `~/input/current_accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | Current acceleration | +| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused` | Current pause state of the vehicle | + +## Outputs + +| Name | Type | Description | +| ------------------------------------- | ---------------------------------------- | -------------------------------------- | +| `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization | +| `~/debug/virtual_wall` | `visualization_msgs::msg::MarkerArray` | Virtual wall marker for visualization | +| `/control/vehicle_cmd_gate/set_pause` | `tier4_control_msgs::srv::SetPause` | Pause service to make the vehicle stop | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticStatus` | Diagnostic status of vehicle | + +## Parameters + +### Node Parameters + +| Name | Type | Description | Default value | +| :---------------------------------- | :------- | :-------------------------------------------------------------------- | :------------ | +| `update_rate` | `double` | The update rate [Hz] | 10.0 | +| `delay_time` | `double` | he time delay considered for the emergency response [s] | 0.17 | +| `max_deceleration` | `double` | Max deceleration for ego vehicle to stop [m/s^2] | 1.5 | +| `resample_interval` | `double` | Interval for resampling trajectory [m] | 0.5 | +| `stop_margin` | `double` | The stopping margin [m] | 0.5 | +| `ego_nearest_dist_threshold` | `double` | The nearest distance threshold for ego vehicle [m] | 3.0 | +| `ego_nearest_yaw_threshold` | `double` | The nearest yaw threshold for ego vehicle [rad] | 1.046 | +| `min_trajectory_check_length` | `double` | The minimum trajectory check length in meters [m] | 1.5 | +| `trajectory_check_time` | `double` | The trajectory check time in seconds. [s] | 3.0 | +| `distinct_point_distance_threshold` | `double` | The distinct point distance threshold [m] | 0.3 | +| `distinct_point_yaw_threshold` | `double` | The distinct point yaw threshold [deg] | 5.0 | +| `filtering_distance_threshold` | `double` | It ignores the objects if distance is higher than this [m] | 1.5 | +| `use_object_prediction` | `bool` | If true, node predicts current pose of the objects wrt delta time [-] | true | + +### Collision Checker Parameters + +| Name | Type | Description | Default value | +| :--------------------------------- | :------- | :---------------------------------------------------------------- | :------------ | +| `width_margin` | `double` | The width margin for collision checking [Hz] | 0.2 | +| `chattering_threshold` | `double` | The chattering threshold for collision detection [s] | 0.2 | +| `z_axis_filtering_buffer` | `double` | The Z-axis filtering buffer [m] | 0.3 | +| `enable_z_axis_obstacle_filtering` | `bool` | A boolean flag indicating if Z-axis obstacle filtering is enabled | false | diff --git a/control/predicted_path_checker/config/predicted_path_checker.param.yaml b/control/predicted_path_checker/config/predicted_path_checker.param.yaml new file mode 100644 index 0000000000000..780d86b5e9804 --- /dev/null +++ b/control/predicted_path_checker/config/predicted_path_checker.param.yaml @@ -0,0 +1,22 @@ +/**: + ros__parameters: + # Node + update_rate: 10.0 + delay_time: 0.17 + max_deceleration: 1.5 + resample_interval: 0.5 + stop_margin: 0.5 # [m] + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] + min_trajectory_check_length: 1.5 # [m] + trajectory_check_time: 3.0 + distinct_point_distance_threshold: 0.3 + distinct_point_yaw_threshold: 5.0 # [deg] + filtering_distance_threshold: 1.5 # [m] + use_object_prediction: true + + collision_checker_params: + width_margin: 0.2 + chattering_threshold: 0.2 + z_axis_filtering_buffer: 0.3 + enable_z_axis_obstacle_filtering: false diff --git a/control/predicted_path_checker/images/FlowChart.png b/control/predicted_path_checker/images/FlowChart.png new file mode 100644 index 0000000000000000000000000000000000000000..15b32417d2a73a0783c0c36e5e244f51459b2ca1 GIT binary patch literal 64755 zcmeFYc{tSX|37NYNM-CT#3+$<#xOHdmN7HN7|dA4Ap5?Lb?j0jBxykuN`;DqP%2qM zQc)?Atc66DtXY5e)cgH8-}C#Pb6wv*&ULQy$GNUCGhTDQp09g(-uL}{J|2%dg<@*B zbNk-y92^`wjfgmN4h}9X_;ZD916PKu<1TS<2!w^=>7o7+9^QWL9CAp5zu)B4;a-6u zp>jx^oVvO*gQ4o??d%!s>>r{U;2sJtf#3ZD-Ml@#-QE6{QHQG|Rp1&b>S#-Kq#RNo zuE~B=S4C)O+5Ro>?CBoxk3o~bC~rSMXE}9(rYanmQlKLca!4%rX5|tV5E}OPJJ~xR zECO7Mz@Qva8h@_^hXb<+xRxsXF!;sL-PJb`SW$-~;NXP-mk8d$?jC{QHUa{n&Nj}j zs(Yxj=ih32n7Z0hND-lQ59cVV5z-7v_hz>clnHka4)G2Q_@^}pRfH<$?+>Ao4EMiR z-Q2^yUESGNk$BKk|8xns*ZZGNFwiu|hG-xIan35nZc(=GrlDbxe~kn<`~PEv0{TG( z)Rb)mO^XP%LYbfl6b(dxO90X{Dh%TnZtLT$MMt26g3t&W0!h(Akc_O1;3RWv0~Zeu zx(mYC6Kif{9!fI^iSqIEF|)Bif|?SHT}hE{Bs1_W*vpNGx9}wSA zRFJVL!{6VCfd<`-rIQWKNI^ji6q$&}X&P8W`nc;`fLm=0Nhag~s*kNcj){mMT6lW< zJCkha>LHfMKr2L)iI+a!mV^#>^EG9H(m}pa8h8@c)zBA5#s`J_TA68Cn+9738zC_! zaEma%C{0_mhl`I1!i?(Xivo`&Q7rVS;M2y|myWiAQ!zNIzZb@o0C%N^L?ZmaFTw1$ z0CPB;ev6|2sQKd#{|2;iLQo7vOx&Nm52`y3?)P`eS9>r{vm!Imf;~ZTO)m> zF*4K(co2c<7Dx@pnHb_VyopS*W)v+_3uS@RC#f6wvuj1dc|`gX&3uVbWX&*ZGt*F# zm$@6<-_+BKL@@PcG6)z8B9@9Ldq<+_Q66CtT9|MRlsie&O2gCN%E*~a@F(gUlJwzL z6nB3syagVZM4^xvmoT(}KD%cuDXvtkhbt2}m(F~T;?+uxs{WfEpa*7ORa+7Jw=7Qt{6bCQoS3We~*p7X#Y>PZ}H(;swVC(kMm<6Eo*1FB77T34<9L6-<*(&Jiwv_NntQ2h_=OuGO{vyQ zteGdA8bRWbGIc~B3(>10{u-uDN>lV zjkURnjdhd()|AAcX{aNxeo^MO&fX-rg+~~|SlvGm5r7J!hod4igEZlwCkZajhVJHA z4QKX1;Y{@1hz5q%W&uo>aE6OBBGALfoyv?LnERU`G^k!y{#MSOKE~mmzFsEb?onuG zf+m3)?BZ+fZ$KijC!qx%LGd*s6J3}QI4yl!3N6^h$0v&E6Nz#UL`NACi9`wkZDNI? z;L#BtRAht~mgs|vU|12XXb5Y35Z0Xrw~F#NHws44X|@*D=Eg*4b#j=Mm8+JCz8Rga zsevQv8(Bs9p)Jjvtr$etAb(GDf`tds0=NQ}X&mHj=tOcm z@~6@?2yWgMT2^pFZ@9IVahQz>#>Ll%J(5-!e}AMQ3FU8Vh_xahsFWyh1DfJSLE;%` zB)eZDDFNVvVjF30<`#?zrG;Qz-2#HaC5>QpjJK5s(kqI+6lvLpdKeJ`^^L>bBLZ-K zI1X_%$HF+M2RHrU#f!O*hN zFlA3ZD=(53#Wm7TJ)CSqurcy9#9-Y>SW|y%8-%U735i5E^h6t=!cm&e)^6%FQ&UeX zPeX(y!`2UOjPP*L_d}^0hX+Ll1=C6XzUCTH41ZH1+LXw&h8t**;FJIohKfT*1lVZ# zXnKJi3rhxnUj7y#)?p@ie^aI(0dE-Uf&oneFZT$#RyYF8JzKOV_}4E0Lkrf7Fc10r zt1T03kXV}#G9@&E;u>mYt~|Q2(m#$dYQUW5a_6Yz#vo@ zff*TOO7pNF+t35F&}8cfngJmwI0%74TZLJB)5EkP2$nvY{$TCF`g%r~__+p#s=HwV zBk4ZomLWdY*1opJL3&E}vOC4kKhV;fPWNSc5~EzhtVtT| zp4PBIhD9Ne1nUT-p;n-cf3St0flWZLk9h>njZOqnK(GnJ)ZEpEX6j+3q9_FP5b`pYlaFmOtxi!j` z;0)U3;}YfSLUjv_(6B<8qixWkI3GIP24lj2v;7DZw+Z$ngoCf48ny=5D4Z+a1%oj( zjdI2T*Ctq7`n$rJQChyJ2=^d(P$UiMu4QeAMn`y=22d?DeAG3Zspc4Gw56{)isoZr zV{72X2-YIvNu&UH-g7~$$gN81{CScSTn61<`?h|sVgq?v61#?q3#?r0c*AQ622 z8z=q=8o~enMA%5Y1ILFD4h|U(Bb>fvgu_%GU$!W|hhQ9h{yIchN0bx6L3nMmBOA_V zX=(ZTIwbiFC-OSPLkQno>eP@Mg2m&Tc@$)}@yL08t1tdCCf%LapVL3xo1gPAzxeo< zpJj}@k0*cHJ-pi;{4Oajt0nWD=S|7>3k_CT=%1xBT(W;&Hh8CdJ2GkpGsRaIyD&MN z-FrGm?v|7Vch?AG_sHMhf!E`~>G3ql$iPImwH!4yz$&EgbF`A+6oMv5f{Pj4Y4m)n z^H4>z)pk2}N!;~Po`bA*61;QfGPHeu*YzK%Cm_jsSt945?Z37u&x{yDGMNGj@j_6x ziApHARp40v7_mx^IMGJkRlHCAKAqA0yPumLupZ#vF_U>x6x@-B;y4v(Tl(y)7VqCvX5W8K zd-}Vn_Rp%VFVPF{44-^Gr`;hH`R#&tru?Io>!)@Ft392?-kDbUafrX*EkzObx4VL6 zpuRBH?rh!oXD`&Fm;9JNt9u^5kdFWICL8hmUUm1DMC`=-!ZM$acMpFRU{Sh?O!}FF zvEwy^acj4h<7ONA9Bw@YjX7N0nfQ_gm4yt4{CX+>B&0*@#m74qFX~o*l$6;$4459f z8W;|&##CUxPu{N&2Y=6e-(J_Z9pzUoJ6>C9_bevz&U7Yp`_8Mv!hiZPnLnXa2s?WP z<$tA*@qA~0VEx7zuaK@hIeX}ertet5fe;_lNh@<{&*E8weiz zaBO|{`E*TRK*%XR!6oe3s@3J0nJtrE(_@R4%PkU)kHUX`t_|z4@3AY}ZpSj!-3YZV z!16LXk1dzJivGelr8d)^sZXWOJH&4Q^id%+T$^Gk33gqho>J;md(3_ zfMMi&OB>6BGjUrROy=6phiip>X1W_y zeX+~^_Jhx2`F&qUFz+*5)&p9Bu#B(PkZp&N@mPtb!g zT?pQ=$$Jks4I!{^#P}qs@D(%bp+Ga8-?#Rm{Id5UJzgvD*b(uO0`9JwtqgoE3K1@S>ZWFk1 z$mGhST~bFLS(s|I2$Y1LWKoV(h2DcCvzLM|kY-zmbM0-~y(^wIE-l+1&b97)0-n*8 zx=R_gW14;fvd^zPL4qThyD7&re*!r3D4v5RS*pZS^zO&Ul5t*B8J5oK_XC0aIhwv% zCH7Bh>UT}HIVVYtsT^(muv`493A+D-ebhwZ=3ZRSb^TLclF!MF-?c1SMZ-kZoSrFGSt_E#>>oqqyc? zSd!$(1HEk6EhsAiYWs*$AfY#VbR_uzEyVQUDH5d6nB2o&=T1PHQ}jv`DL2w7&R&oL z>A00fJ0sdX+tIwcLWjxPyqvHm4KO^hV=6}aULWsfJ{0H0LJ%Ko{O#1-wkP4FVYxXu zkFid?oQQ*&#V2QsviM z`4Q^{mZ=avNyZ7^cQ;!0zI4)@?0rz&>6)rivtqVCMF(qkY%$~EiD72d*5+DtV_TKe z5q+$Jl2nLCYl$KmMDp;;3=`~{(26SjY+)FYv`Juudy+q)b1>9Qx&!S$P~ zC4K%6``15+DOJrT?m@V)9)2F8#QYw;9IZsWkq&)^N)}j8D$f`aU?EZ1yVmy|&P^a= zqGvgQyIfwk6mIgmY#$zEeLN`9akb+Ia&vQYZ6=z}vy_&og3Z|JD`@)pTSC}Gb&tdR zRx9kvgz|dm>~EM9OX@cT)xIB6@LW*$i(T%lKojTM=w-)n`TU3*&lC1aA6P$?AG>6u zR+_#4V0wdLvOo82UF?~)Y;#w|opC8y4=j(AuqcWdp=p07$Z`H}XQ5Znf%y`=E0a!( zkG|hXefk{9d2Qcj8ZTcXGVF(?ZeFGWt9(jEPiKABp#G?o=TzPDK;H%!1ojlw@cE#i z7J7P@)DLkrhpDcn%2T5$8WkLA(z*iz8U8iBt!tyA)#*@|_71n6L2br1@=B~!2~o&mm3!|w z*2g($496$Vqe|MiQf==_(Xa|u=8sMP`qVid7bVsRL}{11;^gTw&ON~4{BDrU{{DCg zY~<=jPtHaW?e4+4oa5J=Qi(_gnM!9*WyqXzvA z8s5u-s6|}W{&@BY*7;S|-!4b7!;`V@RQ+B#mdjg+;>o|4@9+~LnS1jBSnQ$+|D&QA zcw+3II0h%v2kv%{7RUOa#1Cqw1}!}+$<`=zon@ki)d?WvYB-Yv($xXnGLC=&jED<W0mg|tCZQF|E9S1JG;1CA#l}v;uL>!RJa3Eh zw3z`Qw=nFkiGOlMvlnw5W(Rqlf2^+#L|y?y4GHn!iN)!Qy?m90Q;NkPL>uFGBSUh; zZok^4J;iusVRkI>Km|5SWM!!@us?3?C%Q%XkV@rKMPJc+5P|B}&39@#As2{# z7b=k*n6JGy4?LtS&b2^PCPD1J{E}~f#L#UJ!(ys5GAX}Byt<0s1P;c`?_7&9QQmrw zb2w>b?GWkyEl1(hFBs#amGy<7qSl|k?zm}vyCQTQ#f$otA3Z%0^MPuZX&{#Ap236a z5A7PP-`b4suz0n!aS=7(_u$Py4kpOurb6W5pRYwCrYc^1sU6SP9=Tt?`Qzg6FUb6x zwZzfSeqpV9T2Th`C7IAO*Td9hS)0rCTd~@4>nkRsBBJ_WeZN>2wRi>X_4axI?d*lo z;~AA6gN0_-bPPaR;`pSLe_}I2Xl1@r7i2L0%RfKdv9NiUGYR&>Bs)4*;Y#b9WT<+4 z>b1C);UtfDmil+HEq?)(KSVfAlG2VtUn+ckyTlcAN8iwr_n+-SaHNcYC-qU*?G$6ARZ zKVEpx8KrxQy$)%W`fVSd%zJUXdxEmouK1{wz8XZZqhb|qJ_=Do@)vyZ?K)mc-M>>t z7+d=(LEuM*;RfdBJq1_>^-JyL92RA|+FP|-A$=Q^zu@>(gE00&&0QAdRz!$y5V$w+ z>+#zKEQ)H?jUVUP_ob=)_xo&E!4=%EWJSG8ND%xP^s)%R3pP+4iF-+qQLNJ4U6CKs zk)J;S6rv?BI>!hy>Tq?;CGD3N#|6BCgm$?yc%V-=e*wa^^^t(;qXxEL{z(bP^O*S$ zcJAvxT%0>HMTU9WO+2}9!0<6AHzwjp-7H9JF@2Vub!*d40;a#z29(^Z2?ilwPM`1n zx)^Nu+uPA#X7&TG%MNZz*IB*2HhwMs3%`8s`CrFUB=&Roj<7P?%zluY8ImU=z=L5}<_Kjzm<&G#K2$8Rc} z0A2a&gLGG&)+Fn^)Q#_^M_bIi7bU>`Tf0XFnc?O^yXT`|ZI5%iLuW#Gct77SM7=tgLE$E5iyB z3($mf`eAM7Vbh?W7r0H2V9O3B3uEPk6#EBLf1qoSmC}A$YL1)NaFf7}HbpvD6* z**@O`fqR+$scnUd&{HPV--nD#Z~bCOcdCyRJ3l|Y`Fr$0^Es`OaFnQw({I~UB@@l^67BJ?|j`9g~M=t0L3-)j8##U4jbrC4;x7&%bC@uaJK z$`dec2c3WRg6zB>GT{`@P)BPe|LZ+tevZJ(k` zi+*mGJXC4NUWi|F$3;2gFPc5azm_D5g*2Yz4FI#@w^{x0OrqmLcyh&z#qrJ|;8NG* zembUz7r1RCAH`gr{PjwWIX~XHjOFEweT{hWXgoi`k-lQ|N(+o6*3J4tS4na{yc%;3F-H(4L^mc`aFO7G8IpWDg@0FyT8;X1MXO3;}+Fd`p+f(f;mcNVW; zQ$H@1@#z{s25!m@zB{^a0jB+>7wm{_Qj7-9fcA{TTNM!A#pKoB#A=j4Y_yzCS(VR)K5iHgp8gZocbiR#R2b6piF$Yb1D8) z7SD<{wqm$0|%cC{AfPaIrKS5gSVpOIkZxB4fbnXiKYjPz5qB(lDYQ}IXlxSarDq*_Z7*8Itd9?0 z+sUH3M2@}A`Fc$^KT*-@Zp2*DV$vo_delabB9yJ;5Lmaku1C#vFRA+J_?qK=-+Nq& zYUQH<@2>QBv4;g<$}!0wT+@$rg9xfaAh&txW;)1{dpBK$1%5a3Nl&e@r0#rO`VwNM zb^=NP7;RC!^!$a(3apEZ(WO-Vc*SX{H++ij4Z_V&&I%g5S39an%Y|;A|2=e|;PyDC zLZ{`JG=OT0x6T`Wnxl4@4Q#~ti9}e>Z)MnE6&{#bnhF#QKTldy`*Qsz41)UP#Qj2G zuacmkI5&iJjqg(Z0m62^BP!kd%f33l_=UaRh5QH)pQ1BYCyJv(;M_cF3o-|Hd6640 zpqFQz=C31(a4V~ugq>*LZe{ByOZA>{I=|WuY^F*M%Cew>{Eqz~?08b*Q1`h0^!=b` z`&X`Pu6#|8?JYOwO;BTOvvP3L*K1DOjc5`mf@6iS3Q)a1`f8YRN7bXa-I4fQf*TeR z)Hsw7aR|THcNo`i&d(y2-}LZ{UzHiMZWfi|Iz6GbP0QgsH{Y%WtP&M6H`1usvXS8p zAo8WSec_*q>yspmhoR)OkN50)uAUaqK5|J%5ISKVSN1?;c;m99nri8;#szNg`Oz1H zTnOv#fJ_{IWk#~)RhQ30TNJp>=ugAGxc~Luq zTw>2e>y&Rp`nN_1@f?>ZpK_&+em&_uHUgko#dN*Zo^Msqrq^u^(kmNq%Juf&@AaH! zU3SslIfV_UiqE^dI9=hWV406|eiPuG6yOvPI=PM7czGVx=l8XK7Quf>p4)QdMC%FG zcm3o!xzlpr0SLc^0OsCU5p_-NJk0l9^$-z=Vnw zw6Dh}?UFjm=TKB7;n=;x|JD#vh1_(W0CNFu@qp#F0mgrLUskE~@XcoANtpw0<9APG z?>DXjAmpo5ZAh~G>d{yX7>wBUmAQcz$)3sHLm>X^9&D}J9*Yc8yrQ4+?B)EPaE0A^ z1p4C_Hl{0Yq+c`S`wt;*o)O5ZJ)O7>e*p!}B^BDVXTke1e|Z4=vMD!A^4JIQL~p^RBS% zT(lVF`)9?T#+T=cOB`g~h^!1Yhs-q$ok-HVehF;Yx8My!2F*Qw&M;OykHxkX={c)* z>$jlNV_Rvroo%_}A^VkM&Sr~v4$1gS>IMz9mUf0=VtzHili_W{9I1i3x(`3|f*#m$ zBE||~(%R7WMf;Sfdvjw(3?fH88LoFx)$w@DvDrnFy%~u|ehm$`of#C|O1DaF6e_3z zc=3HiTBAXmOe^w`Q$Ei;f_BdO0GZHkf8-pyQEJb*)Ooc(#ATv_&ZZTWoFkdbvE`bC$~ZNc zX!kznH=)6%kcu7od>QxodA4ywtvvD5Z)llf6r?WQp1U<-H z*qbk^+xO2kN;2VLt#xE>cx>X^p~jXQ%@SfBRy;c#dy++8x9*5q->)Pp$lWcy&9LZI zgx4~ldNgTi!{5_F4mjoiiskOnzy zEEOC*E8!(Bh0Q{h>;JK}zt_r_3yA~M^B zmd1P1>%~cZkBA&(Dk`CkMX72rRavYIxSegQig!#b6dsf>sylgN_W9it2D&B9+2VR+ zgqR3k!Gl8|TH)iD&=N)cmN%?X{UG%4iOb{jjeJOH!#fMHAgj{|*#3|RDdaeORn6H( zeI&_y&l0P#y(I%jhb0049cQ63_EQJzT%qcJWaZvmWy^aJkG{*6`<1uTX+jvfg}~Y- z{2rmV+_FMAO=qta(V3&S=U1G{-F0Mk8H;kI{MF>|A=eYVoTaN(#S&`&Y5`u;c{XV+ zV!DZOxgI~mv;c

$>#N+PgcG$AAFOIsWPt+ZhX2D?( zQVw^$Aoqw>@7-3=9PCwOS#Eo%Oh({= znXez+t9Ouvoj}Zy53(_beE{?P{}$%3M+h5U)@@dk>BmRk0l-NSFl+zUR@Km<^ zXZ?bE^Bm_`lwIgb7DXfNsu0Xgo=-OdkXv~XAmhSW2o%}|{DripM1SR#a^%Wqy(uWn zd%iTl;uqQL5h7=~v?Uw?qy_d51Nu*Q<{w+DIy!U!_Fw8}^RCaYb~Nk$r&?^<%{!;f zRRq~sTop($Cl8<+aw+?NC*_R;IAFPXcKCU^4#DJ4{&PF__YEq`9Zqa@RML= zLQ$oI`@UQcMYYjO&Ro3ySMLOAKd)A*pHkoUq3Fet!0L}Q<|qEH>h}AO+qIOrzP?Cj z9@>4#1OTI7%io_hI0C-qEwn4$ulfY=xDkqEX2DsWBjHnz8diVUpj4I^doHzS^%b71 zEG#5BnpOu*^om7#wz8nL=)i%e#y>w-^DRtt700bD{j>)ti9y-9i>VhI)_>3EY2`kC zchj(wongl39*qosqA;&iT-DSMVZM<)6YydQ; zu6{#Xy6Aau(T1gvkb)rBD2&y9Ge75bh}|o{+bzedvfM7-{PkB~0ZnKF3{he_zzbuF z{)6Dl!T>t0LjxR4jw1da+&mdjRg@LF9IMIm?!P#;j4&JMcLjotYj^(T6B3AQcsF)o=A2`2XN2~n5QiK1;oAiHS(5e6V#XBBzVyTIVZ?%@5B&l@llvgU0 z{f8Hmg^B~3g+a{se7W#&rLN_&O1^v0`IZvp;4yk4qmuexh!9~wgrtuRW?4$5J`iRJ zZpB|}F>ZQA_%F-_9;|xG7d!I2jrv_Ge6SZw+;(mkEI0pmyvy0+Z5(QvV3llXO_%f% zx(4I@4>huzZT=xu_#U=$TjDF)oqzZcz3*V3z#9H2-~KT}Qz54OKaSFL2sjFgI&NBZ zFv+HL|Nl0KFIg0oS8lqg`^4zzrq>qUDSA{Mdf3AcBje(_BC`KHw2ddB346Ngr{Nb@ju)TG4q53 z_MY9t&9OVh`JbO!mC)`C6i?!04)Bn7yom+6o6dZ7_x786UQ%+{qLdSmZ(^LRw?P2s zkc{dwL4WKhxl&STqu)@P5m3Lew6+dt6|c);yfoNf9;#}x8X)-#eIw@ijIEKF_I z0r8$2h_2MV^p|l4lorA19$17{Fl z>^T59aKwl9+f~40NXDkie+y^e0GWOeyBb}~CX1I3Q8?L@xr`_9#ZkYmG1HcVtl(l5 zD-m|9lJ^0_kE)Z~=cU2w@wL|8d z>^_w71yfAi7JQ5oKk_`ZR zR=_0_)L|>nPeAt9|3`2LY-UV5J`TN8Vq5L=-d6|s+NTQR#V%yfAs1ot2P1Y?~R zl%D4Ourl|x|E2u3sl;}z@mvePCOm(^9gE_}?ys`6v{~qhIw1BcTpQfl9PhRZtXmLp zVDOzxt-%0@Gv@c!?{Vp#YF}#*E{l9j`|AqsFv<<+e^h*cS5WDx^0r_CF9W) z|50rI1ejXD!uNLpK*dJ4f7pce$_wkB<34<9)7mk1^wCU{fLiD!d9$VN3xHUtWvd3| zY*|@AUl_IUefVE3Vtnj)N8Y6@fk_BA^VGJTUjXs*9W@ve!UT}_6EIxQFVsu%{J4kz z77tV}5f_#~Wu8p;KZ!08{0()8-w5j|Hk0nWiGeLF55^Z~Y_cg5Al#YO*<5jbI8k)9 z%!9{}B}5bFX$5fG#B}{u+}5&)+0l1w-elk|EdzS8XQqB@S_KQ{!%U0QOrVYUBCo^<%@KO>L~1=f+UAK&(B`g~*SCG#UjKtkM^tJO6Z zz3nOBx{|+6+Jiu5o#SO{~tvhs2Af>FIg#VO%_3?qH*<~*w7g7=fEa^`G`rKU|I5-dsbgR3R zb^^6(g3J}9=ehe`Y|ZFh%R3`%HY!n~sQ?4Z@L45Gn4@^;m4KTO#FP?}4>SvQ*+Oi2 ze2lQSL5LZ5Chd+HkK|&*UI_@SS*R(iO6c(i!3*=V&jq6g=ucYDNlhkx6*80@X46W{ zH<~?|B0cYYD}0>qQd`o(&BPC~7KDLVSlFxYQP5Xo-w(Gd>S|Ri%e_}UDpc-l7q{a= ziOfBiNY0!ed$ZQI(88hgokbR}p~IT1E`>2>v`^Ayj)EO?;No+Q9V2{7+zFgBED=)% zm`K&>bP1qCo8D@*goyJg%hIS@fm}Cu-ZkjGx^AMgqdYRttk`-n>{NgS&3Vt;h$LmnY$1^umpp%K=L zEoTM8T_<6X(&dx@#X8CI%+xJATthWw9~s#S0bFa8{sNoQsh`_Ap4x_=m1&2f^!0GV zC9HB|&}kQMypzUV{{ry?lt?jvk7uIhZ~8SD!f3&~LJBGgy$?Lsdb1L^`8J&&Ur&EC zeU$RK>V4gPI*j$mX*G2sy(xinBir@d9Je-%k?%3oy!!SAK2EgERe1Rr5^^_c+54HzzqvVdTff0pYAmVp#(vM)n8e*O*mn8aju97zd^r7u9!=|d+^d{5-X zURtZTA_R?~a|2e!tf-0Wl|+2~@3E`>((&uV#WI(8fuvE|*Ite5op5=2hw9j4Mrtj` zhchVo!u)g|zvKCV(_;NaCRekB5SFFTrpOuqtFpQ40d<@cqh&AaK6&JBg&w3jp#L+% zJ(0-IwJA#$LwtfAgevnW<5mh}9Dp`iHE`GoYUt%uN8AC{b+Gkmt;x=v-Ltl*)?p?bmhSNlOKKPNT?tAD=Zw!q3`q=4n0Gb zQnxZx2=Gzw@YPdZ1+h>U9%W*(@OtW*2atCtNGV%*XxGE0 z3a95})!xbS@Zpp=d_2V~)0OoSKn&&@tEV6t zd&l!=!0cQD=Wb3`Lj#w2 z24D|b0(t6Xl%dJ{a9kriLCsF9AS8VZ){Q77XwLzclcV!1LeABZz^A+ErVhDKm!Xyy zJcbisos4)T-0^0|TVR#!u3}r&Ciwo_^J6O|GJ0neB|1JpVA~imHqa9ZUTZ)hDrG^W z8`8>-|CWZ~o+Iq5VtxlNgY756nR*dQl8=>qTe0cH?-~WIcxkzNata^*64EK$ptSlVO6$D~m{U$Y==+v{A1qkNkN z8L4;Bte~Jx-bx65WgF$a?D<2S(J$hBXO@62C*u&tK3|;EL+7>Uirv0Jl-Xpf`SIg7 zpo;h_6^{!`_bFx-szZ3!!=dKew}cyXlQYBUJA=Ys1ePAu0io(KS*SEuP1CsigK2cC zS&HSw<8n@_nihUVo+e3OA&OVKu#ghSDb+YrvYfl3)4fLXrplmF%SOd&@~HH9Ou54_ zCVf!I!i6qi{P{Xi)O4uatsI4MMz`E+f9_M#o^yy&4xbqOdQf(UmI^By)s&DuaWuCY zDMsf_oY@ibJ)w(H^sD1^41@l{Q*p({JV%a2lD$=*sz+dl%%aRqk3+DGdjI=ZB!E2a zmMV;^f-O@k8Eflr0)cu!-(3%HkRXyovQ;G#yudB^SJrnVfTq*j8k)exj>NaiLY2U0 zh0nP!DrbR`r{Kc`Wsw4;}La(Y+daH8px!O}1xZ5tfe4lQ@yh87ntgk8U+KiaDZ5Iw~ z2vKrh5^MhmRZ=xQUhj^N(0HLyk)pQf^xPKpz=VAN5f1b5go?vS*Yi!^8pdk;hJ|xe z_uP6@VHr*@(^bwDkzLWYgE3g*SH~(RbzcyIwsY)T2quO@Z_69P=GJ7&kL}>|4Y|T` zbv5`TXECMReJAW==*5rrfCO_WsH(gInngoj+kP#Y$6OejyQ^W24B{q+WU2oPxfK)3 zhXYZ*%Rsap#Yug?-z7(H8BowGt}KOp#exiBPEV^t0_(9r8Zl=m$&N_`becK2TxW+G zRS|U~?VWKxxj`@ad7b%KZ`p@sS`k($kuoeG3LKppI=q_9;|J}???pUjWK`= ze6O1FcA9W`^A~V%x^SoRcoOCI_soOCI!8Lf+)j+{mfy8BcGh^CqcT*op5^7!QF#jzulZdTP1w?ooGZ+e(wL->L z4qQ8ll)Yp47@K@_xUu)Tq`wutVV0?E`0W&?$p<$8vE9XCwj&vLZs4T5(FEfjixTkc zpk>K<;L(XYEMKPR^+Ga8kf8HR-o<$}tF!(*d={i!zQ`6a>qqRT>c)U2f5+b72c4g3Y1A=vln!1%9p-&GIl$VlG_%wxn zC)-?qi64d9Mtm(e?%;r#6yhX4`(_11oK=ZX*~wd%OoLxYhMbj%PUL<4 zzR-B1;4{RcSvgClP@?Z zOy~$ap!7&%LSF*(3c~fY@0Ujv5?tV*NH_osmb$P0IRKP4lo1`{U9Q6ev>pkLyAn9p zK}xu2fm(Mf--U(uKX>i}VMxWpw^iVv4RQFQ0t-{MuVh=d5?7Ye`{ErCNo57P8PtLu z(Yp`Nq+q>zE8PcNx~}VUzbN=jmWR1Ph3{;Kt>4YQYp|p6NG3fa9q?iqi47li)8H{z zpdOGspq=Vrhn|}=kOH9dbggax2gqtAawX5F&AJ>(=k66sP2|okcY=8slZtO`mffki z2$`6PGyonSc*R!i%`UD&VJvyhsj!0Et|xo%^OJAuh1%4D+3;WCunXHT7QU1CReP?UsYOofCUIN28l)SAIyiiMk}Lb0j7 z6(})@@*+yn!@;;dZ8Q1pFlEZOl*L;hC=Ost2gD06vYW%cF`Udfo~gp}Pm()2A6D*6b^UF5NT>}7 zmN3{hopnE`CS9|90aSL!4duR8>R!Vy`NyK8Do(f}xOH@NRtn%^)j(RjR`|1pA10YN zcWI*U;e82_WYu*aC;_6l?yp+E9<26=;o~9W>Ga|VtDbrT+SHg6{9q)_w;a%Wtp%Uy z*^HN85Z;NPp=FRO|I9ltbcDPNg4?^$V;J0ndmQHmz6eS;L(Z0W zIPDb7z3zPa+OCtlN}}Qi)^E`(64Rkwapd2_ko+=y@@6y$^`C&b*)*>PmY|&={D4_mXU1zv`BH zi(P0A5L}0{_oN-`!A zg-PjNrFL1?r#-rHPni*|@^=78GtiWoM@4Ufb7o5Pf)JZ0nsa}!QkxvsQ0 zEyCi9yjIYoFt{AcVi|b*WbtJAeuO_uk4O##2N<8_LR5=BL($2{+Hrf%>25}BZDpip z2CNLplddMfF$d)pe18arz$0v3=Ld*IQ(JoD?iW6^D~QGBq4dn6TXH?l-|lIQ=*wKf zLNc|wMW@3tcMm7)=`PUEppRJ3BY+q-VSOHi*;Sq?g&KMhii_mH}fFx9%^v~Nk zuT?AMXX+W^hqsBf`(?e}ovt+iz3bosDX$8f`cx6$*Pc}Er*{61$F3P)E#D)`$d$}g z$SwAw2hQZxeBfy}hVb&11#Ep9lub%*OF2^K;pcYwf!M^ciTj$F~*i^~2(vQxO zgHk+y4tcnI!PHt-{SwF4Hmi4qIfsd?oqPByw6*=uaVvE_V3_-SX5xIGIDK|2MID!~ zpOKjE{TZA$^4~3H4DeV{u(1?4S<1?>A=VU1%oYee25YD<0npkGPEX&S+L4Lf4&Y+* z6QW(l2U#wk;fh(na~0T}L*s26iG8X`xL{=y#9Pn&Z(an5az-8qxq9~~CW->C2@vQ- zmx+*;DrZY&&|{S%<;Vwdl_sWFGmps?%5b%als_1as|h)TK6B+#ZsBuzz@q*=k^~ju z0_Q;+n}fc#`g=sbZn=e1B&Z5!YlR&8Uv#}?TvY8AH>^kuJwr*ujG%N1(lMlf2uLGH zhm?TQ&Crc>ND4@INJw{y(g=dIfC7r7yw~V`pL5>l`8{9wz@FKA_TE>lwf?a}S5z}a zUOfF;{Phm_u*ttDR6r^eQ1Ls1KtXCD9Bi;EDoO>WYlr{^2A{;*^}kT)|M`^@sCDgn zvO)VHhyVp+9>#-+|JDS^xv7BQU{pf8@>^a-2Ef%&|G|t*Wjp=_H^t6(YwJxlImnq6 zAkTMaojO4wb-c!=I+S0X8T=+z<$v{2JyBAiJk;{B!q7`9Tf%3j5d@M`l}{usnKhki z!mmk4JGLH#06lp}Hhwy+uHauTz)h}QVVlVU9DXfdaJu{dJ>AQWKVW1NV1$Vp9I(yf zWm_I_6e^KhVKk5&Sg|h;ogQtzyE^~&?pLFqf&^Xi01)qAEW!VMu#6aUD%V1nwJ1WZ zT$r8ff%I&pvFyL1FLV9?W~|`KB`Pl`%WMS5uB`);9vzG;mc`KGcZp5BZO%XMhBwzC7~? zZ^+Xk0J@_C=84~M!p@olFL>>a3+0@H=Nt~C|3S|f@o~v0kb1hoi<|(XKe3OL+qxcU z@L!40020h7UWdEr+bb$(9t>|b_^+Vre-B!@C?DRBEbso063iSPP=lB2Qa;wG_gJrZ zUwrS%@j;gv&iUmYh0~D4vMGP0_@uE;e?^vgwJ-HA^K}p(t=wBEOkVpyaS#7Ly0Exl zOkL?d_ZA=4;TY(A|LTg(Vyukdm-o83xvOvPLteix)o86tGW?Y(m#o0{7X%CIAd^w+ zG_BRhV^M|d+1o7HRR{M-Jt;saYutuhj=ut1f`o*yt!8^0c7^&;JqsK9%5XQ<=CU%15v`@9} zF@ON-f&zWm+8eqEi((pl6ubYWsYg31Tq@-)tBISU}c#Rn$guz<}}KPV{)l<-*P!l{l7l4AXLiH>~F(=-kAJ86qGa2 zc%_u!pEH!+hXgMO?QRYJm|ds%R(lfAyRu@L`|{Sbq9UJfp;^XZteg zB+36CJj_5JTzG;$*gTN1)_RkuzJ>i_?H|#rR5BXObP59JUlvk{{$$QO8wtOlP5di~ zl@k1O)D|3F6ne4qvDqAC3;V6coq;$2Qu2}+7-Sdzi@fXtu$AnK;7cgp8z+ETIJfqS zJ``S090L23LA1lCx17|xragxsg3B^Hh>xpL=llC=<`lq|-DiIru6f)y*Jjk_`D+iI zpf7uDW&tdGqhrah8?93t17O`LfRBDN)LM;8jQaj`n5%vC&x_ELN1uWaoZ_s{rWrtR zpIQL2<^bYhw;2auLNb{salXs%HmzqhngL3lH$il_8mFy#ru7$f9U%U*fLY<@{-7xe z!Oihv)2Z+-#?46w$)HNwN-D3Jb{eC8llSSN)dOgji5|C-q}M0-pE)1LiTQN8Y|a*_ zDKZtd0lu+h1;9y;LiI3MKqAfoWm5BEglWvgjDF}ozx#LN!dAAV21dc)hCGE>dNHya zBf04hDF1m&&SBSl^N%~dy})B-Uj}?6+eh^Pg5!1vu+W}?yT`pFx~ccdyf%)wxn$IN z0OT~(Dc64y8Jy$jK2Z3q1!BA zgKA#^9JzR^U@-lBXABsspv_#4&=pfXzssLGI;A@qbB3RGx!X``2llhM;LUXti2;{} z=ho+LTbvTWih(}j6yyY}kI(NB_YUV;Dh zVNnPTGmdB7wVAwkJ8Y~j@{X*b)axf`2t2RZ&V;>l{}YHl4AL0X=+3D;u7Fe#K{VyE zw@KX4MG(4k5N}mN@OK~&Ddhy5MiU7<$!XIS`j9IVTGuy~^2C>F`Mt-6=#S*n7(|$5 za=p}BNpTDt0bS4gR_jyK*)ufS0Cj5vJZgQfY@g#ncV;x8>9&z(HCFq*P@GIR%+jJ2 zI;8eT2b?2|A*lT997x+9*0+5r$pp{`xpHf6Dn8I?1|&6HE!F^Rov3jx;AkizdH+Ey zm{`}jzfQ<#m+3lf>@_YMHJ8zewB2SodrBVxQaVSE*Gfqt^D<7-ls0UHf2RqE2!r{3 z>~n|fFVBBHnFqjXj20dz0VR*|Hh=>jPaQUm95!*)p_0@zFg&}tukTXyV2HC&PNk}mi3m;5W|BVHFDu@4$#+q2_V`l$sjK-sOb*(DafAqqXl`!Q?}9P(MgOCqktJ8_{5ry)EbK$UXaF z?W|GqcTJEtrZEt5iRDbSu{nP?1q5Obnx)csOfbA=s#7f!I15eqmz>r$;%Gd801yxZC9=etNOSHyRNOSxWyb+1&P>j$W8$AOm^2g{`ysD5B9(~hoLE*@u zj21oU&D6WlhB1{~4a6(Ic0c(glRuT`z)M~a9>9TX4vHLoB4^VS8YA=h3EXPlB9j~E zc2*^U1I^W}PS>g*fbn^~c-&dE*OWe)s>SkW0Jh#wFeK-?3U9Ob=ZLW(BQWjKy*$7j zyn7|)aj4d^{))`;g9YMRz>uKPq7BWKYYl7FQ?vR!LH z%}zOD*($vi?-d2daR$Bf2D-INY(na~!$5qlfw@jQpg736TEJq`B??C8L*i~y}%Q?^B|2cm^5A|I4gE?m=mK_ zau4!)7{um%Nv;u6+EF)M`8p4m+Prk?zNA-}lns|t3_i98Z2dH=tNR;sek~6ewCZ9= z-dOoUzQR-@B`V_<-CYcH-x)!xccY2(t*b1Z>XHXMMKmuJ)$X*{@ZdpmLz_Y$BrU9pM!{14~Ptt z2{F+G84IgSr@q<->22OQHKQb6GfW~A=2Y=-wi#hzYmnDbQ$2jEO}9y1j{2jhYC9~k zp>E?Ad{=g3>^i84LRx1ltJb<+m?x0X4s8hHzTQO>MBIuSTNy12N6f+oM|MduJB_wKwcSS<&nJjegiCY zeX$IZZN|K#cZ9V%A#5-Aki~+bi*vqGIO*3)EJE`wy?l_4rlKP+l5q1*8?GiIv2e~u z@x@;*Dpp1AK}CC9Bg(UArqnq}aDM>O?WOZWw zq@%JNz7fAbyQlQ0Gj<^hV*QyoU)6VYHZXj1bjcOBa~sFAJg!LmATEL$Lh~Wm?TRM- z>?FLy>=%~qpqO0Pa1FHFJHegZ9k?iSw$w6F6;PvMH){`M=K^IwqvR1z3l*hCD~5n{ z*AOQ5P-3*L(o@GDGT9j*edqR~#Z~5Y&YuC2RS~>Ftasd0Vwp<$L z$@BMeSC75+>ZkI1UwXX(4|zRukAueRj%LT3{ffm>lQ*{JchVlujU(TXPDCFt8!K!H z*)Eipp`Ly|5qz}8TuZz`W5=vJp5fT;!AW|<Yv)zGzTc~03b^n6w?(^wEU zH^^T2LL6{uX6L7K>h0c`QJ8Tr=PmFlTX6@U0&)|9>Wf{CGw;)67K&zNNN*lh9xKST z?mqsKUgE=(ZN)GWgeQ?&+FPHwu3j$Pjhn!4pdQ@7w)`Ir*V-gWJ}zQ^;X!D;qG zw%E13oAakh`F!(gyu%!0$YQuvYbyVb{f%V}D?n_BqI$c?I))~*jEqOvFD!Ek^>6%I zd}BBy5D!(N(mFOwy$*k#uHH%264}emSwpr>6&ZHRw~uoz9XdZMks6AYazC)^%Q=>( zu_wE?RLwCC=+iyB9j(Cq%{|12;EUR&5eGiCQ?4m1%FZ!#!9MbcYdW{|)w3?#>cv&D zRae%OHT#=ONylm&_4W(j8pT=1*i(>+PYOwp>-`dknf#2X5IyD%-Q6rTs$Iu9{=8LK^9$F3kH{~p^ZIHMq-Ec({ctPJ zBJh42wqtdfTKzm-`D8gQ@P(jtW%~&2mrMcE0((3;+g^b&lh<>hZ=b!yzWK9U6=H{oufgcQ1h2T zyD~b^VwyVh8&!fxhwv(x=oJAmEPWoCejItd|}`48Ev>nXO3aLCqpD$plOCy zi*LSl-P^HK8ZG(h$fLf_1%#4U=ic~W)Ge~nv{1}GpTifF@eh+<-1KJA+QiL>c+q86 zF0<@0dH;@9bRh(z4z;Q_1d}(&PPPo*X{f=uSwK zVB8*T9Aoqo8o0T{5+@yLG8tZ9IhSI$Lu>Hh&IJyZbo$1Dc5^`FJytDjZnWP_3pkuj zFTmK!X^aG-M#NqB#wFVW>_~i$+oZ^r`Q_Obyi$Ai85``b9g6KdVR)f9TjV-Y%_l)Uu&xN(l+oV-BcvZ?5C za2#$vUyzmDp4| zYJ5A4sUG{d(#Hco*U$+CxRO~kE0AcChXQ1GP+J2&=f^Lm{T)CS=Qb`8+xI;}BA@xp{o>#qAcyZIQ7>v#aqs z>f}=M?H3L2s`GvX1h!J&TztfZ9N_rargH6Pqdio6R*J4G={{ZU zwT6Rov`6G+Q>V#z6erl%bmG5YoC*bBq z+PnI{SG88a?C=Jm-| ze(Z(L^}H}ID33SYgws7B@wU2?&;rWH9&l8cbn5KgCBKOAWvD1Hy>mi4wb6)s*6|;e z-$XnVJ3yTSAkSxR*I$$S@d(p)Faa^z5)!b_ecwh0ga%1HczuqOgo7UtO6dhcUAjPv zqAb@#)2|?ecp@%fiTHreCYm_D*~utq(uH}IQ$z~*g^@BpY1x-IZ}X(dyh<3m)O%*n zfr8pAyvqfg;@w)^b?*#^HUYE8b=MCpLVupAB;djq96Ip}CdF#sC%2z`1T%KF{$_TL zZ16xLNnp^8Cotp(Y|d5VwiLPcyg(nrs%voF)P$z#)fg-RnRrAo^tKttZLaLs zE_$x!v*10*# z%^2}xU90|IC&XJ^hSo@zhMHgZa4^ty?swl#i|s!YMqh3113w72Mt9(GLf0a5G{(io z@H?g$!LYQM89&gnbc~0GANw%WEilO@pxJF!GR2MGEiP1Mlo31JLLZ{VudvlpX2xJc zm%9O2hen6Ax+IEnrw|iX>DM1RtKLGZ${3AzmAlw?S2>Un!U{O8%Ls4#joQ43%2nfh zaUkdidi-e`4q3?Su9XP(itiq^(V`S|?FG2cw?O_Ngu+7fwf?+ZCT+w5>+K1nQ!4#0 z zSG?Ee+K3CfX-n|drCRRuI-U1>iWerJx~?Y0;oBMqQb%6yiSoFmr}w3dZ|##_(www$ z@_pAn_(0*LBEMKg%TSFxgOqf0m=1Pwho#Wkh?~PgH(zid zo)Og#Xj7&bn2SWd5!sk?YU)!6+ZLJ>TSjD*+*T%54~d<4MUiIeHHlanQ0cTl?2b|; z91|JFCK>-2th#|5kocY%eP6-`+vim+SYKz*r7W~3$NlA8P5}Q|UvfkvKT8PPMu~uoaOc>^Mn13%O z`70I_&#jtlB#%Hy{btjbDos&t2Oa^wYjEnS?@(V*@o}_4Moqu~-2b(lGq@Y*NOs=s z?mectdf`vb0-?nl?ǦlV6ajmMR>+Jt)qXS1N7s*k%b3wmX0z{Y*98vE>vQJGFS zJe69e$|Skhjg^y=tgVofvH8DlD54_=%)lY$HZNv|=M-8%KE;N7$ylx_ymv|ZPwk=0d zJ@olyP!4jUD$h1vZX-X9&DIv?bhjJvTLzNwh||b2Udt>D5hJZ@348BkCGB}~V62+@ z6ozB1{qo@ROQOKrl!;aI#vZHRjpb!gWWPRJM+a`upGhFMts=wfS0}&dNw-!%jhdoK zeV1>Pu@u73WUsDdJBBI4C`%4F69*TwAwj?}T&neEss;so5)rU@do`QRu-A^*WDb50e_J%?RRq>3`HRV0-0dv63a zF$zjYOs)z+K`j+Hklx{>sUfC3(vX8BWnTj9aBg=IE(&@PCc1Qw$7JHyNm&B1r^Y(E zGI@~E-cYW@tjf3!J?hS&`{evA>&8FR=n8g%p~Q%p7h4?)xjxS6EC_KF*%O8l;al#M zvO)ey{#uUV#1Hl6dt4n1!{mnB^WHAZkPW+hq1D}Kd?iQp$|6pYOco4*AKAdi*kc(k zi*-q=4886&5=m>4Na-Gb%Xp-6_=>ueNL%B~1S}JPZM7ze+yW(%1B|eaoBJUD*gDq& zT*sjZC5E**?Yh2Y0b{A|2ZnEmTwisyotB20E&jre~N~?`SlHLGYE$4h(nlZ z*=ONSjII{3IZ?8EAz*R$Mjcgb$oPMs(s^FcMW;Q`>Gr5>IJ=^2Lu)tVKw=CR!7Odc0s}V zh5FA{#cl$?Y~?mA#Q475IWq3ePKWY%f{js_pNh;2%Ac?900G~#8hJ8axjr(Q57$*( zc=v*FleT#)azE6D-^_Z&ZS3pVHBPKZ@Yt*`%1J2y5X(OpV$#zsO*MKjF3EHvc!1S$ zBkTplkGZy0C?n=D4-eKauJRJ@Sw+M+q8RJ-9p(v2MB3*N_+*t4?`tRE?{a>Z1>Yms zLl&3g6j^oMBc{Bh`|$8-sy&^levz)e&}-~$wE)Jhv>&O561D`JgB=Pi#r#*XFJQdN zS2+*#u1V!EOji6z>+y%Ky6H;co+QAPw(ey(9me3U+p~mPjN2fZlcmq^5EQ(({a3e$%M5ApI^AjIGHo^GxGZX)Aw8V5Zu0xG4=w;Zt8D} zT*UlHt&k#c5=TP55?0jL68HBOyhMk&17@97n7)Fbr zkiM7?^g*RY^iGCRUl@<%V8XMzKMhjK5V|-``oGOfJHUmj-&rB zU2@qWUN1+sqm{)f2eogFKKra#Rcl0v_g+(+N;!2ThMOJsg_^YvnCw8XF##OKYbvxeFGkL;%$E8@bq2+PpC_x!@?JN&F>A z0v-LJrtW=dw3Jit;ur9VpqSxmjHF*5n{0l-l{s%?wf#(pB1P%VICW&Z_0V+;SeAe` zplv2I(CQt`7)#e0SeA#0S)fa?IS7Z@=79GRI{r*Ts50G z#Y+E8+Uhs^QcLVvr8J9%Cl6VQ@Qbvr8SdiFO59hrSN?^r1=-E0#b7~1nMjekzMguE zj$hSSNquCZoPN_ z{bWIdOK(mST48S>DBDb~GS;cA4PSsbC+%r9b~Jo*@6=9pUL!cKSoaRI*fdE|uT^|r zZPB4+f7JANj{Vkg)}Wx@V;dT2uKH1`Z?W#76XwXUOYhl$m;5qMb_=w1 zKLbCnP-hnFVT{(>&BudZ!=cQK1sna3+Cd2>!A`SL4hX&CrwS|Ey+&fb)cSDeam_mdBr0aVFMO3bN*EfUPbPf%h z-Q+TZ)ug;=zJBiB?K!3>Jo);}Js&BADfG6p9b;oYxBBHCTuQfU*ts1WH<#AI1N~{){B&DIKB&~#z)!ShxaqE47@A$c1CfmmqP5INa-)M zc2@O^W_Ri;?IYf4r-p`X7<_e!3iQDm@dFqp#jFIkuD^z!R?^p0bj2%}8T@gIEKa8* zb$PYdBQfB5_iA(AwsQ1@%F?OOXU-yplV0eVvD`vidy&2GkQ2WLL(P52X*iK6S0yCbT@*vca*c zv|#X4dA(`RbE7K)pS$&0lHQ2JD^WMDQU`K8hQ-rKmew zTiSXYaN7ATd7CM8R%YT(lGovG`aWoK(Y}sGwrZblD!kYeUn#Rc;5K`y8hn9J`#@Xr zts(YZw+g|>sz*!gYma^^L{tBa4eyY%lydK*&7I41HIbN-OnPexjE;&Q+m98zQP1xS zkO?I!e*vU{07TKSsjaKmd8fgk-b6S%~2|9E1~3c_KOs$6aaJ)^S*Sl?;I4z z_{Zj!@i>Dz&8z-Vv<6dEJZA^he3eP}-rjDD2*^haA{s3=Er6ctRqgBVE&zb~Rowsy zag!81WG9nm)IoI@-VO@^ols@k=0nsr2{SRP=ArZQ4D^imky6_C~hz>%(9x@fL3 zY$WsGOaH@r*LzyBlj`w$p#$V_GEFB(0~6P80Ms-bC-Nx!lyiO4ZbJt!2n*5PrW$UF6^nlmy&0V7`yzR1}ja(3-ZE1Y#3yd1LfCE$B-zq0jEDH0^SWn^w&lbc{w zm8Mo*u9M)1V2r_><|}t7&%67w-NvM_t3`kx>NPJd75==Lp4$B@T#iTPi?pDZQv6FK zONZm|9(UvG)yt84#;=8yw8vgq-mff>2_ev10z{FBhSyS(=Oj(H+ge_{fMeL!TfblZ z0E)5p-oO_2W5c$#PN_Dh^bi6AxO}=v{yxbE#jzi)0FJ%r2i91P!+EQv#p|;EzlzFv zS{8dIl+KGsA`ZGFexzhTO03yC0W8XX=eb_CJ#bkJoPJ~eTc(UsXsmF!WI^6QxYo2o zu=}jPu#;lpsek_naoE%~_L_Euz9NjG42Xy==(}RShhM7X9q~FLQMRM~drF#xW9w=E zC;OQtoynhPEf6MXvE7&6EZ3e#MQCm6{6Rkh!|<(F^Z?dP0*cfn5>cXUfD+dp{dTT5 z-Ns!Dg5qu2`3F7DmydSs$n^vt;S#TRZa)Iu8h*q~pk=gou*RN!7;07=GWtX33YQ|K z;ocWiucNs`ow{tpv18eu{rm1UQps;~E!nZlRMjlH$u|+%D76VqQT@$!bQ>3|Ua38x z%d~hv`Wtu)aD-;wbKRZYK4$z3xpbN)Gr}SVJ_Z51x2g!OjDUac2B*Dzt}3v=8G4P~ zke(#R<8nteQ|QfSu>o%hBGw;*0}Ajp_7Yu+@f z=PN*_(=dB{Lu7a?n2OHxp<3WyhQJ`?xpDAZ`}w}XOkZSFxuPX`RonSy8|XXKz+y+F z`iZ+Vyz5R}Ba%hRCiL*9ZjJe1bOY8^M7z(qHX(!4oJK2}K`}o^oKynXG7CgQG>;oT zK5zWXMzg`4jaYkRCwExhKqn)&P%bt}KIlr)kq4v_)p$)UJ&io5PiNCH{EmwtN|)(+^)o2eB6g6d=GA<>rG>9|Na3fm@?jG&LY+M z!gz0l$few@h(e(TOLQS`VX)(9@7c%ZV@D1t=aAv>ihL!s4GC3%AT5T&d2IcL+6<7) z7S8U(0((@3h^OapXkEq0U7W0j&V$S; z9MxIUI&+1(n;G55Rf!Iha(~B$)9tEzq5cco$3IW~9VQC~sV%a2&9E8X9qm3p(Zj+q z{VF!#?C=1{0I54NXDaGBXU|jJO&*3S*(|>8d)>g$i$|(8)*#1VJoYRlNs#W-oj*)3 z=tGPK=HY{eq1|Jky3%O}roS_ViJw~Bo_q&fgLy>N`X@Vs#LzE#hZpQgD773=eBf1mit#Ct(_%cu zxlk-@6F=_m&fFyC`!*Ut^qLkLsJBhIc-#-F=>!T}9-uqYWfvwiX5!`KfI`aOZWreF z0yeb08!S$FTT0_IUZ+6Wyju1h*hlsk(7mD>Su6E{6RlAe+WvirZ2kgRI$RMO;JT(? zu6(MoDkA%rQFzTNa7DG+H#td^I|6hT(ctH2usaw7osM4D?6InM9|!5)HSGyo7bNzM z=~X}u$q)YJ)|%f8h|yjQ#p6I0&5!rjmYy^}(Z|h=TbQLS;jZQ%@tjRF4jP4>g~Ywo zasGz^DCfUjm5e3#yW3ZAq+)f`*M=FDXY)lXxolRxTl)?ArlcC6LKApHQBqlf7~Iv_ z9El!025fP}>mQBj9F!oe35p(Rh2u@3!1tdaU40Cda~2GE=l5AzdC2EJI2){w$7m zOcS5*K6S00&gq^!=B@?Y0#2X-Ak=xyh8|~gil4>Uf83^Mu`pY4X&WIyzOSo7(+`qL z(6ik9R)9avdl(h%V_`FLzxGW_Z~g{hX%O<$gFs=7F(=<&KmRajxM+od^mdw7ZWwn z5tw~(#Y*w8HVnE-kjYB{Q$=}F(CA;>re;#oLT{Ic$!HpFh7r1#K{$}qL48c)qg#L1 zokYkHUCBV`%nmdnadaPRFh+gnek=0-_*R||j&9FYC~52sU{MqH4vun{DmCa)PZ#G( zmJ5>oJBXM5@O?jkK?@ocf)S8t^ymNoX#)Z;Vuq9PO$4q1|I~kn9}Q58_BKs{|Jcyt z=2|zg{TTuS@_(4E0^mQ?3gCTK{JoPJw9QbI!2={qX5ZXQg2y7FUrWrG7mB>I@D9g7 zY;3i3#r(AFP*))ZL)O#6XQdO9f6k$XX&T~Bk_#bDCCUAc(||gK)KzpdigHk-;`CaJ1>4qr88P;`}{Y{X=cgN@=>uaOF!WGaI-GCwPYh zYB=6h^)(V@Dx7?ZK=w)uMxJ63%{O|FzRuZuzX`6-_!dHo_5?cq4eanjIS!^{ixw@c0k8o2XMqLL9PdYh5u74Fk;;REe6l`fP<5} z4{$kG2vxIFjU1TScrD9P)U7+)l>{ND@GCcjh2diu*w;_p08LpO-9RQ;PL(2PD_X1Y zIo-BG?+mDI#R0={O?56M;oOl8HYDCB_aulUH;=(|F9!j|#xUe=`W#4uO+?#dL>X*D zbs+V*^=Rx|t3XDGw$D{C;eNOpQz)}hG9r!}c&7lfzYHm%2q z+4FlijP3alARv7#kb1zihrhuB(y>U-`^*PcZw_LZntE^q*`ckybY*p?l=vRng;tdQK zGtj0-1Cdl)9^Yhu@ceFfDDgeO;ut+?k#3h-c;&gn`sj@Y^FlvxURkCE;qC@#Z@~K1 zw#3UQ;g68Ie``Yir(QXHQsMC0%6)0#3l~rU4;-n#C|=yZ)(ujd?go#3Hzt@`?nv-h2ff`v=f#Y}`G_3#w!7m(y8HKBZcB+?*o-ZXKqhEotBJ^(4S%GK}7 zXx-8b(77bFH}Yl%xca!*j|rJVz+LZShj{`%$|xWuJNBHuoC9pbQD7Ej0SHDGuOC3^ z3nb&%n)i&+4LoWAi;$Z4j8B2yqX124jSBM|1?n{iG;(=<36}{;B@~?PP{nE!bNi z^p4(wbHW2U2!OX(>&4-4q4WAsGQ`mPpjVrDBW%LRYtQ1fQe2Rs>s&8jW)6FU`%M;; zwg$~rOpbtaNg4xan&=xt2GLk&LIR>kZIAV0lapnJeFHD(%*ns1^?`&bEUrt6yhravj*wkOn@j=f^S4wqEZTsNEF#GzF^aDH z8X>g;);vw2U3{Zjl3Xzoc-Ro=zG7zk7<6_#1&X;*fKS`T(PsgkyF9H&8t7n{2+IX> zIvS^AO?lO(jYd#2pb;}eh&O{SJ)F33w7y+?5|!8wgAarXMeu>eqtXZaK5SWG(gkpX z^jFvHW8F3Dmld3Aff+x|PB+Cs2BVEP&cdB}VviNSxZ6WNN{P2)br6_TqM!Y5*u5{l zc1nLNuY|&DzioYkEIieFI85 zkGA|lHx?`nQC5AUX||vC{HHi@(kg|*cXJ-U69m3>rF@zQ%`WzMS3?*J2iTzSBK@Ro zbr6XP2+U?EadLw^lO$6Ci=r0Lk(lc@I<(=z&>ajbutD2+RYgnBhbS`E$aFA@b{T9hA&SW-)y5k1&YSdMjAZr&V!U z80?%F8D-JB-(x$M9ke(+DD zlIrKWicf_jq~0T-!ib*vn6=-a2}o7W^79W5rbhxIxq%H?edq?DNKN^Mv@sXrrRthz z>`a&2&iG6#ANvCZe}?$+Z1?n_i2&X<7}@nHfmJ5VPQ~a(vjXtWPXt~DB|fFC4eT&B zDC`Wd(RK?A>#XAictRjfF9|TZr=D*rKx$k-{x^$@$7-s9I9-VlE-40ECmrM=g*Hz$8K12K+AK2Ywz4@`T=J zI%>MRVv5@k_t`z2eK^vbezzLi3b$us=&(qHVIPQ4Ez{f$V2OBMVhY#MQ;Nu z56fYBQzR7>p-8$7yf~|6!-v_RxDHTs^Rfm*TD9gqWu@E5i`a3{Nynhj{`*(bM!(MM zdzg*>&DVNzvm(V8S$u&FfIZzwmW)~F@;1-vA))<-2JBvxLKLOR0>;6LGhv1077og~ z3x#{ELA;)OeZ_HeR%pvk9YMy6=rKmhgSjB$0?H!Tbp3de{WcgkHb4WEex+>C8A*SI zFJA-f-o;U<1{qiw2ZkODu0j+yd#m#s=jt#ynOVW89 zjbrQkr8PO`kh91KucGM?r~+(J4V62pLMxcIJ8`-*963he6Tlb5`oW!N8G*JG5b~QY zar<@+bC$_D(k#XJq=YI=j5h~mAk8VB#%|~-vBK%a| z-)b`-8~;FeFwQ0HO$77wgZ0xVWC59sapbVT3FC-sR9))j%1x{$FE>o_alPmrzMKBO z22)y>K-H-E9ejw-K>JgV$V1N69O%q$MGz8yy-ShHo>c~3Kh(3zvZ#HMtycZs zp(rBX4$11&q_fVHL`dt*(b#~cSFa3*B&&LSZn*XV>`7#0lS^z_9Z{=4-u>jm?W=1Lw$63ZtJZH@6 zk{_rPE6^&Ygg-i&kx9}4J#^R!k4WI(bLlk`xJ+VuYKuSd-B&^(8Ida9KMGWnf}3JW z4;DvO3$^7)2|IA=JM1rQk>i!PFdfbLI6R7@CyFxh~g)C=^Ac(P&Q80WMQVicYmn_@v` zG~rX8FiK1e8+=<4L2>t=3gk%k&HAj`r8 zB8HBxEL0_ZRF(%XPvQ}Ey#>j3Rt+u8G-0(2>Qnb*==PzQ#VHJ~2<-`_!Bx@k3drG> zCmzNRlni|?3#q=XO6s9kd*38@K@E{ecy>l7ms0>8NQDFtF$D)95+XwgY4mZLp_E|+ zI64H$di`+`&oUy{6j0w6y4}J!2h+oMaB7frvzWQA-3^d!l7Pne4Jq#v`eWg;{@K2& zS6}$B@@+`2-hu3`YzSgHRbeQqJQUXo#iU81VaVlo1y?l@TPQnR4+B5!|I(_5z z>{XK|vBbxIY3df{W2`VWk*K(>&yZAQF(~V9MqI^vqhorW&OXTCO%m1=wTFRJm;&er zfg>A+0Hh)>_K-Df3Q~mdVe7JhY)>SVkW!N3G85xOA`b{&TlB-Bc?!4hjPrTKi^1cS zV#LPZT)QP0cFn>G@3Y&ye2;so1UEJ}IdJ0+hk9D5l8x2YXXwVk9Y~)YuB=;59v~2T z>pM*2E>j=aVd(%sVt%lQ`FZ{pH_x72>j-OvZISyd_RTW}OD0cdWEuzVSs6kt0b(wP zT1AnCIuq@d^){>SUyH3{Nx?m5+|8VQG)oM3fWB4%Nf#{?p31!{E$eaOAhos~{UgRi zzoaxN>^h7Wu@eM&7KHyrhf##& zUYwzhnx+t}*2Z6f)8*)qh?4CyJd()_&As{7KoKYCOZ1O@H+v|FKUu$_jBIqDfGi~D z7II4#7L17KWr^xR$@RYit9xXjo@^-D(J&3+hCQoTNL`fr1@+)pqLTX zb%TWoM?)5=5asK&J^9)~77`Nki2M2066Q;Vq6c&+%=@ZJs2Y#gKSd+_5ph@;at~28 z9R=p?d9o}IWd~?JfBZJj7{4A5X|@fE!&Ai6xl0EN+%?vUJtG{1;yMRO^CzFGB2X}@ z5e6Ay66{4q^SFJX@VJ-8xwmCz(p3OGPL58op%pire3HIE&Ku_q`BM=Rd%3rm@105D zKYHr0__jzU*&Rxx&IE)7#`Xh3R^`i##9~DH&R{p^ z1`EZ1O;xn#Vd*<#VE3wwhBgSs;qQh%6AR{q_RT_BZ+g%#(A)V}ZG?W8ih}G8)yo&i zElPZGDLdzVCjT@^1=VyMUWaqben;vp$~tNJ{b1f$*EQ-4hM~@qY)@+%dpFsz1wzHl zDtzadwkC1^_w<7Dvp6eUbf&?caUz+~YLxL?Q}V_B>`~g~ke(aYnUM#SMVWOP;cC~z z)ZW?#%`O^WL}B7|1@TgnFmS_=H8-xW%rnk>;O)CDpL*|5tlEZAL}(IuPeo7B_1TVG znxg+RcPwe?cM*oFY?8=0c!ebLISWHVQ1v}0TgAGd3=8kE2lswI6jw}W?VD6AZ6Z?2 zD&Ar=s-tq-rBstyWaPLQ^2bdy3HpUxm8R7#XISkkk^eDljHIInA z8$&5v?cW?AN4D5HwBaI#6{dmE{LL}Aip!)cMA2Q%WF&>324Wu)pK=3O7#2vSOnFtB zZ3@skwQ!o=EQWMQm78;}g0qIYKQak|b?rWbI;Ij0cnr*9zm-tno0mUy5SpOD7@a02 zTLcIZBM_8dzOjSLkbT*vDce>;1+El4WW`{P>Zl+oc1djdHrs^aWtpo`hLp_l9ou91 zH|9aQo_ah@JQ2RQ^H9r1;1r$1x5H-6$O2~J6PlT;F{I;{FU)3~?Vkn1;;OHt5kkA{ zSeIXSE4p5X=zuGJui}_X0CkSt3EO`1fDTAl{sJpWVXk^f95Mj^J&sxEwcoovl}kFS z$+sH=-n02fk>uj}7djGJ5tu?Y*A9}8ydkhC0OWEYbQ)d)-J>o*qmpNGWD%8 zkl?60z9jcnWLvrx9xANRSC`(ioXI38J);BLM0?i$(DprBq7YjmgvlMy#B-TkZ=5f5@^(~n|)9jz(Z zH}?ni&L{RO7tVm1AbZW*zDZ>2949oPO8=A)OpPo>qhzZ`1k)ug?XNCnzla@;w#>Al5d$+3p zR=S;Fc7_5^6o$uHmsTn$H!y#;BY3j^aQmL1Hjd|iHjX-)hPLa*wtk>bSY4An^^xuV z^O42qV8!g%b3!kBcyIh|S>pfs&(%kFDe9_i;~%JZ{NJ|?r-i~$stN^T+=^f^T)LnG zKK`dys6Xsr9`x{k|KB%HDDUoEh8UWI8)DS|Kkf=yGjNXQf4?gSlg*|7{gIG#51rxv z`#)EsTbj%lR{9PVD=w@Dc(onW>*fs=$``%M2-6j!O|<&k5XYRe+-vc6ejadR|%kaM>rlV*60oRteoAA_8kzauk8jD{Ay zc5qtjH#krYWvnBAc5)75`tBP2aj4IRUdd{=YW0~$-lXdzi{djrVmMhJ z3LG_}whzEDE7;ElhfoOu377 zjbrgO9r*srD>w@p$0Sb?C2?MN$+6MJsnYUeBssvvQMbT-vm}DF4EeVF>}}0nNN=`c+3OzAnwLp?rRkuy)fx|4hr1& z(!ZboEcJRTEFOj?T7;l0eU%Ls8NxCXRXrj`Fp)ytpDGF)J`%f$(Z{ap{u1vGv5Roi zN%b(YrUQ%9oEgLYz2SHB@^lL-_>8=NP$Of(XrDybMxVOrolV?)aP}fCrH*ORDd=Qh zWskz(Ct3c}llv6&Y)zBRTa*NXU+#E5(!BZF@S-kQkapfiJGy`JzDYi?+Iiuf_;;d_ zq*K%@)EJu^UAV1FJU7e3k`C+c?gQOAlj zS~1P27b6YeL}OM26sHdW#Ua3g35>Z4$@ zov*w*eVX&c7X`p75Pme~02z2!23-`11}3&SAUL7770_#vseTHHAkRX=l-XeAU(Zic z{)^;YKackW6ruKJPyn=L6H>RheW)MOVO;J7tm7YpXyxz-gcQ8m0IOH7^nKD?zgQhk zh2sJr;|UmDTM+MEV+as~MC(!`I0-_BzJ2WH+Pi!#1zmIFk8q-am)PH??iB)kkn4V{ z$zO-4S_QZ}qf|Erhv`f2lDNSZcM3>oBhLY2l$*T=7{7e+zi;e3iP~H@Cjq!{TJQ}} z>9GQ1^a{0Jw5CP*R0B8{uipen*t0YjodeZ>wQx5(_!YQF-Mh8-4-dal>&TA|QUa78 zdzGi?fJEcIfw6??*ql!bQpv{*zSKl|^ch zh^b4*mDYdm=P$%~lW(my5~JUoIN4xjI13;o)$qL^s9KjItCzU_hhW^-{RI>g0H8=@ zGR$v8fLl7wMSZ)L!J?n^okkjb%zD1fgsuuvJG4}X+Mx-S|N43bp0Gk)vAC%lFSKR~ z^+SE~7`OxzxJ0q!UnJDk2DE?YaD^0nM1#7A(5KECP|iElk2KX&iR2;b0xHP@mI3_` zzAW$&(BNfE3|rR81}ehKe*gyI03d_Q?6)Oy#cG%HyyfnJJqRxVGX&8yZ4z4uw~bUD zgyCjAsQETI1L#m1IF6J%%Q`$rwE>kForRv%_9M2sGCZ<&-@j2YJ=^M?k<)j;Avel9 zeGXmxe&yBo`xuBP4y2w53ImKzW%1gbG>b?1-oTDV2^TX^X?cf-nm1v6*Su)j*jQR1 z;SzAq*S`z!wZnK5`DYD7poHsa4^%=;F#taK$L7D15GZ#r?tXTIT4czSYl85Z zR{FCYg(m?TWdhtL28MwRPVfghRk4(6#|EE?%@?3x1m+zg6Cx0W9P%3Slg zxI}VmKn0+)8swZl6N0J-V46P_y!pKU8~8nr9sot|GANasIlcreLH|W=58%p*pMr^% zx&}`LzRU$SKrimM?X5+BpvJ)GJZ#(Rrv}eNw=aZSbPq2Bi)JNY{trF)4X~elDvStz zI2YXV_?KtDrk5!ua`sF6VOlO7G{FFYtB}o(h*<*Ze*IdJz(yvu(uytoIMDXeH@!Uh z$v1m{W*}ENl{9^{k!ldpb6z|$dYcN>5Qh~&C_;_bc)+{Z76j^TGw}6!Xbx(G3oNk@ z7!o?sF+zulToon(+F>299f&H*5Hun36({sia_2SGm9 z;Avaeko6)Y4rCuaS{=TVfc+aW1K-I0=mK1y9JclQV`S5b9l`d!^B)a~E-3S;Lg3%u` z(x5-rXJ$ahDc?F6DFl=6iG`q#ynJG$Z?m1TseHizVQlH4qMY|K@8Wwef0=l+G7DI} zRR&`^pLt*D&z2_3IUt!*{WfR62Z^SAq6cu_gonLMqg8$}iG!>vV+BQqjDxrviNgda zY7QK;lK}$fZwgq#gKtXoQl|iT+qPFtMdKbsQ$5b?#B;wJ6RB9J*!=k|K=htiug%qC zL@Fio%BzBk@dJhu5TCvbdZIQGU{46wDPv^?&q-h}mRd+)V!2-%KMWb0 z7vM=(&joZcTwR#)ypt-+NKhbD)m{hnexE}iC3jc*0o+zPf9?DHA%u|6=#C=!8oYo@ z+V=jru#LDZL^_h*h0_R!e%lg!(IgX8=mmp;l2Iv#$NwRov{7mMu<)p%+0$E|y*RH| z^fK_0d@3N5#!iGg$=KHT7#USx)i7Os%dR%%lDG<{|BL14$^h5cj*cLB&!)<4|KkHk zwVA^$;kvTs9kRI}Sbl{|wiAk8Cn0J3rXuCW` zWD4=#ne=;{&Hm}<7mt5W`spKQpuHj|!jGUawt=TI#7_+X=a&c9m*TjtR?Q!22if&0VMJ*>0~t4G{PhY`BoxF{H4Xqsp3f!_Sb!Ah zRyP8R96PH6uuxPSXa}?WL^VHwFyq|bKfUIU#U0i_yEGmmIxiH3J%nRVpG$}N*{dcm zZ#o(cYJc@{h3Sy$S>Q2FI+6v(jEP;qZFW%crI5RtOiQW@^B@)NRht9BI@xd(G-cIAr3f|9`^`%+o8LA*s6+eN)!ad&@$n^M$sYKd4*DRq?(Bcsdo2z3I4W zQMwc|0XQ#8Egi1%TInv|HW@ey6^6VnM^6+>Z{CuDS3|GUwmV|BOcO0SHpX6Z)53N* z;W%`MLAQ88fkg>&HQJ(fqYcml0L)!RQ}7L?*e7MLVxZwu;rx#8j|F)OX@p~l`^YTa z?1ApSko<&AX?~<4-|rGIbI7++S@vbcL$l5SH@QJDIr{g}=RY=q#Zv(<3L6PleVlNl z`<1sj#C0w*!cGIsmObnHnKzHUs``!Os%!y|D5=Y$y)#QM;|hOuRoQ1;!9>9GV!f^+ zUu?1%j?6vk0^^=c%Va2J-qk_S>*86i%qL1reiLKB4MJB<-plo(J=iElxKyL33ONaO z(aa-Q|IK}E_gBp&bXS-WN!_pEs(;R51a0)s2xDIR+a2F)LUaz9WAGM>U1LpzY^xl|=R)fO?@R|>mGbF^ z@%k8BO+Z5pxj-wBHdk5Cm#hUcqYWJu>8>JB1pa*G5d{REE?-Q`JT8mRN#G&AcYS~U zO<&`4{wTexAPUybfLNK_UU!>c&9C6}5n&JMP(APireK70fdY#hsx{A1aZSA#?!c#PDi^B{mHo!u zsWkKRzGXrf+X4D)>1N#{&sjk|oAVCeh~@`eUQ&)m)=PggPLrUiBcMXZXqPN6M{O*m zj87%Cr8)(r1jW~QMkS`~=C6GTYR+P?j3$Qo5^Ei!$vOdK>A?B~*^<1jtB_SzgqY2V zuw^VRGvO<{AnL1*bTpng$W0d)sIo0aNb%&%muR|+R0fcG{^RvNA{2$vtq$H@%64>% z$Kd^z?cDH(!tk!zyloc(J1q@UF9Q*M2sYTK7)n1J1X*pd+ zg`}QB(v#c$oPF@Cx`c-vpQ$MKBKPA$G2Hly3ok=wAafQMgn1sIIjX=NW&gI#*Dkx; z8?mxa@Zp3ozKcCDT*4r%+Gg@QU@m?5aH;Cg%?6f~)-@y25%;KLW{DR35Wq&M-Cia^ zeZ@|R%I^C16~KnF!!j06BF;`4MQr~%7<{0@wyW@iAKPL({|juT;Vem+T6dvPx|5Pu_rrORRis7lajfO3;Zswv?!N>fUj;46kS-Wm^H^#HW z>0&zN&tFf3T@fppjpq`S!83PV0idnJ2N8_UK`J}I;s*I*Xm;oM%Z>r6$l8MO@utV! z*GO2E%-Ex8zs7R9v2|tiRcx1ZQ|_%s9={KmWfx@Lg@AYKejXTgnz6sRAxwz)kL+tl zvzz1MFiBc&9bXY&8O$edI>3VHhrW&RxyPZ@%2?{>5SP?2HkA`(g>niMrLyVGE3Hzi z0YZ|`ghqe5v*T84uriL(J(KHnkamoknIUy(j;fr@2{wDzchD)${j7yAGQ35NM}sSj2$r@>cn(Uz|SKNG;raSZcaMGZc(!GBVbilFa{G?_li z4)2N_wx!rkyTDQ(FN#nSV|k8A{ZF456Rk|?{qj*SI;NRV7gS84X{O1dqh34mxW`FP z2J&%}#X%p^r*1jBqKP&~4sBY05L9m;N;#oCD9zy{>06F@nR5s~ev5mhm&~%sjPBM3 z^CNW9o<1Y3%*-e&8^6+J<+x+7t>7i_5zZApnOx{s*`kl20>DqKYFmsej974VD<3{v z%T0o!0vX(U#7<8F!b-niF#5&I2k|EgiaA!6eXHwO$GSI*<^zbKv{y@;>0N3{bUQH7 znW?`b{2vfrU|Vta-L!izCu41d9Sz&LFZ+ob>UF5F%Ux`K#}<)uzSu~2YwqNa@@n1; z?qfW1$77Q7q8N$WBUjtI_xg`I51e=-5b!yh*cg5NBNoX~HuO#PSpJk%F2MStBpodQ`#7-lR^>P=5!VqcQOYYpWA4~riY1MrUTf=7_rRBq`ZH*AD4 zEA)BkR;mT*4LIwb=e_aeE3&XsIZz^+MbpzQy)%wsf%uB4iOh(_kriypG1_RN2l>=3 zR{mYne|4SfYC>`oJ$y|fx2TMRa>6JcdQvAf9N(P2n8-g1BSGhG(JM3cdWSM+ zsjC)_yFFP{ySkLMq_uM9{?pLBwG%^LM;D`$RdU2<*O;j3@5W^WLm|OC%}1oCqGll3-=2>@(OelA%nBD*<-p z(y@Wd;V)9U@{t^0{Hr+R$f~BNtgwDJ^kO>X^0|*06J?&ZO2RamDyBSERMoE5leJA{ zW5lsI-Sm+amLwg5ppEXnMQ6Vr7FTn0YVo#(twZ@p!}_3m?g zs;+XEQ?sGeGu?Jsva&hGa;Ja3q0iUwK(DSc*qdu#wdd9gA#Hbp+l0_gN>Dnqd@E`w zW}qrBHTgy1D;mD9cTb@13MSBOvGwen+`o_))DM;gbU`S*ooT&l0kfbegvNK+YF4@- zo9vXjq5Gv-FLzv|U~rrqt%m+uqZyzuYh8TxxTD+s`0`QGhY|@VD5T zO!R>x!2*Wo!|6$=k;&%$-rAvmtyz9fzNa)CA}}xSpOV{Any*JJEeBV~Z15JE>)M*qHy|hC#GWj46c9$2qh~qj#>Ee*oo! ze^`85sNc@8gV?TdnE~)FVv5UNa|=b&2-_ zCRyO_#KF}KjIORUv>3KlY}RN;JX3L{j$$)F{4V2MN3 zl?3C{GeZ)5W~*Aa*FkyW7A#mfhgB&>-U{UpHZ=#!f@KUi?MtJT^>0o#&_DK5f|stt zB01=s_T~)ysx<}8t@!l%S>gq%-;YWG@FKUANVnen6-y6kl0*_FF!989;?t#!BHT9#sL05W`@?g&Ns zuE19EX2eetQaU!d^v)g)f98i1oSS%Un2va1JroW zv=fxox;v}fP^xxz1(yccB?Y;YCD&+G1;dQ%0?Ez02|4_3i>K0{b;G(5>OU8Br%j08 zWnVL{%ufcgW~H({O#}PcYZIdc0=s%kaLg}6iH*n`4Gl5&m}BWXBFX-}_+B{j$73>2 zn4^!~cV2Wjy%HKXj;mjVi^AU7R_lyL;3eOUx1{#%(&{-R+<^H+kb8=gUi^q!T)84@ z1kXPS$vuvwCvc)p!2YqR@vg@?yv|(A{kU{U`Bl{ww zugKWmvZr&mr6*}98O7kWLT6!78|A{S4+-@hkS->(H0p@&h%7!g${;VdIws`L4V~1( zFfWUT8ytb6aBfUKZ6~V6I)O?nAoTzfvA8xPrFcN=C(C!Q{cnB zip3TD39RjBOt6%Zrs1(th4YwRyco>^>PkK^@M1HF-r%AHVJf-l z0U6V-gqm?ickj>ngaxe9DVmGpV2n04&0I!it&s$xg{9K#x>T`1Lf|(KD&TJ9L!YKipZyz*9L zFBu|GfC{Z#=QETzvS>fm9-!m?kr@EO47x@|QtF_x8l*>5Ppwx(IV@l9G2)cp#PBHb zTz#0}|7RVGJ4nroWhitsAcU)zp!+ZAUWAJW;@pSlZL){#Cc#9zc`YvRWc3Nd(Wxp8 z4El1(vHcl7EtwgbgFWPtq%ao5<#!X%k{}mTq+ANW6>CqQFX^%>sjflrMA%(aU|_^+ zK2BA6v+tGm7VmVp2@%e}(nH?Jm5Dj$3xpNT+3^^a+oBiMX2=3o*~ytYEu(!iL$|v5 zO(Zz0&*hRi<>796J&Z|UBB?#QK#op&@7>NAt(%l+?KE$o^g3NKeFTnrf5xw=#DHF= z-IKm`MmG_L;Zri`+Tmla$>?yM9dvzZLOdwdba7wey@IkwFE%ZQ+|hX>;=_m1neCm5 z$w_av@15GJ7ETw!d7RZ8*>tu8QVmpDk>Otu1%dj4t34V;n0?fzVRrtl&UzX&kE{CW z^yk|UFu-2h5v3C1xX|76earofoiS;4!sbhr=pAheCdD$tU*tTFINoc2-%I(ft11M8 zN5N5=#~(S~5eQzqD~ZMVw35%NdaJtIXcx5Jw)Cz5=X@b60gPhhb@DeC(Vhc>I1aRy z<*>`S?y$LipQr{dK_Q>FP}tX>)9Y$83ghR4?;ah0i$q2J&?As9>>3e2Jvx^=b(Pha z=yVLC8n{R7=FV3oVSS(1q#iCr@#dGCCc@T*YYfTpJNkmNzhA{ehtFWV`0fTvmikuq z+6jW03yiMCZZNvkj3L{05#DA>a5q!({W`6{@3@}^G2HE-w5BRRG$CtnTpq|q0@eyF z^G9xs7|=Zlp}WOj;0%_z6N_Y53`sBYxSCj_l5>Pq@b%hhfY2;vlAtATU?+qDgf5>zzED?B4n1q*>K7!+p z^p7i?8842y$Ex}@U;vLMnoAN?-J?Bdth&YHE{o@ay~gD0ocamZ?o@X6Z7rAtL7?yR zv!a%4u;jPvjf?iGnMK;&*eQDSc0RIlvpGzcUl(w1k&>&*3{)uVMWRI~lj0&{bB4yY ztWR6FDxU5|qT+|l3xygu;uiN8V*GhWLyT|yUb=NU={^erwkv1*Sj0m2FTz#e2nNaKnxFBY&#pfB4_p*{f zZII8s=JL4XtoirC#KTMiWw>vY+epio;m6BbigFtONF?sL`iMcVn4nsjGz%aN^e2V` zzw`lREROc2qo_3mphM)LSUaQzwOCSn25kfXK^M_PSk6V1L@hkHDMfc4Pmo>XO^ypY z(nZ3R-AR(enqps7Do0>!gMpC-Y4^^`Et^EKqtO)MN4 zq)`)YXYg=ZFl7A?A~V+jH7%7eRTha#ul_~3p1pYI8|i&up^$}NXKHK36e`Nlp>v0> z$~EBIr9R(2Ah{|;!Cz`d*W}+Pk0aC46cJXdaGRfHuV?iZPcUf7&VlR zkD78Qki%D2;5+`l9@`x+mpP$5sU2rb=SlBy47nwvGU;tNurBYjHxp!9J!LciD&kc6i*3GW57 zJP)@OM?^SkNMth@cp2A>+5?18M138Ww{DuL0O?M!nmO z^-N~v%Sn^WZzl|HiacN}WDsO{puPqb0@l5ey-#OO$82cO0CYTfxtC)5S6jF4S=0( zDz->XpT%13spuDnz&gY~f!eTPs((+%Qn>Ggn3rX zxV=A6*#q^U`uWnv#O5zKV+bm1cFg(XFg}2r`Zvk{{96t5M`rxF+n!;fyNM14)gs#W!=L;j(jF|l7ya%V+lX}E>LT)BdX-WGt!zhVS782rFraN{-5`eJ30qNmVzhQ|C;|u?Twq~PD^e2^?mZTC7x@& z^>^5V_fq*U#iL_WpE#OsO_b0joO<=%iKVK=Aq8+qBy^D! zJ7n$TrKnI8dE zlt6wd-@8l zRADP}HrD3e-(mq^xLoG=OBN%a1S1%1R90qe(3!!czEn!ire6C{UT2bBCQiGhEQ^USO5q4|M7o8og@pev<-p znJUP92=??W4D8`w2J472mBmB_r9GFP#Q3!=@qBox8_O7HwcVY_HTZq*`B5Ig+ilbM zH5EJvwi7rh$3zPxpCeU0n%$@Ae>CKDp3-^aXF4+@2`p)UX*F3YmtMp$uulI5q0rFF zyXQLvPYbYr8Udt@ArL#G_p00^yT(%BY}!?MAX6JWUwQ_>lZsw;UFmfcTPo$PJA0>Dm1 zaP3o)OV?TZGNHrojkBaD3Z$QbD(AmN=y~Z3AH#q_dIi{Yl{A27y3W@$?v`u@c1wTQ zvz@Pd{ms-?h*WvO8ckKpwgU6}igWD|fBn`9>*1Pw4@J7GA7>G1PI=C>h2NoiMa3WZ zQPa=tt`yq|KJ^?%iv`d0PCuIkrop+#0m16b$ouhxR1zE9Z5~8!b)*QN7^7;pFMt5I zWnew005~}*SYcl0kdk!X=R@-H@F##D_F$A4$KtjLRK0op$`^9pnw=QblF7emU*{H= zt9&sn3wi4H@pm91jeh$PK+0bCCSSNPRNxhHuwh8$G;D$0P%fZ(^iXZgE&%$CsYVgi z3_SQbyP6&43P^p(^>5p)_|o!HsH3;Qp5`%7ihrU? z>v%&0zP`PCQ)9I(!zU8#S$sjJfs%5-td4cL@sSfEfK@XTsbGm-Z2ywTU3rK6+Yxvb zR9#8B5-9sPqVLAD?*MII-3A;Z8UFsx*<=Hk9?(V3^7muCIr1v?S?Dq(%vG6$0y-|I zkE^7dK&GQ`WQQVK-P-3RaFj-F?t{k3e!owx z$AFPi)1}6z>un!G0MS#HWC8Np27r^DZ=_)moe|Z};Nz!jAFtS&Aml(&JpHil72>2! z)*ky6!AqK>?v)$P*H^KRz(#Z;QFnLYRXzRB=K&E!0l*hecL7^0d;_p(kT=qxAhURQ$W`IaZ-Q(Hj}r<Rf&T0{yg2J++c|gcrm?dmpv6vdgVVd-n0k+B^3kR}b7N9IbRGK{=g$o7E4J>X})^xRH@x{5(ufloxj01*8an_*1va2xPRu$N+k5b=&E zlMjARANW5bi3-D|Iax=~X2$goadK*-@gKZafE~ew5Ahz%Mn^Dk;1FSM*Z!NW0A(_} z1X{tWy-GzoMS!C9%?uiKnM&A=fwiGl&%mFwZGq<@f&lNn?MEfWavT15apg zN$3y=mmf4ZDb|47aO+X2CB>lNdt5G|*(l{BV%9y2AV6v&Ix?iQ2b!_p(EQ ztHy&pA7Eas3hb|39nhD7?we0>r@s18EXVQQwG;Ym-dGiO!5ZngSW}emhWHxp%HkJI zKo66TU1J`JNxBE(-p`y}K1EKHC<+@PUH8a3ynBzAng2XmA3SYsY=eu$VuGPGkQc^6 zH=aRq>xgFf+f+l8N9QAeStO4mVnn{o11wC>%5icM)WA2#s#6vUt@A&)ndB_h%z660 zjRju1*8O3!?&9mhSpO%h6#{VoyQ;5aJM?+aHt291eq22Fe1XN(g9M{-8%SMKy@9MbUKYmWOSW|*y>#5Dh9XSwqKKVUFX8`>mq-ug4$c1 zZEj<7kHjA3HCmaTg$sQPW)@#&ZiF;K?XBl$AbJ}BopI49uC;et8KVQs19>x(ZLS&k zuEJG7dTSaHZ@-?vE_g6|?~=M|c$p!=6aU`7kBxC;oWb>%Ug+I|pd=NKWsPj485u!J5aS4o z{|aAZbN>dM4X3EYyjusz&KPyqoAgy(Dl}JB3BF{!EA*SKJmVWPl`|qdfYS~-nF&wG zc^VD9?n@-6JgDmt;JFC*cgRO#04Wb&+4bv zXD&w-u0dT8X!om+M`0hLleyttA$DvJ?|a0vL)?@=yya8NJ0n5T5)yDX&$@P<@Yt`M z(vPX0Dv5~K$e(lmh;8kmo3QRhAnC6k;mAhP5<>A=u0|Y9jm-kE?oY+3yvb?iHg94 z{O%=g)MRa|yL({*4mfr@6$QQ@?fOsl*jMa2HRF1aYxfrW0sN z!6kGDu2C-`2CSX_KQ3`Z9OrZjFuU-%u`fV4=ZDJmZh;R}E0jhIfjs7fzqe8`9?c}; z=^hpsJb5XJEVmEHCeHuzOT=;X5WYCIXxOG3+=E99RJ=2T}6 z-!XK&x!RmX$R^RhJf%0y>gP1yHU|tnn$i>ld-;!9imHhQK8GE%l>hnkNz0YKRHHBl z_{E#Vmsj`Jcr~rNPUlbwqzQ$)W-iGxtbYCoo!tCFUCPitQu6!{(-u|S{i^%E#M7X} zQF#8(!ug{7+WWDx%%#KibbFsxrCBLo6s{7%;-kNJaPLtROIW>+b(3p>uvNckJ zobz})bYp%H&VxN0&}T}5JwVbZCf4V=LzsHhZmC(`&Ca=@{puEUE)8%<3TyS}hG=bh zWqWphpy7=1A<^V=o4(KAm2J01A37a`>qdNs$D1pR@iG6j@Ke4#oy}@l!I=q^{@56L zJE!O!Dt5bnq)R9ri356pB6g9%L!^z*HrN)oU5Q4N9UyPJMH;8|efnM4bZWIf7CQId z4=kj$H$>LCaI{W6FpnwutY2*#&laz7wBMN3dlO8T70dO_`(TX4wh5GkmED$_T_9;T< zrOz13A}(n5A(X;hHb_L52-zLa?<1r1Wc%j)%@|Fu&&LKT0XgUs^`+U|d@fZp@v0(T zXpvzzR_rU-245ZiMQNDT+gs1TwxYhsOSTp;K5E#S{``px9bF`)na<_FxC+g$%ZAQ9 zPYR8N#A+C|YW`tb)ko_E;Yl<0vm2-N?F4w5LA+XbL>e(M6~YV^q(z9SGKLXC6JZht zm^|4w$cgRzdjlr`lIUkvH#*3iA2Hi0bk5o5nEJ-zAw-;!ep$ARy-#Xg2<1ioF>%cnZ%9EEkRr3@`_Ado2$)Clrud5B|vy&qJ! z5n9}MP-H<{v>WCu2j4s%d;8>#?Z{^n+J&1>&^fhuXzT{ZEBf> z=rXs4<}=cWhiV)@gjA>YigQxZPR@;(CC3bHwJjEZOdN=uoHi$CIN%z0-`!^Th z&+Z5oi|ag>@CE?!A{kMMOqR$2MtXw-x$csX+zXcHhhM2WoW#VyL`{*-m~llbA?I7g z!)p_!S0~Q%KJC3D?xJ|1tB#hzOL+|l1iAJo}{T z?gO8vxkDJrtOgd?OPj1Ep-%#^PmDz%LC5&H`7(r9aIHMz_g;nj%d{~|sbsaXOW;*# z8qz{ZR`wuIh6KNK)Yy{z=yXb-7)2|;cKi61`fB?og|}*5Z@vqw(od#}S*@>TZtF0H z8s^-gZl2e)%2Z(}ZE7b2y~+q9kHPQ33ol*i-ia=z?Nz-JR7%ie{A~P$>*Q~lxzM7o zR?%7s3T*3yHU$P$KM7hgWUP`M0}w6=l5B?n)XQax>U|aYt?ij6cT4$Ob1NBIPuk$>D;^d=-l)Y zIO}d{BTz!W5vDoxOo@^ZSevbLD9{ErDr;r1qX@zZHyHaSP9ViSC`j?RPaU7^-;6IB z$zCr|)tJ*jXP#=fF3uyHSIbv0K7ogF2p)4d=^GvHej^H6c-8G;tb;rupZS$;_q7NL zQUQPGWw45ma4CM)2BB&*+L_$-s=yTFk$5u6%c3-QPW1LQwQfkwv6sZU1mE-caWcef z{pf@ViH-}kvMZ?6ZScja9uK8U;f^ITU0G%{=ngJ{q)C=T`3K5IX4JlK-^$8W)8I5n@QnJ z6jKgOLM<}B^)0N#hVRXVvWBW@rZI*w4pkvUS;BU}yuLI``V?>@c{SO!&&*Hss4&55 zuD=2s&~PntEsq#^@xa!5%Fa1ulnu_!wsQdQC8X??m`g@Gp=ikk=& zhX&yc$ll;%a0z@JKylBy+kc&R5m3N^oEcu2u{AGg*A$#3bV_0WCvsVSj z$0aEl)~Ud3s#omi)@DiJEW>wIb_xP%6j;( z&+lI&vH_m# zfNtXJj1OXUxmpz7)kCBB)E3<>v}h6iggy4m{0yq^r(A#QTg~S;QXv@(d7+<47Se1X zg%Ox`&e_*<%u0)F9;@2nM4GzQT7!U&K<0N0zVpK7*~Veh7oYlECd+U7HG7Vw!CW6q zHMZKh146a??Mrg!0pA9Li&zpX_xKak@l$-7zDrMc>{b4?G-ydDp((q}Dz)YKFn4Wy zEhm}0?8e!UD%*N*p&T2m>ZK3UbZe zI_Njyo+DSh`=zf63p8z?7$~4`!~B<7|tx7@Pto+^)H}#*0NW0uD0T#BeBCW zz)sTjOhdm{jCYx98A6*MNOqDT=1{n_lG!tp7#PZDIOk5KyWJF@BaZ{7U;=?Mdqi5# z@r6&pP&LRDCR*kDRamRo1-{-e_W&0H{nD+}6_iO{^IFxEA0Xv=UUZL$WD}Fu`Zm z@TZRM1d|Cpz8hm#mhaonQ)eqK6@hi}EgnIXpF<#F8-X&FI#?E7XY`$yXe=7Nc`QN0 z{6>fxnOGr)t9*buJDS?S<<|4Sa67sf-}@|>O`O*+s zosZB|cV|WPN~o^-syknTaqLw~Cc%7LmH1PPcO6qM?8?o|d9b18%bCTpzQ@n<{4^G! z8=Z;uauG!JFzLD3I1nz@mbgA20+d0?HpXBjzbwcq4N%^ert@50Cmy4dp z#)|2TL&NiSa0aTlfmoYhAob{(dU=LhO$HW7U|X{qJ2$PeoV_Ks2qY{^?hPra5}%cV zqh{)!Eu8$0D7m~f^g(=@`O6qmC@0v0O9={AV2?!F+2HGapeRopRJ+#7 z=^nFG%l0L`HWD|O)*Y2!$!Dau*#CE(i|tw88rr0#SozSmLV8c0lY{p+y8U+Eh3d>V z`#?IybV=%jx^rbF0Kj>9MvXYjw{&1+rc}wK4iD_&lmT565ap7?K>)T2r|N}n#dh<4 zkR4qmu#vkMjKDG)+mW%BdmKa#J}!Vr?($=C6KKJaUluE59cKSVPyJ;kjNuKRVpJ@l&rQjft4hSFsuf+~y*$10 z=uWwvEzgz~C?7l*vl1=1af&jq`o$aARgIqhv%DB*ySzb3l)#5QeL_j>)XTIZjFnR) zx&ur>M?BXB&{fzQmNpcOiSgF~jHBk&{E!ec%x~ZsUh*vd90`Vh5jx%Nu3V_L(Vd?L zEl;|2%EMjLRIjq&*GEuzpQ&Ek5u6bpde1rSm-wOkowH^<;+^2H6(S?D$Ew7dM-W$% zU+{2`Rcn3g-LTbtiR93ALt-QBfRvPIo-9RSG7%4v?*=ARLQe(=&c5AcqF<0INY@P!hr1Cpo9jJUkjzJ&$FLn zjVphV65Hy6o89yz zFK29>jx1XaZ##Zg`lpBDy7dGIH{-|V0?gm22HSFTagy@E=qet%z62|Ngw@98cC?i- zQnb@k_Gj7=hkBGzz0F`nK~^kIQBWZd0Dn8hM=tAi=nAOnI!nYr6yri*k>Y@i$UDr7 zCJsMNK5lBt=QciD{3bu9JHc&JJrSnsW>ncnx^3MObYAcmvm*xKN~5(i8(Z%=4q^9z z_f(K*ZpWF~`#|ITf}Gc~ca;|M{oAUtv-=w;tzt-~D*wBtxmMb-Nxw0;dnWv9Mt(A( zX^YyJ*I8e%xXQt_^eK}d*1iSIwG1}+b<53$va7{7R*C8$M&bLjcUBVZyN*`=QF*~c zyRFNwyevT(uoLfoxJv-<()hSb*uYmsjO5nx;3LMqkRc3N3=j3sYvwYiJ`29Xs#Te* zHjPi8&xNaOI3Mf;phspT-e6R5gwXAe*UkwB+*CItqx(BKNS`(Sogm*wiX}r9%Z6VxyOx;zt&Z9*1Am(12mw^hYD0Ts`}Ut#(5U2P;g*aMdsUb=N>o znEaQ$1}kPpoWJXB_uI6ui@&NUycgayN@`A8Vt&Q~H%MDCmegTYh@K9bM;6A#PT2kK zGDDiS#_AGlnJL+?T<$4{?_HKLLiNja-@X2G?ZUy%MttwHZoAIxo9Czle_I28Upz#r zjnwSAtkQac6pKLF;~zkvC&X;r$Rbk*dLzi=_`X8mEREIQ(dp76f$hX2pyf8%>SC22 zubc@h>Ya%*26%*+JboXAYP}oq*1|dv6}7I|5~gU1LtO4Jz8Hc3V01%G>RpWhhaw(vBv^^hpf&$wzcl}}`>d~Fx z9$BCsh%U8DDTo!C?kBWu^$j7gygErzVXA3;NV{E¶NM%+d8IdLX1kW2aNF*EOI?nX8}qZtO%$0vk~tMecD3 zKmmsA&y`7a4!-@Ed8R7;O9M~D9_E_W+|=^D&+c{gD2VDYB)Po3gTgH4M0Ehv!6aHV4;cf5<&=)7*_zYDD*n>FQ3$Vb_W;zSpj7f8fXEc0( z#`~dO8BZtoKgLL%?j(8Gs0?6F8w1Iu)qi+HUF3s*hy(+;13IF(?gQc?u9vvb3e10g zhpWyZ&i{u9IIDU0)wcDkX3Ht?uQ^0rJ{+OL)o|{EsQfy2A))s@{xGZglFe-$}q{~ zrY)Xs|ABA(k!CoR3SB*Lt{Eh)va%3m3;;m(%}w<~X38m2eex2MNtpGfZpy_$7TYRA z*sE-+OZdR&DQciuTdH~L54+gUJbwBpS;&$EW#50W|Fw6(D8>BVUcg*i$^BntPxo4F z6m`eDO>~SlAEkUX7L*KT?xB>Kh?tYvTzA~Ti3>$P+ZKm)-NlI#AWr`}C+M_C510lH z?0l2$@xnfl3^D`GB!+k)vsDKqnj(XoL+E<892YS+YL)x;2_Yo*PqLJZI&t#tz<-h@ zv5n`G(kD6Yhqh1`V*^XbGtVxZPlc@5PyVR~6w6*bIMTAp&e0znpas#iS_pYGXAJTt zTD~_6`$4hcK3x&8naU2+C0kS{B2t8~xhf+2*rs%AXP98C?yzGVjhxVt_bRT>% zv0HY-#_LD|q7!F>fbb;Hjr>gZIF&MZZd0HqoR}j8rge*BLc&MVeCeyy6SF%|n|MYq z%QOinm~3wM8q)7ZhO5u?km85nifBBC7_S#40@^m&qpttgl$U;?7Hx+8hE<<6lRiYO zyRRSlf^GP&V^tD*Z0XvH7UPdSa;$27H3^{_W%5t7dNgyMG;U|(v*ZFcf10OaU9K_KjUr0Q+3vp*xg{#(s+UNMwMxMVTxn0{I^8Y6?es!W$t+ z(ybvC*5??6Fd*6%{k0#d0_5!a!p;*Bz1&h0gM#SHZx?#ZRDO{6l1MQw7PlNuzOE4v zm8W#eE>CwZ3E<>&4XX3$S}b%nE1g8-g@JF+qj}F$G#1LG#*N}p_)sR~7fe&86MI2b z^o676p418Gqc|WZ6H<^+CD&Z#?ar8Z1_sXc7mJ%UIZl>W#s&k@6C@M$x>izLj25pc z5d2Jfic`_%6^nW8Ri*e?i?TZ=$DTrjqX)O8kFU%Qb@L#=vt@BGtI$g7Ki`f6eL9iG z);)Bj^Hn56OZwbkm{kaMvL7U@{q8L5pRmzK2|=kY(%E-aU61R4Nn*Gp4**#X=t?{* zCV}aB1hGtRdd$e-!;=Ld)ziyu{rcVatNH0VV8A&m zUAawBPUW;5RK~qZQT^PJf8wGeU>F@yPfX9k9d94lNN9|Ur!o&Twu`>8u-Y-gS;kB*@^w7)}|!obm2yFn)evgKjB%e^!23I5?KN&PgCw z^4nl%_K9XH`UmJUEkE2V5@Ugd*zbiQdWpi;d_A|_)g%*MTxOYj#R&UhEz5&TKP>g3TIE{%>U*_%rJ@R7gl2t2W3?M3(n}PM^hX&whEcdu7^5R21Kx}DG0iUh^ z&@a%*e6GiWYABV;gyC^)xB8T8F~i;A?B6>2=qwT|Jc-bVnp9_Lvx?@ibDpdx=QVOlwMcZf2OcudqD=l z?=G;fxkD6QlenNKX;Jlmbe%Gn;Hq99Nh# zXzIR!B_~Wd55V1tNg%%9=w{1B4;h}0#c=-W3g4l}`UXS_a%CqHq?9<7+Q3~!3XmiX zRnW=fdB^xzrXl=NZ1Pqw@Sp|sYmJszrJ-Po1B>PvidThJc3+E(-Kqf)!$1&72>Gf4 zJ|K6?W&c|_TEW#CAS0tN-l@Xjp(a?~&7Kr^_4C2J>Os%73n2iADKC@nyJ};C_iPu( zhlc-b6fo(60z^&_?IrVFu5J60fVSe_bcc_eHt-c0?t{QaulZphHl%}>E;``RPc^SK zX)Nay#U}zRV|O>-=$@+V?5x~Gxli4PbJq!vlimgz2xx*0%;6{bUVUodT5Ki;#g3lu zAQWF2$kAq|M?R<+`!B~gf0{e$_!6Y8G#iPJwEt*V)a;=v$TIcb{jusX9ln3-lWY^u{d!wq zjZmQHwI^GnoNM}3YyLEXn<&B4vD@0N8UHUd1#}H$0lH1YpPi1y0Zo-!e7l+v#8v|$ zSNWTUJ)8$}yiV?e9q}okqw5DjK@MweKkX9y`+;G@MKf@%)wq#+HyETLjqkT$)g1aj zE+wPabO^s_MqjyESBvFk zC?gcpt{Ye*wBazWyoTwJUS*HbK7c+6IRmB6BB1jWz{k!U_ikD31F1yrES*fpJS}K~ z&;NrdE4{2M?IV(Dhv_8c^VPZUfQ0MU0^T5J@cpii#Nl_~CFOn2M*98o>j1!NLyx9= zhU8s2fZYAO|JQ}ZwG@s_k!o`;%Dz!8q?dVEgNmpD)Fb$cv-q9wa_CUlVTbv35XCh9 z^#wQwR}|DJO)YE9z2885lQYPRN*BP#(M!m}I+n1s`Hw(2R5d21_o$`I4oJ`(g&bBd zOQ1jsyWkck@lZf_&em<5?1`aKIqkEnS zcMb{0zM}igF4cRwt+nrm)dX!g(nE@lu_!5 z)NX6Rfz5De7)*qYv3Dk!6qTI&D!tIQAu9z;;lf){&yy);x~QFFc)IH8(>gh?!0Rsm z`^*6lF+k4;F?d{`MGsg+MKeh4CQBe6o6-1B+$cMA8_27F3G^VhaY>Y;r`1Tj`>NA$ zBkp!J-rmi(v!G95Ul#|$3tCeok&<&SKa{7j!!B~=ftu-i|1TpwV8T^+mY&C|Nf&YxWXC~0KXVYKc zwQdcY{J2i0{5l~a9MqdGQ$zX=aPi7cwtyT`dnG|dBZ7-l`i*DsLjFw;OO??gC*HI( zUy3>N&K&hx^{B#7RFL?ts8sNFiv-s#XSIIfZh3LN^?MR{?^Gk-sXvF0gmNDP2h}#) z8BWf6DTFAN9l@=7<-UBM&U}IrduQSK3cGaA4`mnMzfn1Nee|LW5YeJ8X~ow{*y1zs zS>1Pqd=iV=2v96X07iQpXx#dMOwLQhcmau~;pX(3=&m-aM?*!tGM65J?Zd^M^UbLx z?gAC7aP^Su|+7?9t=TYCW0&tpmn#r%d= zzb)M%CsWX-T{R16yiSukdWxDZXOuD93hOWjd5}(W6>xeEVl7>WHb5cu3-t%&aOlJc zyK$ZRFeiMtnBQ(_~=7B~HkS|^7bnum(JfL%LJ17dc8Iw}1-A`XKNR>-@qQ zynt5N;|rA{nUucyqonDUGY7G*L>W(##qYe$R|u`Mv51<$nHLd@U}dcO1)L6A49K}7 zZ>0E7f{n!JG|OgOJ~YQP-X-V>uw4#I(D$^A%-fv2Obw}<=ZwCms(4?c@*0I!m|HIE zH#K^y7j$PzIPSJ${;h9l)i6sTrx6polOUJ?&7Ol$+w(p#pWQfO^^ZRBhDkgw*9##{K#?;tKA^T*pN){U^r9SlBk1+`kXW{V%Ac$o zzV8nm6Cm>(JBURN$7Hf7ifzPt?oedkTnTRIzQ#x?3&DFa-st(bw8B*?{YH{-N9Sh6 zK#LocRE$Q4VBeq!Wb5k)`EDM?&UNfbe433}sattzl@8<0-x*me;FL5w6!!%06M;*D z{50OCK`Co94Z0eY8D@wi^THy?7W}5pO5K&2SRy zQ6K*>-uceocjdF8=0fV(G7?u$zM60GtbHUmvP5>+Q8e82h|$dOw4R^ye>;RTHlA(c8|M91ZOU;=vvKrLU*q zelfUdGD1>3Uxav!NHlGncvkXn3^EvMKzIa}qQ|19fabO#;xd=FHQz}`Z=p8+fR2bV z+bKF-SWrXM41Q!pMLgpqQM9(++LQpnbJl5wct(3Z#G#;6blC3rhkZ4!8je# zwQpoG#+Ndzyc9irQwi`YF~rGpgsbsyH8Y?@euWNB6sP-n&%ZPoU>^gA%e6B&3U|Y7 zXpejaoN0v`okT4#Pd^Mcvc@iDY!hHr83OSbKlyBwUzjr9XC(5=UXM7SVhM`3;lx|S z$q-U1DlY=kCE93ddBS{cSdBnZ7DCiBn)uFH9JI#!6-B(%q4pw`zwvGqfw;_HPk~TP z{oJn83JR=R{@u;F+Msb!29x^jFHDU`)?U5=h1Ph7F3M5|ke3dr3CwD5KvG@V6`v>v z1_FN7beUIHIv?aG3FAv-&1Z^v(z1nJC(RaM(ex5`J@|ED?`$eVA z%_ULpJbCpJ&VuYbze`@-u>e(iXe;XgU-ycZxA|cB@O^Qc7aGxxkH$GY9mSFll z4ZIAD0)r2aNLF@Y3gnzT)%lTEQas^N8z}Z-Mg>XCDWr)*9ADH$x`>(k^OquzW%M+3 zgZDvWjI;S^^^UJQpIKrF;9RzM{qG6YHzZx7635tijo8)cEm7I1xqW6~bKa137L}~L z!<_xvS$?nflGLco3)RTux3-3E)CeC~1>0>GEB5I|cZbnOHtC#s9iRZcCWq?#HQ(Wy z+36*R@5_y21?$wuv0u0_8oFCX+2wj?t8kJarvTOf%pX8~HgdJ~RZ#5`ljMCln#I6s z4PK;8<(xRR&Q2et1>ZCvjfFNXM2uy3x_!^wZwS;ur&;+5ghwpizQE$0f*^?4u*cP9 zq@i|rlfw}m`bd(@gk>XQlJUv!?kVYqrUYX)J`DL>BbgTo9cq-&S0JPyoOvUj&~|T` z*#^ZKewP24b924rGlQbiyIV^SKmW}6IsNnR-_Y=%>n-h1*iBVf)2>n_XqHBt3#JRr z%BQ;im{rJ4F84*?&~fjvZjaLwT5?`8Yxm^#^~SgcXvk@v_!!VotwhKCw{-8_ZCuORdC*W8walc#Qb`rZg$r%h9}4>g&4 zkfDO;)=?hV1{#q7p_gBu71b1`q&|PH!`D2=kf?r=3a@j4^ZGukC@&A`4G8A!{94o4 zxa@C?vP`9=wBuY8R0Ui!aOJd68tIZY<}N}u;Bw&2Qag*hJGXIVs53UbF*L8KNeSj3 z<0K1VSW>Zi6ayNbV&#LuE1%!)k6~iFAuTPf&R$ehls){z>MkNrFSUt9M&4rDz~s(v zq$3%Mg}Qv-4ITom{s)9L^hn+1_0h3cvM((mkfl5lwqo$=c$_?j)ceqa$Nddi+(5FY zSmit8aS>-s>gdqWtmf0}s{sqm6y{C%gL^OUVMweM5CM{7-RH=_?;R>|Bl5=$B>*4W z+~Qa7f#)6+MaC4zjszjlP(GYx7T^nA(0^qot!F&08r(;P_5Fx z65TN*K2~#H_@PfnYPIw3A4A_k=n;qn@lFl4wmW1C*Uq{e1X(D;o=|+IL|qYPgo{q! zJJ>2!`vsMp;l1{O!IAX!ce9@@;RA{znNzkR3Ql2$BTEH?Y-s|sZumfw+&)mBWo&>O z;}O3dx0W6|`N*v<6i-8d!|M|ZHL*@te+X)K;kRgqZ4F6llTGt)*j7)5eowv~=S31= zVN1c9H9pORkUDyd{3IYN7o8=f1F|&HN)Y9TX*Subi1>wxZ#Dq<|ERiYl+Ua|*A=yhI79l1d zf4kqMRJ3Sg+2pv6O0X{$_DJ5C9PK|2TdHBvx7$3*#LPU^N?o5}j3*;@h~GE{&bqi1 zy<*t5`5fw6jVnbdkv6(tzdrsPVPZ1JC7nVw ziPaJ;sH#%n`+@G$cTF&s*OH+gK)#&|RF1&0A^_amR@Ry7H+UF1V@B3XUk;VaM>?xViX z#Ncy=IKrDKw|rlc-RYEhDz$S#;q-LQ7Ak=_J-D;eWRfG=$LCI;Bce4u&>xXV*4X+{ zPFN2Lg-K;E&)jB(VL%Ah_OC)PS-ftM%!<2nn`|+(7G(T`_W^Xv8R)socm@Zd4FQ>13o=d)t>G#aI|b0Tg&mT6Uo73$sa9On7y zS=d{>DyI{GQ6fVku&1K^8eohX?GeG_VG$s+%5tDxsbKx5p@G4sUrX)4QABRRU90;@Dc|jGK9VU`TKv1|QsPJ>ek^w`&mFe)P<;RcR#h zQqcowOyiSUfe#)E=i!PNHnFv}ZNGH4EhjcM_Iz_oi;0Yk3}ZCrk@J%z^Lb~FYkMR9 z?SIEUeY$zVC!Gu}tR}g&>muZ@&b}U9oAnM452tL`5;qXJA9|LTmwiT+Ok>I{cRPuL zMWgi#YvbeNygKxlK(wuV!awV|drS z%|aDqEQs&~_@A1(I+d%ANxji-z0{vPiAItM{;)qB}N;>pjQzENZ=VsRXB)`NL zo12?8;5b6hB-UD*R0&ypuz=g(4mdtZ$e;+o(mQwl?CtHH!07cj*wkjo+@16E_V)I0 zaan3l|9KRTgYx6RKbO)xHndRox9Xl(?M43-6%)HGbHBG-DHzN)g7xLv&Icmmcz*wh? zSFbjgEBoz0V5C##Q*>R literal 0 HcmV?d00001 diff --git a/control/predicted_path_checker/images/Z_axis_filtering.png b/control/predicted_path_checker/images/Z_axis_filtering.png new file mode 100644 index 0000000000000000000000000000000000000000..0c064aecae42e81f470ea19efb7422c412173beb GIT binary patch literal 22632 zcmeHu2|ShU);B^MVVg@bY-4QmEb|mH53xzYHqY}sWXP~HR%Q}XnQbJQGG!}7C{#ou z86q-d`mS9(PtS9nbIv!M_x;}Uy0oK+Y3bs{=W62(n!tM(H>88DgAMX$A0YuDVO{|dUO{m~@K#tsL=^lH6z7u^7B~Ib z-_p*;^>9Khw?GGHXG?Y=RZ%_xFcp`Hpdh=jB6u>g@^SU{`T4Br;OgTK+WaNNEW|{9 zwt4zurxlRo6F3XroVT%das#sn2?z>+|AL@N)xp!o)(s3(6%-W0PLAEGjkl%U&uywI zxtzVHrkxB1zMwDEPY{?T1n zgP5O#ToLmFA?2&idZ@D9A-=xgDur>UdhX{v+m z@#}0ybGrDS_q24k*K$MJID;vW{zqgMKRgpM;Ae}VprG*2W$ZkU7CKxP+hE`jc(^0M z!+~}_4oDlX!@-9~baQj|c5we=qqUo>tBv&`#|~$-^z?M|`(rm-H|N9S9rketC;!9j z*aib@OXt6BHFiLH+yCquvi?$X8bsPkcim%Sy@&F|;Hq{9n@3oBo~md-wh#QoXe%2l+P){ryZoj`@!Xd&C1E(l%}`Hr}2AV9$QP zv;K(l_P=obsLj*H+0xs=_m84uc_@K)zfJd7cj#B%{`!pNv#y&1&_zJC{1hHR;X~FO z-YGF*$)AKgTFJ}J$J5&8a1@p%|6C;g$&w;}T2e&pPfL1RdfM4||8FcQtOPdq3$6b# za}Im|5d44E^Kb5l;9m*lp*H$QLitCz^RsvGwlQ$G1gce7*$-&FKMSch5P=_>nHLZZ zYbJgOy^W{0%@O~8Ho05=pUdTMdiIYJAtG`3`xm(t#IhK>m0tx{?6B=fa(esK=~gu)K_<)ZviYaRLwf1%whogM73-m*1tZ)||p zQv?pQw*zqV6b@gwI3ST&FXS&V#UByip;`P7MRdQpkw=0kdT5P*(T7K}`YWJ20v|`h z^M4=Efmq4kx_y5o1(qNT^8cmar@wjRDf|h*h#dX?_mhJE8C?In^Z$R=MThE2wD!TJx-LVsTJ{~4TibdXdFd+{+E|>iFS<*_7BLpHJ;}2qHCdNDbL=yX;YATy;hA)4?T2Jm4hZl zOaq~$0lU=qI(LYmT8shXi)_^X&F-&kogp*<5Sb^x6!v_|jb%vo5)r<7F}T zcIWYzk4=-=yOVBKY>M&_2rdgVJ~4Au97b_R>F2L_7G^Zpwd7DbydUpb(E}XNACFfH zAhGax#UjGPap(cYf7uHz^zk%)3s=)x`|6gF*A`(e-`D4@T^l~85mz=XJ$%#K6tLEN z?n{&P#mdVrYE9iP>?dO3~Y)-ycY>4=D9HRW{rDzVk6&G6fWflK%wGc1ga)s*8B2 z*1ZL4EC)=*R+@6YI>aA?ucYHVz=yZ(2XK8%I`^&_;oS@}~jmN7Wy0ZnXpJq6Ylw_t`gpN5s zo-Zb|E4BT>v@i-NLZWl+Mc95#z(RR_S&m74m(N9F`J|v#Y%65I+lhyJ=ey-k!f+^)g_jk98FQoI$ z_K8o{@_(_Kz6tj%5(r$kui2i>&aQbO`{p}`=Yg#K0P|Wq3BQR8vL}`ORXPg=R zhGPmKpPgQnRkuVEQE_B|CHe>HCD~Ehxl);Xo0F55{NDN4&cD2SaqY>SkTwFk40746 zCt0Nvp*t>Kt6g;2@BAj;$uovN&yn;ht5k;JM}{YN{?jZ5jFeOaQf>!m;6 zFYERC{zW z*hR~ty{6ggOI``zVn9#vmh}hPU=1~XJQ!-d!ll8FQ2l)v%Xh~cT4I9M-HCPDK5Ra>nANm zNyv!OM&Mg#CLy62&u;PTL}2o}Ux)^7g8f#v+%q zdOK{tVNbjOEy24S^J!}HSl8nEojuBfI$A_$SOQ)qOVL%gyMf<6UmV)nY&~#!5jc}w z(sZIMW|AautnuOw`@Rz&I`)8`K&s<%b71CiM*k}^h!YGF19VG@G(;oNNSxmPU z!BAV3dila{tx&vhy_rt_zj%8^n9YJF3m;iMa?xz8^I9^&J*LsX3bD2nM;QDs2SLS0)fG=#vG6oyt? zWP~EveCKy_A7R=pq(`#P;TItub@0T$f}yx{lcq9YsFQpe*NWKy&8$#h0@XXcx=0G* zUV3HQ`<=_UDJoX!{gZJh_->a$bAu9)oO4YD_Ppp=yl*EH&!S_e3Xt|1V1xV*F|d6n z>SrfZXzx5JQ$SwR_`3^NGown3O0%$6*BL0%3PG&um>;+kDLG$s!&ylBd&7Kjc5e^o zZRgU~I=9IQj;@85cTo-l`By*eTLgTh8LG69TdQx~>suAoKAq>)FIns`R6M&_^TIbU z&P5!FfqLyM)xVJxlzZv@*~@$)Bo@dOQaRIcB|SkvCd8{2!izG(b_r>E>P63?8IqR+y=YWVa+5~iI3=|~L& zR}$LB=W#y*If%6eA>X~Er zYwL2r>O;qoXYk8Sqt2~+8?!)9)sATo;tfD9mYdq#FCVCPvslBhCXMdDIQK<*ExmR3 zZhQ6Yi`#D6=axjyiGMG>Se~`SX;EWRYoF4v`t-C`0=;N2&|YpEqt@gd&%b|rZ6Qv2 zT1-SKhD=l|OQf3LvSm11+|4yF^t*#_=&t|JJsqYU8l8E)?*Tx?@Uq;YS#c9}dR4m_ zNvQPd;^m3q>iurHeZAzZlVNXM>P6RP`)VZ4c=Za}wnq^Gv-7Z8wCcgdbXM)!3Z}5# z!S%(lA?dC6k;Sg>9v8d3dFUIu-WHZ5x9=}(+k4W??lkEggL~T7)<5j#iUH-CE_txK z_L?dIQ_y)&C7s|}I0hzQAF?o7ZQF*rIXu#If~4ni(2~u9q#v*|nwdgfm%{eGrEx)9 zmRonfUbq0si1Wv_4t$o@rN*`o z9$+52{n7SS8RFIOdjGjE86Tggn-%1z9ou~N_G=HGP~f)_W39dK-Fj&nRHs+I-iyEKmQqLDCW6H?mV(0oux3C!0`98*lX1PsM_ zJNRX2Lo47NfrUh`j4|ESN^24P%g=%xSP#CxZY`U{Nr5XXIDQ7%6V2~i1a#uXovHdXUNh6%$3GMV%-rZIQoqsL9!;_&3M_=^ zm*&m9`N6`N5pL$wGm+%7Rh_d@@P}_)&!KSfDw+o5_ST0|F?b1tFCTv{DRmAjqMOSD z3=q9Qo3TwV>S*Zh6bDxf{VMX7@P{^HiBBcqzt~d16tb@8HEJ0{(_9F!KzFFaT68Ze zgU&e-{^+EZ_1i}tvo~~P5jupxkYA6o_1JvpmklIrREk7=GPs64J-(9^G0;uKnR8_W z5RUZmX=(qe2S%l7J`wx~bm#uXG`N8H)FoAcJ$Y!kS^dM#)n4HjqhSYgat98g?3_qk z2TOXzfl+}QQ*LKV5m&Oz8hy)047p#znI#$awzE218<%YR9Fn^LE_18oWrLv#%*;sf zl;x2W%#UP!%#-0<*CJ(_R8X|9q6{^e2`+aw*>Apale++si1k9Q0$`B=2sOF_i-ZW| zGE1W0`&~|x@Dw!94gEg9;j}$$pz|#nzH(=U;Kg%c&Mg+l*A*_gGrdImTuE@|<7#}j z;mqk1%|Vr|ga^k$Bw0vy#ge%rTJd8<`Gc34UQV@UJQY~@_`Nh1&SljZBFHoYfX~D6 zq5YjNZasKIG#}#N@k>|s7-@idxu5JO>#Bf$UmEiC`l(mfr`Yu4=(s!EFHpVN1`xtA z_gPaDR8fV6ilItbZq;g*AcK|U^3&1DRn!#@DWD6=b#T3ORHhAU9xbAuJAV}4WnLdE zHRj*>NU9+3*gM9H5D*)u>bDW0#lssA$w?;IPbDCs9n~*xS`4WE;5>IXaTZV2_;SGA zQ)130dgu%(pOO*G%65L-0F$kB@Oc=?9C@UvY7Dr+Qj58Xmy+SJ4DBy;n*+an+oL#R zgxODUJGLX6Xm&YI!bR}Ld-sn7qzYFqKAw)}DGS57VUP$9-TP*|)6St0n?7;&UXGQL z6^~KLOdLEm@$ttrv)&GJIp?1Fw_Y#m$ecxT^a@qSxOiKZ-=z|;T^-sftLYb#+g*ud zKfg>8rp40MmSBO8elqn!l1PnEm|>qDf;ahM@H~^tjzxo~je;;WJu?!wr|R+*t+4lS zwEK6i`{p-+I2R~Sj*Y5>(Dpy>vE}KvdXL{-8KD|(aeT>sV{t6*n6fOYlNV9K0SA`U z^DzgBqO73bO->1y>ZzTxTYOvJ`E}em)(R5!1v00G4*>hh)#LaBTb4ga&wX8?L?}0) zM6f0_=!qzz^B_&6LmzwpBeE)1NpOMPS0j9#gn>yoQ;pPMV0zDQJ?@DIw)!WCVHWPJ zZ13^VkAk|Sv7+LrII~dvEu(oYob=^}gB=#!o?}hg02VF$ghgUXuzrXK2Ry4rpm7n6 zA{}BMA?^?2FTmobsmsr|IfRIn&yMr!6?}1aPKg%I(I1Ofx|-9)Ou<@&tL}a>Imfuj zXA>5<)tpb)Uzdy@<1)x0`KYH`(Xe2TlIClFHw{9foBI~8E0xy_bikbQ6yQ~v3>u(@(w%qOl}en+C9;T+bp$rCCjOzdYr(NfIk`c7@>cQ zvCM8B%ZZ|6LQ*$uj}NhC#;PajFp3HBKqhC+$(jKX%P5nCubSkQ9^x=;t2 z@cbCL%Vt;K+98d%7Ay`N3CS4jhvw(*t*{KW14HfL^iY`v94~*_RWk)(8T~Q(CA>G^ z@)aV32e|=u8sjLs({`NTRC3xiQN9Gmeo<52R5uFNKc6(2&y}o8Zp9wN87? zBcPB{8SfYp{j#mG!9!-(b>M3&uPlU{ZGT=T41Fq}g(TI8mXcd1^+~F8_sm%mgIbGr z<;L1-W!k$_X&I$kC7p}Rz|UZog{)}m-x5{_VC|)&inOr^RFit9qy5xnwe`Cv_pS6C zQhK`$A1W8VcV`df5?8!tEkePqkm@4^9}FcTnSa?$fw-AyK3maQ-_~%V_LdceLEu)$ z8%FBqRxWBK>URY6-x>r|8%|yCdwKVQ3+=}$Ky-M~1*Y>zJt^PJ##E}woJC(s(YrBn zIZLbWBgw~Mh+*3{egz_ACEoU8!(5*2c&)=jHN;84U&3b-PoW6^4t`)^T1Vvy2K(?9BvPGNpJaO4+U?s&WL05G*wx) zMg15vMvHYr*&Qflrf)w%QX9@tesf@EBMv6u9p_hIU2G0ZiS|2TFnLU}Xn<{io9pU@ zMOoz4Vm&>wvogE;t|Sp8T%^+eD_6XBSG!MZ5WFridOoS}Y<=SyPvz+NSDuSyRSS$o z_O1AF6>BZG{ilh5FaE}pANYjbf;GG>gp{P7gpuR#4e{B|E!9tknpygqf@xjSPYK(a z@Zv2cN-gwdi*Hu0!DM^@w0Kn>b3cN5DV=$ccS4+rFlS}16^qq@FFo-ImZIXpApFj% zKS$E{!*vG9m);AnTteQVfa~$(lJ}sBuXUZnkS*)=^_8GWv76LS@z=3#J5b{j>z_xS zW8L71bH0Pe9xN=3l-f=Be;_qz^mU;#e(Y4TH%-RhYWiUfCLb}E5t##F8T!(Cu&-+Q z8b;Ep`$A1lo9FZV6n}_PvQ!S#M*9kIoVH)#pR7G~f515g#2~yhEDInSwH6VI4|hY? zI?A_$5N(~8HUug$u(zTbyY?{D`?Ta;O#o(UTJnpvvQ1qofJ@m%`l-q;*vMz$Rb3q6 z^C8$7UxTVlOglL>YI7BAn!de!RkXm!kS!x`knbzGZ9F{wLn&b=KJ#(HNI365=sOVWbral_ zd}GUEvc7WejYtt0C+~6f-2txe-xln+fsS9CPBuH^Gptt(@X1E>hkEs8U zv__ZWg2@8;u*kS^>ET1+6cu-;o6qv@BDQMEfE?CE7po>Qx&*F&vOF_-Zm}v-F8BB_ zN#X`D7H@6u_BG4z(rX~6=u!_BVxWHdFKfC5*rjN8t&PonZp}}&9uV63E7NLI7&ab+ znyb~U)KU#RTW*+R+!3HOSLSlA9-hi0T;H~0SYE`{M{S*jksJ)LmF3E)CeV|4NYTKS z0yh>NAog7R^%D*Zn3#>~8GmdOkbL7wcE84P`lxba)jm%P_Q8HB^ zCY3#tG|T{yB|3!5Z_I{{PNuWwEQJ{eOH_^|f=Jev=WHspciS+T40x=rt$V)NMr{vt zP16?!ymMbkQy?T%jF*ttJgbj027Y2G)k@|*)*8k1pu_wGlmF0Ijollnk4&LkmV=5W z+8nHC_lTDVd_Im?@ec#%=-pU93w3S05}9OY@Jie9i;-VpFx2#?RhITsboDJT3~p*T zyeSsbP7-*k=_0avs^ErH4j1M0m|4B6OHB=7u9UR)n?!?mQ1*Z_(5=8~9TLVqoaJuv z(no@(-9oZeNurGDxQq&|c8c-2%sbB(uOez}!TLq6b8m5{Vh~iR150(UFVAM$kAv?YFfwbV)G}lcNJMKX ztB*eD_)vRhd%A1vI|#<5OY^359pry_KMNpxJjV_XxpTpCtUfE-D>n+nR)YPro#*IN z@g3epWVZz@BiJD@4K8AK!E2OdTh3~-#__T6eUxb^1x#GRGlR^_K6$(x)7}?<--Y7^ z;tD56;+cXc!;18)5=CT5L%uWY-Ns93o97JOOLH&e7LaVc=tdnuhuVRGD{p7`pOOh%)^9SX+5#OF1BNsu@ z@_L5o%QzvYY9V@KM$_BpfCu7Yi&Gmfpn{~}7(aWGX9=nB*}a{#`rzx`>+wvRK=q7Y zjDqr;KOWXCe3yU(#+sJp*_1zUKgL3O!U~}&D%AHm{d;Ru2`IRLr*S($o+UL(cU19| z1XWYiRc#sL2%pcx4=OG@BA7|EFIh`v$%Z^G6~%0`Z~4awod(f^5%dEO`X0PFcxW7d z>+VaAy`2uG{ru>S@F!Rl`hu_tgccKn)P)gS=1~~I`?BHypq#KDZ@fT-xMF?p?TmGj zjIGzwiXDpgq1q1RHP5gp_q>pevDxdw{q7={>3d-JVaka(r1>cr7>@4HTN`=|o&ga( ze%!uVbstMHP>@MOErs|_*|H=zMyl&Pk%lW>59Txh6rz zhv8O@z@6t`9LE_hv9&Sd9_wP`MN+6z6j+%Merv?QxR3_pZu&h|Ands@{Ovqb2y$uO zE-%2gsK5QDYZ9p|in0t?0#OjueLM#CO+4ZGvIo`+Q`TELlcHjX;q-o6ltOSyPn}M? zh-yApQXp*uOMg?=^NF?J9uIyZR@d)=#e?qU!02$#P)WJ!WANr+r_QOb`>hY2e=|I# zz-D~mlh8G>rAtH?^)r|PW>RPpvz5`YB@jJ(l(8-I;VNv&UBE?j;A@znbh3rrGf3_k zp?rM(j&?R>8Ff_PT&^Ft$R2wfyns)U`t$ThQVw)1@pe?=X*)FY*V(4E{+iW2d+K5O z3Fn$V%6RX`#ai0DuowN3b2*-Fa3Ul{I`%*(4wsThC5XwxOe%X*RqrWJa{2f-iJfFO z8C;2_9d|4(7t6Yo(*tu=i7hZi91Tbz=owvp^nuI^LwvS>r8=9n?`Y!sL&ylqk|Q@6G#wHOC>J&AXgEY+v^H*MT)-u+Cdx^FUt9m3iF`I z(@CchoedB@!*i-g3YxD(XT%-R%svu6mA6AS5T$y_s+S&g+^ET1Zd%j+kyzlgm{z1! z=Tf8|aYVrj6lL#xP*tHFO_0<)4}ov*s-L-BsG1N}knzsJdR!7jUtCI-c6G<^_rTP_ zLB?fZBikan0q@5N zP467Dzn2$S(wuIfEooC&p*EuR;3**~z4MCMD6;K&^f+YLZvw;~%BjqNU2q-G5bf^t zdWEnXgQ+*z*WJg!;;=N-PKmxSL(<$d8Mwe})?A`nol}LG-k!T%Z{ZrPc%CGMK>fYI z+D(~4CjNsk=!MP z+U0kRZx)DjF4Z~S8i8lb85x_~kUTEr?adZ{(X=&!cN9E3$CjG!ehq5cKr-qPjHWCfIIv2qDIh%1aVCe%35TB3Ek|^5`7+u>w!me z{G?031AT$H#|srTua%@hgrYPD-*NP{pw{5=C}lZGH1in(3YOhBAbaC#5qq-`zOWSu z=cXseW2O;7u7HROqWumrU8bQIvaJKtEUyWj66|KMCSp5Jn}K&ZaGpJ~Ax2UTq^^pA zrVwlP9?eqWLldDR+=;4BH1=23bgPzYD1^)C5VT)CcHFcauT_PQ;KIo)71Ma=t%zo( z*T^h5P8EpX`F4Co%YtxUA2y=rvUU87ck-T4_eRoisqu?z_FvDnrr4lC-2c^byl(_K z>v`JUa~9k7>=2jj_|K!&9|F5HJX6SS5y6J4rD2J8WEa+7@wr8*_gmM|{qBb3 zR;JxuGuYf>jr){hsb1BqkK2P0Fq8-_*j*(u4=xb0(*E?OQWgXgaT9-wZAE-0Dp)Ba z0od3E1rfslWHm5UW>o#a$93lKRtg`vL?#Hr%T*q8_Q!HGLIH>u8zqUx;MJU3 zoyy2!j-ur?HF?PzarQiI@@cPd{J58j*5SIISt=~(@VANQmLHMMei&)Xb*r~+u{Y5; z!#MK6BKT4;!aitGxONm+rLje@y^A|GFDy&gpWyW+gO|H+f#%y#!uc8wSL@Q(+=Zdw z)GXZBY;;>u@K7b%Mmc2oWcYF1wz2EvI9@NX0mxhrWoeqgfVuNZ6c_rR=TwFSliAbO zIgX4o(|-neiF!RbaL zvID%LACdTU)-+OBi1RWAsC}vXS{0!el)}BF5>AUhq{q8OfFd&Gxwo**Z1qJRuHD6h zKt6z^7-;Uf0ypLKV8*wY4`*UFsw_Lo_`@ANwgk4m@~CLAJmi>b#=)Wah^+-6(WHyo zqOCDY7h*D}I)Miz*oOySa&1oow#-TGeBz6W<0=XDGE8$?lU+h;T@ zwIgXXSWsnH(I$`oY2nfoX_N6~sd__Fw^+lK=v&jNh745vBGqXO;CIHgLChH03`wDq zjx zR&$I$djQRiR>dZYo{LGC9y^?5I9LRMGjuThIv&+JO zpwz*SC1yiD1`*3V&Mi;~Ft#R>bN4K$qKJ-%(kSsYO{&mNS?0#4G$pH4OZ7g$;hGX} z!_|2?JYz?}^(_se(!HoATW6)81~-hlf6{8|fDa8}T2oH?6b*m0s_2tJMXIU6@#3of z%Qrjh+B?s&vu;N>9qh%zce_PKWCGbyppyaVB|O)}p%+L-=0;Dx=H< zEN7Lediqd%YPnSUpLvw3(?_^N%P|EKs*Wi!;l}8Ep7yiqUhPD_>g$mA0~|KJz~6D| z(_xrqBSd(aDt> zS$i_9!qTTXW_a+ChQ(8n7D|ranMuTr&}dfy0e`-UIHo>+G!BH4PJiYTNXcqQ^QkxB z>U-piCO>XDpTsEU<3EH3zXt2sJ1-cXkib#cKTJ2k2CzG0N zQ|&ioAGaJAdo7Y#nck{=jSGT>$GV}g6rCAFO|&(ho_yT?10q<*gdj#Ry( z7*cRs+%|9j{>x}^%=MiNJKpHJA>O9oLS%P zW5WqL!q$x39LlOG{dx4Ou8?B|`MxrB@9W2GPdmM8e@lR%PDLZz;+!;1v}a8hFRwSs zfq2bso1k#>mKJU2h{5Ze(S2;Vg|J(|^(NF%AF1n2!YOQb33Fq{kom;tvit-INuQ*L zo*ks(CTmm#oh#8R6ye(sGJtifoxOCtp&`?$djDIaAHFi}gkY~BdFTy-_Y@`SQw%UK zwh0Q8de>YTKY3gpPuA7w%b(+%HT9c=HXoW25#i1!wQxb6N)i+MrPRlH&lVMQBk>)Q z&4|$++{s;ZFAS?+H#~@(stv1KO9YPBxoer9Ss-W}`4Xb91gIf1eJMO7N+tV!SKo!x zx09I^ad}EO6p0O`qa?P(H8%8JZ=I;Q$-&1yHVG7{Th_OC9iU9XU#_@B7L-6BDWFvj zOJZE;Cl&o9$L;ES$(Q}eqr<;%EIH*q%ZPaF_l0r%6uw+MR8n1UnUThPiW3yY$PM<& zTPUOu45X~qf=t@Oj z$wAU$N_3Z9+k$9X?Cs@bE(ggK3U}h{6@6{-7#0OO7D_}^b^Kv0*UR|07A=m3vJT&6 zIWL@aua2nl)Jg}CyG>(;@V^5Ibfv2tWiVwJN;hq2h#5wrvtsjQS^$G*2>ee+i|aG^ z&q~~@5|mt^1UB00tlAjIla#(cPEq>#w^NuP6?*qQy3g2wyklC=i9!ZlHfq}QV*Lxi zzqp2e&CCTWZRb;DLqlu=WI)lv3cke#9~lZRpH_Acsr*sI^CGeGc)b&WveBeNf3hm(gyivUlHWGPQiL+3X2=UbY znWycGO2tNsmK_@1=GYL($>TrP!r!)k0}?`Aavp^-TC}5!gWSLo>mR;#g`I(>6`!73 z{Xor+@)|X_qM`lnA$J0P<4y+$2~M7%^);=^+%(Fv97B+)e%^?ZfLvTf^FOS)5}q|1iphi|$YL~+$ic3_u^ zJ#bRMRs4LX+i6o&Mq?Y9bdXbwHJTO_>~>MZOzH)=>WnurkI$pq!1Kg2#QN1g^HmzW*7CIQc2Fm$Re8A??`&H<>lr(dZ+F!j z6h6voH0wKM4b1?!Dg^ZWBw^6ihalXLhpmMuSH0R7R?{a&65NWf7sa)rZY%)On@1_F zTc%H=;3g2t1&T91BSyyCXg-Pl7g7swrH4i$=|`|pto9qIyb=dB8t6ILM9>$RY{p#x zIR)hQw+j0vqgXSgFBUC8SbhAHRbqp2YofH3Y1zYRzp}$n$+$Y--Sx8Xcy5J&!h%_l zx)B3)Zr_40;@Wv*>l5&ygBo2k6^+YiwrKUm@MC0E9QUen+QIGT_Wh!;2XF#0;@~?C zVk^dmBQTx>fg`q$t8WAKFf5U$E*g6>`F@sKH0}T-f2Wqw?8-8c>)#uL5#8 zzZ2Z%dao{=Fm45u_1WYFetwWj)?nGsLQS{SNvby^h8H!MuW&5r8XhI7BDmqyoiY;h z2FNM=yT9F^ZuJ@*X-)a96YhMCG$kt()ia&zhI*515}hJZqo$qThG$^=S0y>*{6n0k0;gFB;|keqSRtY?S|1r6vb7+wG& zY#7~y4-BR{dy-0*?2Eb7s8{xyLnV$M3r+$B*V9x|2KCM&(q z-(V>H6p}AeDJuRZ_pFLQKov^f-%*hp|i|Aue192<+cAS;Gu|-H7?}(JsD|is`+fs>D1lwImiZNWHS+-#FkV@KG4!tpWXNa_pEh}N31m9$T; z9@7HWQFCd$PV3v}5tnJXN=547i)Jo1eOGMQ9S%98x16p&ms7W(Oe<*O@zUjnZv_9e z1hHtLdd4QWM`ka>*|?1>oIA9i2Pt@x>&fmiq@J6KqFU^6)Tm08Nh_;Lepg;U;V6u; zBAdPW853vmYhZ*4$z9DF;ThNl-~WMi})7y0_v7g5BD2Rl}en!XzOg-e)^v!voyPH`lB0YAmly zaQ1km0xfrU`u(@Z_cu|mx9z3^Q=&VJ@}vXMGVD22FE3L03{{vn5(hn56m*-AUc;6j zjg%T^Z!EpZ231~TroB1RjIzOxwCei8)UPJkR)bR1@eXrClMm6?Etn^BrrjSWX7^XCXL7f^L(Q#EvE#o$Vb0c*;}#Z>ljw%P6D-~x3U+LM49WUrykzDDZK|O z5wtq4k&?#DuY4@J1JN5yfa*rx-@G7Sf2+h*g_a%8=|k#&hf1r1&QApov7*Rcr+ioA z*5Y##DhQjf>syJ_b;mMqvXeaXP}C-&Ri4uFyz9}J%e8s9#B`+OQZ*$(^jt#b{2DbZ9n zzm7vW#){ObvR^(PowvYX>yK+Gl+-|ktIYQ$C_Ua0C8mIzJ#=P8XNWkw{0=Du_i~hf z5~%5kkDAZj$keZtc89)ry)s&Dt+l!Q?##CYUj;S_i_g)u@sEO4oE3st<6`xsv5;tZY*-h>!B7G-TD8;Pc zJw>^>9IwPokfeD4!ar@#Q#V$uQ$N3JtoNBPkiC^IIr;@ayNxI@+j0<)A1)T|HdK2$ zPQu?bV4LH^no$lij5MYpuHs%Ow9Ctl3pY(vL?2?>3BDp}yPVg7AWM~4=yD_E$!@N% zqu~D4cizpMyFUn5E26tuyZCanR=U|k_g(2EHO##)?t_89@q7b2kj1(g63Vr~({4>u z61R0Pff6sAY^hn!V9vXav}AWQY--A=Su{RQ&ld+PfD9${sc^qUhm^1%NUD4@z7+8B zRCbhhRq=)s)e&XpBtH+Xe$CYl3jA5RX(Y0VmwC@y7xQNwoNH2IqdXybk;gX=&#*v8qH6_@XUqS(%mtULYu&sAa- z2}~o|i9um%LE42Iq?EWcEX0)N-*UZy@1bs?&^IZJ$E`SXO3^177qpx*LB6xa>%QZl z${WUfWyEC*@;)6nhASSgW5>X-lms8`Z|3@_2vaK1^RvyXK9%dB=QP2<$^?|zrzsv9 z(4lKgg3AeZk&|B}R;~1YNJmbzDE8h($*gFD2Z);DQ&QgdLCHCaMWs<0RO{PedrlQy z6jYE`iM2vQvs7mR36ZWnHv&4q61_e{WkZo*2^pu%@pF0<1;c#hu5bk~h_coMkzEoG zwQ=e4Y4uPAN17Yy`Scc?sF0-cO`<3$C^O;LdB|}OraWfq_Ra+jre$+#)<0(k$guSC z^Hco@FfBVt=I|SEz@mzr*<4XTafI1w#@otxXcv-2gp}ny6jU!M-sU{KZ7x&2l7@(L7%Ur8a`~l_36d-5qhmvLyM=4{jfkqW9gX+AWyAe zl)M<0I@Y-%trx(kGx|PY<^(YFtLId$Jz&)CD`jCyJWFO~H00RFo7eBal+Pm6-d%%o z;euTg<=ZS#f?bctFN8)vc?yVF%fqdVomdi7-DAM_DFc^BAz(W%i$RrrJ{)+AOL3%7 zT?&{B1boc#Oi3PjC&ZEh`x_tpCI$mATl?VD*WGa5wLsltfqb&)Iye=>lpW0U-X%dH zv|!b$t*2-|5ODs8w%1o=eg#&wYL?#D)&{u%P?Jq`6^@N=@)hej0j+TE6&Rpaqv-A!~G%=k~JsUm@+h-MTO9n{XcEbJdRCtOwfO%)fjsuxV zjUIM^T+IN~jh%0o$V~%2FrXkf;R@h5$d*%FE`=#mL{#31mL~+{Wpf`iisHJ8Dn3hc zHHwuPtu}xMl6Mib7Yu0d&VnPSgcBH16yU>Ci@EUifg2BdN&e|!Wp2UgFq{`lY{fI@u$B-ZOOfzLgJ)6IP| z8@@i{Aq`9a@m16`5EEFDkwU3_qx^Guu`wi-*Z28**+6v<<{PPjqeAh1T<7V}*n()v zPRp1xwtJMj#y>tkQNsiKPBxy8&r(*fy76@AV^i$ml8Ny5nS|a| VQxBWK18~4Ubro&p3dJiC{{yym>&pND literal 0 HcmV?d00001 diff --git a/control/predicted_path_checker/images/general-structure.png b/control/predicted_path_checker/images/general-structure.png new file mode 100644 index 0000000000000000000000000000000000000000..86f3016db5c2c492a0f1c57dfad87823919ea2e0 GIT binary patch literal 84656 zcmeEt1zeQdx;G&rAfN)G2!b0C5osxr5(Sk~x_e;g?gkZ62?J0h6+~jlp;IXdVd(CX z7?2tozV#COu;ZM)@ArN8-m}j+{`~38n)R-=-lw1c^DO`SG7`juRD@VqSj2bl+>*n> z!bM5reN?W=Zo0;h{aEf!YvV)~At8j2IaNPu-6!q*a?CkeGOBq|(JApALJ|1lz zupIQejRUkSJ3lKs6Zj?p(>JvQt8lV&u!FxGU`X8924-jp9uw!_;Di>3jtaBWHQGC- zw6v6p#RFq@TU|*9&Ra@yqS_|BdvhFMHnzr=7W;3*!OFqPxA%jcvlVP_)BxsStPg`m zxo$JCi-Lurzg%Mbv;ki%bj|nY-UqMC0uBUiZ@f< z$38KP?2Qdzw)+q7J!xlYX=Z0^^~*thOA8B_{yxX{SJbt!v2^_9G($_X{ny){V+G#) z7pp@9^7^`Fza3RJHn4;5&E@0XKOQvu4$RmHey}rO?cOQPbq^-*57@$W4J;k^p5Glh z%$7Yu9+1<<(h{t9h--&`KA2?gBnC5ssC_`k{e6Hi{nwxUnHn}fi~ax0a-w?L+Db;+ zoIDTi--O@3ZDGYDV{;&14!UOc`^4R&l&zif0ks|B#&$4yD_tPBT(=y7NC7>9+nJkz zZ;;CA+FI@L*wEMsQstY#448>onpxWHl9kKQ(2!GKAB@}D*;tyw4*g=lqsPO|1Af`t z5~z9e&nXZL}v4zoZmyNY~8R2(o4RK=@#Qmp38)7y~5~-T%Sd*uVgik(<9KAU2lv76!YP z9hkTO9R^?fErWS}hre9=WB)wt1)gk|zlU@XW&jM}zDio!*ugE0EG=}+ZvSoc<}V5U zw>eUlmOx>5HPr-WXXm_c&2{bVECFK<>G9s4&_=s@%lY?Ikdxik(%xp*7=TB9S3bG+ z*0R&JF#@iBHvstEc|6d(f2jQ-bS;J*AG zvabxBJZ6v`H86GnAB~`oR!}U#$!-N%USJ>hCje7_XwGls;FntZdxrCnM*ed;Qm$V+ z(gUM<$dPg!IM>6D^kCjUk)rMzAYjn`7VP{F7+T;@ZJbs1ZSw9HRq%zKl^Y1v?$=vT z$O5^|y>C#!@;4{?|HVM>8>l}t&>Xv&!oQw^@4x>)R&WQHnWet59TY?U+bj9cLGWKt z$@in>|0N|qq}RJ@{K(iZ5O9bDG}{0DP_01lV|D~R#-SNnICpI?d(2k$(LtG(RLizp)E23j45~Ii1F_e5e`!HD z55W0@yl@X*+J6f2(|-)H?rrvWx8ylUJr8`|!4VF&+>4R^FSg_Y?gU`FKf}%cm@@mb zcOl+Cn)xS@;C)*DM+N~Q{?16=zW0 z1Qo5_VjmPXT7Wnc9NN-mKc;j9g+NeYhT_d%o(N1dHiS?GfWSjRCUj<7XInd%Ie3rX zKkC12w(=XxeW0j^TpH&=MswJw9gH0)^glt-?ziFQZ^DINMg;#=co@d>cj~OX{CdF2 z{_m9Gt{rgV5XZQGFG1iqnD|e`vi%6}5K!&Dr?MH4{@8fK#l+`qV)rG*g)3=E7xNgnv}{kXsk!jOI?d;bFV{nzC`?t}F0kbVD) zl>ZKp&;MET{&V?{ot2#z^f~TT`BeW@32^t>`!E}0(A)&|Aw0c8&p~j#Z@#&I51Tsxiw+0d z2V)0j{(nE%K6sNueEd)K2e_kakSB+n(mso?9T4h}^*#X04s-Zm+CLA6Irl@2|5@X` zN4!4#w1%T)!>W?#Id6`sM(q`?qEBuPrjq@5Ix85f=X*4*V;={-+-Hn_2|NLG-lml6ekX z^ntJa|6hyXI;ejg1bWx@vB`ri|FU`d!PTZiPDR|(%nTG@50G|nw+jaD(?FN7e&J)- z!MpF4AZ(5Gz^#UVUxRcI>K%yV0fuuZLIfAa_6YRP9U^l57PQd^!eG36Ne|#OP$u{9 zeP@REz}Xz61Ak#=A6&xx2RGUN3*P4tO7nAHwkiNzpc`*{ZQ1{?{hNOn`0t7D|3%<` zNK+4d%V9-582i)E{hv7k{9Y-^`$XAivF@7I=pQwE0~x7x3z+rX^tVc-(?{Gul9D2#)} zW$RplnvEFjAfr`=TLNn-W53!!nj-BWAfMI;%K;k!i)1F|0?O-%~CB%$3zEx3&x3k zpo1@J3b%KGxFpmII$^)JQvKAqZ6PwfiyJgZ=1fx zLVPasW>gQvlXL^{Ej$1M6!d0PpOsfk4j;j!Uy-UKtn z;~qqUBZv?hgn*S(?&V&8X94z_Z1Cz11H-5!j{W!1l@~!#L=CXPLus&GC+9~eF$6ej>!kb- zIXZf9D#Ke;Z3Me#R041REy*r9Yf=fk+U)U9?qC=Y0rx*d_zw~O+azHW4?)Yi(>is& z>9a$(u|!8Tjbm>UNs?yYY(aJyx6srHfsq-l_T>n9gmkju-431H=ITyI(~mu~g>#`x z1&Xmix<3O&p-OSPOtq){U$udxZ&OVYFa=-3TkvhCSC6n1cIR)&&YjpPlkbKi8sWX#mTxN#^SwLac6*Tvn@ryHHBY6-g+miZue$&Tczh51VIW3`alF|+mO zYf8l?c;~JU+L-nuW3%U)J4l2+Jr-PVmCUi9wsdfr);C`=FQ)kvdUc?cCndtfRCtGT zeB&L@mVqW44ScR%btYnfrF7F|%f%u4boKf3D#a9Uz(IF_mL@&8uxIWLnTOzGNQ?f> ztUk_#ldelOH95L%3Z@8d*{i|@fxcI(hIZ;Y4O#FGD|+6D_T~Y zw!SN|vSDo5kgY0D>wqd)^`+pM2#`R}mG0#D22B~r3f|}xO`uiNFZLW7(zd!`-U6WtBAWtU@$2;TLXHVIY zEWESXVlt!MHCtF5P17@)j>aH8=Er!IN_ssSI3lmDNl&|%Yw}ynDBvUfc=$VbQr*k% zy}oQg>t57--41OjIT7fHy64W9_ByL!+K8s%#^>%F;|v;Pl?!dUW|mzhy0B4ecI#=j zOh$F}adPBiv*TAZfO%^>vug{1698X`)vL%YKT&)oRoik^qZgPGenPNfzlsi0&(IT4 z{YIr|hACpmM|ER*g&OSR-7;+uws1kXxofv|dq&^g*}3bQR*~6t*zI@V0hnz8`RZI8K0Q>fh06;A7VMn1R??rRYg$ z+YZ1T;50D_<0=M0XZd;F(R>K00cFnH;7x zYZR31%+1E)>~ohV3B(cy-Ybrfqd+8 z>&mK`0&ocds>;#8tp(!@0nwwH-DFx1W(qzIAA3ERy#BnN|G4o}#d`0r=fmvMU1`I0 zzP-DKTx=A~{u;CV&ayO@wGY4ZJwvq023Me?HPW-9nLN}#Yh zBu7_|qGF97c^1kxcjYZY1YPCMS?G)RJu)U6>-{9fo{_ne4E!~+HmxcP@~i2bvn54D zCO#prFN%NI6sMF&g;$7vOxg_%|6DYn$fxP4@l_<2oaS+&e>3gbDmIAc#DYj1p zND~h?KvUbPeT5VbkcbERXr)j}lXL>IVrLE>Cw1q!xs- zs9%>>%QgvlZrH5l7sHKa>SV?GdlC}NhAdfZ5a7}Ma)h!kNijJ^BIrDc0;6>Js0Xip z)p65qWFl^lU6{H>e&zP&qUx-B(HL~>3EmTJ-5b& zzZh#~8u*(URX(L4IY#U9{X()r;>)3nBeO#(>qK-y9-B_4P46GMhB;wp43+!!_}~$x zJoq5a`dB}ZzpC<%Kr2P^j!}g5<4BuYCI=8-b(VSK_FXLdj88kW{#Yp?+M~c$5F07U znqR?>@EXIXbsN4$DImqtkH#TAtz^A6zeO=w$zI0oDvRFO7g7YJwp+$D zJsG}vH_|$M5IGB29mV*g$Io(Be3X7?m}U4a6Nr1SXn$*>yr%nAGCacq`|0ui z$(DqmDBMh_AKtRDkn~5UB9bQy1i~%zm1EC_T0{oeUNR^ zmg2sFA-O+M|8~TS$m@1kF{ay80A4(mluWJAo+w}4aAQ%5l9BGTWTOh|-oQ_h?a65+ z-Jdz^b1nM1Ki^o#^`v!QdSM&ZgipD(HW8gmAIPUC5U{z7J>z?n*rsv*#;apZRXFZX zLWoG!5=RiZ{Wi|{QmTQoe(gY?+beloF=o6ZHEmgqLZ+lQet1d0<*aN^)ttUWhLS;r zN9}xin5ZZ!{U$TSDFyNn^AA4o?)#A^gyhH61nj3qds9r)ig_s2yxF#~WxQ`M`pPhBK$SeJXr#Vn)5NV5aZ@v8ZsVxYci{~h%?lvQ&|=aKM8x*Qf+C>n>$izpZ` zZ#^tP+*ir#c&^pZmq)2lAO!6fJ&8qO`T50F<2s$nrJ z?Rd8vehWVva|c^ZHRpM<6#sNemWH-9)0PY~9Z|#skS_?Sm$>EI8t6~HJ%$w8T57?7 zv4R3%G%eKgZQ_&9-LD?HvHtx--+=R4x!`ix9e8DARb+gO@--H#O8qe$I3CaB#8g{y z)kq+vw>1+Ycc zUXYj>iE~@1VI;ZXr4%|i96H?lK&63@eZg_F{L~=tfLjYV&;+$c3_BI_yv^4O7CUI$ zMnUCI71-8D-#ULuDK|dX?`5a@Elbuene2EA#ZV|ysl@-VTo2$Ozl8nW@glOhkwd>K z&&cYOs>ic)RFaLiVnPUTLXXH738t1!%Lt%HLp63Lgm-G%o{iPLz&b;K;A_!x&p_(v z{iMX|rKDjKdO2$*qF4T>SPJPgyyTZiQ4i;j&78MyeK5NsWytPN2WM#wZhRVs$Ne_a zaE#Xl`1!Yjvuy%UFe7s5N-#S}R*Jgvt!*b7`YdFbNvQCCV4v|47$gs7(JIPAXqWZ_ z`%k-ad@d`RY!$2Bs_wD}98byXx&{|v`Und)O#31U%Ldz8pD(EpoYrGiR{<_K8TYDJ zPHQ-RaH`oMO{@4+?L+`?|94EzaC6Dd_J#vW6raDE^sD~BRVymQ&UxhU8P|jpxQL~5 zjv=Rr)UY#wuNe>=0iL3&nd>BcIr{vJ@EiXKRO!N4U7>@uAU_|WW3M)N-H9%N0KU-E zV7h0$1{Pw8Vj@B~%Po9@9#Eq8HPgt=c|b|*s<#PjB*J7^KJ5=5J1GfW{Zuo9WG-!5 zWQKp6u}(WkGQA(hguqIbwVdOvgM%Dh(j8BbNnXR|$ME1p_g}bugH&NKW+*jtiQG$n z&QuFUsDdMU!*ALVh(#eo3|13x*l3bIY&ah~!DZMQ2VLcPiSBs}@)T*q9B^MV7!eyR z@;YaJ_Nh9yQachR3o{Qqm3Tp~W6D8%DtdLX_bQG)teO&*G0lvjkk*^5xrpt@JH)ph zDYrh}acN;mXQz?cY8gDr=XERkl5}{UU>sl93)GbHw)?RQK4rxqSHVXT_HoN5~47-JldqH}e|@q)e5m=ZZGZj9Hk z8O~GcvWim%y?nhAgM{g${FrXJq849Owd2o+sBk2(I2cb?Ul-&HkI@*zxr%Fxb)AtO zS2yBgJx%r!i_#81`Q^7LLbycBD|t75*qJxai+DTIh}WKsA#KnkW!3$Mp3h zsMMzApBFRLQnRYHLTao6f$;7DuCJ*tcUt%fQ+P`)w$cox;Q*JP8i(i?Z&|7PM-bna z26bQ;kKdP1X+4RlURmVCQR~7fd#E-Vr}iM=Df?*dr}Zf1VxM-qPpLe+Ihb4wJK-n+ z;!$Gf4y(lk0b|4f^=D_I zZI5YMt8a9LwQo(uV){2|yJYwi;RebX+Hmbn9qb`1_edhLsuS?GmljhUojo+f>1R3?g7!m=j0#?_aTD9pZl)ghc) z9`pBbdt^mzS>{q|dDPX0c6FrY51UQ*W`q!#&eeW1pAqEkhIRCsNE!-*l8D~D+jn6e z@=ehO*%#o_p)Zpp_A(B+c#(TC6uBW|iCD}e2uDiOr0Z{u(G|^Pl(ysbZ}VMYT!?{; z^~L25Z2`wf{}Z*qz0QSZP{yS{#qx`GFORquIr9@-I_LePWVVp|xX6;#+8zw%H9kV_ z5odd>sb{igR(L?sn2CU}QH=$_7rHcawM?TCJvso~s8;g)@!q%ePC|BNhF_07DQV8z zIaV910$6zpb$YL}034w*O+M|s*5xq$P0*~W9QH)p^6D+%dn-chQ6lE|^oJ zi>as2?VNF@Re2`QmK|Lh4og-_%PNqtOBb*mo&ng-IZnM4X;<6SZ{h3dn!Mk&HolLv z%|^17Y_^Bz%osKb3pNMBRXRkcjalAx3({|lN-521<;5}laJaJgPDI7nQvnr@A-{~Z zpN8J|el23^!!c>i6l{a%)%MP!poYKoYL?;J^^LGsnnw^dN=U2a_){ic?Q*VjLn1F1 zwughz^|r1(-7vN;$8)@@{(X+NgX^C&LmbSKXL&rUMwGI|+dM_5Tl6h6$O3yDENv36 zRCm?hZZyxWrbx)H6;0^=XwMb#O!9Mv;39IoeQ+>59~G|rPGlO8EQ3s)da|EQbL)tD ziH?u?s*66!{bj^Dt2jf>#YI|sN~NwKbVMGXo!iBW{tv_i9%!ZM9pWR+ZY$YxdoY|+J!XuBzj#XEEPf2gq;G5rv* z4egwn)cWAb15cUbi{V%U0d1&p{@PE1OB-g7(&+KB@x0o`))A+ zyKH6H;xz(ZnROM-lc@DX^)ODbf4V!n5}l%9ioU**O*;NOD-t90#e_zM+o*JkBp)5s zzpir5tvsGYO}S`C!=e|jQAN-4BWJ1q?a=-NJj7(1Hewg{h!YtGNj+JIl%>HFoU8QX z*GyAo76@ggLEaH^K~RBrL4NRqa${z9z{co`irn^jcwa(m6ptG!Cw_W{&D>q7;ci!* zc1E~gPWE<(L-t~W1v+ATYdSYW*;eLw;5c$!mNXC#!H0O)Q5s@P*OP<0*uYm9hTHy_ z*kt0Ssg}7MTksTGa9NBwxWMgm1v&;WdIp=ZYa=TPxiE&Y^~`nowjLq2`Bb$sA|55e zT=$B|Wy)HVj1C@<8WEEnLw|2>AEPKBKgNR0=0vxJ9NV8keYvHnKY6-PPn)t+sd#?DLqe7%Pp1|fdaFT3IZ&+brOsUKyQ1!^$Be+VTDKgRve-)38^R;pUi1ufr_Ag6w0pT< zJA%NLeDdYR9`NEuapx>jT+2TAYa4#VvBEmmLbHyyXSobjT0TmT2r=+JfNyQB4D=}q zKVoVc9;Z61!_*QR;+y+*-U}P|)JqUJ+N4<^)Uszl%9PO}E8NFtb)^ed@#JLwk6L@` zUd?1dpv(MD%kfldjsT#4m#gy@p}v!C9nt z)rE4;IxLs`zlKl*B=Ywb#kFmDP#oz1t^x(rxJy+wVa-$#|&m-T_Zeg^>LiKV-xYfj!2J~5@ z2;GDTo2Rw|M@gvRj8RVS?a|$BpXElX0wT$(`=VLYkm(?u?aA{DDGn;*vTpIvvbL>8 zOR^Ok*iW}-KO&-3dqha_eA<+6$xkKE^tFH-Q?H;T^*LS(Cx_(w}oLL1Bxjs^V;s`1znKIzGIJj{RG@rtz2g^s%J`fw$iY-_+on#>$v$L>|r9> zvp>IySvl2iB~obNE4|VOabjglWWlGfh?l~%lY*N+%F(fY=QBx#H&J5`n@c@C9UDzj z=0&L^mg@NE#O-K=JvJR=n19GdSQgDct~EOGX%Uzj*6x;Yv)e?<+DWm? zU&WVazOb4TQrZbwW+g0tJv~tftM1a0dpp2x%mOEt&mm9VGo3LaAlWOJPPIkVyhO2; ztbP%t#DFw$%;dh$={@0O}1=U^XXgS@P@d5XJqwr4eC7B3#Tau#fibrO| zncxdNSfLvNr2L~N1O~luHC%i7iN1P38Pr36`m{XyNF}T{1^G#j3(c@ch|lrx<#_^- zi~1T)*3RZ)P{3KPD(oxiH$1o9@UyVib^T}z0S3{Z+6YuB3$U_Pl+GRASCE};+)_2^ z8zuEq`V8h9?||JF!()(kb!@$L154H~8-4b{JDtpmwNZvkitMp0?NiG!o3uL;*Hk%& ztfHb7aax{gLj$17!5pV?$=9E7XF=!sW1$N4R^cb%lINrVT>V=tQNExfYC5zK^1Uaq z-tv7${Gzj{>Gd|{o;{rR{t3L7IEWBtWd~Q{Afa_QC2qqxuj@9WckmPQjJ?iR?r>S? z`;pL?0rKQ$;Og;Zh@73ne%thP0%@aI)0=M_$weSic_pf)wpo^1T!f@duaO~@odXx~ zEu7mn_)cs9w`qGP$d3g?$#}`CU#)y+W0$~gZNkbu<;V$W8M{205Z&;IK0?ha(joDg zyjeao`M{%V{s+AP9h8hO?_$SLltl=-=9r$#(}wczn_KacVXP!>*y4n9!#8nbMJ~9S zzCGr&(&rdCA{hyPqA#uclNV;dbH|0*3&lJmSiy~ZTJ?HutQa{+cL2emQen#~Md~EF*TS=$)X$vMl{-HB2C>M~js&8P>=8NOsSPd~bE|132_=2y> zR`MG%Axp6`8ih8@qO2uALX({k;K)uUA|3td8IMYm+DECk8gR`5yF^<}CJ2D>Mcsj$ z_ZQ{kTG%ZRTCI1RrsdOGzwx#ne1`)f%;9IL(rN5XUfaCAR${FMUha0Dv-uO-@wxbj z{tFUKRrv0>?*Sgv)VuQ5sZ5g+DNch~hrOgY(Rxu8<)xUgDK7gYhFb?$acI9sp@fi} zO3fI1wd@76c1e+`6%f+d6Z8>xbxtz230&yd;Trs!snL5otu(ff>~zBAjYs-lS- z@XF;Bx%j6&uWIE~bl#FV*M%_WYEB5kVN9HO$}hGMSFw+ZgKz$3JvrumHoe1@&jg$> zdet09(w~HF3#|_Nrm||>jnIuS48@10nhF#VTCHRkm>*dXh$6 zHfDUx;4K@cSEWuU!Z@5;GXgN$DUNFD5t+O(cDhh}S5$2wmUjHTkch&2W}7swG}gEP zfBzm)BStFN%3pgmtCAxSU(IuS%?Q8#g>t`>@COnY1DC1$p6o(eonAfs?3M zF>?Sf=jqr#CNYex`>F7&9ca6+(K41T>|w(_mMSOqc1i&M9aM8rH&l(;#jAx%ICkaS(LqD$S9+!v)@`lx}a)pOl6w!`uU#tk2Um4-|LYgtiVO^Rf;fM ztxuy^ z(+MC4GDm=)>IIU@95GB8Y(7Qbz%N;at)(}jF!psiuzB&pU72K`_R;TeQq+!c*T`|c zS%zv_I)PHR8TM*g{#Sy@;nK*a^cz9v>IgFP0B}4$NQa48U-5q&&$d=sv!D~9S*95O z9n{fIN9N!qs@|7B(e`Drfg&u*NO()Lt-#J?Y_?Ld>!VjFzUjq7EkolkIH)B;^KnvU z-d5(1dVP?(>eJq8poj{miX1V&jiO4E8*&TWEXkdv+nD1@>s*j+?{qfGb-+@eE=wAk zx&QM0Y2>B5cNFRO&dCINh{8D&efv(-*{Dv{ztjkM4@T7Nx=+U$ikgZl5H$*HZ+fh? z%l|A>$zsjH}w@sade_^-r!hir21wnO=jJdS; z?S>Oc5l%WwIJXVS%p@X6+F{WfA&a8wY`x6BmpDF&^@>px6C;ef4^k__%D@jyqNdb8 zJz9O?w6O7|&XOM=6OcP#+rUg>_Mx1F&^m3AR1+0mD(}~%8>H@F| z#$kdA+BY*s;8#w5^8avM@Dw{9Vfa9ha!TLPbbb;uba)$e*@Emxa#7Qd269`w^;rX` z@;XzK?LvhGs-@U>eApiP+>}mtiG}Mi1wY>`nQwQ)Pv0E*Y}%RDcoI3qwNLd!tr<|o zbNwYY_vfh>@8_9=XEt4aP+_KRk090~4l(`bJ~7{r5(6=?K?3~gl~UbOAqiAcX+SIDwE7tKpB`Vi#J?hl=peZ1n4(d;fjsgE2u-&&)O$IdFQEt(lyaK%loN-eunLQ%9guxyJEm$&Sj5Ky%qk&OHyK{mSiad90O@73xYm)%CWuTZC6SE%(eF9IrUU)d?8BIN{=6RpI_ z9&dgefhO}+XT1-FQYV8fz)-V>lJzJX@Dx+njmD$pSlplGu4*(rWGPK700gH>wN8QW z;caD7pxuTJPY&uDc%)a|q|h02DUfyYWHgR$gXQQlzvlHQJwM)*cdwzU{!bsS>}O}> zpw2BF+i7wSs-1Hm{c;>^!1<=h;HF3Mv~4Hu#)z!CjVvu|J;Vu}K;;9Qk^ze7CU$qc z_aC>uRvG3PQm+&PH8Qq*HqeZwhktu>AvSPVZZaXlw8yE@5c_~PqBFSQs^6&_^f$`Cb?E&Bsec?2uS+8JzfZZkN|rf$oOiu2E8MNdqsm#s78u*b&6rw_CR$;WZrR zyPvj8{I@CK*H>A|C3rGmk3qSYir55dTmpUbTF?n*tYZ!!Z1C%TpOjB~*~d%FIAln} z+u@pw^l6Qg&50qA)+V+qjzHMX;!h<)GE65AH6Px))E0CfG!78o4(H^#BWhapk*6`x z+MPu5NkS?2K%`J_97cTFYI$jfT&#GPXLr2sLyZHlZgM*nK%&Rowr@}7#&B~jsGzi* zIwo%XK$T;vN#YS8#GH*Lq_{$kyjQ&6dhqK;43mp-^5E3h+el@4u9Ei0b^{~oxih_; z2uhZ$Wk(tB-QMji1!o=f1kB^6iW4D(S}X6HaWAt0fuSO|Sh$L>W|~VK&0KZPvgid- zdelbu``bkR`+h-=jvYLQ`-%|nB{BGrb)oJv`a=wNUjAK}fiq3TXntB<7+Wr=hd1)4 z^JPFyt-IW8P6Q;6oJvNgK=#_6iB!Q458-`JqT9!RVrh`AbhG)q=ce^US?E>O=FiM0 z>Y^3*R0?#E9KcnoP)kj6OEjiI;^s6cE*cA>=M&q=jZ9JE__P+veBQ zDS_sy-mZa$8|b{5(@aFJ(U+0y3@$^DD5=Y2qJ;!ketZPghZ{tS3SoN+L>B^xAfK4i zUF!m35zD=5C;r9b6Y4rEuhZn}JwzseFMD;|V6E6T3?E9O95QlcJvwq$XdxYI-}Z)YE2p zYdtp5q~$QQtvw~^`+HiCPIn1Sz`0F;*nYg#G2xUzI=xO!EVEzRcmg){`Sj5-P6Qi> z*XjFw#Y%9z8@YcbHqOh?wlDL?(h5!OHuTc5fH!aXsgUZM5;%NU z_}bFSx!mREsB(0z=-mTqsmd`ci!PH;%OliYMiCnt+3AGMY#xrVJ`xhnww+McOBFK-?3=qp{6?0EI|!BOFV$83y=~9fSMv(1#GBL+j-u3 z^knjb507n)1rwG5eu6Yj&98zdOTFdfF=@SQ+VU}A!=|s;)iD|5l~D5x)T*@KK4aNY z1K9md%Vm-QzoSBg1|WXf0P(B`#&CGbU#WD!xUaxId3mO*g>N(2tSfWYne=O-FP~D1 zimo56N1oskN(=+Km-su9>?WJ59ufrGO~!{&&F%n!FvrCwv17eDr{%uf=1|X3-3eM5 z3>yVkBDmqGvXZs&a}#nvQyP_0HTX9`qe+TFqFk!B)QQ1<4rHpt45z^eeI%+a)OQ=Fxl zDg^au%^Q3rd7(?WdX-Px({;tlPhPy#EfNN>ov+@)b0}WV`Sr5AH=MAiE$y^oDIRXi zZORs76-CaA8q`I$<8O@$lB}z&zvo9GGz%Sg0A9FabPvLRHdp6QPrFwcgO(}H^Hv$^ zIcCQCRnId{G2E-5u5LXiNn0fybY8HRk1?n<*JXJM9nlVo>7SQf0lJwQ*^W@3==I-z zR}7NtstwoK`~;|5$=v}of{kACT@~an9e!%Q5?^f+?Z%an0dM+=_wlOKf`H8!Mfcd^ z*Ejk%f*Z3MuGg*h-dL>XVx5J$y8>Pn3E|glu}L&h&8@C92p7O^LUd0a0WCgNnT zYnG0wBSiqHD+{Vdi4NykkC=T8zF_h8VmiT-e&>lulH;peNaGeWkSXs>y%{jZqcWZY zJwY6^TLFrtv}$WVk-E(@oJZmGVx6R$Il%6)3EYp6d`}oK76jbal)^e;AzasLo$E#B zWnbonO>*-InbQQn-MeeXFE_ZbM%dtPH4I^A^w0cQ?wAKfLV;s%uiGc#Mlz=rHGgJ% zbz~_~yVN6eNnGUlZQR`xbd>kn%8>r)DXjgaCzfo~=0 zY0{Bu?!La10s2MCvZYDhM#KbZiArKUCBUYmO&~>SfYv+fGPM*B_QzJWXBjIhH>3iC zJul&V@#zFvf9=$N>0N~7h9sMIkD=gwy~3=RG)fod@tm5UbDZx|pvpQszi9NQR|H2i z9LqRtoY|wv2G@aibbQ4wyuJFt9e{E?Mh(=30Bh1Ld2^gj8P0DzF2!4-LLg$HpDkNb zQ@y#i*jGWI`u)~N%U z7vH%a?I1QX1F)I{IGI51^DkkZ5j86Vo&)YRD-lX%AKlg#8pg1PvBV-cK|{sYD6z^g zV>Hmb<=epU>%A`G;h!9=d~o!M*BwF~oF#004|pGFgz61* zZyfqEhWAv4e~f(MIPT?sPFR_j@P`XAGVi?Jl-|0TTuOq(dk%G*tPqk=&5xjP%tZv< zWAF3oIImA86l!4K&U4xJ+4#1I6`Mf040_#a0K#to6w*HCF8@S?ZN!MQ`zX%p5+Ukg zIq|9s9@_ib)E8JBd#snoD7TU@RwC%4E0`~j^Hx^j)C9GPUcqE3&KP{M3sM$X%1BZv zaaUS+Wj>x6)AQSUpV2|wfla)jLf?r1q%@O}DnuL78`bfCn&Z^HL3H64J*m@l1I z`6wgo-41&Fx&xRAtvLzGj2M(QX5{f~KvyZc|H2NODqmLMl-eY-{@aMb)|bjc${9lV zBJO9N#&923m3w{`N(lAk5(Dpeqpk$|qox?dIwmgiT>Ze05NW#07&+QY0{}^pSzz~i z6zjtrk>f-wz@xs~aP+#Ee$2oL1^TLju2T~JIO4R1Gb_rxL1wMVl;>F{e*;x3r$y&= zD%pcn4g09ZU7fF`I~43Aj$YU5?$STQN94gpnD5WB$m2crdhv|4;42H+(X!&3J61SN zLGkr@%~zY2K%drf2X-rNE8jaU>?d&2yjUR(#e1(VNk`=H#qqr;Q@n`8c>-qYE>qH- z{(kh~jT3lYU187O9lMI-s-;Vf(zY3=h-p(NL2r>%zwO1!%vkph`d~O$D7Gyj%0HQg00HOM zt2mZBdiIlnF{gfHo^_J-v!)7Ljl?>EH~u;L*^POli(cxopD7r3)e2Y0I(bjlc z`tBq@MfP~QpTSum5;YY$70aFp#aa%ds>QEyM3v}X0lQeZvrb(ivhe5>qr{jeVMTXK zH)uM|H3(xv^BIY}S6+SDXd@mPwlH?K`k9X;(V2NJNk4BTx?_WQ%_<*jPTj`@xBz%^ zfg$?kG2}3o4I%S#Mbf|=@)Tw2xj;5NWt)jeM~AX{Y#uFID|~Mqy4pD;&Vw67A`etD ziJoM7k~@AQ?Vcf9#>_&{t%a(gf&W7#Pnxoy%rkOBZ%S1Jv=F9TB1B-3x-*_8{6Tzw z$1s&?Apt4JT!~ zhn2Kvp#9r(q(7$~F>af^;tCtqw9bzA`+^Y35lI%74`FH|FHJ! ztg9)x7a^v>6G4wfv^pxu?>Ro;_MqCmf%F4R%-^_CRNM6u7_kI-~uO z%v%YwJ%p2)(B`XDf~rm$AVu(5df}m}b+wAho8>VYID=O!Va6=8G#NOfBFw=!og72n z^7R{K+<}+so~+H;UqrrVeMRqeDwU}&h+Y!(ja?qL!?`gdZLok<%;$Cal-{tlS`CE- zhqE3(PbyX2-8Ct!Op$Z0YTg`PuQjp~C@8}jhOg3IO1tXKz_UY8(;uZ|heO_#T$4B7 zTcxt4Q-sO|S#MJS!vfYSZVUC5+c&PN;+I&xczYEquZ>!ML z)l}8gapaP;8YVH}X#A^dw=JUr9L9+CaaM29SK+N^#C5+r;g5%SUZ;X)#6gUD2_j!4 z;BR;aF7rQGc{pRY6D_-YDRCv9dDWZ(Q>al)pX%}wo z!c7sca$7!s@=j4noj|raK&PFH+U;BT9M-|d8*yl`&S+Pt9wj_cO@zpEyxOuP0}$Sj zsEVX$-0&h-HEa@{=Yo$bp5aPeCC7{|!1{YipS3)F=GbGNn~xigMLy{3`wB~O@Z2X{ zCF_OvqRd_d><<{NZ-oNf+(^}i7=^8n*Ux7rA}bmbvi_Ccz3T1JU@yD%V;UI|VHhil zt^~>R-i`~>(KW(4jhuQU*b9_kUgx>Gi&(Cgn}{c_rFpP-iEJmnyL)W5BA$f=2Ul&9 zt$`Cus_=%SmQ^)VM!%6WyQEGH6MOxwBRUNWvnnHeZ0=9It#OZyU>5kX>OY7wzrud* zY3al(vps{3+!z|=fAl%E2XU07ie?W3#}Vux7~<&+UF@DYpzXq>w;aOR1HEVh73Oe$b5U1}!_r?Kw{H-V=NdgrFnjQVWf#+aoSV_?uV?I zr&{!8UA^;|vB#DO=Va>K!8ZJq)_zx&cYz&hg)Qox*~(F-pfc-`&`sQ<9-Um+i5DrG zl@R6r&7uJI9HFsP+~fJ4%w&yTiBdS-%O6T+dzpff06*W#k~#Id>n$d}@R^Q)++%7Z ziuW2BAjiY0pgCj%e`Yr>HDpxeIf;|R8lxppbX1g-7u4ceoZGmorpP$$?ArsJb~ z0)#ucf{uTC)P5Ad19?=sA%mDXVVv3~15Nb|>JWM$mgdkf9VBdCvt}#1;!5{=Jy7Vq z2P<+x%5QO_C#jNc)G$`&1BM6Fe-7a!F1zt8ueqi;1mdQg zf-Yw}Pl$I+RPZ`<;nvv5M<8M-NN9fwim0t^ZdW*+3d0WNKN}XG_ErPSZZ7x(-O{TX z#?T%c^S{SJ%p=}Tzr|clCU2r{%Q8XC%wn2)7w3i=)TFkdvV|&`6LM9p%Mt9oAhT^# zCmqJ$k=-wff2Pib{@DbwgmanZ9QXQN*Tpe&i5dA%bMINrXS}9D>S;+mdF@8&F@06i z4h^a-=w1un?gczD+?cqVUnK!`-A2ki-ANKYxFnFeGJdU6eirp|ucgNTx=Z!=X_D9h ze3H480Y?!YWBsPuE$M9dMd2L(I>9^47r`9a#Nru$>$J|~mi|5xHPWQwQm7qMJ>$FQ z3JI&)LI!Sw`kdPhe7BvSO9oiB=-i(O$B{^|IL&D4>#j;`9u#tFmN{ueYe-i4`I zea|K|qGq4@;R~tca2m;)37dLW&08cc1fR?83rg4}yZ^*vF^!@YKLDT&uTK&~7?y}G zgj*?z7PH~)R|8a6;T|ZLMwT3xlLx&3xFhJbuz}2usB@9F?&V6R?(J zYt;Ipa+X;&&CUqso7TWI&a_KIB0oN_bRECOq(yGR@(6|L4jv-TANJXEb3UP%bBI-7 zDOgSQO}>@ADoQQSZE`*0%Va|CF7kv2E~TlRXj=*-zZ?TFDjdQvhO{w!#5>*{GjzdH zMm()Yxa|pfhQg&U4i4yL$4>X$m8=BDqEJ`aX-4Lbf)-EVRjYW#%em2g_NHtCsjBn| z!}PL6a4`$lixU-JSsYC2KJxqVEj#*Mj?* z29Sf~l?N5Qzcfp!f8*=6;VkQT?L|+~Ine*MpxyEP`^8aDA|^P?%;nY%IXY~hY6MT> zCCKE;JjbMOvRxV)I_KAGsh+cGl1o~6i6A@i>3aw%c6*eCUXK8=T!%&8&`|)IRXOO{ znAfrv&b&-7%ZLdnkMh{ras;T4_!s-rOOL(^U2JC5Tb?}?zjbRwmRya@pG8a1>ytMw zK0wf)^)mOOodKGm)|CmP(RdWUnmv)K(C~23qtEGC47XOi+)6|Yx4l|21CtKG41C_- z*O_jQ*jSdCH59YRYn8da{pmgYCzHtfFFQZVgB(<{8ZfQpzI{v;S3ZN%r5cjIc4^uc zTqA>nJ7t;RYD85x(pVql*w!6&R=JETIjkz_WwW5|#c_bK5xRDR#y4d90o}yXp$t2n zho_XUS4oQSY6`v}1~tuD&zX0of96Mkj-}jPbjy9|K@R|-Y;nQuw#ck_T?uv2(gAgg z<@rFZFA)Be|0-Zjz;j6G0d)%08Md{)+-J|Xgr_pP2D;h!t10hbcBDM9&Ub%|0C3CE zV-I!?+Rvuh(jD7XjmCKRO!ZZy{+fy>Y6+C`ofE5&Lx_Q7WL#F$S3pt{! z1VOpLhO+!6gXDHRugp&!(^j>cT}IL7mc`4VpvR}KEkL{?2i)XRDRe-at^OFAZOpiL z5xLMyA9Q1QJxV(gwBdjS=PNBZKY$iFnGS$0^80b9X@4qI$= z@Q*C##c|a{x%lHO-V;t6qs9ZE7NSfV@42%r1Ku^x*rw*QY4v&66hUzJMybfj8dnXU zn92ZT^3l(NR#NkZ2`4(MsE8ONtbCyBbMRNqnJ~%ew&bvlE?c#;D}pXd=}?~=xKT9` zrCRJ_uaFSp_;n1Vv{O?~KR)Bz=3r*VhKIPy3A&^%9b86xzeTE!WdaJ8bOeMKT%n4S zY^r=$%d^xo5sShZ1LzD6+}^Nv3L$BwU2yxL?}H>Z1v`P;i6%;Dayek2FW^ZH+Rv?1 zr{V$lT?lm1k$t-GCTKlUiH=Zb?AZeFb!k^sFyW zqd$EJwDBsR^SX_-0<{rA-GN&(enK(!0EPQM?7d}Fm0h?lyy%jUP6?$Oqy-i&-6>sy z7=Y3pN_Q(It+0s2Lg|u51!)kFl- zh<5-f#gOMW0}z%u5w~?cL)KiBOq5pW1{jy;B&-^UC!?QQw)^P=Kn|2{&SP`p$NBM= z(=U*lZpl=YMz}uafOhXV;}e2(GM30RY<--DZjItCib*TJ;Wi}wUyR!oUBAww&(F8c z370zXU;tj|TIsV8!CHTM@UdFASc9OWrxS$s8*{f#<%=EsP-;PpquSZac*SZ6rA{K) zJMOLU<0?*>=%GrY9obtT#qM5LYE+enB!~I2Y&V^li^#Oi)Ta+! zc$vUjw-(ByCBM=L~ zpvydnc&F5MRfwO-1r1il#LZp0$?$Z<3rj(buj(cV$1p*Odx#aca=Ev-z;A_ex&>19 z+F0-DO@36%M-UUNgMb-r1@(K5^sl<7{!L1;v}>7a+XaP?O{nl-+OWx)+OS z4has7OL^IuC9*+N{BguR%djL&&lGmuE)n$2s#)Bdbd59YrI!w8SSTLH8|C2SU!M|T=>j4<9-#halao0JF^&0*gYO)S;-CH4Yj1P1nhtS(q$t6H zUGi>!y|kjVk@_ja*{N}dr_EG-vt>=t6i(_9oH^&0b7+Y z*jHV9e!bj=Me7{+?!G1bH7cc`F$um&hOL#dSm!%zYkEPO>|Sh^h{pCMNN!FqfYJ0$ z>isul42z`abY+c(nQffp-pw!279avr$3OirQ6P0lhfii zI|RpK);Jhv0!ha05-`cOiZchPtq--Pq=Y6eD8?T*YT}%5DdlB1KrLxAlf!a^lUBH+ zdUFz)Aw~nqnAIDqVlty+jU49Zsp7**xhdCZBMF^R^la8tLaM$&&{?~HJgoCsA3(5M zg=m{ThTNG@RGK~6z|V!>`x!;&X3o&~*efYjnpR{eSbml%t=tKzlwXs^Fnq(ZQwI`t;V?y{=53u9vQ%KfiUL_b1!b?CdDrL%`+fTTduxH6iI$t(4Yl;x?(i z-N6RO8DIsgnL=WtU#1e!6VkPKw?BMYMXsMbt0F#+Qw_9~#2zTY-TK9_IEIOdW{3q5-xO|7jC zJ=3U&iV)#?&aXkO^(H^esn1;9@-8_-AVNl36pB-c>g+rYdDt@B7^kb|yBQ?!!$K0s zLeR>jk-H0s*=`IE?*y9Asm7sMKse1}ni1(6VVWV`2uBch&z9?&s z7difg`Vms~`9+#tPpfE{c7k9aeerRkDo=0qmC;}yPUBK^Rh#s6yT8TG91nqjtSSRP zIf7WVtg2POq3ga)P6TxqrOQQ@0pjrjI>Rzm*Ig`0g_kB73?SbSBKH36^t`noc}WQK zp6Ez435%0J6M#8Lep$E3MOq6EI$T9u+d^>m?H0^5NQLnp`;EYHQVI+lXE1RMH&l0DQuCz2j|#8 zHDHCPCM_qqRmEL$oqL@N8bh8ZY6rw`%xyn{q+I>jlqrd zcMUx~g7fE@Uwhwp1HcVb8{m7mRowJHlZFFm*+<3%4GfOa=XR>Ie&Q>Yv6917Z>BVm zz^(hNx!7-4AWQ_)^WT*S&a6I^Amy8M7?O})o{sQnS}Rg1%{Nj&yp`_Q!6ApotL>v< z|7OFqfOv%RDu^F&m@sp3HK=|+p}L+YHQ07uif5r4(K}so_QPhy^f`fdJ+c9Z{`8Tg z*G$NIMHGpzn0oq!dfgp>ATW>k{2@ScB`$;0y(CRHDoKBWwcqgvEq3Zx9W5Bu%5+9w zM9+15jZ$V=RJMXXF7?a2J-+OTif{VrAGU4ss)`l>h@V5xfJSHT2F$?|HULDzg3&KQ zo%8{&;vjze6JTh4hqnE^0kyapA?Cn!;7-0|OyLyLODt~X{ye>~jZ4R836)I&sdhp8 zua!W&U@BH(d$pWro}n<9Jm4#mLgLcGVMxH7Ul~XJkSmfT!^ZL3hr!tzi67@gcqT{? zz_!Zr_o+M%3_ALV4!LVlJx*#wQC?g`$J>vOF<>8bOtu~d(Sie?8gmVamDD&28UB6%)Q=jRy=(p>a}m{vBU0B?7G52U8>`D`|`Z|-Z9F45C$Iwe(TWD;g^V{4Sw4xI6NPtJbyAW1Q zjd>r6nZ40sFTY-uufkmx6-#xCV;dygLFDwa^J_%= z?A0eg!7-~tl4crqUd^=zZGRsE_zynKt1Gd4$F9qLbq&t!poad^BV~wwe>=;d@#&<1 z;ffNypDkckyfP$2**}@}sc4|__>rf8sYUO#l(6mbU}qfAy+C1q`(zql)gnXGGFrPf zC)H?>ksOf?qJ(YS0w6&#$pa!gWYyV+0-usIfw{uvfy;uthk)Q>XfMWggIT3zTT54% zA?pb~+ElcDwvDgrz#lOBxs1l(y$UH8dd@Ruq{>GBYlANKMcOTww6(j>1x|Wh$2XB` z98)Zq=jv^@fA7@pq-4Id*eWxwjy#v9Q7~x9t?4Py@6?(51vak`$Iu7JV7>QRlmSfT z^{Y4UKMfymR}=cxK_hKxo>z=e~ajB^*LbD-1<(*qUkh>;|F}8P3>& zOtF`=P}E3G$s;|UOmA%<2M47-LhF)ybF0d6xTubI^pd6rf_gIjQqu_#Z!Yq_Q@GB} z!~1yzEpKH(@TM>vP7fT5vwk;kqD=+LmDQKm{ieo>YNd?d0}zdRQ6y}`#tXKB6b1`K zLm+Fm<1Enop1YPtK=KL9{>+Y-$d1 z-wp#Xj88{v!`*G%Vs&dp@22mygABpyb++AR;Bm&|Y)1etOCRpvF%fX&a~Nb0IqnrC zA%#o>^^uuVn|3r$xrWQ!lwK?rnZToO??f7yKdrih5cBzXlhGoj1E#p+JCBS^%Y{Jy zPFE8oWL_i{G79nq-6%WwBGYqHg1+wIS+)D?!yr7|?gAN^M|Yy{tg3Sbe!%*D2O)49 zHTQeA_a(Mc(1ph(j&KxEm!L-iP7-LFoxg_MJ$k$IgTYxuq^=&&G7Z532fM3MaO zP@28et9^_V-u^o!sN!oJqK**uxp29zI|JXdBqT&)crOA;ETKsCjWU4Q83Xb{TkYTQ zOprn63S}yrc6;5+#+FxWIvYcUQ6Yk+Ot=<3DEthhY)k#(<+Qu6r2^5*rvaX2`k?g@ z5ZBlO)J2auQdl`M2Kt=TlmlwqVLpUdq$@-vyAVDDuG&h`%2xE(%VhwQ6BpG+2m{y_ z7k*a{uFBsn(xWd;K)nx2@;Rn?{(8P^em^gczTgr_rwpbtSCa94nT+6*!7GZX>->~}He!>xiO4cAIc(Jnk#AkScUX`hbMd4zwfZH9oIwJfI=9s<<} zDuqEo)lI${QP^TZn7RSaOjE|gGzC?Kt&7@*kDlne8(34%H&7L z9uy_{M`{Wlge!?N+0epo6b2kjEp|)-wR7ZL6?e_--4bNnp z&IO1*kpu+k;_|R)P+N3Ns&2?|#z#2G_u`ot3q&rev~i zQFxlL3nqJQOyD6%@qG$0@QA40Hx^hRpU*%p}x~a04!^V_fH` z)K+SSPJay*^T9~Lh;q`oXz?c9%*mX-hC^v-2RqYT`o+L&K`EDQNrpq~r4@%o${8=h zupycduF-BsjBAh~^6~qDi*+X~gFgzd9*Nf+&IIG_yrq5#fjM;5E(~yWuc(v)S)E5L zyli(6@O$rWW<5>zGi)?#b4R7?j~;_MgEeprM?roZ_BBdLbj{1<{s$KI z#>BV4juCf&zu z=Rrw(+Tgflfn}k6pE;mC6i0ibT?TD>mEMQbMQ&0lsybX3vkZl`%GxT9B^MKWkYxas ztJd7Vm`sG+p=f6l$Bvg98YxGE14HyKL7*!1KU#oD6we(7oWQo;LZ0XDu<;Vc~rOnmSlAyJH&!*Sfe6UZzD3{Fn%Oolf z3SU z0!QdO_pV~J!69;DGD;05XaI?^Qc z(W(ZwcFS9qgwEPSF}A830qn5r`2*LI1Zn%j6N+QuB)oH620TyW zTLJzr1jzdmQfCx>vv9=l&5)y5rOA{H3V|9|JHzb zVjsz-FUdn*3ooYI9uI~DkL78y6ca3Rz>9U5PjM?NXrRlMWdJAmMeK6cuf8o#$~!c8 ztkPb?B@CH6XB#G;%Khc()STt-&fDiE^+ zU0nR9>^BeMOX}DM?0vV8qLhpgx3opJn*%b<0uGvv4^k~&=Kp4h47zHVJZj>-rL9T& z1P)gJ?+ROBa=x{A{3WCjA(cnx`!kpbs(@J{V&(04zxe6-*>b>O{lWLn&Cff(Lu-|A zn~*$^{L(ab)@yS6r>kavil$m%k6{W4MsnT-|FE|;LHqSVV%u-3oaR;BvzhaTEz?_R zH4R5QZvJzN27w9ye|Hp6zuxgHkIH84<7far?s#k&P`H@c>&+?xl;K_xuJTv&Jeu2y z%O-cqjXr=7t9fS+V%X!H!&Ur=C7%muBFuR7)yO5hmXN5s4&Cs_eH8vq*S=2CZnn{@ z{6=lgSqN`@GIu7LN^s#xmxZAz?`6w#^NZdwzFEpV$ux&+yy4W@7=YSz@)ciWXOAZZ z9am1!P9cQ`E!f5O*6r+m&TKebf|Em=qrI7bzSn2HwjXhxEf@TeTLaSH?w5PrKaqZ| zS)Nx)XB5RFkm-E}q$+DjG(_8w6+=+NGByonrm5AzfFYgg4OYwZ#W-nd)kQHJ8g7W+ zwt39w(+!RuiT{%~y6qwL-0uZ-R(0GFDvj%+9Y%>vg>5A2Bql8L^a5bGS7zU@o2jDB zC36`I=nlQTedP%t+@OkQCG4?nx^f6GSmtA&Us!m!VQIMmIPV0gj9MdO#|Dpm0O0<{)#$j0g^gq`>$4y_xY}^3%BkRj$CxwNm=Zo?3 zETCBUa$8V7L&~qdzrX!8-piw9SmS3P z#zbmhE}O0Z4E^)88>GUd^XIhyPz27>$dd4IznTJ*u$lStJAb*^LJKP>Hh)G+TL=TW z1}Bgsrop>88we0sILO%60OXh~l$xA>MDc^xHvy%?s$tV-#Qg*$(4ZJe0vK=s=TAc`&59hN;5rG);FAOl*RbG$>e z%2v|nE?ZMIFwGn(0{^oab&(lcq}GS?GC<$54XOlO23#Nie3AA0lIHVyA7{lYFKdA| zi%X3Suz$88Bw;jKwrfXVN}qzVVAL7G9KGrn5L%1vK5h?=2uFhZgFm&%ri$EvGOqi$ znMWNz>eKQ^pT9c>U4ujg;+^x{02f`cShB_4Cue2v9!0IL#jMh*>qE%>9RKh!`SXr0@!oY$ZTOM2Q%URvgEtInlCF8 zbEP!KduASeZ=a7ppJsJBr+%R%NM_=;@S^c9!^8jzYHcQfKfC4z9$-8KRa~=I2}m$C zSmKYNw=lw6`Ik<(9^28gmB0U7OxB7LjFffRp5*32G#`yUB9M*1D>*j3ABZQi15(x^ zsrRbO$gyH2%aH-(2A*mhyQmKkc6Gh5KL{~sO*<)}ph!j462t+;8>f+y`MK0!r^)wn zSmPeoF-7Kp>mG~7F()kTuuNx3sFYDSwu&E(+J3zdX21e*Aj!>t0HV)w$=RqQ4Rh+m zM-23V1(5jK5yQn(3W;4d?`4uOw#g8mjrvB@WRuQ0Cf--Jk1&J6D{taF@)0(^dNNNu3?!1s5z77NOdI%TQltiqVG!Ged z6@-S;-M$yEMg?(@!zM0etnib?SsN7BKbw-QqM$zb?2Ig_^)><dj=GyP@fu!|I;Suh4)+ER(B_U#H z1&L1y67}oikLB`ru=~LGY(YiGnck6iKbPZ1VwQO0A--2JfDUM&-6UQE$|jO`^T~{| zRdQS6IY)oLi*(TNWcA|)xgzJ(exR#(XcxifCg>F?%oTBm-#YYVk)#8&e|ZF&%2mH_ z#{JeKERzpW;x508;n5+gR<3AlfC9hLLWLyomzWelO_6Ibk=>3E=ZOxBn+dg!e}4ZR zcZbo47RNKu!ZPiBAAr~tE3t9pR@E|2?90h0ury}%(V+L;m7yTv!w#Pd?wlG%>I{gl zqy+fPQNrYa6RoO_qHam(LC=SdaN)TEs8vETRD>saPhMpsx#fc7`a-Dr^*6_Eq(+S& zU^wK30e$xz(o|eD`5(Dt4>69}fiMDsn8tF846+r3b%&4P!5xQAG0Z;W;I9&*sR_(O zj)m$BX{ho;*(>orJBqSST=-{_7q6R|T5DI363!t6;tmab&Ilk2yZ|%^Rc@K)Now^< z2i~^{+v~WtC1|v!;`IrPh-<6E0sm7)Haq->SBNW0`NFd6C%3Gw5Z$rOyqEZJ56Ffr|c5Z;sXTpsJ zBi~^@@M4``sINkh#s5)HEtCLKoS%F!4as)Bfou^iWtQhc#P_zlMMkZ!hcS~~Pj6qm zaQS6q|JA<_s62PK5m)wRt7J+6pu#CJ&s|j`7l|P5)J*teL30PYy89Z8BoNUbTyBNj z45`aChe>{wRmTW#)JnC~riM*^#S4eQ_0digHPPPMSkYu~V8fjx_W8J6SDHw?yen@W zr&0{xuc z!0sOyCQX-%bAnaS>j}dxy2{YIG`xr*iqsgi5e^Kbd;;IhVJazGJg{=8y#}_tI8P$g zra&-r`Gx|yBN?P*9q~}v(BdKVts&BAk%8#TASQS3eWZNx#daG)^>lm|EuAdV8m`J5|@MsVRFi z219T19p2Xk>X#;!H;0xQb)CkAgj{%-ltGoA?^7tf#wGkRvYh=@ zNEs#%@lPs$14of2W(?edz}y-roNQ7x_$^bCFcBG0Lmej4D+c}|=7LR9f(+yqr1CRi zs;-@JHs0>XjtU4-pZ_CEsp!{*W3^>mNBTH9V5oPO)ozCw0i#%%$oe0v)z`tUBEut$ z!-vwP?-Z+8T>a~lQi@b}y3#>`^)uN9CrRMy1xEjsp zVTmA7$^`aEPvDDpl_qB=p(-PF4bX2V8NJSBiLl13zYhQI#BhT@68}M)KpFpS zQ=Q?!Fj5`?KX2tn!M_68FP;*{!j5t9=xGkGS_Qre{jsS^}YX(ulxc1V+S7U|9imzauzO0hkmIB-2-_U^}KZD z5*4i^^xJ5rulL_QKpIq(JFB?rfB4lH;9IL1de^O;!HZzBQphv?J7Kz*mwyr~|E z2%P%#?42_mhL;rxu-rSz3@LrW63s5BrhRsrx@4&TNa_pz-%INMT8(VJT3~qe>FnO; zLyzmrv*(jLK!t1j8uqVR0YfIPtU__<<>){F zPhJVO4d5lO_`FW%!&pn}roAtx?xC4n9i4?-fwIw4AZ4#CdJPVPYcfs)1QsO&s(plCeTj>V$q7w<$j<#RKe!{3cxVRvnZX~*4K28@5|T{Jgs&2YU}Fdm z4lOx7lWqyS*%aZD=>NKzk@52P92b{=Xke~F$e%RDFH#k ze7~L?>6YW$Te&=%i`~4KK)fzHnp#K&w46_m+6eUG#f2-|S*LrvWe4Bhcg)K$2ww)E zTEq*=pWv{2AAl8f&@nvBOAo^By0~v3Kq%k)0_XGX%jPuXfzhZ9(%X(#Rp9J7g4l1Q z-0VV1N_)d+DM4uo$acL2L6~@r{eooGE-9XeHgxK5kw8`9$3^Evo-?C{ltpV;g zpqA<^2KfV2qv;Cn^_?p893V5dQ4P8(az(H9l(PWQm6}_(qxZJmgNt8YMtO2$t zli=g;dc9!b!N$Vu^5Qc9%w=5yIIlzX5+2(?VBRzVIGm*;^#U+ZDA3C2_*JNxW3GVZ zma7@x3f6TGxB{VI06I0@4~la7&Y*zs!7WGJP31LNo;U#gtO4ll;pz{$jdsx=70C41 z;wd9c`f@iUP#erh0VoP_l>Ges!f-ygHCVOLb8kCo3Xwb>P3h4Qja98Pm!!46HU<6c8Sz;tQNc(@x6xIM0{i)9Yg17cS9*n9f8(>E!f*6;EDzy6C3Uxx; zm5@|n(SeFMQFO*$%#Wm(6Xi$nVxWiTnN z+US>wV?Z*&Nw^feK*A92S^Et`alGs%r@|WoDh*qk1Tr`7J$d0>lb39 zEFI3Bf%7WyFDxqaM!?q{5BfKMV#J90A1POt=MP{izB1ilF!!8B1oY za-zm5Pwx6=AHazEf8;9(-Q9>N<{c6T8JGQs_08p1WY{Y}ok8EkrRVms zOCGJXziB7hP$YA0Pb$;Pu;Aq~B1K>P8*mo@5T1MpQfNXUcr4P69(f$E z+_C@2R!(y7P{Cvm1tk?dzxKFR)_k!eB44!}sVVr-+^CsWACj-aDKK1q{-CNAizM3` z#Ze)ZRyDb5leS7Jf&*q5>=hDBNZ5*ga2tN{``wH6UyEOE`G!LsV@8ubI{GydsFlzK z-^#pk@_jK~OIEi@(T*#lAWQ~7pMFUwO`0&YJ`!-7Rs<&=f-{j8I56aWb?)}9Yipuv zu~`z=22#f_udt#k% z*p3yLSklE+b@Yr+kezkqB}(VeU_2_f6VqL!XJP+LIe`)8pFG|mnk?hxrAVsg;#tWME{k&Fy&vV^ zOX#sJv+GWozQhqi*kDrXE=p3gqYRmdG1wheV~wj?ef?T=&BnOF>WH}x_3L2v6{td^1I z!{7oTO+8p>Ey1JUad<;IIT1$I!!Z>?#8?{UZyCW&pfCH2;eB>joLZ?d=n}XVM-G*C zHZ(flr-;<%4ShzK6-a;xAHeZH@P(aXgTsk+uuXl&o!^Sie1WPXObYJP=xP8nlR6yTNG_i?!v(rscKU7 zfYeyq_HNZH@}Wq-c+scW9`(o@CF-NejTJYCLr>?;G&2kkVVf++&3Vv~N-WY0^>8Kv z%XVU0+r@alP*WMMJ=)``yjl&yt|99lO8A@jyV(31T*t4xonN7}>hr|g_0XczvEG$y zcYZ6dUD7%6Acj5+HG>^Sw6*HFKx&aGa4RF)`$f3~O~)7ABC_#q=xL<~CB25_2X2Di zNR#OeIko9RAr^8`JJEWhT2;<;+2=sBrs!m=p1y*(+-Wb)p=wNi~T1G@Pk6ahcgU|`7x z1ef9A7?|^m@}0W}JNP{u1Oe?~*r(1Y^pzHdBNev`oISP&b@^O?->; zrKqoWc*U-(Lu%?hT?3&?r%Be*JuP_fh9588#bV?s9vHA633>C#Yf*6n(ZmiPGxz9)&I8B zhFNo7-1V%lLT9YDufQll#BOgNAO32#Npjxe`80c$Ph3)y#XvKmUj62Bp~`CyyZyt? zt}tDf0CMZf!o<=5V30*@cw6=Xe1xV5IEx_lzh?^IEgwD!_~ z^5^~Q5y_NAsm+pK9>j%SO7(6ftarNygg!++;Cc3LH2!zYM~9hrEt8xuiMaS2UW=6A zvzL9L31-n>HnY?Ag~WS&6b_soQA(WKEAn$T+aEF)i8qSvjenH_LsrmR|JP3nmSJEU zihRnMS}bdn@ZN2vlR6sZjSWq!bJV+Wg|vcq{xr=YX2)M-1DfJWrNssp>()G1Uv-!2Psn+G&mbCS8lyaGQ-Oxb+da z*_e+*H);mIhr-hSoVWVLI+P`Ln?orsCxNN(UGLbD?lN$3N4-V-_?3w}ZsL+*W9A$w zoJ_BO^%V|9F1KxGjB&P5mXgBLk~oW7MxNUB$9}ozxDc8kyPk*PsQ&ie`!XXw75}TP z`bM+*57G3q$|_{MMC`Ef@?Y`Mb(~bsz9zq_C`cAIQ*;Ob4Hpg7@3fx4uS(ZW%D23$ z>%V3USyVnH8#1Yd@??lS%YFLOtCdn!x6aEYYP!+w=jtP`X9wRJjSE$G_FOMJ#EhFQ z3sg1;rOguur0F;uby(xfv}R_7Ih%{zdSFZznwLV=gvkbij&PoT);$q+Bm&ch;b{oE zwHyRKRcgrljnpbbD7C`o20rbFwW zd&*o<5aD|6USaS0dm1BU3`O!CZo$#^3yC8%lH~XK_I+^_{=%uV*UZKi>YT8*(+zj} z^(#jvk`M&cQtJM7-s(>b*cof6kMD=>9biFZOwzv5GiC@jms!Y-?c{5c_nq6(E2(S% z21eoJ;A_=rxr4+;2A3Q^lf-)(VC9kZH*$}1kPV-dj_H58cr z?Xa>yK>sJu!MWPWUfnsSPeSk($2(T1DcPIk_*CW63N8r%-?N6-XzbnB_~xGh)BM}K z*Gg{=n?6s|!EqTq_FFdE^f{k8{ZbL~{IbE}Tw$ee?u7%hfh0 zHa_+3x>QGhUG=@}KV;z*C|IfAp zzA+ohPZiGNu7$(7lHw)ryw^zq1KE;Jri;72&>*R@eE@gr0~ZJ$R)N~`8hdm3g%ka8 zc~*0*lWXPpH@AWE&wkzP?Z68j+XykmD8`ASmo^8AWEF{P;&f9##U&-f)PTZlJKQoe z@T;2B(}<}ZKfyxG;PeM_ICt-!MRR2fs=;kFN^daX%cVTr(IMf~BkZhB&C}(R0|!b5 zy(7l`OocVERS(FqnA9mpM<-Ui3~*bWW97_KlSes5y47zC}>o`}Oo!MZc992Juwwk)@dP zY3Mx>d6%z)1pDsqADm>}iI&WdA5?#I+;?yj!P(Rxd|x1L$B}IKRvq2${Qbqy%||-* z@-mWTYjg8S2}6-dVF!vM3FGhr-S_g2Pey9IuY|>X925|PjIhy|=-ugoF-rQeOU9TD z!{k{r>h*owG{i!u%8rM?l(^ldg*v0UbjiJ~hPcGI7?Oa!>P^j}ZcR#L~#{XT$c?E{%#X2+p|;080ZmR&h=pq%&7 z zgA6JasAK7sXw9Rzg9*P}nEkQbpSA(lGIa`;Fifc925Rh$04Dy{Zv)&zNXl)Zw>}0d zg&&SRdf~`I;1NL1t|Jf9EnKw$WIpta1FIraaUXuwF1mxlAJ`@2lMNmf_cXIjo&E3$ zu4F8n$Jw?YDzwd1uXckB05d3Gds}dyLW>A!MRQNuvn4z%>~Dkd=N@gVo1$8gnyIJ%j3t%AmjqnhS^~(BAZrohGWn1ut z04zS>@Bk>k4^cAS3F3%1%)CP@^4!$!h5LNAL3Kn|xm0zbkb4vV&Q>-WOG)&?;le5i z{i#L_&HZ;T-gq@(`>ES^izKsNzg159ug6Z&Lnom0ttVb}?-0<2bs|C_NJ%?XGkY`8*TLL8CUCEmlMI&pua?Z17v|R$QS6N^1lJ#C}n_?NFfA)7dltNOr=#w zEQMc*erf8p$bt~SyVC*l>C>TWMzndjHrx;3Duuvu_T6T}=h@CM&^$mRz6RM$P+!$W zsP_nV1rc&gr(xMx&V~3lTuTq$i1JAj5mtkc^J!jH>A=$u5oRx}Z-kpQ84li-YCUTT z#=KBqhh6U&e;SHtsmh_dDGmBxMjnXPyH!{<`<@USL4V`xhyiiJ6g}Hv25+|Q0L4#L z0Qlhw@|!(XvK{deuaoB`AKJg_WQNs#wC#?idz%po_2S@h>>SdxrDSv^;@Yf}H=y); zgCOV7d&qnn;~8KPD4KiD&$6fY*fT9gobZ|V(BjI|-HM0Fy9*TgE2SWBCROaaEl@+` zj)Oc#|H~5# zgGzsMM}a<1rL?s zGNM8sIGH5)g`;Y3%cA~9_mt??+^|zy`mhP;6VnU*A1<4c@?*E);f494udq9>21Mhr z7eb!qlrq?i=gOd+frQtQ>>i}_$$2RCi;H@k>#f|Um^SM)%wy*LEAjc~Wj8LB|cA6@keB*v(oP|%FG zsS!Pi#&LU(&!w-Pcu%h)+!9CpFd(_C(kEEGiI!{D zW`F_2(y*`@2L1iO08k;!gWE?XxUehe4i>_Dh|cLO)Y z3xz0eE+^ke>&QJhW}JKGyXCx4kuky!vmdfGL;8|X)HVhpCr1TVM*5zfCLmKdwK80+ zxxuK|qb5t9pzM|vzj6II{BG(pQ^P8Uakahv?Jq+{jTHJ_rUP{aWX1zP!*eZLkL&eo zu0{D-+up>P?g3_)QDk&*8PoA+YaBk?5%6Coa3j2eS?mR<{FFRsIFF5HM8~`8^kmh{ zE^}KO#aFba%&_MojQ#M7ABZ|98uakOM@|C}F_NDxeD@RzfLl}*Mh>z?!JMhnvhCWH z(j@({j3GwWc-l(#q({~_90g=$2{npsHN4@R*VEvTb=YXnw!wyN&>>I&XS`K%c-_%I zkZMZao*HA)F8uy+uZ~H`-ayVXPfnqGcABXCo#W4ev7J&VDKdFC+Z}G)k$rcS<-U-X- z1$*#`B31v>1pZ@16JYf}w9_8-(TwM`5lkxDDzuSZXjkb`7^~Y#H6<9>ssZVTd!>dI zso*YN6=~X5qlC6j8N+z9-AssYO87B8${Xvu{lmBTT1WRK+B(UTnBV>>8uGTYWFX!Bz zb3@+3QR(OIUYt*S%~-E_jH|SFIy!6a*UGZloL>Z(PuTfhA14j1^NKYq{S8w%P>UXN zYV0cfeh#o#U1lM18*M70?QIoD%m!fVmYH9-@ZzeGWGZ9Iu#Nu-uL<}KDBN)I)Mp*nX zs4mD6TD4z@>fyq1wL1hURe?6o?@YoSQG?*tyM?G_9~^)Bbx^rQAKV`#Tya#-{BhjZPz{C z8a{&@Cl66WYs=pLHGT}}Y%qRHg|6HEri^71f>B?Y2}E(bWgYP~^w?o-^PA5&lMP-d zPacSMiJaGJn{0{YOgbU`U1tHXA(HZUgx$INJ9Qp&_1fXkqNq5{O`Pir7Ix7;Ira+M zD@05^{sb@nYf1QT=OSGFz{P{-w0 z3_%RUAY!}yi9%ibNn@bxq7tQrN@+|nZ-d64#RGnZmLX`6C>QbOO+&E(yU|_5dFlyAV;IYIbH#YF5HK>{&stg`{-Ns*3brkZ^sFM2YwEz8%|Nf!>zl;+h zC?1_Te7ABmKhS5zHx_6b?bTj9vajSgU?%rGV9R0rvkDndRKeOZVY;;$s*9~VT@XO}g_bNjfuuSo(p9stUS&o*(V1+3&7~|u3$^{Os<1TWFf4G9h3NOUzusG|_ZUCzBE@QJ3g)iHIU&{x#Hd1Qeg z5Dm0PM`TOzLmYhNSj1}*A6Z8?>k)cd!NQb-Kh^4otCnqp?kHQ9yvx*T@ zQlUk+{`U3n7!b*Ww_s?u*S&`{7ud|eISo=Rjcf~oV4z`N9$$RdHF-uatBd)!uYbn?n+d!HkySN2FmJ8|p^1<}q2xo@ zuyEn1c$KB*-z0M*SX)Q2Bsokd$hD{5)}wL>dT>Nyn%<=7h+iG(DvC6ya}dxT7J7yp zf&jGVWZ@I5=)-@g7G z19Y&Cgu*K)Nx_;E!;%I4r&c*1b9^`SB+ICh^{?@s1q;)=J3=!dkR1#~k)ZJ z9Zc57?qu^pvcb==!DN)bef=lV4%QL1{KV5p&{`(id(eM;X1@)Dx?_z7*96Jm&9VbV zr)jaH_+cw47z$lJII%S7!4*1%eTBBk7p`1d0^|&f27>lbWPTIDz}5sk60KrQHegk} zYU9^65hMKD+NL6S^f|n$>8Y>+XzhED z7U=(*#AdgtB7_Mm`R3oP8R!Atch+S=_nsv#7z(guBXYne{=sChznY< z@z(jR1O-IYFXQN|Jv)m(C8$#QNKsJ6HuZk2)qS9+3c>AnyBV=Rx73S#hpF*+Bp0lz)jo(?xFPHx&R_rAJpH$M`fu~}{|)m5 zakOI$(z@MqIV&r+uaKf8G~muH&!8KDLB5XP@b}n*qVfZa;ra$?ZUg18AAtL6_W?Uc z+!jI0^5;YcpF?ZGCV!)PPmBO*Mm({B^(e;v_hBF%Fe6)(LdO}%qx}-tg(Ea-i+>#> zVr(&Rg!Ra^5+ECZIs+pd@+AZI*SLef(^Y{j<iS;W?;mgI!y5Ywb-DLO$mNp zOcH5!TN#06B*3{Jru@fafudRgEBrr?ToR;3!~B2yPDV+sAeUDNlqdE9X1EuSf|dZ% zPXO?vkar0HsJMT^;~YQXso0(AI$aZl10&D`{2!!!byU<{7p@>mNGSq>bVv#cBA^n2 zbV@UHJER~XEg}ulAW|<1A~AGHswj;yNJ@7j-EjBM@BMD9b?;wyEthMJ&ivw>efHUB z@8@|o>HJ4%+`$HUvG^B+6fL#skdIbe{FRpj`M}*?A=i0z`Xy*~a%JhZ9u6TMgfK#{ zDh8>LCw&nEA71N+d|u6p)Pu&MkKq|By^ zn@-}OQ_4A4-oiAzF4%p9QaP7ckTwDCY08dO_?z6y7J%r_XTcF14OKvMa#<=uxSzb5m>o+8BS zzeq3Oi9vex_Uj3m!FtbfYZauQCZ&BO*Zt>s8_(x&eebMPSWY!sC`%by77= zeo2=92n9_YLC&(jwpxM%`zp!vFMM$jyvdvy{0!;V0w&PCkh%j1#WogbPW?c@s)G&S zhH;?k(+OFVY#?j8OnxCu9GFM)D{ere5&%%?J}B5t!&l<28{DSsg+|d26nV1Al*0Rq zj0#*rs;&yzvH|Ez3k}_NN{Gh52D0U2SzI<}RT33+*MF@6P;vh7DZVdcxQl1Nwh|$M ziKYkQ7v;T8Kw~K;^BGO=txuKt?7O4@D^XGriWXduPpN{yV(0{S{jE10uXXN9VKMJ=pgX{ z>cIdb*or@)){f>492)V~;Uts~s0XcZNm@ zfc7P7U$)d>{tcE+bjk&fYi~r5CAfoua6*O%>Ax8 zge)1FO3PZKHUZANZ4t&tM2eOgd3qijcB%%@V!!JM(ot*h*pyxExMlMx4P7SwfTh4{aNPNW6*VuhN^C@NlxSY0CS5BM1KYNdbR%$YNI+)juD3C|Z)5?qE zV-0@nu`;B6dII_#F+gI}L^Zrl@!?d57YShL^!khAG=~pV7YOPg${H6A0BK2$T^DFv zCB0PRwVl7(9T^&`PADCW)h@H{&hLCw`bFp(TE-e#Yr3K@c%>#wiYYp+jJ#2?KOY>& z+=u2+i@}mox0&-0R9mqO2PWeHLDu<>9dDWTXG$C{3+}@(SV5{EwKh`cVFlJ)`(hVU zJ*q(pwBYDvva;?Ni>T8|9!jlwZuF|A1WXE$Il{{q&m_=W}}3 z20N&`VFM5fWLf%qq__(E#t@VIG7ESmT|CFJModZ%(0=B{)t5K#xh*DG8zZGlxUug2 zuVq7^SX1ytiohaHht3gi|HVHOv~;|iU+>Guh(q!-lKII}e{ugzbBxTdFzHlCy1{&$ zXsKbtJzOB++4%w%2p8a*lc8Cc?QNPvyV6WbljbtBWwgu~Am^N)QZxoTOUgR8#UI{R z{;9x|q#4L1P{d#V=Q*FQ9|)>)VvM_$#V`NoAa2$0*QjoSQAM14F#?-eC?yVWw?kiH zN*W+f)$oJWSvsGNf5b^trqjVI4SGM*k+_tVi$8UPg_p2pR4MBi0AQcrP(5?X(Gl7JfrKsoZ!i4zLIOw@qqxX45eP%ot6W@Uo0_e4H!hMt?xOd z-J#pg(6Rhy47&Ev_csH~vE>pvlH$;oxk{uE4Wd`2;nXJA0tR4+TY?$J{-zABA_)Ob zy=Csd+e_$~oteq;9LO!^5wIFWh!wRNm&ZuHhb>iFCSwIWVvJ-A?A?LHY2C5N?oUEE zzN$;Woy5o*e+X_0fr1DrbxPr8A&!L7v+A!SxevPziNNFA|H+ymOYh&K%ikl7UM8Fw z4sM#0A-4SY4+C5ex6R>}g=z-B%NvT;ErO#y1Z z4PN4l((Fn}3DC}Rf1A&Q<=@27r4nIy)P||n(RgGE`tgnub#%`{cfELMjnx5Atz*el z*fewR0Wg5!(B;qgeOSq1Y^rGkODDAw1kNvujMNgv3MKs9p7Z~mL_$1J8Rmhy#qF$X zI`?tE!O*y=7K&=XFosavFhK3DjqwAHJr?Svo#3r3p%5tm*lQXg+XMs>E@1PG7x4HA z#54k--tV^ITUcPSBUNk5&LoxdWDLBX8`D4MyD}wAK=hnT*l{=pAoT5EDu9aTE;Ij1 z)Q$lBH=$^!1RM1Xga)Wxh>O@j1?Ww7HoEreZ3*AbPa8QzyVBEVG z_GfnQo#m`Y84hvJ9KbpquHJD^L#Ew_m7D+_RKj9$##M+;f{w$KoQ1YTh;&qG-$Vm-DPEAK|+K$DDL~dgW4>%(bwDA3I*W|4`?67 z0g*xhAdyzk0UHa5F$9LLSm^B!1>Scl6F}{5qfooY3}ujnY=wA5C)%pamww04n7BKoP)u6UzP(q#q05 zJ>`O&A-Dq=1@GAt497twtbGEJ(X>}!BL7=ygvxKph9p%5NTHWKo(M};2>mY>K&tx@peA&8OKX*uK%R+- zT`}Q@blPy|$AGg!OJF)WpUOs&`lU!Dp$1aGXp>V%beV4zERHhXLj=?k2jE+ZFy+%= z9#xU_cUArtCQ^l+`Y5^dWIn@Fjx&Pw`w#E^jfV$bN-||rvV=+}sWasw8#7W-2N@tM zK+nRe4tp>)s!~FDDAL61}M8iB6ngD?cY&CJea?` zPyF9el41IRMOY*Vtb(D#73z0f+W855l*Sd09s<4+cydbdb&Qja9{apBS0*;F#x+r0IhZ9-EMOe?wz2C@oRKNcwt|+??RY^gW#U-Me;g; z`8iMux+9h|iigcsV1*zv0Yo}}dowMI5x#f@hlMXVrkjX+BOQ?kjaH&0Ve;+X}q7ecn! zZh3Fr0ddVGBmo18cJ}8N{bp3`7uV;E46~jr^V|O8+VS%pciwypw8e<}5_2ddV6&N(M+$ z=vCRLjDB6LWxexbrPS}zV_dKtiZ@+1k$vpheI_kzctZhsKaHTZe42>!bE*ggR9j}* z{zfT@L-`eXh5H~meD4Wi8iJ`YpK-^V`-b!K-VG%5n&?3NUCu6eQtX(dv!6&2M=JS@ z;7c^4iJDlenF}vG>MU%nuUq4U{yNzla7@?f8QfpOT+QOKu&`{2_kkYAZy&a0aYDUl zupqCSX3@yNF(CyroKGE!$Q>ksgjld!T7~Wjz6Jk@3wb@-Gw%!I5K}9J6sGOh_a$m@ zFL#a`aP(=2pgi2sN4^KCNZKn!g^9@bcp(LC`yr-88oqa%xWocb4$O^_4E&t&ESQ@}Xv!RJE+4gL;l48=IV?!UVzxh$T`bX(SL zsWt2fk+NU2Vx9@XTViOacfyhTDswj6@rs6$^Q#`HV zsC^XjO)2DHF0f_)3f?}F02FLw*Uag~yj-tDg-bKz*g>`dbys|u9(k%)NG6q5Q3vWG z_w77<`20c=Hji!bKG@EJ+CxQ%FO^UuJM-k%(Z3cp24D4iI~4&VOM2}G$kFUW zv<6Hxang4mv*INm!B|OmW@3 zEl~~EkPsjfe&;LOLJi|ar#ueiJTK}cpu5p42S{_^*ui9>SV@=9A!CJ?1|u*YI*8oh z%EeGdiN?`ZT7KjDe@6fbmu!Xy{uW#Rq<{t5So#Ejd1n1D^SqmICPW3d52CqBoqGdU zk+CAU0%P^@N0~V+u^TKusvHa&qM_6BZL?e4q(OMgh&iq8mF^#N1`rq%t3_O!2 z>`D{WTr-ETKUkXZ@c^5t3cQqEOHjk#xTXhQN=2@N2pQH;B%`v;*9S+4pr7wWE+agv zguXwataA5}O^OUsKE9kR;>Zn5agP`l#>bz`CxNY1`h*VLM;`G;$QgfT!4OtEFj)c# zTv3dk*!W1@2Ns){}ZR)2r}| zzh8vYBcXD@0IcVhmyBaD3vg9vzxuOI4?2X<|L3Za$W=LC(Gek6{qg{+RQrD)_ms87 zbJMyZ!VSWSFW}5foslB~e(1>GBn-0oceAv6!DKG0jzh8lCbQD6e_8zUP9`2ml42J_ zZ#;=X29rq&J;Mtx#}wd=xcaxdrSDYJU(3S}?ZGM71~Qc*hQ?-k9>$#JvObVuo{bPD z@S7N~SzOmGN*f9Jd=mf60&Yi7VN3dp2=Aqe=NF<8Q${*PCQa$sx0xcYe_#c{vtPjz zM=?V}kL7v?d595#rM)R$jbb$2ju>IYMXD_1OuZf;G(y|6469haCYb z9U{9^GF)0&v1ub#*(iN`q>z%Tey;v)Lf$7ZsBw)WMki%MXBHV ze%k`6UMW2@+X84)~BxdW(# zz}BMQ$PVFm^Fl6zCaRh1HQfccjWXWeG2gve{2ZZslaE?B|4nU-Lm9Fl`JX*KNrFGg zHP1|~a=)&&EWF|d=BEes*|O3jY?-~n%?S$ zUwImehS+clq2F5!%onbaCcOz7C1{R)A`cI*3M=T)ctOmk%pP^C9W70^v<_o^r(T@o zC3ca5Ph1(d6jD`D^b(CLcLQ=Ynz+78Q0p>8Y`!u)+NV}X+B>&@xN5Y2JbQhAbJ=~2 zXZr7n(ex&6Kk%jUHj?7(T5szY+AOQu!|PZm-S8RBtq4}*6}|LtD#EPqEAy1K{C6^r zwvoEiwkOV+iC2RQTW;c2nS@-!R%$ogxOyF_aE(8}MH}<8UX{q?%+su&|1uG>4b?U? zdgILVa_DVBYDPGD|LZ|^qCfVaH`>!BPQ!inAq^UH*QRP;muBp*E4*-#)1)cT(VtVd zz9h)&YtbtiFjFdp!+vRWr9ZJK?!PB?vtPZ;Q#SI&W)yDvf2N!#<(porWGi*nn%$BD zKP>+-4DaCOE8bJuvuiz#u@oU|p-_&bzi3^2UID#LM8U{{^8|4{6KyCTj`5BvfzHLd zcN@WBnTp|d_BkdkY$kVN7g!7XMR(e6!G!gw4>htNCCfIPfiN&d)t^MgjV!;1p3LJd zjOB5a7fw*3UIydQ$&C+3Q|G7$$-21r6SYQ-pwGUmxO06nqg4q#WB6at&F$3M=#S9! zH}U++VJAnkH*#81z4O<58Q`u9%Cx1>ce0OeejjuvUat91!PDilCKOd4q*zoCgLzv? zWzl~klTP|`Ev9rj;|~LqoE$DGEBV8`(k;h+d7`<_nUdJMPihi$Y+A%>A3_GsM?iR*w)ZM_mrGx7JUKl#~iR@{whI-pK1N# zr5h3O54>@+zoheShP>6bGxM={NlaRn(lCEv-+n{g^I+y^qce+tX3!>MXHTPj2xHY- zcvC#+@L)}t-J#LNM)dxFU_?D78TcI13r~rgb1Gc;{tE~JNL1Q2A!8=O0=dszjSCye z$=o?(iS6aoom0`Rv9oSGO1piG1b!(_J>D;-zsIZe*$?x1_jSc}|K3$xGG?8gYSV(7F?^SmFIO^;N55z@}o7G;Ueff;;f2i=sSfl3njd;W) z(l~`3N0VH>UTB|hT**<4FDf%1>5pTd;wkg!w#U?&wFZ>!etg)zW9vmTT=`+T{UrZe z=FiNo#EbE>@^sSAQtM3~2CKdLs%)os+(~p|`tP1|*b}HGzI9y#CCzxYU<-yWQND!n zB@))@ylPgkEtKh_Z6M6ulS0K`Q{?2aei>&{Y?NHhBb>N?_O3!Wb-esZ8RHfFo?E|s z{hG#ZBu&wNpZp;F#43?5u%&ZdbIx)L&ACNkni@fT2vY|56ZUa0e0On75b;|E4rZ)$;P{` zhWWxv*G#UbF>%FnB!-jRX|KkQ7S%v`{zw%a!{^?jpwZK1(^P!2cp(Ip=1O%oM~Ke4 zP}Bkr5zVocNH!D_j%Gwd(i3b#_XDlp2E!c2^pzgpl@F`lw8N97P3bx-rzUS8t;NbM z{PXxdw48shoLa3s%WuH3H0z)J{n&NAEW1lnv55XRCwaH?**h{BwRawOX${&~+QJdI zE&Rl9GbwU@L}yH5;~4$H{Wy49P~Za}TFiGxn< z&~=3OMs=*eo)DJO zRmE>lgRzT|B2N;i5O{(-(@E1+)x!y=>jwdADk5L0vEjt%*)4vanjE{^?Q%WT9gn3i z+ud4Y4iUcot7*;7QnU4{f95Uc&xdDe&@?5&FO=dfTU@!kth zY<1FSyA3~ksWChd68_v|es@`dn?BO2t`fDA5ZHn>ObE;5!_L&iODq@pH+t_DEg!6w zSSvDP-oO2ky1QDv{KKgt?Qnh47klUFuKp%fi=BKM^fw3z!wVM5+- z)y7k6XUpkE!6D_;YeBAz8>MWwcE0bNoGLFVS}fcsvr@e$CXzO5^6VtctB{H@a*IJ+ zKRK^_SnVTvGaTFDkHZI#{q5YPe^El-KNEWV_Phhw3K{;c6cgn*c^$tyihhDgOR`$F zyo@{c*Nd~g7R1qFPRk12Mw7nX9kX4pA09+!uo!r(C6i>T{Ni1nr623SqMM@{imS6F z_k-i^8oWEqraUxJ>PPdK@Si-GJmx6kTe)t+A~hsbl!x`&{6Zy0Ks4AxwVf{?NISE2 zhBI_?U#vjOr9gOzFI7A<+Z)9ryvlO7NsKL-tca}O!%Vm7XY`4upmBM|r?8&94ReLV zYvuXG1zu^livjc^MC{(LUrEU)W18P=Lp&s^e7%q3RokQeKFtU&9*P7AI{Vz{H3k6-)agv|6wzV7dA zYx&NhxOkcWe7XMbikS*e8FXd$FXE?iTo@d=uJiKa{;>G_UX%PEtEATYTMhnB?0J}r zKgp6J-4gyZdj|~SS3dd8gReb2$hXMw7^;6$amU(L!u6koz7UqlvuvhFSnSE${k|Or zsN2*!_YSy`1f9Fx_1?ZDx%m<*DKnF+ZhpVabW^L$e#JUP-2Vfw z(2jZe7v+D=@Z$f|49$F+xznny63=CNFgFwS1##bsE3eaey8FC2)8+5G_*}y#L*_7<^L8&c>eFdhF}0k{@)}0YMyrP@tH_r{RGHEmi^Y5@PEXg~p!V#2CH|^$+od(}C9JYNJ6sm)YC%ZT@vr2Mpe7NB> zRQGtjihg%4!%r0Yh0H5??xf^?&0HTZ`dYEy(N6jHmPS4Z-@v*l9BB7HHtr!J?^AZf|%Rt8Z~g{NS@omX{!vDNsiJeS>732dD& zYq5#1o_~E2Couytm_g{{iWM+ubTBANi@*>v z#w3JJ7EDS{TMr$bloI;>VYLZVg?dk=&X(`-#2ThTJY&Oc|1c!om~02C_i-ST^m%IY zVGc&Nd{w~P$DhWgtw8wFiksXp!*GpT^kpB`kbBm{l?zKx)uZYvR$I&Y2x9ip?b#|`Jb-e zE6f|m)8!a0=4q9M5-*Hm_txa5`^Sou-m0FpnyPAu`yATy%5P;R`mVoXdY`YWtgC~* zE^HYpUSyeUJ=DX$J@h);8cCP2%5(UWQ=^%DvN})K6U^eol+OF^V9FTheK9qgy$WCZ z*Z5V<1)oePS~RZ2(!5>kzDc!4DB@r|XoLc#D7C4%!}w8rkr!&>Cwo&HkOOKR2!xWO zA|M$oq2O_Ujq7io>YsS95)7rUiT_@m($e52^&83{0k_}n;cg~MNs+~EqeA6Yhofh) zwfzM9`yJZSxS%fZ$%yU#(cC#VBRJ`Zx*jEDtFFHxUK>QAw(0mfqdeBGNKB7%#q^UW z_-z1Xe1yY+Jd&aLT3f5E*y^#U>vHz3)q8p`ef&`Z^>K>7+s!O3$gXZnKhkJOP;MM9 zQ!-XfG(7Ky=DWE6WC{D-F;F6oriDL-pIwRfL%CJl1Hvh#Q&p9j53c8CuqTxoBjlF5 z>-SQ3u77x9o!UV}Bv2--=7aHPIBqwjxru%mt{K0})A5I}{8kUDR@DHlQTN6g#r$vt z)f*ohd`}E*N${IiKWSE$ippyF*FLwlZq!X{)l6Ay+NK}Y@~K1XSfblNbd#SXlQ05m zKMhd(uO)jq9CpCb=e>P~;EA}+&(|(qtpWMd z?HLvA?T^~t`|;>**2TF4m;GdubN$;#_5SJiwiCqsoCTNa5{k!6x@`Lwx0fZ38h)XB zB8*`}N# zAOi(+P+7bj1IU_o5PRmH`<|H$0#_X%sKgGvQ3QYs2&ju_BumO?T+a#RsyImbFC9Wb zDIRj!ZJ=Py2FRbj+XuUA{22!Uk=9^j?0|~c05noZCw~(!lw9IMLkU1G*k6k-cKrpp z1s&9?e%Vm*8}mXYvER8K^mnlzp4f1KF!{7$OC88$B?D$-VGJo7K$42Xsv{v5@G0_1 zy!x*95FW9BDAWbm6yhGpMqbIyaz#xR+uvG=TxZuqq7kq(uj9rARF27IyS zJ>`$2e7M%#Pvp_ZB{n?mTJOtTZFd@f73;{(@*1-?h^KLKRPn{Gsb$9VUeva#V7}Fs zL~=z;yU$^i*&s(7`(FpY^?`UbQ01B$IzjDBB~#P#{~};*(<8VMIzCpNb+H z4q8VbZY<#ZNLbPC7B+mz$*_~#|K_Z|;g@zjT%YkaLN5Ddis4GzcSKD}sNXv@Tr^P< z(rgb)Ox#|?mX4IV)ZW|dFsQwJB=P-gZ*jL#nQR9ozrD}18yp&_^-t2h0&9wx@U;cO z3(3XFd7DY?L!MJY;;YA>juvgYR-BSldTsv9ouk0VKgcYfS=`+|jF>r3{txMdxQp^9AhyIx(*bXojl@!iG$cjg1HMd}Q<eQH*@*Sl4@}Ip` zmA9yMU6=-2zAb*khuftQYTt9*O(wqEi3lCi${WhBapJySlpm`j!1zph(4rsR0uHY}K^dO@eYr}3+igMZhqwCem zAyE2sXFT|vY!xQz7e)gG2Z1+hSt^$1a8?^tw7bfh8rkeuVn}JVROGAiEhc3?+wGg& z+3ZJI!doA{=#M47O_hkBn-u5i_iM^hGL2PPwKy{Oo>_DfaQe)oSn!Dp2+JV?+I+_^ z1%?~eN-E~XyT;vA%4lg$oZQ!v9fEJvD%s`8rq6VdQ4@bVt(z0_#D2LWS4Ne0t%+gq z&&Q_QyJ8qm#d>~Luj-#`H@daD_11`kCWd{eA{l_b>I4!*%T z>9=oK^gTW?Z@5?@r^7a0;YrDfRsmF3$CWP>=YUqI4dFx)mPoYg)y=oz=D^3d9BcGF z7>4D+FB5+GXQUmV52pbI&=USWciS!oWqCO^{lx>8hzN~*Ev`I+>LRUtErruaUcRz7 zF@W@W4?SjyHh?034)?;RB2;BQ9YS)&v^}pCcn_M(KO$GbiwfC=^KyNm^)-QRYrZ{C zAw~bZHZ~{d9;?MN_bQes)*spm2t|Y!@_wNkv~bFywD?_W5n`Rf{Kfk6zghOD2%Cfb zJ(Jk%5}CgTI!fU(q%k8u?=4(#%ggrG7hYpE3f@0nV4Qw?KYd$CPj>MAl%!Eh%I$3> zT&|W0nN{q8_n;DqDd;h$7Q_4@t z4hMW}rHqkPv{-&|y}nN44N}}E$$hiIzmjh5BuMc+EFn5G=wMk{I(>Ms-n|i7dYS$l zjovRI#{Imx|6&2qeQ!VspqF^vHJz>gQLpanoK}2+7@ggde_j$Xy8iMlqCbdA+Vs|0 z2)U%n)9B~#Ow0JPUji5Q%o7l$%K!xh-L|%1DOsa)1y3ih7;rP?tYUSF3`HpwgNl2j36}`A5O_*>rOEPAWH6r7#&M>=D6Yp_cfd90@G981D}H}w9)Gb`EN#g=1-tvEA}Y4G!EGtY+PThk;mvU*HGhV6 zVd-Vj+d){`t6o0Ics`I|-@d5#jByv)-YQ^w<3p$KEwC7|+}&?py>p2-ad3OYtUZ|{ zm1wSAaL#J@qw0as@?9C4-%Wwm4<#P;uZ`l|bxhG&mie?$s3#CPnSl9 z6<6iaaQKqfhxV)qp!LKT$ocv zb$L5{iw{prKRbyth4I&;9#E;$vN$y*$}z=U;#{ ze}nF8ZzQ*v90#5(Mpc;8+}sI}QZ<@^Cf*1zDxaKXNz`#zjdbGfh( z#N@q*pG_9$1&W{E5_Kcll-^3Exs^}Xy&0vXh)Z`Vy{$@}wiRYB5g@{C>@)c@4t>#A$|{?VXk12YgB57Wy~+`tSSyx58ha zyhV7^3)ckRJj|)oXjbJW^}Tybr}WE-27zhkfi`P$E1yZ7e(`VT6*1Rsd~Z}KV?|x5 z$wl2}Qw0|rRy(3FG0&ZPIdu9x@6uKOuNgYL3g2|!HClVT?W{1r7qjppnCX6SY4#Nd z;=?`7#6G9UnU5kcVsr7XYIZX%My`YlGl5HtBB5M&&|euk*&!$QT6Ai9htb1zK_H2U zUqCg>?O@^4p75WnPoCMOty|^o(n1uA8G|pzS(Q%eOF1edooldb!j80zNp#6ZEhna+ z>GG%ezC|wD{W8bdDe`M{vq4KPd`2fSw6ywuAO-L4UB7;x2=8gU^Xtb^vm-gaQz&fJ zLy`qi`!`)j_Bki<4_e$~0G3jQ_tOgXa4terv8^Ju!BzE6>E6LHJ9*?Evr9UT<0XQ- zc7^vUXTERibVjZzl~em>J?UAV^pkM5vIrt5J1!y{H1DAzecy~Ptmk~`_w#tHW|J|> z?=Edxj~3=ev3X}#^6pu748SHS+WG*GHVtr%SbSdSHQC044ti=X!MRg<`_T_cfEvBk zS0Z3Yng!I;bbI{v50+)Ln8#`wpF)c_vJ*X>2%^5Hxb*(vTUUuC2#Q@L_ylU&@a8%a zm4ZG%c;aP>1ym5DWKPNOB{`W%HYF~{OjJNbcvcR44QOOXR~S^9bL*itW*~GIv^by@AFo1$wOAQ z$~q#$Wod^|)}6Z?0-DP99m<}ZO6S)+JJo)T8$2m?bf(kX5KrRVQ;8&|A@%yqpGO{N zDC@m9(*>~U6oX|Mqnb%2&+fL4oEw&c$DLd`5v9L=xq02#si=*{mm+?6ynwahvLB|Y z?Ho`8H<;+{a;QtwyQfP)H|ZXk*T+ zKvEGjYC*X~$Ham*)N`hL_(HEB9MS+bX=LR9=n!r@y|Re2(dzZ>6;c~9DqoBTpoNb- zpq;mWWPv=P!ReS0YTL#k^)^7?q>QksEHXqRb73>(aM0=r6thA(>s{J;^+w#D@RR>r zzst*|GCNl`!hI_qb^^UZtEjY^Qpq#+_-mu)@fob1Y6D;r619I3{*;}iF)aB-Ld z%QUQBU>y1|rWV=LTW+mf;PcI7PhYXgK(d8o{(X~utJdt*0ue9TT#!!k{4GLR7ZbVq z+gLUI@L~R~)%?Ty(vGuJR|Y<$6y1>Ye>}9KVKIZh(_byP_p3-tBUO?Y61UlN7pH}i z8vV`fk3V)$gDx8WB4yEl-XcxG1=JN~VW+XB-b5R{k#UE0D`QWC+{8?f4iX4M)G`z6 z7E7ICl-efo8GTHT3Xe6*Ux73W6I=O(op|#F{-i~g3$&^b2dMs>*5g$}vZv&tDN zyGqj-4x`>u!SrNr=NPW#Ns_LND&BVd{g#JK)zRS1uIQ3Gj>YNCBGJ$N_eMTkF#GJ= zSrg&e+&7|8=6-dAV~r1^*KybTr>*|}!lV85$?qH13kPNEn_;yA&8bVZkIRJ1ss?uc zELARPv_0C4O4DcH##BZoveg*WDF2ia7#J`iQ3!fAMa!?&?<>z{>m6(x^PRv=BguF*gnVE6V7nn#H(*ThB>~7!R>~hUP!|69QS;iqX1XozKDJ5wzAP={^8H_TX?Y%%V9a-FPPj42WQD>{ zt8eewB;7YkT{5#7HTDd)PpSTz>^b9*uH*L&aHU=NpF;|COALEe&-H)%=er1wo_`-L zjr?$dt^57+RGsOiu2+Y=n#mz2OUt{+R)mfyhxZb!>G$>Jrm^O^%>sENI7Ny@+HSRFd z@DnPk_BuFKdmg+s@y*Bs{nc;XJ<3)Q&Dt4S_DLlB0)9zD+G378I%=VoZ`qeo_@DUk zMytz)Q5E0YBwH$SDO37NHRn22r%p^SFMBE-p&stLuh@%y?Cx2e)0JT^|6TN*bI@*!=A7;m2LXXRnb8hALn|E(D?0B(wKctWO1FJKY;-02%QbfK zm@HN*g z+x;Z-ak?`0QPAD%!hm4pQl9!_s~9C?1L604xqS`HG)k-ML>IFuF9qcc7d=0o4ZGCR zQ;=aZ_a(#ylD?XUmEd|x0($#-nw(`qi^Ta&gkO|Ndn zHa-~|I6Zj5{Gc^bbHtlCm0a{Z?`gWDd719R_F`cBPLN}L@i1u(h&O$(sjEmA7%9_q}$+JCpP=QvqT z*#BLMAuKo?+l+*5=^Xm@%#Zdj`a_uRWl{=bd2N@LifCn(l!mxVlRrm#788 z*1ML;MLjx=#z$da1RJ9h1;q(Cop?X+>Fkk2V}}Zt$Q#`4o;$njpRW=_rMv3~Q8ij&=@(_X93 zZ55L*wUfpvKIVNCvi%hUt;NpQ1H$d&#d4fWdk7T1(pp;N^v{^>(dY2OkFryvTDu!1 zOO)oU7_x%913$^;_RgoW|Mrgg*dT^sq}A^`4Oh~+d)r%L<34O5(>FcBhBO)sZ;EwO z9r8*n52AM8PJTgopo6Y>);^)8`3?#8MTTzs;&=UrS(;b%wIJhJ7EFH_eY2<*<-b-- zES=!|ks@5U-|g6pz$jw1*)qs^fNE94(Xj40^Wd1c7Rtu=B&hU-;7fbq)w8Onirivi zSCkwrYFF(7n5&8o!`2(xk27>&I4wSpVsR|q(d6QfVXf*YLR@<(B1hKHZr>{=Gb|#V zR^HJMjj75i8+ArF@7~I$%4N4GUf+7yKtkiU+6Yvwrke-B-ev2b0)&wvTwt;2dqPa)oK#gtcvrmZ!B@(Aj%qcD)}n{wcgmy zrX%jX3dSZ^nFfKg9*yq8N#7=J#6EYLPEcsBZNJds`#K|Ah+J0~&+E3{^~L}O{v|TL zfTuJ>o#{Z%J!!#FUM*W(4C3D;!yd>kc)9p}&mSjck;hChT{G+tq{z`Xm;0FPNm-Xv zxL$R*@X2F!_-9XGAbSA>{J$E?BdGb4K$Kl6>=L`>YqM*+{AOj2dQ`g^DhK>z`zs-a*jyDd3;8; z3BNvsA>m&~J$xE|hm`&f8??|%f{t3=K2DJCz(Uva28l|9uev%c3XGU=;AVDPoSHA< zw}?F+mu#rT-z|kazwV&~uq{3rThN_c4(0I~ZUtqRmL`Rr&j>ZUkEkNiZ*OWba=u7Y z@8**PYgP3a=8u%5GLaPbt7&xwO-;y(9IiO-F_o^l<;3xwklwE>Jxn{?bblkU^po?(|P<$3dXlnbW>-Dz9dX z=Ekn3$1!~q6xItagY$}?pMLKYojQCu2q`;*O-E_5mVazg&&PUe21-cd_PpWWi2;AG4IRmWxV z7gw~#05pdb6TP|yBn2rz^7B9~r+*0z?R)Z9?m=k~$y{<7R25d(^{?WfSAJI)Lz~wJ zL{z+-HFdXZcbR&CN&R!lN&P!?kjMd&F?Zcfg@RnaU-{ZfOTYh>1r4X>=L1wdqGBcg0I)BxbtpBdaI2Xyr3*&){h!g_8<|@{)1l5#Lgv5r)6C2hMKsmqd__rh zitk%N3njIlYUmPO{_TZ){3_^?vCAVUtDJ_}xNG%54MRpVnBb|{&X`-hSb5pzyGg=B z-``<&YCNR2pH9l%S{j8fA3cwna~5l@*zer_){@g=EL1?r^R?gBejxKB#UHB^v-TCQ zsf-^2EghkcdG)Fy53R@ADEEAwCfdIHDmQi&JT{$Z%oQ4#3P@mPJ@8*iC#+Y|{{4}W zawd^nKu5ren@dqE_>B~c6or#h!fXOt`66Wst+;0RqJw1E=Ylvx4NlrN-j0L=;=Guw z>a}~F9y{KvD;Y~^Pj*Lb$WD(BM@)w6c2VCCBzRiK%KL^;D!-P1O?xBpA*Kx0LXVcA zwjq?O9P^`*7E1em^@>Z+aL!t;Pk!B4F8{6wH3!Uu>+-_G4H7;VP(oE z3)rsQ=z`^MKr4Tzmg%?}v1q0I;{s)ADv9o3)FQPHsol&0>z z?LP(cK^3)PJPTyFd>18AM!4yw!o~-G`e&XDHmI77ot%|LMceC}U#G-MD_1$5KCE@O zXLY!<8zaQ1Cd6udRdl4x!IEFw9ZP%IK|JzZ=LXA%V}_%(pSAm){mGcvoUmfq$u@3` zTC~5>iOu9?E>F}?WlM>t772sY)s9ovG?bp*f&F~^GyQh4dZ+Ai`BFkF9nH3G{{*1cQ@Wii#sMQX%?QA*M+)MM5)_RP^wD(tn%7K-#I zx;d*(E{SSLwC4BS^S_c;X1ROkoR~_5X-=nQ_w(v(<-8u^kKU{yFA}&Pb}j2V{GPd} zPrpA{nk}@$M2)FfT)Nsqwq%jfqbr;serl_U?(NsGOw*^Csic6Up<_K?`rrFT7FnqpUoR-v=HXEZ@pu?%%w{-yA z*KVwl5!M^1V-Xo?1>Rb2YiKBMP6K45I<^n?n^z4OFA(Kb+L(;sr$rl8+9sv}Up`L4 z->>{~4KO@`oK0J3?{S~uc1=1m;gW}a7H&}j4 zub|8;j~5v0!&)!2QBz;3i^V7(B|f1vq0emb?%>M3;wg)2-peJoAU%hZ-D`l}!G<(7 znlsn+bJZVW2^L)a{oAjK&YLM)sx3iN-uPgL)tJaU06q=G1Xj35YiUt;zuHUgk?WV^ ze(N>gIo$p>M>dhj#bnH^jK_tkXS!6)x-M5+t;cn;mqhjZd9bI2Qf%Wc!Hv`U6tj$xmav7;1v~VO4!hf0 zWmVQ^1CTQPIa9~o2+g)`CMp@}+yh!*ikNqmzE{EVe3oo2nb1xLH)hD+{X5#K#%F(H z15_4Xi*KLFna|LPVmJ0#AA;6cDX|uqqx`}pV*`w;9lLIhHc!hP)m(m3bC=gQR-~DN z&tMlZ3*(h!r_08<|8SgLB5X497Uw$PKIdy=d)ihun)>^eU@m@ZypEXj-sjnL77jw) zIKPoQHeOc2*7wJ@2R!EWo(Qb7Q)k$ylJOZ=$Pf1f+Q=F^ne7%_P0JuL?kw(^%JFr& zXnNww!+1aJn*ApC2sR(?gKpwZ@giCS;;&PFq!+44hj)9Njj|T_o?n+ed%YPwr!SWC zy8+AeO~{qJSNRo-_vO8w#S=NivdAdsR6**;+}MILwhwBg?a&mgYeJ#c|JG8SO}V0- z>i4`eb~0K#1*o6BGo8@d7@-k=jpEPUacx?G3P-mhPvR(|;Q=rND`;?xZ26v`m@yNl z#OJ)_FPPo?u>KZlKo?$&j#GDNiSM2_P;!69n9R}iNc(`BunwWX1GC3o$G~Qj2p#jf z!?bFL-mP}`cG&f?w-%j`?fe$gXNWuPOu#062CWjDw7DC+(mdw*W^^&&$cu;3EFyTH zx+=0xa1@+Pc5epwm;5!IUfrF&Z7J08YthLa_P>UD;de+=_558%l6|*~n_}u{E7BSz zCVT&)98J{r3sbRBh24;YV{A{@wo!p!V~mc3P4s}T+!^$?PRHk+iM8to-{hqI(ruJM zbKuEGBV4n?C)d-li;mU{D|&b_eHIrd=g3Q9&tPp?m9M@gf?JWa`SjxLZ)?}$QuA)| zKjzy0@(f-R(zeQX)$jr;yjV!^E>hvj7gP&4^oN{o?Fv|8?=s5whR_W^#W25Y zDLM+i!Yuv$9L}r>-}w3?LtL{|U3s2U!3Ts)n67gKHS%ZBZWrBgjb>(>Psg4}goQ31 z3W#njzOs&5QVOw4RXl8?PWB*}zScBKb1rr-@yzD;;*?GIVdN?W)pKuCe==hnvG0n1 zP=MzXIv02RC$ro%O!=jjmbMmufx*)I_f__^p2;X_C_LM@;ZPY0 zzeMo6UJ8|RsD(;7OD=yFCtHZ*$CG)1ShCaoh$I??-r@`2^33z|Rt#>(#$PzwziKRrM;-DYb~SU}%ky~^0@O_7EHFp!Q9yu4(3a+4yBiM+})2o&uijs~w`7r)nEtW<;1 z8C5h~4mkyh{^;<=W>orbn8;%n?BJWqO`Vf@Y#SM5neKjDiNeh=gC zas!hVCqdVzRwIS&H;K@0Hpcr|)Y-jc`YbIUVN2=B>=3%zSWc>WU~zhF!L`$9_C^lT zOvgJ35tfof$$=9W8=iZ#okB3hMHjJ>w~?g~qjMr0dPx$Ma)JCYSkxlR@~fiTeoei1 zlo!T7N%xs4diELdYrp1KQB&Vq6bUFfp84kdk88|jxQ0eejp+TqW@7i8f)%ft8N)0u zAiYHj1KVI|KXUTc?G<)4bN97v{;DQMzoCrs!GQSD$lJB3jJ9Io4e~zDSN>+*_2pBf z$mFJ{tNGc|qqjAyLVI^%JZv=gdcKV$y>}d~Y94xvs{XaPsB2eph$XmR3v|-CL`Y^uDh^bMsX? zvry}U>7}b}#l<~xf5@Mm!F$w+vg*A$U%Likq8REP5=c1-cuSeduL(9Cp z_f^qtwms+FG#xMJDbS?zGq5!^%2V8zI)h6dsss{MF0it22(kmtU|-+c;<~7y{V@He zO@P}gjfiW>*T&Rut1$^xnQqy$PWPARG2bANJcCOJx1{)dIXv}!MKDV@F_s6s!54v> z#|@Eh54BiLRw~}m?$;LSPkmufo#Z1`RKQJAb0X;pU>8@pXeJG}{Ri7YCvpuZd;3*r z4G!)OTy3g6ZF@q>Zr`cAWjlIRGs?$#P>xA$0jdn{qe^y+4^hw0;4z)S;UH8;yTuhX z4auU86Zzk4i^3Xwf;-2nXHNhA*ZAZn@>9C^2vIN7W+-)n6*Jm(;o*I_<1@(}m`!Wh zgfsus)MH`M>aS^Fd>yqlRkZcRK71TA?0O(lCxvQG@sK6KA&~r748Liz{&rFmt8G^_ zD*3vJ1N}R#Dpx`!Ev3u+iy>O)1Z1;?V$fFV$Q0tjG?3Wa#xq>OzP=^E@JwQf) zQx_vlsqLF-JE6I%8uIGDSOAC^mAT5ccDDrBHA7nBt6=mTq!px*>oASug3X<46JNkZ zZ!5eZ5WIr-oD*Yr+sB6fU4rl$A*>_Hjzkh!Ebc9$Y$3^ZOHSlhQj&1@`*9R>EZEmf zoQ{G2qq#2+hx&W_m$WBJK0?VZl#1+YvJ6=YGng!ut+7>>Y}r~!*~^+J#yTe3NOtXZ z84V>;S;{C>cHwuQQJ?2~{jTrz{QdaPHSe?C=RW6N&+87^!*WHC%v(ROu);}G1k4M8 zu|wGf3GJn8IouSq;QrmXzYQ@@NQe;7N4I#P^dym%L=ZYxzhknD;{HD;&b$l!h^aKm zMcF+MZt)UWt(&a`x^Od@9yv|#r$M(h28lMNRieV{MKHFr1TH9KhwFa{TY*E+_Dc&o!i zC}+){;MK$jW@xVeD8_tAZ!9yrC5l*3h|9NV$H2vm4wCvOeq0i4g4EhvWd>0iIf1g~ zbIj9+s62UI(z}W{8lxyur|K5+@^tE&6Q#}wV;#!1eq`G0mHXj$4Wve#7W;a}kix*G zh8s=yxohH&#W}7Y`SsXV9GztB<<35$AuTNB2 znkZ{|wvR2;iv5NdIzG;Oav+uO%gki-2KsT@`sHsr3D}_;3wT z<`Qa^_hJkj((2$3y!m-@vMs?KWZWia_6uln8*x0>8JA%AMt<_P(CIT}7!uoUoyKL6 z$)+1BrLMs~rr7=(xA1b^@=I;b%X@alEh5$YuG#i|i~+TvnZZ?H)`Uu?HQ}2eYfx9z zINh}lx4$L!Zm;*DMYAzl{^*5{BW(Z6G}**MA?wgGa{=;Qk3-D7&Af>tmq9-jaz5gVj;N){^3+8U9 z-X(2)J(7vaQ+iFt3T?caSO{f7B~&gfhSIz7cFt41(*RVaW9HPJM$jq;vyEEg_sjJR zKlxLdkPWvim7ZY0zJaksVNIC8H4}yLqxffWPC5jcP(XOIYzcl}?&73X4!ZWMZ|exE zqJr)hVu3RJR;lP(VOsMZnC_9#wA)n->Ab1#M|X~7S(ikS0=!<-xT&G&Hli#yzRyJ- z4v~8%W%Upl!L@XcI9Bv14i`Lp>zuXdA*c4;E*Z|x!dMTSYy$^~ejGnp{LG-hT_Cz# zD3P1N&lj%4sGA*ay{=Fe1Pxh6MMEKT@~&!uf1U{h%gKQSkg440y$j=l)FLTpSsC&w zYxQ1Ng3-N)(!aA4rd^#kwi+@+3TUZTIbi-c#uP}c955l?AOl4neI}L&!)o-H2QM*( z#7M#sBG0MP6d@hdi#_r#OQMvu^Y@dG16n9j0FW{l_HQN94pWY#F~NsxFn!F7OClZi z&R>+1z{Wx_4A{J}!H~Hn>0pHxmXz1|?2eK8l9%~Y@U}cL5W9$&ij^_c|95GQuVDt; zv1FSmb?u$ZO#B1Qbv;?f@YcFU;fLXJ$XWZ}4uLU8C`9(*XnuVSGDoDpD7AeH!B8x4 zK?iBv3;`zw_fSUZuxlZPRCbxrugwfVe&yRtd%4RI=(GSYH8vK{MU*m%&4E+3(kRi z%i+L^TW99=a%0&>_SkNIr=CT-WBA$(A;D|tRE4e}b)J=>mp;KV13X-b7MATKwZIvY z?1dq*>*33|hhXJ;#pT)}!N;~LXN-Y4)E!#hS;XO-=BfVI8UO+dWjE{KrkCg}p0i@+qmj^iq_=vJ@Uq=-* z8w?3cLuEz84pTe(52psDOVoWQW7B-k);*pFsuVDIc~#zL!h%OMA8y=lTi4f{45W+`2R; zBMt(Td-C;n54$mv7AfCnYL~E6;sKGpIzhPQR%z+HFZ~|ww3{ZmmSXdSFYc8y&FiUy z52Jr-tb8)5H!}1)|3T1l;Y0lpImI)3Hny`gpu?uJaC}~RJm1FT+ncBHc@(46<+`ao zO1PjUw|YudL8yxAq`>^lBH5#*c4_F1;Ps)F^WFLhH-`-hmjq|sK0G=$URi!m#eae< zx(@kI!(svqjTDxBM|(@6w(|ZuC_x@gOPfMkHa{<{v`f%|aKNVKF_KD5zl! z>Y|<-hBDnbL=9pQ!r9G#4$$>8M#t?T_bf4Qq$Q~X&MshedWDb7FUR3Ja`(MnGXl!p zwmpf>NmgP)>%DH1cxku8GkkG%0}EGMef@}iy>9oRXvbt$ssEB_WzCZH&nD%A%}``n zK$fRGDR*xGX1tnwRprOjt)EYRzxerMic9zrwLscxrT0B?<*M~isFXZUXkk}jR?W(S zr%4&!p|Yam#Jt-;ke}Ndj!k-`m#_2O%$t&xjEtf%Q{@$E!ftnMrOaik-aeOcdwQs{ z^r$t*m&EVt%{`trNp8x^=JZE%)!Vx|>a4`&`)J2UjJSoIw1_o^U-*8$xG9Tqx$mTY z^(qs~Z%|UE?Yh$<6i30aj4*KbK?NcMg-oa2iQUF(^I8BUD+Co%%B=Bhd@8#vn=hU1 zD}j3G7IN-=`|u71LrS=S2ufUZfLA13`Vh6Uk0=;7AJ$I;`$j=>oRXnoL{$pzl=i#O zfU%reCgs?XLmaj0e5}h}dgu?YFO{&ndFMc&SN@81T*26AoNa}xe@a^La9gR%u$3Yh zTlTc7C~DnUR_W)grr@}4ogSZrd)v>bj6WWhtq6~qvMx01j(C_d$`N* z@;>M;aPGul%}K@n`KK!ubbY`o7uUUoj-Ur9dTpF_8mg`&N)Y((JYMbbX#^zJA&6BZ z9=3al0q`{8bc&--Fq9c1LPexfmmVoafCAJW7|74eEz+L{oIPz)72ui1Lo)qct*?RL zX^qPWT4*vmA23Z90kM8N@xZBO-t#pA0MHj7y!KAh(=rve#I*A&P~Z4VZkgVL0)W`(e(0`GLz_k<;*MBN3oB0n*6Y z9?G2Q&@(Ro)Z=&$PZAHXsZ55vDYCWw5HO+IS)1|W^J74~9q#k&NPx_W)E|$WY0Z5B zm+d#*eUx>%hkQzm%f|OXBmld5agW11OTWuk$Y9?`jh}=k92MTl@h@K`@PJ;x5OASs z#Od-1rX#USB?zkkEE3I*4=(o zp&n1BeDL+zYstOcww5pGH@En%xW1scW|ygE+qybeYo+5hQO_?HUQMp@3f63o*nhN} z?*1*iCE@)gKP}ur-%}dV)lSs6{a!A-g-@ls&b~@d{Kwo%;vOJS4Gi$* zKkcshydM0Z1RY7VudYHNl-@AvEbBW_G5{aO!Iq*2f(=E7@cdR?eRw(6GkLA8;5 zstX5Ot^)8C-z)-!ecL$1X>KI|*nT;HLUGV>{zsI8`}XWPKHv|-=s(Tt1CuLMSLO9 zI})%^c1x-XgJP1(m%gzF1|EkPm_Y!W`tM?Rr~xZhq!C_tabVX$3Bk_fdt@NI6y-Q^VX)4F62K;XAEc8k#jJR<+#UwW+ zqyPP)xHvX5`CSo@WQ3yLSljH3tTZVJhwss1@%dd0kD0)1?lr?_+&wg(Ozjmb<(g@X zQ@6h`5zzLlC9x>c@NUI%n*Q*Pc-;5;bM^x+D=S5m_OxFmmdtIVPx3nsq$B0v?$KdXsgPdhPQNbG@XRI5Sg(MhRqT^ST~n>8NxqsFF=m(%UWB>R8W z(XZvjiI#5iDS!#dXwyw|SI;ZxBl;=Bc)6)_ULLd@f5TD$;k1`Z(?L z2?7~_gSYY=X{0>B+6@Cf-eI90{0rLgS4nYk0$=X=*z^Tqn>i|vWOUx717_cWlTp#~ zadMBv4uh-WY}(@%Ze~S2l2jJ0r>^EuVUbmk-kwp#ufeTPbSCPKXe#v&*#1nva3vBF z)U6l4`%sJ9_T-!;;0m9c$w3ri7NhOm85xlofVa!`4jj6lTh-Gi;VBlA$b*$=3=9l! zdf4azqB6taoH1!!y*9^G-g0urSA4hSj>6*sr)>K%4_z(f1qe&-Ei;Yj-5=t&E55Us zrPu`P8Uvi8{LikNitQn}*i|zDjM3B`k2KWo#8VtAa=jr9Tj#af!dD@v)5ikvfUa6H zV@D0dw3eB7P6Tm2L1oI|@u9H9$*OO-btjU#ojK*l1dJ@RvD(IL_EgE4)1sWZ*7(Bj zx^;-Y;Z&fftFyj={= z3?3rwYK$09I4VH4Y0^>{ROlXkBH>peK)Wzx0~syi(rD z7i^H|@5@P1R8>f)&?BC4$xZB3)O#F1HP)?onjRQ7t334`Mx9Gu?E7lVkF3Mi{)efsmlshM34S{=(c1>K;B~ogy7~6Ki4kMz#xM1lS9iCD=H34GOV1^;`V?TXTnDuN1RmXRqht8Gc)_z0}!t#Lvi zaF{)W)icF<9Ia*X;x zHW8|+r|ipf+1Mni^LvTv&f~^AA6k@TD&~Q*(a90zdGlB{_7!7d`kbXw=oG(lpI|i<0kY^s0903$hR?~xN5JmpfT5YZqxSuespZ0tWLn&rI z0m|Die*FeH0!@{Sk9xC|v7pauZM4`Z6tgR0kMbqgM*;Z3{-WGz=NHD>Co_6_B)-;G z^(jcWm57@WzJI8#ezzfYIzhgqJ_GVzEH5bo{=DKS@pfeKs0}2=@dh{~bXSj8wU`A= zpU#K9zPprNMR|Pm>iR?qJ^r-nWxd%FXKzbVg+zfw*+9^un<$}8JldC9Kr`d~2(s;{ z6uk2vg$#azdD5X*W9CV^W1Q9t%7E^O%uCbOVKp$@0!OCiY?*`NaQz*@K1Bi-O?zfT z+fAzMFPf|>{%gP;v0mkg-qsFSq?-52IY8-l0LiXXJjIl=Md~sC_~?@#<2{eOM#*(6 zW=X&3*)8r^Y41w6?&GuebV8N?xlX$n)x=R%GVI>*K-jK@0Kl#;YqhX@Q$R4H76dqB zb-ay{wbpjV6g1roKj9gE(LEL8KD3LYrFIFns^u2OOHpLX$WC1q0!`%E@x=YxInR;% z=*yN6h^G$vE;(QJzi;N1QBWt8Bv}il+SE_BfQg{E?&2>IKNxmm12V*91oZg{gmrsZ z_+!DLAnSceB_#kZs&Q^AGs7Hz46#4&nDU+gO?+*-z~?PH;iD6yQ~jr7aRb?rA&7;q zfqR>WKIO!t@~r5BT7Wn}-|2IDXa-PQGM75N_P8}s04=_xh*!|G6<=H2-dkL)W z?%XLG;72|K{TrI#ue;+Kwi%fPOt1_l;m#ZZVPW!uKUd(oCx-I< zc#OJOf*G37oE7Tp*cdBN>iNZpTgfXAo-Q`k=;#`sjIN!sKO6WWwvHpQWf3MZ@nv7* zc0uydb}2j1kNMeEVC|D0sq`BltpeL3>%x)PO<7R%oxJ~589Qh#i7SkS+7gX~+8 zmlym~02Ej3_qUSIdXEKS`sP6QYk{f`k;Q^!K0SS@XBv3VO|h>$89JpJ->PeA)t@qM*Uw0oVrnKlBtFrf0j2DOw+hlsIeeOl5S5J&}_=LqHW=~uL)<$(OkcEd{<5S$;;{fW~HK=Wv72G|HAYE*Cv19s0#LAOmaAq_7JO$}? z`lL%drwFv+Jc&ZQH}>%5rRknwu>BJObh!~k%X#ATr|`XUEm!Qa%b^;j&gN++i7Xf= zW_ugVy9tUaOCYRmaCJ)f*2ql2aUSRF18j+b8=3mwVYZ@0wKJ)4-H;{F z3~#w2Tj{~@+F5e%<;l=b-tCHCk_TK`OL{Dw?uh`5_3cycl`o!8KIbrQ1o-913)^^( zU`E>SNy6JC*`+{uPHj@@;V^bF0Vy^-Pr5~Fc#1ZE2B**vsP9p{q-aaYZk{sf*I%$B zgMsQKN}>d|S)~HZ4np7Kk~qsKNeQx&W5PGkhK_^CBtjxxSY@+fl02Nh4~@qxvNZ6| zQvF^sqE{Kf^1&l*5s=7+BR`_svyakWl>=lS#LmV?OwK~O?M5ws|Sb37)fzz1|XCDz|o}yoa8gSEcQVp7pZv~Z=rnCnKQWPnd?HzUxptE)0h50V|76U-#ZRpF*D z%vN}7k0e5>6?aLuLu-}2;*5zRHWb&1-iBLdk`9l}PWMQZRvBX#Yd(eX8Vs8xDd{3Q zCb4ntJJE2t8tCV!_)y=a#$tL=NsiOq`sw=utfXgA^TkU8J=r}a0eJ7olSWm~=V0HZ zk5jThaF!;NEoW)oxvf_8yRzme_%?L-p7N}t@^rV#xFf-{EDTWtne#?k_lhZWJ`+_H zy;;gjebV@_nK(7O2-1DZXR2XsklDnk)X!T$>wuKnyUiJo9$d*2ELgNGznLeU2g3%t zdKr^ussUy;AD+R#b1K*%Gy23>;vr-K$t@m(9KG>S+9ir}fuR4>O=9+Ea{~wmYo|;+ zH{O2@yS$urdQU)?IQ?>b_30&A|r_;D7S zPn&}fip$G~i(c5}gyWY6%tK=maQ(WZ06bzcT4$CnZ)LRp7$B}2Bv`P8D}iFvT6fm^ z^X^j*E}h-c>G&B$%0j+xE+O)vVvPhM{3>O`^V zjmmK7zsY>8A2i0`OcEI=qwlFK`|G$4le(mOrhu3NJKA4q(B`Bs;tCJIK&n-oOJRX*4k*rYFx{ibx`AZq}@vG!>K1<-M(L+Rou z^;Eylm=8R;ZLf{_p_I@79Thct$dwjl6=J)L}pqV-7qp;SJWD=0dz`OlIv z_&4e+g*-O$+B=h&m^N?M(>iJfY<~vWKJ4(=yBKbQgA|HcVo0}&N9hNlZ_(BG>wk$f zVk9}hwTwLax#t!4iN1EWd zhBho=U8T>j1gN7huCz28{HU6IhyD6BO4z=4TRG8On4sAjOs3&62mvtb$*^M}Ae*{T zWc=by;G3P6y{my)G5E^p#I)003`zZ$&KordYb+eOxMmZiK41j7QDiza7Up&={wqsZ z1Vxl7;uQaBE)Y^u@s!(}fqyxjRaS!;qd?J|TQ#!_+W6We4#CZb;Kzoh=_jvaaeIX$ z*jEV+-$-*aq6f?@R=@;R8}B|`1`0ZuFz6SO0ka8ePAn@CB)9*}_TFfZ8VFEP@CYJm z4D~eB9!S=-W6Ssw#^r)%OQQ;M8J{-M+_{6wlW8x*NKIISf?)g+a;y*p+a%nk!M`2I zC3xrYD{&yLsXc*<5$sKauau6`GTa2o%dNa87m=ac^?aSMD^Zp|!hRRp3{99Dt6nC@ zE>UVKldyYeh&Aw7HHK;=0qkIw_hT!=1R>r6%1ZtGFvdlI?5Z348sJAJv7Qabj^P7i zg*6$Wr8V$Pv;Dv6Vj=pHTr5=W`_5c(w81W#@)9FCg^49z>_qmjj3)Kx9HHP3TrZWq zbtBr#85RgI2n8=Ug{%Ks%a}Zpl#nHYGK$bf#zMY?tb>=z##IY9M1u#YM7U<$gI3Xp zD6Mz5juEW_J}rj1ICJUshN8oJ5lHfiL(7-JdvF!oqx7n?XgfA;fK9Ftdn3Jrnr z)+U|^L;KCohhp-7?CFq@y)ar(E6-u1>taw+7Rn=m-l0DY{_j8zN^S@f`tLEMhI)s~ z)H1dkJ~(|4c*!Q6vK~HcCcyKZBG7w=hwqu|tsnqU`BS4ho2OJ_F;V@cw16Ig~5;s&KmuYPJ(*DXuZNeYHO( z7OD{(b%@@h-b0YA@loj7@^_vyK#A|edM>CZ6%-tYw1TaF<{7#hdNN=`{RqGvA12(n zm1FfM_CrAVh8@ip5cQq|<4DW@Ww1#hpnHs^kRxg)4%eJ%$V;KW^CaE_KAzdh&LF!F z^gezehwJY=zX2tBTW)uwo_s+L(cisc-}QH%is&-uL;d`@Ke$C#f8sw4ln)WP^9fOJ zRr)gMh*&@u=)Sj1rxi6bftx9ZDN6V6JTXX9_%6=Rgoa-Y)EgZ9JI}X3iSi^dGGf(9 z@KMnIW;~(~JfJ6ORn*Tlxc+(KwPXxdm!Q!AD_V1W-!sJi>@jfA`rvcMfDJJ~cXh|8 zWOUesHBO@%M^OC;m1psyztPL2E}+tZ74H4^5aXtTamj)Qf%tbJv1&l>g|}U3d)K_$ zl;%a%Gs!z4pa{^1!w@Nog;yR;aYDNGkmHU@Q@vWuUHd<6PeEJvO+QsoTQ-heD!#>2 zV?D;(_NuuKVI5>4051)1%lxJ~byasOKKOe!nL@K*!j8xt6dJH xd-?Ys{ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using PointArray = std::vector; + +namespace bg = boost::geometry; + +struct CollisionCheckerParam +{ + double width_margin; + double z_axis_filtering_buffer; + bool enable_z_axis_obstacle_filtering; + double chattering_threshold; +}; + +struct PredictedObjectWithDetectionTime +{ + explicit PredictedObjectWithDetectionTime( + const rclcpp::Time & t, geometry_msgs::msg::Point & p, PredictedObject obj) + : detection_time(t), point(p), object(std::move(obj)) + { + } + + rclcpp::Time detection_time; + geometry_msgs::msg::Point point; + PredictedObject object; +}; + +class CollisionChecker +{ +public: + explicit CollisionChecker( + rclcpp::Node * node, std::shared_ptr debug_ptr); + + boost::optional> + checkTrajectoryForCollision( + TrajectoryPoints & predicted_trajectory_array, + PredictedObjects::ConstSharedPtr dynamic_objects); + + void setParam(const CollisionCheckerParam & param); + +private: + // Functions + + boost::optional> checkObstacleHistory( + const Pose & base_pose, const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, + const double z_max); + + boost::optional> checkDynamicObjects( + const Pose & base_pose, PredictedObjects::ConstSharedPtr dynamic_objects, + const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, const double z_max); + + void updatePredictedObjectHistory(const rclcpp::Time & now) + { + for (auto itr = predicted_object_history_.begin(); itr != predicted_object_history_.end();) { + const auto expired = (now - itr->detection_time).seconds() > param_.chattering_threshold; + + if (expired) { + itr = predicted_object_history_.erase(itr); + continue; + } + + itr++; + } + } + + // Parameter + CollisionCheckerParam param_; + + // Variables + std::shared_ptr debug_ptr_; + rclcpp::Node * node_; + vehicle_info_util::VehicleInfo vehicle_info_; + std::vector predicted_object_history_{}; +}; +} // namespace autoware::motion::control::predicted_path_checker + +#endif // PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp new file mode 100644 index 0000000000000..02ade624d630c --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp @@ -0,0 +1,92 @@ +// Copyright 2023 LeoDrive A.Ş. All rights reserved. +// +// 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 PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ +#define PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#define EIGEN_MPL2_ONLY + +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ + +enum class PolygonType : int8_t { Vehicle = 0, Collision }; + +enum class PointType : int8_t { Stop = 0 }; + +enum class PoseType : int8_t { Stop = 0, Collision }; + +class PredictedPathCheckerDebugNode +{ +public: + explicit PredictedPathCheckerDebugNode(rclcpp::Node * node, const double base_link2front); + + ~PredictedPathCheckerDebugNode() {} + + bool pushPolygon( + const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type); + + bool pushPolygon(const std::vector & polygon, const PolygonType & type); + + bool pushPolyhedron( + const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const PolygonType & type); + + bool pushPolyhedron(const std::vector & polyhedron, const PolygonType & type); + + bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type); + + bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type); + + visualization_msgs::msg::MarkerArray makeVirtualWallMarker(); + + visualization_msgs::msg::MarkerArray makeVisualizationMarker(); + + void publish(); + +private: + rclcpp::Publisher::SharedPtr virtual_wall_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Node * node_; + double base_link2front_; + + std::shared_ptr stop_pose_ptr_; + std::shared_ptr collision_pose_ptr_; + std::shared_ptr stop_obstacle_point_ptr_; + std::vector> vehicle_polygons_; + std::vector> collision_polygons_; + std::vector> vehicle_polyhedrons_; + std::vector> collision_polyhedrons_; +}; + +} // namespace autoware::motion::control::predicted_path_checker + +#endif // PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp new file mode 100644 index 0000000000000..e3c3b5cccfb8f --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -0,0 +1,178 @@ +// Copyright 2023 LeoDrive A.Ş. All rights reserved. +// +// 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 PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ +#define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +struct NodeParam +{ + double update_rate; + double delay_time; + double max_deceleration; + double resample_interval; + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + double stop_margin; + double min_trajectory_check_length; + double trajectory_check_time; + double distinct_point_distance_threshold; + double distinct_point_yaw_threshold; + double filtering_distance_threshold; + bool use_object_prediction; +}; + +enum class State { + DRIVE = 0, + EMERGENCY = 1, + STOP = 2, +}; + +class PredictedPathCheckerNode : public rclcpp::Node +{ +public: + explicit PredictedPathCheckerNode(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::CallbackGroup::SharedPtr group_cli_; + + // Subscriber + std::shared_ptr self_pose_listener_; + rclcpp::Subscription::SharedPtr sub_dynamic_objects_; + rclcpp::Subscription::SharedPtr + sub_reference_trajectory_; + rclcpp::Subscription::SharedPtr + sub_predicted_trajectory_; + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr sub_accel_; + component_interface_utils::Subscription::SharedPtr sub_stop_state_; + + // Client + component_interface_utils::Client::SharedPtr cli_set_stop_; + + // Data Buffer + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; + geometry_msgs::msg::Twist::ConstSharedPtr current_twist_; + geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_; + PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + control_interface::IsStopped::Message::ConstSharedPtr is_stopped_ptr_{nullptr}; + + // Core + std::unique_ptr collision_checker_; + std::shared_ptr debug_ptr_; + + // Variables + State current_state_{State::DRIVE}; + vehicle_info_util::VehicleInfo vehicle_info_; + bool is_calling_set_stop_{false}; + bool is_stopped_by_node_{false}; + + // Callback + void onDynamicObjects(PredictedObjects::ConstSharedPtr msg); + void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); + void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + void onIsStopped(const control_interface::IsStopped::Message::ConstSharedPtr msg); + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + void initTimer(double period_s); + + // Functions + bool isDataReady(); + + bool isDataTimeout(); + + bool isThereStopPointOnReferenceTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & reference_trajectory_array); + + void onTimer(); + + void checkVehicleState(diagnostic_updater::DiagnosticStatusWrapper & stat); + + TrajectoryPoints trimTrajectoryFromSelfPose( + const TrajectoryPoints & input, const Pose & self_pose) const; + + void sendRequest(bool make_stop_vehicle); + + bool isItDiscretePoint( + const TrajectoryPoints & reference_trajectory, const TrajectoryPoint & collision_point) const; + + static Trajectory cutTrajectory(const Trajectory & trajectory, const double length); + + size_t insertStopPoint( + TrajectoryPoints & trajectory, const geometry_msgs::msg::Point collision_point); + + void extendTrajectoryPointsArray(TrajectoryPoints & trajectory); + + static std::pair calculateProjectedVelAndAcc( + const PredictedObject & object, const TrajectoryPoint & trajectory_point); + + void filterObstacles( + const Pose & ego_pose, const TrajectoryPoints & traj, const double dist_threshold, + const bool use_prediction, PredictedObjects & filtered_objects); + + // Parameters + CollisionCheckerParam collision_checker_param_; + NodeParam node_param_; + + // Diagnostic Updater + diagnostic_updater::Updater updater_; +}; +} // namespace autoware::motion::control::predicted_path_checker + +#endif // PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp new file mode 100644 index 0000000000000..75e90e624a17e --- /dev/null +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -0,0 +1,104 @@ +// Copyright 2023 LeoDrive A.Ş. All rights reserved. +// +// 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 PREDICTED_PATH_CHECKER__UTILS_HPP_ +#define PREDICTED_PATH_CHECKER__UTILS_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace utils +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using std_msgs::msg::Header; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using vehicle_info_util::VehicleInfo; +using PointArray = std::vector; + +using TrajectoryPoints = std::vector; + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point); + +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width); + +TrajectoryPoint calcInterpolatedPoint( + const TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & target_point, + const size_t segment_idx, const bool use_zero_order_hold_for_twist); + +std::pair findStopPoint( + TrajectoryPoints & predicted_trajectory_array, const size_t collision_idx, + const double stop_margin, vehicle_info_util::VehicleInfo & vehicle_info); + +bool isInBrakeDistance( + const TrajectoryPoints & trajectory, const size_t stop_idx, const double relative_velocity, + const double relative_acceleration, const double max_deceleration, const double delay_time_sec); + +TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point); + +bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max); + +double getNearestPointAndDistanceForPredictedObject( + const PointArray & points, const Pose & base_pose, + geometry_msgs::msg::Point * nearest_collision_point); + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width); + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + +Polygon2d convertObjToPolygon(const PredictedObject & obj); + +double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape); + +void getCurrentObjectPose( + PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time); + +bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos); +} // namespace utils + +#endif // PREDICTED_PATH_CHECKER__UTILS_HPP_ diff --git a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml new file mode 100755 index 0000000000000..a35c11b80c1f7 --- /dev/null +++ b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml new file mode 100644 index 0000000000000..35f971907696a --- /dev/null +++ b/control/predicted_path_checker/package.xml @@ -0,0 +1,47 @@ + + + + predicted_path_checker + 1.0.0 + The predicted_path_checker package + + Berkay Karaman + Apache 2.0 + Berkay Karaman + + ament_cmake + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + boost + component_interface_specs + component_interface_utils + diagnostic_updater + eigen + geometry_msgs + motion_utils + nav_msgs + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_control_msgs + tier4_external_api_msgs + tier4_planning_msgs + vehicle_info_util + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp new file mode 100644 index 0000000000000..5e8d96805b832 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp @@ -0,0 +1,231 @@ +// Copyright 2022 LeoDrive A.Ş. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "predicted_path_checker/collision_checker.hpp" + +#include +#include + +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ +CollisionChecker::CollisionChecker( + rclcpp::Node * node, std::shared_ptr debug_ptr) +: debug_ptr_(std::move(debug_ptr)), + node_(node), + vehicle_info_(vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo()) +{ +} + +void CollisionChecker::setParam(const CollisionCheckerParam & param) +{ + param_ = param; +} + +boost::optional> +CollisionChecker::checkTrajectoryForCollision( + TrajectoryPoints & predicted_trajectory_array, PredictedObjects::ConstSharedPtr dynamic_objects) +{ + // It checks the collision, if there is a collision, it updates the predicted_trajectory_array and + // returns the index of the stop point. + // If there is no collision, it returns boost::none. + const auto now = node_->now(); + + updatePredictedObjectHistory(now); + if (dynamic_objects->objects.empty() && predicted_object_history_.empty()) { + return boost::none; + } + + for (size_t i = 0; i < predicted_trajectory_array.size() - 1; i++) { + // create one step circle center for vehicle + const auto & p_front = predicted_trajectory_array.at(i).pose; + const auto & p_back = predicted_trajectory_array.at(i + 1).pose; + const auto z_min = p_front.position.z; + const auto z_max = + p_front.position.z + vehicle_info_.max_height_offset_m + param_.z_axis_filtering_buffer; + + // create one-step polygon for vehicle + Polygon2d one_step_move_vehicle_polygon2d = + utils::createOneStepPolygon(p_front, p_back, vehicle_info_, param_.width_margin); + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_vehicle_polygon2d, z_min, z_max, PolygonType::Vehicle); + } else { + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon2d, p_front.position.z, PolygonType::Vehicle); + } + + // check obstacle history + auto found_collision_at_history = + checkObstacleHistory(p_front, one_step_move_vehicle_polygon2d, z_min, z_max); + + auto found_collision_at_dynamic_objects = + checkDynamicObjects(p_front, dynamic_objects, one_step_move_vehicle_polygon2d, z_min, z_max); + + if (found_collision_at_dynamic_objects || found_collision_at_history) { + double distance_to_current = std::numeric_limits::max(); + double distance_to_history = std::numeric_limits::max(); + if (found_collision_at_dynamic_objects) { + distance_to_current = tier4_autoware_utils::calcDistance2d( + p_front, found_collision_at_dynamic_objects.get().first); + } + if (found_collision_at_history) { + distance_to_history = + tier4_autoware_utils::calcDistance2d(p_front, found_collision_at_history.get().first); + } else { + predicted_object_history_.clear(); + } + + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron( + one_step_move_vehicle_polygon2d, z_min, z_max, PolygonType::Collision); + } else { + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon2d, p_front.position.z, PolygonType::Collision); + } + + if (distance_to_current > distance_to_history) { + debug_ptr_->pushObstaclePoint(found_collision_at_history->first, PointType::Stop); + return found_collision_at_history; + } + + predicted_object_history_.emplace_back( + now, found_collision_at_dynamic_objects.get().first, + found_collision_at_dynamic_objects.get().second); + debug_ptr_->pushObstaclePoint(found_collision_at_dynamic_objects->first, PointType::Stop); + return found_collision_at_dynamic_objects; + } + } + return boost::none; +} + +boost::optional> +CollisionChecker::checkObstacleHistory( + const Pose & base_pose, const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, + const double z_max) +{ + if (predicted_object_history_.empty()) { + return boost::none; + } + + std::vector> collision_points_in_history; + for (const auto & obj_history : predicted_object_history_) { + if (param_.enable_z_axis_obstacle_filtering) { + if (!utils::intersectsInZAxis(obj_history.object, z_min, z_max)) { + continue; + } + } + const auto & point = obj_history.point; + const Point2d point2d(point.x, point.y); + if (bg::within(point2d, one_step_move_vehicle_polygon2d)) { + collision_points_in_history.emplace_back(point, obj_history.object); + } + } + if (!collision_points_in_history.empty()) { + double min_norm = 0.0; + bool is_init = false; + std::pair nearest_collision_object_with_point; + for (const auto & p : collision_points_in_history) { + double norm = tier4_autoware_utils::calcDistance2d(p.first, base_pose); + if (norm < min_norm || !is_init) { + min_norm = norm; + nearest_collision_object_with_point = p; + is_init = true; + } + } + return boost::make_optional(nearest_collision_object_with_point); + } + return boost::none; +} + +boost::optional> +CollisionChecker::checkDynamicObjects( + const Pose & base_pose, PredictedObjects::ConstSharedPtr dynamic_objects, + const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, const double z_max) +{ + if (dynamic_objects->objects.empty()) { + return boost::none; + } + double min_norm_collision_norm = 0.0; + bool is_init = false; + size_t nearest_collision_object_index = 0; + geometry_msgs::msg::Point nearest_collision_point; + + for (size_t i = 0; i < dynamic_objects->objects.size(); ++i) { + const auto & obj = dynamic_objects->objects.at(i); + if (param_.enable_z_axis_obstacle_filtering) { + if (!utils::intersectsInZAxis(obj, z_min, z_max)) { + continue; + } + } + const auto object_polygon = utils::convertObjToPolygon(obj); + if (object_polygon.outer().empty()) { + // unsupported type + continue; + } + + const auto found_collision_points = + bg::intersects(one_step_move_vehicle_polygon2d, object_polygon); + + if (found_collision_points) { + std::vector collision_points; + PointArray collision_point_array; + bg::intersection(one_step_move_vehicle_polygon2d, object_polygon, collision_points); + for (const auto & point : collision_points) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + collision_point_array.push_back(p); + } + + // Also check the corner points + + for (const auto & point : object_polygon.outer()) { + if (bg::within(point, one_step_move_vehicle_polygon2d)) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + collision_point_array.push_back(p); + } + } + geometry_msgs::msg::Point nearest_collision_point_tmp; + + double norm = utils::getNearestPointAndDistanceForPredictedObject( + collision_point_array, base_pose, &nearest_collision_point_tmp); + if (norm < min_norm_collision_norm || !is_init) { + min_norm_collision_norm = norm; + nearest_collision_point = nearest_collision_point_tmp; + is_init = true; + nearest_collision_object_index = i; + } + } + } + if (is_init) { + const auto & obj = dynamic_objects->objects.at(nearest_collision_object_index); + const auto obstacle_polygon = utils::convertObjToPolygon(obj); + if (param_.enable_z_axis_obstacle_filtering) { + debug_ptr_->pushPolyhedron(obstacle_polygon, z_min, z_max, PolygonType::Collision); + } else { + debug_ptr_->pushPolygon( + obstacle_polygon, obj.kinematics.initial_pose_with_covariance.pose.position.z, + PolygonType::Collision); + } + return boost::make_optional(std::make_pair(nearest_collision_point, obj)); + } + return boost::none; +} +} // namespace autoware::motion::control::predicted_path_checker diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp new file mode 100644 index 0000000000000..3fb7b69d531c0 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp @@ -0,0 +1,329 @@ +// Copyright 2023 LeoDrive A.Ş. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "predicted_path_checker/debug_marker.hpp" + +#include +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +#include +#include + +using motion_utils::createDeletedStopVirtualWallMarker; +using motion_utils::createStopVirtualWallMarker; +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; + +namespace autoware::motion::control::predicted_path_checker +{ +PredictedPathCheckerDebugNode::PredictedPathCheckerDebugNode( + rclcpp::Node * node, const double base_link2front) +: node_(node), base_link2front_(base_link2front) +{ + virtual_wall_pub_ = + node->create_publisher("~/debug/virtual_wall", 1); + debug_viz_pub_ = + node_->create_publisher("~/debug/marker", 1); +} + +bool PredictedPathCheckerDebugNode::pushPolygon( + const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type) +{ + std::vector eigen_polygon; + for (const auto & point : polygon.outer()) { + Eigen::Vector3d eigen_point; + eigen_point << point.x(), point.y(), z; + eigen_polygon.push_back(eigen_point); + } + return pushPolygon(eigen_polygon, type); +} + +bool PredictedPathCheckerDebugNode::pushPolygon( + const std::vector & polygon, const PolygonType & type) +{ + switch (type) { + case PolygonType::Vehicle: + if (!polygon.empty()) { + vehicle_polygons_.push_back(polygon); + } + return true; + case PolygonType::Collision: + if (!polygon.empty()) { + collision_polygons_.push_back(polygon); + } + return true; + default: + return false; + } +} + +bool PredictedPathCheckerDebugNode::pushPolyhedron( + const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max, + const PolygonType & type) +{ + std::vector eigen_polyhedron; + for (const auto & point : polyhedron.outer()) { + eigen_polyhedron.emplace_back(point.x(), point.y(), z_min); + eigen_polyhedron.emplace_back(point.x(), point.y(), z_max); + } + + return pushPolyhedron(eigen_polyhedron, type); +} + +bool PredictedPathCheckerDebugNode::pushPolyhedron( + const std::vector & polyhedron, const PolygonType & type) +{ + switch (type) { + case PolygonType::Vehicle: + if (!polyhedron.empty()) { + vehicle_polyhedrons_.push_back(polyhedron); + } + return true; + case PolygonType::Collision: + if (!polyhedron.empty()) { + collision_polyhedrons_.push_back(polyhedron); + } + return true; + default: + return false; + } +} + +bool PredictedPathCheckerDebugNode::pushPose( + const geometry_msgs::msg::Pose & pose, const PoseType & type) +{ + switch (type) { + case PoseType::Stop: + stop_pose_ptr_ = std::make_shared(pose); + return true; + case PoseType::Collision: + collision_pose_ptr_ = std::make_shared(pose); + return true; + default: + return false; + } +} + +bool PredictedPathCheckerDebugNode::pushObstaclePoint( + const geometry_msgs::msg::Point & obstacle_point, const PointType & type) +{ + switch (type) { + case PointType::Stop: + stop_obstacle_point_ptr_ = std::make_shared(obstacle_point); + return true; + default: + return false; + } +} + +void PredictedPathCheckerDebugNode::publish() +{ + /* publish debug marker for rviz */ + const auto virtual_wall_msg = makeVirtualWallMarker(); + virtual_wall_pub_->publish(virtual_wall_msg); + + /* publish debug marker for rviz */ + const auto visualization_msg = makeVisualizationMarker(); + debug_viz_pub_->publish(visualization_msg); + + /* reset variables */ + vehicle_polygons_.clear(); + collision_polygons_.clear(); + vehicle_polyhedrons_.clear(); + collision_polyhedrons_.clear(); + collision_pose_ptr_ = nullptr; + stop_pose_ptr_ = nullptr; + stop_obstacle_point_ptr_ = nullptr; +} + +visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVirtualWallMarker() +{ + visualization_msgs::msg::MarkerArray msg; + rclcpp::Time current_time = node_->now(); + + if (stop_pose_ptr_ != nullptr) { + const auto p = calcOffsetPose(*stop_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto markers = + createStopVirtualWallMarker(p, "obstacle on predicted path", current_time, 0); + appendMarkerArray(markers, &msg); + } else { + const auto markers = createDeletedStopVirtualWallMarker(current_time, 0); + appendMarkerArray(markers, &msg); + } + + if (collision_pose_ptr_ != nullptr) { + const auto markers = + createStopVirtualWallMarker(*collision_pose_ptr_, "collision_point", current_time, 1); + appendMarkerArray(markers, &msg); + } else { + const auto markers = createDeletedStopVirtualWallMarker(current_time, 1); + appendMarkerArray(markers, &msg); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVisualizationMarker() +{ + visualization_msgs::msg::MarkerArray msg; + rclcpp::Time current_time = node_->now(); + + // cube + if (!vehicle_polyhedrons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "detection_cubes", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) { + for (size_t j = 0; j < vehicle_polyhedrons_.at(i).size(); ++j) { + const auto & p = vehicle_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + + for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) { + for (size_t j = 0; j + 2 < vehicle_polyhedrons_.at(i).size(); ++j) { + const auto & p = vehicle_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = vehicle_polyhedrons_.at(i).at(j + 2); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + } + const auto & p = vehicle_polyhedrons_.at(i).at(1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 1); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + const auto & p2 = vehicle_polyhedrons_.at(i).at(0); + marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + const auto & p3 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 2); + marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + } + + msg.markers.push_back(marker); + } + + if (!collision_polyhedrons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "collision_cubes", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + + for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) { + for (size_t j = 0; j < collision_polyhedrons_.at(i).size(); ++j) { + const auto & p = collision_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + + for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) { + for (size_t j = 0; j + 2 < collision_polyhedrons_.at(i).size(); ++j) { + const auto & p = collision_polyhedrons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = collision_polyhedrons_.at(i).at(j + 2); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + } + const auto & p = collision_polyhedrons_.at(i).at(1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + const auto & p1 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 1); + marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z())); + const auto & p2 = collision_polyhedrons_.at(i).at(0); + marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z())); + const auto & p3 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 2); + marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z())); + } + + msg.markers.push_back(marker); + } + + // polygon + if (!vehicle_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "detection_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (size_t i = 0; i < vehicle_polygons_.size(); ++i) { + for (size_t j = 0; j < vehicle_polygons_.at(i).size(); ++j) { + { + const auto & p = vehicle_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == vehicle_polygons_.at(i).size()) { + const auto & p = vehicle_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = vehicle_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (!collision_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "collision_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + + for (size_t i = 0; i < collision_polygons_.size(); ++i) { + for (size_t j = 0; j < collision_polygons_.at(i).size(); ++j) { + { + const auto & p = collision_polygons_.at(i).at(j); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + if (j + 1 == collision_polygons_.at(i).size()) { + const auto & p = collision_polygons_.at(i).at(0); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } else { + const auto & p = collision_polygons_.at(i).at(j + 1); + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + } + } + msg.markers.push_back(marker); + } + + if (stop_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "stop_obstacle_point", 0, visualization_msgs::msg::Marker::SPHERE, + createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + marker.pose.position = *stop_obstacle_point_ptr_; + msg.markers.push_back(marker); + } + + if (stop_obstacle_point_ptr_ != nullptr) { + auto marker = createDefaultMarker( + "map", current_time, "stop_obstacle_text", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), + createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.pose.position = *stop_obstacle_point_ptr_; + marker.pose.position.z += 2.0; + marker.text = "!"; + msg.markers.push_back(marker); + } + + return msg; +} + +} // namespace autoware::motion::control::predicted_path_checker diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp new file mode 100644 index 0000000000000..0cf2548d69746 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -0,0 +1,585 @@ +// Copyright 2023 LeoDrive A.Ş. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "predicted_path_checker/predicted_path_checker_node.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::motion::control::predicted_path_checker +{ + +PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & node_options) +: Node("predicted_path_checker_node", node_options), updater_(this) +{ + using std::placeholders::_1; + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + adaptor.init_cli(cli_set_stop_, group_cli_); + adaptor.init_sub(sub_stop_state_, this, &PredictedPathCheckerNode::onIsStopped); + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + + // Node Parameter + node_param_.update_rate = declare_parameter("update_rate", 10.0); + node_param_.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold", 3.0); + node_param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold", 1.046); + node_param_.max_deceleration = declare_parameter("max_deceleration", 1.5); + node_param_.delay_time = declare_parameter("delay_time", 0.17); + node_param_.stop_margin = declare_parameter("stop_margin", 0.5); + node_param_.min_trajectory_check_length = declare_parameter("min_trajectory_check_length", 1.5); + node_param_.trajectory_check_time = declare_parameter("trajectory_check_time", 3.0); + node_param_.resample_interval = declare_parameter("resample_interval", 0.5); + node_param_.distinct_point_distance_threshold = + declare_parameter("distinct_point_distance_threshold", 0.3); + node_param_.distinct_point_yaw_threshold = declare_parameter("distinct_point_yaw_threshold", 5.0); + node_param_.filtering_distance_threshold = declare_parameter("filtering_distance_threshold", 1.5); + node_param_.use_object_prediction = declare_parameter("use_object_prediction", true); + + // Collision Checker Parameter + collision_checker_param_.width_margin = + declare_parameter("collision_checker_params.width_margin", 0.2); + collision_checker_param_.enable_z_axis_obstacle_filtering = + declare_parameter("collision_checker_params.enable_z_axis_obstacle_filtering", false); + collision_checker_param_.z_axis_filtering_buffer = + declare_parameter("collision_checker_params.z_axis_filtering_buffer", 0.3); + collision_checker_param_.chattering_threshold = + declare_parameter("collision_checker_params.chattering_threshold", 0.2); + + // Subscriber + self_pose_listener_ = std::make_shared(this); + + sub_dynamic_objects_ = create_subscription( + "~/input/objects", rclcpp::SensorDataQoS(), + std::bind(&PredictedPathCheckerNode::onDynamicObjects, this, _1)); + sub_reference_trajectory_ = create_subscription( + "~/input/reference_trajectory", 1, + std::bind(&PredictedPathCheckerNode::onReferenceTrajectory, this, _1)); + sub_predicted_trajectory_ = create_subscription( + "~/input/predicted_trajectory", 1, + std::bind(&PredictedPathCheckerNode::onPredictedTrajectory, this, _1)); + sub_odom_ = create_subscription( + "~/input/odometry", 1, std::bind(&PredictedPathCheckerNode::onOdom, this, _1)); + sub_accel_ = create_subscription( + "~/input/current_accel", rclcpp::QoS{1}, + std::bind(&PredictedPathCheckerNode::onAccel, this, _1)); + + debug_ptr_ = + std::make_shared(this, vehicle_info_.max_longitudinal_offset_m); + + // Core + + collision_checker_ = std::make_unique(this, debug_ptr_); + collision_checker_->setParam(collision_checker_param_); + + // Diagnostic Updater + updater_.setHardwareID("predicted_path_checker"); + updater_.add("predicted_path_checker", this, &PredictedPathCheckerNode::checkVehicleState); + + // Wait for first self pose + self_pose_listener_->waitForFirstPose(); + + // Timer + initTimer(1.0 / node_param_.update_rate); +} + +void PredictedPathCheckerNode::onDynamicObjects(const PredictedObjects::ConstSharedPtr msg) +{ + object_ptr_ = msg; +} + +void PredictedPathCheckerNode::onReferenceTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + reference_trajectory_ = msg; +} + +void PredictedPathCheckerNode::onPredictedTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +{ + predicted_trajectory_ = msg; +} + +void PredictedPathCheckerNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + current_twist_ = std::make_shared(msg->twist.twist); +} + +void PredictedPathCheckerNode::onAccel( + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + current_accel_ = msg; +} + +void PredictedPathCheckerNode::onIsStopped( + const control_interface::IsStopped::Message::ConstSharedPtr msg) +{ + is_stopped_ptr_ = msg; + + is_stopped_by_node_ = + is_stopped_ptr_->data && + std::find( + is_stopped_ptr_->requested_sources.begin(), is_stopped_ptr_->requested_sources.end(), + "predicted_path_checker") != is_stopped_ptr_->requested_sources.end(); +} + +void PredictedPathCheckerNode::initTimer(double period_s) +{ + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&PredictedPathCheckerNode::onTimer, this)); +} + +bool PredictedPathCheckerNode::isDataReady() +{ + if (!current_pose_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_pose..."); + return false; + } + + if (!object_ptr_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic objects msg..."); + return false; + } + + if (!reference_trajectory_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "waiting for reference_trajectory msg..."); + return false; + } + + if (!predicted_trajectory_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "waiting for predicted_trajectory msg..."); + return false; + } + + if (!current_twist_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_twist msg..."); + return false; + } + + if (!current_accel_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current_accel msg..."); + return false; + } + + if (!is_stopped_ptr_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for is_stopped msg..."); + return false; + } + + if (!cli_set_stop_->service_is_ready()) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for stop service..."); + return false; + } + + return true; +} + +bool PredictedPathCheckerNode::isDataTimeout() +{ + const auto now = this->now(); + + constexpr double th_pose_timeout = 1.0; + const auto pose_time_diff = rclcpp::Time(current_pose_->header.stamp).seconds() - now.seconds(); + if (pose_time_diff > th_pose_timeout) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, "pose is timeout..."); + return true; + } + + return false; +} + +void PredictedPathCheckerNode::onTimer() +{ + current_pose_ = self_pose_listener_->getCurrentPose(); + + if (!isDataReady()) { + return; + } + + if (isDataTimeout()) { + return; + } + + // Cut trajectory + const auto cut_trajectory = cutTrajectory( + *predicted_trajectory_, std::max( + node_param_.min_trajectory_check_length, + current_twist_->linear.x * node_param_.trajectory_check_time)); + + // Convert to trajectory array + + TrajectoryPoints predicted_trajectory_array = motion_utils::convertToTrajectoryPointArray( + motion_utils::resampleTrajectory(cut_trajectory, node_param_.resample_interval)); + + // Filter the objects + + PredictedObjects filtered_objects; + filterObstacles( + current_pose_.get()->pose, predicted_trajectory_array, node_param_.filtering_distance_threshold, + node_param_.use_object_prediction, filtered_objects); + + PredictedObjects::ConstSharedPtr filtered_obj_ptr = + std::make_shared(filtered_objects); + + // Check collision + + const auto collision_checker_output = + collision_checker_->checkTrajectoryForCollision(predicted_trajectory_array, filtered_obj_ptr); + + if (!collision_checker_output) { + // There is no need to stop + if (is_stopped_by_node_) { + sendRequest(false); + } + current_state_ = State::DRIVE; + updater_.force_update(); + debug_ptr_->publish(); + + return; + } + + // Extend trajectory + + extendTrajectoryPointsArray(predicted_trajectory_array); + + // Insert collision and stop points + + const auto stop_idx = + insertStopPoint(predicted_trajectory_array, collision_checker_output.get().first); + + // Check ego vehicle is stopped or not + constexpr double th_stopped_velocity = 0.001; + const bool is_ego_vehicle_stopped = current_twist_->linear.x < th_stopped_velocity; + + // If ego vehicle is not stopped, check obstacle is in the brake distance + if (!is_ego_vehicle_stopped) { + // Calculate projected velocity and acceleration of the object on the trajectory point + + const auto projected_obj_vel_acc = calculateProjectedVelAndAcc( + collision_checker_output->second, predicted_trajectory_array.at(stop_idx)); + + // Calculate relative velocity and acceleration wrt the object + + const double relative_velocity = current_twist_->linear.x - projected_obj_vel_acc.first; + const double relative_acceleration = + current_accel_->accel.accel.linear.x - projected_obj_vel_acc.second; + + // Check if the vehicle is in the brake distance + + const bool is_in_brake_distance = utils::isInBrakeDistance( + predicted_trajectory_array, stop_idx, relative_velocity, relative_acceleration, + node_param_.max_deceleration, node_param_.delay_time); + + if (is_in_brake_distance) { + // Send emergency and stop request + current_state_ = State::EMERGENCY; + updater_.force_update(); + if (!is_stopped_by_node_) { + sendRequest(true); + } + debug_ptr_->publish(); + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "There is an obstacle in the brake distance. Sending emergency and stop request..."); + return; + } + } + + // If it is not in the brake distance, check if the collision point is discrete from the reference + // trajectory or not + + const auto reference_trajectory_array = + motion_utils::convertToTrajectoryPointArray(*reference_trajectory_); + + const auto is_discrete_point = + isItDiscretePoint(reference_trajectory_array, predicted_trajectory_array.at(stop_idx)); + + if (is_discrete_point) { + // Check reference trajectory has stop point or not + + const auto is_there_stop_in_ref_trajectory = isThereStopPointOnReferenceTrajectory( + predicted_trajectory_array.at(stop_idx).pose, reference_trajectory_array); + + if (!is_there_stop_in_ref_trajectory) { + // Send stop + if (!is_stopped_by_node_) { + sendRequest(true); + } + current_state_ = State::STOP; + updater_.force_update(); + + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000 /* ms */, + "There is an obstacle on predicted path. Sending stop request..."); + + debug_ptr_->publish(); + return; + } + } + + // If it is not discrete point, planning should handle it. Send drive. + current_state_ = State::DRIVE; + updater_.force_update(); + + if (is_stopped_by_node_) { + sendRequest(false); + } + + debug_ptr_->publish(); +} + +TrajectoryPoints PredictedPathCheckerNode::trimTrajectoryFromSelfPose( + const TrajectoryPoints & input, const Pose & self_pose) const +{ + TrajectoryPoints output{}; + + const size_t min_distance_index = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, self_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold) + + 1; + + for (size_t i = min_distance_index; i < input.size(); ++i) { + output.push_back(input.at(i)); + } + + return output; +} + +bool PredictedPathCheckerNode::isThereStopPointOnReferenceTrajectory( + const geometry_msgs::msg::Pose & pose, const TrajectoryPoints & reference_trajectory_array) +{ + const auto trimmed_reference_trajectory_array = + trimTrajectoryFromSelfPose(reference_trajectory_array, current_pose_.get()->pose); + + const auto nearest_stop_point_on_ref_trajectory = + motion_utils::findNearestIndex(trimmed_reference_trajectory_array, pose); + + const auto stop_point_on_trajectory = motion_utils::searchZeroVelocityIndex( + trimmed_reference_trajectory_array, 0, *nearest_stop_point_on_ref_trajectory); + + return !!stop_point_on_trajectory; +} + +void PredictedPathCheckerNode::checkVehicleState(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (current_state_ == State::EMERGENCY) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "vehicle will collide with obstacles"; + } + if (current_state_ == State::STOP) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "vehicle will stop due to obstacle"; + } + + stat.summary(level, msg); +} + +void PredictedPathCheckerNode::sendRequest(bool make_stop_vehicle) +{ + if (!is_calling_set_stop_ && cli_set_stop_->service_is_ready()) { + const auto req = std::make_shared(); + req->stop = make_stop_vehicle; + req->request_source = "predicted_path_checker"; + is_calling_set_stop_ = true; + cli_set_stop_->async_send_request(req, [this](auto) { is_calling_set_stop_ = false; }); + } +} + +bool PredictedPathCheckerNode::isItDiscretePoint( + const TrajectoryPoints & reference_trajectory, const TrajectoryPoint & collision_point) const +{ + const auto nearest_segment = + motion_utils::findNearestSegmentIndex(reference_trajectory, collision_point.pose); + + const auto nearest_point = utils::calcInterpolatedPoint( + reference_trajectory, collision_point.pose.position, *nearest_segment, false); + + const auto distance = tier4_autoware_utils::calcDistance2d( + nearest_point.pose.position, collision_point.pose.position); + + const auto yaw_diff = + std::abs(tier4_autoware_utils::calcYawDeviation(nearest_point.pose, collision_point.pose)); + return distance >= node_param_.distinct_point_distance_threshold || + yaw_diff >= tier4_autoware_utils::deg2rad(node_param_.distinct_point_yaw_threshold); +} + +Trajectory PredictedPathCheckerNode::cutTrajectory( + const Trajectory & trajectory, const double length) +{ + Trajectory cut; + cut.header = trajectory.header; + if (trajectory.points.empty()) { + return cut; + } + double total_length = 0.0; + cut.points.push_back(trajectory.points.front()); + for (size_t i = 1; i < trajectory.points.size(); ++i) { + const auto & point = trajectory.points.at(i); + + const auto p1 = tier4_autoware_utils::fromMsg(cut.points.back().pose.position); + const auto p2 = tier4_autoware_utils::fromMsg(point.pose.position); + const auto points_distance = boost::geometry::distance(p1.to_2d(), p2.to_2d()); + + const auto remain_distance = length - total_length; + + // Over length + if (remain_distance <= 0.001) { + break; + } + + // Require interpolation + if (remain_distance <= points_distance) { + const Eigen::Vector3d p_interpolated = p1 + remain_distance * (p2 - p1).normalized(); + + TrajectoryPoint p; + p.pose.position.x = p_interpolated.x(); + p.pose.position.y = p_interpolated.y(); + p.pose.position.z = p_interpolated.z(); + p.pose.orientation = point.pose.orientation; + + cut.points.push_back(p); + break; + } + + cut.points.push_back(point); + total_length += points_distance; + } + motion_utils::removeOverlapPoints(cut.points); + + return cut; +} + +void PredictedPathCheckerNode::extendTrajectoryPointsArray(TrajectoryPoints & trajectory) +{ + // It extends the trajectory to the end of the footprint of the vehicle to get better distance to + // collision_point. + const double extend_distance = vehicle_info_.max_longitudinal_offset_m + node_param_.stop_margin; + const auto & goal_point = trajectory.back(); + const auto trajectory_point_extend = utils::getExtendTrajectoryPoint(extend_distance, goal_point); + trajectory.push_back(trajectory_point_extend); +} + +size_t PredictedPathCheckerNode::insertStopPoint( + TrajectoryPoints & trajectory, const geometry_msgs::msg::Point collision_point) +{ + const auto nearest_collision_segment = + motion_utils::findNearestSegmentIndex(trajectory, collision_point); + + const auto nearest_collision_point = + utils::calcInterpolatedPoint(trajectory, collision_point, nearest_collision_segment, true); + + const size_t collision_idx = nearest_collision_segment + 1; + + trajectory.insert(trajectory.begin() + static_cast(collision_idx), nearest_collision_point); + + const auto stop_point = + utils::findStopPoint(trajectory, collision_idx, node_param_.stop_margin, vehicle_info_); + + const size_t stop_idx = stop_point.first + 1; + trajectory.insert(trajectory.begin() + static_cast(stop_idx), stop_point.second); + + debug_ptr_->pushPose(stop_point.second.pose, PoseType::Stop); + return stop_idx; +} + +std::pair PredictedPathCheckerNode::calculateProjectedVelAndAcc( + const PredictedObject & object, const TrajectoryPoint & trajectory_point) +{ + const auto & orientation_obj = object.kinematics.initial_pose_with_covariance.pose.orientation; + const auto & orientation_stop_point = trajectory_point.pose.orientation; + const auto velocity_obj = object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto acceleration_obj = + object.kinematics.initial_acceleration_with_covariance.accel.linear.x; + const auto k = std::cos(tier4_autoware_utils::normalizeRadian( + tf2::getYaw(orientation_obj) - tf2::getYaw(orientation_stop_point))); + const auto projected_velocity = velocity_obj * k; + const auto projected_acceleration = acceleration_obj * k; + return std::make_pair(projected_velocity, projected_acceleration); +} + +void PredictedPathCheckerNode::filterObstacles( + const Pose & ego_pose, const TrajectoryPoints & traj, const double dist_threshold, + const bool use_prediction, PredictedObjects & filtered_objects) +{ + filtered_objects.header.frame_id = object_ptr_.get()->header.frame_id; + filtered_objects.header.stamp = this->now(); + + for (auto & object : object_ptr_.get()->objects) { + // Check is it in front of ego vehicle + if (!utils::isFrontObstacle( + ego_pose, object.kinematics.initial_pose_with_covariance.pose.position)) { + continue; + } + + // Check is it near to trajectory + const double max_length = utils::calcObstacleMaxLength(object.shape); + const size_t seg_idx = motion_utils::findNearestSegmentIndex( + traj, object.kinematics.initial_pose_with_covariance.pose.position); + const auto p_front = tier4_autoware_utils::getPoint(traj.at(seg_idx)); + const auto p_back = tier4_autoware_utils::getPoint(traj.at(seg_idx + 1)); + const auto & p_target = object.kinematics.initial_pose_with_covariance.pose.position; + const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; + const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; + + if (seg_idx == traj.size() - 2) { + // Calculate longitudinal offset + const auto longitudinal_dist = std::abs(segment_vec.dot(target_vec) / segment_vec.norm()); + if ( + longitudinal_dist - max_length - vehicle_info_.max_longitudinal_offset_m - dist_threshold > + 0.0) { + continue; + } + } + const auto lateral_dist = std::abs(segment_vec.cross(target_vec)(2) / segment_vec.norm()); + if (lateral_dist - max_length - vehicle_info_.max_lateral_offset_m - dist_threshold > 0.0) { + continue; + } + PredictedObject filtered_object = object; + if (use_prediction) { + utils::getCurrentObjectPose(filtered_object, object_ptr_.get()->header.stamp, this->now()); + } + filtered_objects.objects.push_back(filtered_object); + } +} + +} // namespace autoware::motion::control::predicted_path_checker + +#include +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode) diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp new file mode 100644 index 0000000000000..9cb095e908d41 --- /dev/null +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -0,0 +1,429 @@ +// Copyright 2023 LeoDrive A.Ş. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "predicted_path_checker/utils.hpp" + +#include +#include +#include + +namespace utils +{ + +using motion_utils::findFirstNearestIndexWithSoftConstraints; +using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::getRPY; + +// Utils Functions +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) +{ + Polygon2d polygon; + + const double longitudinal_offset = vehicle_info.max_longitudinal_offset_m; + const double width = vehicle_info.vehicle_width_m / 2.0 + expand_width; + const double rear_overhang = vehicle_info.rear_overhang_m; + + { // base step + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_step_pose, -rear_overhang, width, 0.0).position); + } + + { // next step + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, longitudinal_offset, -width, 0.0) + .position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_step_pose, -rear_overhang, width, 0.0).position); + } + + polygon = tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); + + Polygon2d hull_polygon; + boost::geometry::convex_hull(polygon, hull_polygon); + boost::geometry::correct(hull_polygon); + + return hull_polygon; +} + +TrajectoryPoint calcInterpolatedPoint( + const TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & target_point, + const size_t segment_idx, const bool use_zero_order_hold_for_twist) +{ + if (trajectory.empty()) { + TrajectoryPoint interpolated_point{}; + interpolated_point.pose.position = target_point; + return interpolated_point; + } else if (trajectory.size() == 1) { + return trajectory.front(); + } + + // Calculate interpolation ratio + const auto & curr_pt = trajectory.at(segment_idx); + const auto & next_pt = trajectory.at(segment_idx + 1); + const auto v1 = tier4_autoware_utils::point2tfVector(curr_pt, next_pt); + const auto v2 = tier4_autoware_utils::point2tfVector(curr_pt, target_point); + if (v1.length2() < 1e-3) { + return curr_pt; + } + + const double ratio = v1.dot(v2) / v1.length2(); + const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + + // Interpolate + TrajectoryPoint interpolated_point{}; + + // pose interpolation + interpolated_point.pose = + tier4_autoware_utils::calcInterpolatedPose(curr_pt, next_pt, clamped_ratio); + + // twist interpolation + if (use_zero_order_hold_for_twist) { + interpolated_point.longitudinal_velocity_mps = curr_pt.longitudinal_velocity_mps; + interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; + interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; + } else { + interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); + interpolated_point.lateral_velocity_mps = interpolation::lerp( + curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); + interpolated_point.acceleration_mps2 = + interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + } + + // heading rate interpolation + interpolated_point.heading_rate_rps = + interpolation::lerp(curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + + // wheel interpolation + interpolated_point.front_wheel_angle_rad = interpolation::lerp( + curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); + interpolated_point.rear_wheel_angle_rad = + interpolation::lerp(curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + + // time interpolation + const double interpolated_time = interpolation::lerp( + rclcpp::Duration(curr_pt.time_from_start).seconds(), + rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); + interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); + + return interpolated_point; +} + +std::pair findStopPoint( + TrajectoryPoints & trajectory_array, const size_t collision_idx, const double stop_margin, + vehicle_info_util::VehicleInfo & vehicle_info) +{ + // It returns the stop point and segment of the point on trajectory. + + double desired_distance_base_link_to_collision = + vehicle_info.max_longitudinal_offset_m + stop_margin; + size_t stop_segment_idx = 0; + bool found_stop_point = false; + double distance_point_to_collision = 0.0; + + for (size_t i = collision_idx; i > 0; i--) { + distance_point_to_collision = + motion_utils::calcSignedArcLength(trajectory_array, i - 1, collision_idx); + if (distance_point_to_collision >= desired_distance_base_link_to_collision) { + stop_segment_idx = i - 1; + found_stop_point = true; + break; + } + } + if (found_stop_point) { + const auto & base_point = trajectory_array.at(stop_segment_idx); + const auto & next_point = trajectory_array.at(stop_segment_idx + 1); + + double ratio = (distance_point_to_collision - desired_distance_base_link_to_collision) / + (std::hypot( + base_point.pose.position.x - next_point.pose.position.x, + base_point.pose.position.y - next_point.pose.position.y)); + + geometry_msgs::msg::Pose interpolated_pose = + tier4_autoware_utils::calcInterpolatedPose(base_point.pose, next_point.pose, ratio, false); + TrajectoryPoint output; + output.set__pose(interpolated_pose); + return std::make_pair(stop_segment_idx, output); + } else { + // It means that there is no enough distance between vehicle and collision point. + // So, we insert a stop at the first point of the trajectory. + return std::make_pair(0, trajectory_array.front()); + } +} + +bool isInBrakeDistance( + const TrajectoryPoints & trajectory, const size_t stop_idx, const double relative_velocity, + const double relative_acceleration, const double max_deceleration, const double delay_time_sec) +{ + if (relative_velocity < 0.0) { + return false; + } + + const auto distance_to_obstacle = motion_utils::calcSignedArcLength( + trajectory, trajectory.front().pose.position, trajectory.at(stop_idx).pose.position); + + const double distance_in_delay = relative_velocity * delay_time_sec + + relative_acceleration * delay_time_sec * delay_time_sec * 0.5; + + const double velocity_after_delay = relative_velocity + relative_acceleration * delay_time_sec; + + const double time_to_stop = velocity_after_delay / std::abs(max_deceleration); + const double distance_after_delay = + velocity_after_delay * time_to_stop - 0.5 * abs(max_deceleration) * time_to_stop * time_to_stop; + const double brake_distance = distance_in_delay + distance_after_delay; + + return brake_distance > distance_to_obstacle; +} + +TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point) +{ + tf2::Transform map2goal; + tf2::fromMsg(goal_point.pose, map2goal); + tf2::Transform local_extend_point; + local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0)); + tf2::Quaternion q; + q.setRPY(0, 0, 0); + local_extend_point.setRotation(q); + const auto map2extend_point = map2goal * local_extend_point; + Pose extend_pose; + tf2::toMsg(map2extend_point, extend_pose); + TrajectoryPoint extend_trajectory_point; + extend_trajectory_point.pose = extend_pose; + extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; + extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; + extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; + return extend_trajectory_point; +} + +bool intersectsInZAxis(const PredictedObject & object, const double z_min, const double z_max) +{ + const auto & obj_pose = object.kinematics.initial_pose_with_covariance; + const auto & obj_height = object.shape.dimensions.z; + return obj_pose.pose.position.z - obj_height / 2.0 <= z_max && + obj_pose.pose.position.z + obj_height / 2.0 >= z_min; +} + +double getNearestPointAndDistanceForPredictedObject( + const PointArray & points, const Pose & base_pose, + geometry_msgs::msg::Point * nearest_collision_point) +{ + double min_norm = 0.0; + bool is_init = false; + + for (const auto & p : points) { + double norm = tier4_autoware_utils::calcDistance2d(p, base_pose); + if (norm < min_norm || !is_init) { + min_norm = norm; + *nearest_collision_point = p; + is_init = true; + } + } + return min_norm; +} + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width) +{ + const auto mapped_point = [](const double & length_scalar, const double & width_scalar) { + tf2::Vector3 map; + map.setX(length_scalar); + map.setY(width_scalar); + map.setZ(0.0); + map.setW(1.0); + return map; + }; + + // set vertices at map coordinate + const tf2::Vector3 p1_map = std::invoke(mapped_point, base_to_front, -base_to_width); + const tf2::Vector3 p2_map = std::invoke(mapped_point, base_to_front, base_to_width); + const tf2::Vector3 p3_map = std::invoke(mapped_point, -base_to_rear, base_to_width); + const tf2::Vector3 p4_map = std::invoke(mapped_point, -base_to_rear, -base_to_width); + + // transform vertices from map coordinate to object coordinate + tf2::Transform tf_map2obj; + tf2::fromMsg(current_pose, tf_map2obj); + const tf2::Vector3 p1_obj = tf_map2obj * p1_map; + const tf2::Vector3 p2_obj = tf_map2obj * p2_map; + const tf2::Vector3 p3_obj = tf_map2obj * p3_map; + const tf2::Vector3 p4_obj = tf_map2obj * p4_map; + + Polygon2d object_polygon; + object_polygon.outer().reserve(5); + object_polygon.outer().emplace_back(p1_obj.x(), p1_obj.y()); + object_polygon.outer().emplace_back(p2_obj.x(), p2_obj.y()); + object_polygon.outer().emplace_back(p3_obj.x(), p3_obj.y()); + object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + + const double obj_x = current_pose.position.x; + const double obj_y = current_pose.position.y; + + constexpr int N = 20; + const double r = obj_shape.dimensions.x / 2; + object_polygon.outer().reserve(N + 1); + for (int i = 0; i < N; ++i) { + object_polygon.outer().emplace_back( + obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i)); + } + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + tf2::Transform tf_map2obj; + fromMsg(current_pose, tf_map2obj); + const auto obj_points = obj_shape.footprint.points; + object_polygon.outer().reserve(obj_points.size() + 1); + for (const auto & obj_point : obj_points) { + tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); + tf2::Vector3 tf_obj = tf_map2obj * obj; + object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); + } + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + + boost::geometry::append(polygon.outer(), point); +} + +Polygon2d convertObjToPolygon(const PredictedObject & obj) +{ + Polygon2d object_polygon{}; + if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + object_polygon = utils::convertCylindricalObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + const double & length_m = obj.shape.dimensions.x / 2; + const double & width_m = obj.shape.dimensions.y / 2; + object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); + } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + object_polygon = utils::convertPolygonObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else { + throw std::runtime_error("Unsupported shape type"); + } + return object_polygon; +} + +bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos) +{ + const auto yaw = tier4_autoware_utils::getRPY(ego_pose).z; + const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw)); + const Eigen::Vector2d obstacle_vec( + obstacle_pos.x - ego_pose.position.x, obstacle_pos.y - ego_pose.position.y); + + return base_pose_vec.dot(obstacle_vec) >= 0; +} + +double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + return shape.dimensions.x / 2.0; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + double max_length_to_point = 0.0; + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (max_length_to_point < length_to_point) { + max_length_to_point = length_to_point; + } + } + return max_length_to_point; + } + + throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); +} + +void getCurrentObjectPose( + PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time) +{ + const double yaw = tier4_autoware_utils::getRPY( + predicted_object.kinematics.initial_pose_with_covariance.pose.orientation) + .z; + const double vx = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double ax = predicted_object.kinematics.initial_acceleration_with_covariance.accel.linear.x; + + const double dt = (current_time - obj_base_time).seconds(); + const double ds = vx * dt + 0.5 * ax * dt * dt; + const double delta_yaw = + predicted_object.kinematics.initial_twist_with_covariance.twist.angular.z * dt; + geometry_msgs::msg::Transform transform; + transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); + transform.rotation = tier4_autoware_utils::createQuaternionFromRPY(0.0, 0.0, yaw); + + tf2::Transform tf_pose; + tf2::Transform tf_offset; + tf2::fromMsg(transform, tf_offset); + tf2::fromMsg(predicted_object.kinematics.initial_pose_with_covariance.pose, tf_pose); + tf2::toMsg(tf_pose * tf_offset, predicted_object.kinematics.initial_pose_with_covariance.pose); + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x += ax * dt; + predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = + tier4_autoware_utils::createQuaternionFromRPY( + 0.0, 0.0, tier4_autoware_utils::normalizeRadian(yaw + delta_yaw)); +} + +} // namespace utils diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 375819f8e7579..af5b82349f180 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -63,6 +63,8 @@ def launch_setup(context, *args, **kwargs): obstacle_collision_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("aeb_param_path").perform(context), "r") as f: aeb_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("predicted_path_checker_param_path").perform(context), "r") as f: + predicted_path_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] controller_component = ComposableNode( package="trajectory_follower_node", @@ -176,6 +178,34 @@ def launch_setup(context, *args, **kwargs): target_container="/control/control_container", ) + # autonomous emergency braking + predicted_path_checker = ComposableNode( + package="predicted_path_checker", + plugin="autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode", + name="predicted_path_checker", + remappings=[ + ("~/input/objects", "/perception/object_recognition/objects"), + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ("~/input/current_accel", "/localization/acceleration"), + ("~/input/odometry", "/localization/kinematic_state"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ], + parameters=[ + vehicle_info_param, + predicted_path_checker_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + predicted_path_checker_loader = LoadComposableNodes( + condition=IfCondition(LaunchConfiguration("enable_predicted_path_checker")), + composable_node_descriptions=[predicted_path_checker], + target_container="/control/control_container", + ) + # vehicle cmd gate vehicle_cmd_gate_component = ComposableNode( package="vehicle_cmd_gate", @@ -338,6 +368,7 @@ def launch_setup(context, *args, **kwargs): external_cmd_converter_loader, obstacle_collision_checker_loader, autonomous_emergency_braking_loader, + predicted_path_checker_loader, ] ) @@ -372,6 +403,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("obstacle_collision_checker_param_path") add_launch_arg("external_cmd_selector_param_path") add_launch_arg("aeb_param_path") + add_launch_arg("predicted_path_checker_param_path") add_launch_arg("enable_autonomous_emergency_braking") add_launch_arg("check_external_emergency_heartbeat") From b32f8ef5f1791c268aab7265937e87f8eb23ad69 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Sun, 1 Oct 2023 17:06:10 +0300 Subject: [PATCH 44/97] feat(tier4_control_launch): add launch argument for predicted path checker (#5186) Signed-off-by: Berkay Karaman --- launch/tier4_control_launch/launch/control.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index af5b82349f180..06718a2cf0ffa 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -404,6 +404,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("external_cmd_selector_param_path") add_launch_arg("aeb_param_path") add_launch_arg("predicted_path_checker_param_path") + add_launch_arg("enable_predicted_path_checker") add_launch_arg("enable_autonomous_emergency_braking") add_launch_arg("check_external_emergency_heartbeat") From c99c6b2fc7062e6e21edb80a1b96797f59d04efb Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 2 Oct 2023 10:55:00 +0900 Subject: [PATCH 45/97] feat(lane_change): expand target lanes for object filtering (#5167) * feat(lane_change): expand target lanes for object filtering Signed-off-by: Muhammad Zulfaqar Azmi * use expanded target lanes in collision check Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/lane_change/lane_change.param.yaml | 5 +++++ .../lane_change/lane_change_module_data.hpp | 2 ++ .../utils/lane_change/utils.hpp | 19 +++++++++++++++++++ .../src/scene_module/lane_change/manager.cpp | 5 ++++- .../src/scene_module/lane_change/normal.cpp | 16 ++++++++++++---- .../src/utils/lane_change/utils.cpp | 9 +++++++++ 6 files changed, 51 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 583bdeaaf8dc5..fda7390abdb9e 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -38,6 +38,11 @@ longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + # lane expansion for object filtering + lane_expansion: + left_offset: 0.0 # [m] + right_offset: 0.0 # [m] + # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 27735777c27cc..0ba35635a9143 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -67,6 +67,8 @@ struct LaneChangeParameters bool check_objects_on_current_lanes{true}; bool check_objects_on_other_lanes{true}; bool use_all_predicted_path{false}; + double lane_expansion_left_offset{0.0}; + double lane_expansion_right_offset{0.0}; // regulatory elements bool regulate_on_crosswalk{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 8a78d2665fa8f..b2592bc1b899b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -186,5 +186,24 @@ ExtendedPredictedObject transform( bool isCollidedPolygonsInLanelet( const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); + +/** + * @brief Generates expanded lanelets based on the given direction and offsets. + * + * Expands the provided lanelets in either the left or right direction based on + * the specified direction. If the direction is 'LEFT', the lanelets are expanded + * using the left_offset; if 'RIGHT', they are expanded using the right_offset. + * Otherwise, no expansion occurs. + * + * @param lanes The lanelets to be expanded. + * @param direction The direction of expansion: either LEFT or RIGHT. + * @param left_offset The offset value for left expansion. + * @param right_offset The offset value for right expansion. + * @return lanelet::ConstLanelets A collection of expanded lanelets. + */ +lanelet::ConstLanelets generateExpandedLanelets( + const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, + const double right_offset); + } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index a502c9d8e43ec..9c45f79d00263 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -77,7 +77,10 @@ LaneChangeModuleManager::LaneChangeModuleManager( getOrDeclareParameter(*node, parameter("check_objects_on_other_lanes")); p.use_all_predicted_path = getOrDeclareParameter(*node, parameter("use_all_predicted_path")); - + p.lane_expansion_left_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.left_offset")); + p.lane_expansion_right_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.right_offset")); // lane change regulations p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); p.regulate_on_intersection = diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 1bcd3e6b0cdcd..d8fa9cb5630e9 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -674,8 +674,11 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto current_polygon = utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); - const auto target_polygon = - utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset, + lane_change_parameters_->lane_expansion_right_offset); + const auto target_polygon = utils::lane_change::createPolygon( + expanded_target_lanes, 0.0, std::numeric_limits::max()); const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); std::vector> target_backward_polygons; @@ -1449,6 +1452,11 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( target_objects.other_lane.end()); } + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + lane_change_path.info.target_lanes, direction_, + lane_change_parameters_->lane_expansion_left_offset, + lane_change_parameters_->lane_expansion_right_offset); + for (const auto & obj : collision_check_objects) { auto current_debug_data = marker_utils::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( @@ -1464,8 +1472,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet( collided_polygons, lane_change_path.info.current_lanes); - const auto collision_in_target_lanes = utils::lane_change::isCollidedPolygonsInLanelet( - collided_polygons, lane_change_path.info.target_lanes); + const auto collision_in_target_lanes = + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); if (!collision_in_current_lanes && !collision_in_target_lanes) { continue; diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 0ae599a74e439..df609dce48512 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -1104,4 +1104,13 @@ bool isCollidedPolygonsInLanelet( return std::any_of(collided_polygons.begin(), collided_polygons.end(), is_in_lanes); } + +lanelet::ConstLanelets generateExpandedLanelets( + const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, + const double right_offset) +{ + const auto left_extend_offset = (direction == Direction::LEFT) ? left_offset : 0.0; + const auto right_extend_offset = (direction == Direction::RIGHT) ? -right_offset : 0.0; + return lanelet::utils::getExpandedLanelets(lanes, left_extend_offset, right_extend_offset); +} } // namespace behavior_path_planner::utils::lane_change From de83f251d3e9bc9399530107df5ab4e938c3413f Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Mon, 2 Oct 2023 11:37:47 +0900 Subject: [PATCH 46/97] fix(crosswalk_traffic_light_estimator): change the shape of pedestrian light to CIRCLE (#5168) Signed-off-by: Tomohito Ando --- perception/crosswalk_traffic_light_estimator/src/node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index e45e3494dadce..870b8bc7c13f5 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -231,6 +231,7 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( 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(); From 3c5a0b84d195a60f045c5e829024d085c77735a2 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Mon, 2 Oct 2023 11:38:20 +0900 Subject: [PATCH 47/97] fix(crosswalk): check all elements in traffic signal (#5169) Signed-off-by: Tomohito Ando --- .../src/scene_crosswalk.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 9ea03996c88dd..b99bd522b81f8 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -962,8 +962,10 @@ bool CrosswalkModule::isRedSignalForPedestrians() const continue; } - if (lights.front().color == TrafficSignalElement::RED) { - return true; + for (const auto & element : lights) { + if ( + element.color == TrafficSignalElement::RED && element.shape == TrafficSignalElement::CIRCLE) + return true; } } From a44830788f70b4e84ccdd874d56eea2e84d7ca1b Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 2 Oct 2023 12:06:24 +0900 Subject: [PATCH 48/97] feat(behavior_path_planner): safety check against dynamic objects after approval for start_planner (#5056) * add stop judgement after approval Signed-off-by: kyoichi-sugahara * update param Signed-off-by: kyoichi-sugahara * add status of has_stop_point Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../goal_planner/goal_planner.param.yaml | 10 ++-- .../start_planner/start_planner.param.yaml | 10 ++-- .../start_planner/start_planner_module.hpp | 3 ++ .../goal_planner/goal_planner_module.cpp | 7 +-- .../start_planner/start_planner_module.cpp | 51 ++++++++++++++++--- 5 files changed, 59 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 0ca028e053fbc..5ffac49847ce6 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -132,7 +132,7 @@ # detection range object_check_forward_distance: 10.0 object_check_backward_distance: 100.0 - ignore_object_velocity_threshold: 0.0 + ignore_object_velocity_threshold: 1.0 # ObjectTypesToCheck object_types_to_check: check_car: true @@ -164,11 +164,11 @@ check_all_predicted_path: true publish_debug_marker: false rss_params: - rear_vehicle_reaction_time: 1.0 + rear_vehicle_reaction_time: 2.0 rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 1.0 - longitudinal_distance_min_threshold: 1.0 - longitudinal_velocity_delta_time: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 # hysteresis factor to expand/shrink polygon with the value hysteresis_factor_expand_rate: 1.0 # temporary diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index 6760ec787bb03..4a42da9bc5ac3 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -95,7 +95,7 @@ # detection range object_check_forward_distance: 10.0 object_check_backward_distance: 100.0 - ignore_object_velocity_threshold: 0.0 + ignore_object_velocity_threshold: 1.0 # ObjectTypesToCheck object_types_to_check: check_car: true @@ -127,11 +127,11 @@ check_all_predicted_path: true publish_debug_marker: false rss_params: - rear_vehicle_reaction_time: 1.0 + rear_vehicle_reaction_time: 2.0 rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 1.0 - longitudinal_distance_min_threshold: 1.0 - longitudinal_velocity_delta_time: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 # hysteresis factor to expand/shrink polygon hysteresis_factor_expand_rate: 1.0 # temporary diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 9bbd1203ee6b0..7ab404d99bb12 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -67,6 +67,9 @@ struct PullOutStatus false}; // after backward driving is complete, this is set to true (warning: this is set to // false at next cycle after backward driving is complete) Pose pull_out_start_pose{}; + bool prev_is_safe_dynamic_objects{false}; + std::shared_ptr prev_stop_path_after_approval{nullptr}; + bool has_stop_point{false}; PullOutStatus() {} }; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 5d9058890f351..574a1a38fc66a 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -365,12 +365,7 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { - // TODO(Sugahara): should check safe or not but in the current flow, it is not possible. - if (status_.pull_over_path == nullptr) { - return true; - } - - if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + if (parameters_->safety_check_params.enable_safety_check && isWaitingApproval()) { if (!isSafePath()) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); return false; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 819686c20ff54..092b91499252c 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -136,6 +136,12 @@ void StartPlannerModule::updateData() if (has_received_new_route) { status_ = PullOutStatus(); } + // check safety status after back finished + if (parameters_->safety_check_params.enable_safety_check && status_.back_finished) { + status_.is_safe_dynamic_objects = isSafePath(); + } else { + status_.is_safe_dynamic_objects = true; + } } bool StartPlannerModule::isExecutionRequested() const @@ -179,17 +185,19 @@ bool StartPlannerModule::isExecutionRequested() const bool StartPlannerModule::isExecutionReady() const { + // when is_safe_static_objects is false,the path is not generated and approval shouldn't be + // allowed if (!status_.is_safe_static_objects) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against static objects"); return false; } - if (status_.pull_out_path.partial_paths.empty()) { - return true; - } - - if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + if ( + parameters_->safety_check_params.enable_safety_check && status_.back_finished && + isWaitingApproval()) { if (!isSafePath()) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + stop_pose_ = planner_data_->self_odometry->pose.pose; return false; } } @@ -221,6 +229,7 @@ void StartPlannerModule::updateCurrentState() if (status_.backward_driving_complete) { current_state_ = ModuleStatus::SUCCESS; // for breaking loop } + print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); } @@ -252,12 +261,42 @@ BehaviorModuleOutput StartPlannerModule::plan() } PathWithLaneId path; + + // Check if backward motion is finished if (status_.back_finished) { + // Increment path index if the current path is finished if (hasFinishedCurrentPath()) { RCLCPP_INFO(getLogger(), "Increment path index"); incrementPathIndex(); } - path = getCurrentPath(); + + if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.has_stop_point) { + auto current_path = getCurrentPath(); + const auto stop_path = + behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( + current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, + parameters_->maximum_jerk_for_stop); + + // Insert stop point in the path if needed + if (stop_path) { + RCLCPP_ERROR_THROTTLE( + getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects"); + path = *stop_path; + status_.prev_stop_path_after_approval = std::make_shared(path); + status_.has_stop_point = true; + } else { + path = current_path; + } + } else if (!isWaitingApproval() && status_.has_stop_point) { + // Delete stop point if conditions are met + if (status_.is_safe_dynamic_objects && isStopped()) { + status_.has_stop_point = false; + path = getCurrentPath(); + } + path = *status_.prev_stop_path_after_approval; + } else { + path = getCurrentPath(); + } } else { path = status_.backward_path; } From 14ef695b33325ac946438063278a16d1835c18f0 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 2 Oct 2023 12:11:33 +0900 Subject: [PATCH 49/97] fix(trajectory_visualizer): fix plotter error (#5181) fix(trajectory_visualizer): fix plotter Signed-off-by: Mamoru Sobue --- .../planning_debug_tools/scripts/trajectory_visualizer.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index 73e41b052cd90..10bd41c5c9c93 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -178,15 +178,15 @@ def __init__(self): # main process if PLOT_TYPE == "VEL_ACC_JERK": + self.setPlotTrajectory() self.ani = animation.FuncAnimation( self.fig, self.plotTrajectory, interval=100, blit=True ) - self.setPlotTrajectory() else: + self.setPlotTrajectoryVelocity() self.ani = animation.FuncAnimation( self.fig, self.plotTrajectoryVelocity, interval=100, blit=True ) - self.setPlotTrajectoryVelocity() plt.show() From 93b61184520d93ab8bcdb653f51d94a2de5bfbf9 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 2 Oct 2023 15:00:43 +0900 Subject: [PATCH 50/97] feat(intersection): ignore occlusion behind blocking vehicle (#5122) Signed-off-by: Mamoru Sobue --- .../README.md | 10 + .../docs/occlusion_grid.drawio.svg | 2616 +++++++++++++++++ .../src/debug.cpp | 6 + .../src/scene_intersection.cpp | 87 +- .../src/util.cpp | 19 +- .../src/util.hpp | 7 +- .../src/util_type.hpp | 1 + 7 files changed, 2714 insertions(+), 32 deletions(-) create mode 100644 planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 5f26ef34c1186..e8a823a7e790e 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -94,6 +94,16 @@ If a stopline is associated with the intersection lane, that line is used as the To avoid a rapid braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) are needed to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position. +### Occlusion detection + +If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough the ego vehicle will once stop at the _default stop line_ for `occlusion.before_creep_stop_time`, and then slowly creep toward _occlusion peeking stop line_ at the speed of `occlusion.occlusion_creep_velocity` if `occlusion.enable_creeping` is true. During the creeping if collision is detected this module inserts a stop line instantly, and if the FOV gets sufficiently clear the _intersection occlusion_ wall will disappear. If occlusion is cleared and no collision is detected the ego vehicle will pass the intersection. + +The occlusion is detected as the common area of occlusion attention area(which is partially the same as the normal attention area) and the unknown cells of the occupancy grid map. The occupancy grid map is denoised using morphology with the window size of `occlusion.denoise_kernel`. The occlusion attention area lanes are discretized to line strings and they are used to generate a grid whose each cell represents the distance from the ego path along the lane as shown below. + +![occlusion_detection](./docs/occlusion_grid.drawio.svg) + +If the nearest occlusion cell value is below the threshold, occlusion is detected. It is expected that the occlusion gets cleared as the vehicle approaches the occlusion peeking stop line. + ### Module Parameters | Parameter | Type | Description | diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg b/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg new file mode 100644 index 0000000000000..fc09a4212070a --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg @@ -0,0 +1,2616 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + default stop line + +
+
+
+ + default stop line + + + + + + + + +
+
+
+ + occlusion peeking line + +
+
+
+ + occlusion peeking line + + + + + + + + + + + + + + + + + +
+
+
+ + ego + +
+
+
+ + ego + + + + + + + + +
+
+
+ occlusion attention area +
+
+
+ + occlusion attent... + + + + + + + + + + + +
+
+
+ + + stopped vehicle on attention area +
+ (blocking vehicle) + + +
+
+
+ + stopped vehicle on attentio... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + The cells behind blocking vehicles +
+ are not marked as occluded +
+ + +
+
+
+ + The cells behind blocking vehicles... + + + + + + + + +
+
+
+ + + mark the cells which is unknown and +
+ belong to attention area are +
+ marked as occluded + +
+ +
+
+
+ + mark the cells which is unknown and... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 13 +
+
+
+ + 13 + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + 4 +
+
+ +
+
+
+ + 4... + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + nearest occlusion cell + +
+
+
+ + nearest occlusion... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index af8972424f54a..fbb82d6ace175 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -230,6 +230,12 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99, + 0.99, 0.6), + &debug_marker_array, now); + /* appendMarkerArray( createPoseMarkerArray( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index ca1cbfdd0ccc7..89bb65f403095 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -929,28 +929,29 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (!occlusion_attention_divisions_) { occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0)); + planner_data_->occupancy_grid->info.resolution); } const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); - std::vector parked_attention_objects; + std::vector blocking_attention_objects; std::copy_if( target_objects.objects.begin(), target_objects.objects.end(), - std::back_inserter(parked_attention_objects), + std::back_inserter(blocking_attention_objects), [thresh = planner_param_.occlusion.ignore_parked_vehicle_speed_threshold](const auto & object) { return std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh; }); + debug_data_.blocking_attention_objects.objects = blocking_attention_objects; const bool is_occlusion_cleared = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) ? isOcclusionCleared( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, occlusion_attention_divisions, - parked_attention_objects, occlusion_dist_thr) + blocking_attention_objects, occlusion_dist_thr) : true; occlusion_stop_state_machine_.setStateWithMarginTime( is_occlusion_cleared ? StateMachine::State::GO : StateMachine::STOP, @@ -1064,9 +1065,11 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); const auto is_in_adjacent_lanelets = util::checkAngleForTargetLanelets( - object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_thr, + object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, + adjacent_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin); + planner_param_.common.attention_area_margin, + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold); if (is_in_adjacent_lanelets) { continue; } @@ -1078,17 +1081,19 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT if (is_in_intersection_area) { target_objects.objects.push_back(object); } else if (util::checkAngleForTargetLanelets( - object_direction, attention_area_lanelets, - planner_param_.common.attention_area_angle_thr, + object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, + attention_area_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin)) { + planner_param_.common.attention_area_margin, + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { target_objects.objects.push_back(object); } } else if (util::checkAngleForTargetLanelets( - object_direction, attention_area_lanelets, - planner_param_.common.attention_area_angle_thr, + object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, + attention_area_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin)) { + planner_param_.common.attention_area_margin, + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { // intersection_area is not available, use detection_area_with_margin as before target_objects.objects.push_back(object); } @@ -1240,7 +1245,7 @@ bool IntersectionModule::isOcclusionCleared( const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - [[maybe_unused]] const std::vector & + const std::vector & blocking_attention_objects, const double occlusion_dist_thr) { @@ -1265,6 +1270,13 @@ bool IntersectionModule::isOcclusionCleared( const int height = occ_grid.info.height; const double resolution = occ_grid.info.resolution; const auto & origin = occ_grid.info.origin.position; + auto coord2index = [&](const double x, const double y) { + const int idx_x = (x - origin.x) / resolution; + const int idx_y = (y - origin.y) / resolution; + if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); + if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); + return std::make_tuple(true, idx_x, idx_y); + }; Polygon2d grid_poly; grid_poly.outer().emplace_back(origin.x, origin.y); @@ -1351,8 +1363,39 @@ bool IntersectionModule::isOcclusionCleared( cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); // (3) occlusion mask + static constexpr unsigned char OCCLUDED = 255; + static constexpr unsigned char BLOCKED = 127; cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask); + // re-use attention_mask + attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); + // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded + std::vector> blocking_polygons; + for (const auto & blocking_attention_object : blocking_attention_objects) { + const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object); + findCommonCvPolygons(obj_poly.outer(), blocking_polygons); + } + for (const auto & blocking_polygon : blocking_polygons) { + cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); + } + for (const auto & lane_division : lane_divisions) { + const auto & divisions = lane_division.divisions; + for (const auto & division : divisions) { + bool blocking_vehicle_found = false; + for (const auto & point_it : division) { + const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); + if (!valid) continue; + if (blocking_vehicle_found) { + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + continue; + } + if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { + blocking_vehicle_found = true; + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + } + } + } + } // (4) extract occlusion polygons const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; @@ -1397,19 +1440,11 @@ bool IntersectionModule::isOcclusionCleared( } // (4.1) re-draw occluded cells using valid_contours occlusion_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); - for (unsigned i = 0; i < valid_contours.size(); ++i) { + for (const auto & valid_contour : valid_contours) { // NOTE: drawContour does not work well - cv::fillPoly(occlusion_mask, valid_contours[i], cv::Scalar(255), cv::LINE_AA); + cv::fillPoly(occlusion_mask, valid_contour, cv::Scalar(OCCLUDED), cv::LINE_AA); } - auto coord2index = [&](const double x, const double y) { - const int idx_x = (x - origin.x) / resolution; - const int idx_y = (y - origin.y) / resolution; - if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); - if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); - return std::make_tuple(true, idx_x, idx_y); - }; - // (5) find distance // (5.1) discretize path_ip with resolution for computational cost LineString2d path_linestring; @@ -1496,7 +1531,11 @@ bool IntersectionModule::isOcclusionCleared( const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); // TODO(Mamoru Sobue): add handling for blocking vehicles if (!valid) continue; - if (occlusion_mask.at(height - 1 - idx_y, idx_x) == 255) { + const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); + if (pixel == BLOCKED) { + break; + } + if (pixel == OCCLUDED) { if (acc_dist < min_dist) { min_dist = acc_dist; nearest_occlusion_point = { diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index fff1d08223e5f..37dc83cbdf761 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1058,17 +1058,18 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( } bool 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 margin) + const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, + const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, + const bool consider_wrong_direction_vehicle, const double dist_margin, + const double parked_vehicle_speed_threshold) { for (const auto & ll : target_lanelets) { - if (!lanelet::utils::isInLanelet(pose, ll, margin)) { + 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); + const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); if (consider_wrong_direction_vehicle) { if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { return true; @@ -1077,6 +1078,14 @@ bool checkAngleForTargetLanelets( if (std::fabs(angle_diff) < detection_area_angle_thr) { return true; } + // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is + // positive + if ( + std::fabs(longitudinal_velocity) < parked_vehicle_speed_threshold && + (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return true; + } } } return false; diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index f02362e3f803d..7ba894bd9d32a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -136,9 +136,10 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); bool 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 margin = 0.0); + const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, + const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, + const bool consider_wrong_direction_vehicle, const double dist_margin, + const double parked_vehicle_speed_threshold); void cutPredictPathWithDuration( autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 4135884f03c74..7e5bff89485ed 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -50,6 +50,7 @@ struct DebugData std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; + autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; }; struct InterpolatedPathInfo From 89404e6070a43526f86a0bc88b231865ce2344af Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Mon, 2 Oct 2023 14:24:46 +0800 Subject: [PATCH 51/97] perf(system_monitor): fix program command line reading (#5191) * Fix program command line reading Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * fix spelling commandline->command_line Signed-off-by: Owen-Liuyuxuan --------- Signed-off-by: Owen-Liuyuxuan Co-authored-by: Owen-Liuyuxuan Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../process_monitor/process_monitor.hpp | 8 +++++++ .../src/process_monitor/process_monitor.cpp | 24 +++++++++++++++++-- 2 files changed, 30 insertions(+), 2 deletions(-) diff --git a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp index e114d58770883..42bd2a3ea9236 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp @@ -88,6 +88,14 @@ class ProcessMonitor : public rclcpp::Node */ void getHighMemoryProcesses(const std::string & output); + /** + * @brief get command line from process id + * @param [in] pid process id + * @param [out] command output command line + * @return true if success to get command line name + */ + bool getCommandLineFromPiD(const std::string & pid, std::string * command); + /** * @brief get top-rated processes * @param [in] tasks list of diagnostics tasks for high load procs diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index f8ee314e5a0ef..6f58339f2ff4b 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -414,6 +414,19 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) getTopratedProcesses(&memory_tasks_, &p2); } +bool ProcessMonitor::getCommandLineFromPiD(const std::string & pid, std::string * command) +{ + std::string commandLineFilePath = "/proc/" + pid + "/cmdline"; + std::ifstream commandFile(commandLineFilePath); + if (commandFile.is_open()) { + std::getline(commandFile, *command); + commandFile.close(); + return true; + } else { + return false; + } +} + void ProcessMonitor::getTopratedProcesses( std::vector> * tasks, bp::pipe * p) { @@ -462,7 +475,14 @@ void ProcessMonitor::getTopratedProcesses( info.virtualImage >> info.residentSize >> info.sharedMemSize >> info.processStatus >> info.cpuUsage >> info.memoryUsage >> info.cpuTime; - std::getline(stream, info.commandName); + std::string program_name; + std::getline(stream, program_name); + + bool flag_find_command_line = getCommandLineFromPiD(info.processId, &info.commandName); + + if (!flag_find_command_line) { + info.commandName = program_name; // if command line is not found, use program name instead + } tasks->at(index)->setDiagnosticsStatus(DiagStatus::OK, "OK"); tasks->at(index)->setProcessInformation(info); @@ -515,7 +535,7 @@ void ProcessMonitor::onTimer() std::ostringstream os; // Get processes - bp::child c("top -bcn1 -o %CPU -w 256", bp::std_out > is_out, bp::std_err > is_err); + bp::child c("top -bn1 -o %CPU -w 128", bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { From c6b802fc2908592cb4f27ae12feb6ca42015a51e Mon Sep 17 00:00:00 2001 From: beyzanurkaya <32412808+beyzanurkaya@users.noreply.github.com> Date: Mon, 2 Oct 2023 11:29:52 +0300 Subject: [PATCH 52/97] fix(behavior_path_planner): if ego leaves the current lane turn the signal on (#4348) * if ego leaves its lane due the avoidance, turn the signal on Signed-off-by: beyza * add right_bound check Signed-off-by: beyza * style(pre-commit): autofix --------- Signed-off-by: beyza Co-authored-by: beyza Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../avoidance/avoidance_module.cpp | 27 +++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 83789818c456b..9117efcc447da 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/marker_utils/avoidance/debug.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -2543,9 +2544,31 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; } } else { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::DISABLE; + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const auto local_vehicle_footprint = + createVehicleFootprint(planner_data_->parameters.vehicle_info); + boost::geometry::model::ring shifted_vehicle_footprint; + for (const auto & cl : current_lanes) { + // get left and right bounds of current lane + const auto lane_left_bound = cl.leftBound2d().basicLineString(); + const auto lane_right_bound = cl.rightBound2d().basicLineString(); + for (size_t i = start_idx; i < end_idx; ++i) { + // transform vehicle footprint onto path points + shifted_vehicle_footprint = transformVector( + local_vehicle_footprint, + tier4_autoware_utils::pose2transform(path.path.points.at(i).point.pose)); + if ( + boost::geometry::intersects(lane_left_bound, shifted_vehicle_footprint) || + boost::geometry::intersects(lane_right_bound, shifted_vehicle_footprint)) { + if (segment_shift_length > 0.0) { + turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else { + turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + } + } + } + } } - if (ego_front_to_shift_start > 0.0) { turn_signal_info.desired_start_point = planner_data_->self_odometry->pose.pose; } else { From de07fb8011cb9330b435e13dadac74c875f9bae2 Mon Sep 17 00:00:00 2001 From: Alexey Panferov <37497658+lexavtanke@users.noreply.github.com> Date: Mon, 2 Oct 2023 12:04:32 +0300 Subject: [PATCH 53/97] fix(lidar_apollo_instance_segmentation): add arg data_path, update model path (#5139) Signed-off-by: Alexey Panferov --- .../launch/lidar_apollo_instance_segmentation.launch.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml index bceb928dd6692..7f5024929b531 100644 --- a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml +++ b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml @@ -7,7 +7,8 @@ - + + From bab14534a60e5741bd57b70979035bdc874c65cb Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 2 Oct 2023 21:01:09 +0900 Subject: [PATCH 54/97] feat(logger_level_reconfigure_plugin): use node interface and some cosmetic updates (#5204) * use node service Signed-off-by: Takamasa Horibe * enable yaml configuration Signed-off-by: Takamasa Horibe * update yaml loading Signed-off-by: Takamasa Horibe * make it scrollable Signed-off-by: Takamasa Horibe * change function order Signed-off-by: Takamasa Horibe * change color for level Signed-off-by: Takamasa Horibe * fix depend Signed-off-by: Takamasa Horibe * Update common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp Co-authored-by: Kosuke Takeuchi * Update common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp --------- Signed-off-by: Takamasa Horibe Co-authored-by: Kosuke Takeuchi --- .../CMakeLists.txt | 1 + .../config/logger_config.yaml | 59 +++++ .../logging_level_configure.hpp | 14 +- .../package.xml | 1 + .../src/logging_level_configure.cpp | 230 ++++++++---------- 5 files changed, 164 insertions(+), 141 deletions(-) create mode 100644 common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml diff --git a/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt index f86b6d4d2efdc..cc7da7e322d19 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt +++ b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt @@ -24,4 +24,5 @@ pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description. ament_auto_package( INSTALL_TO_SHARE plugins + config ) 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 new file mode 100644 index 0000000000000..da5cc757682c5 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -0,0 +1,59 @@ +# logger_config.yaml + +# ============================================================ +# planning +# ============================================================ + +behavior_path_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 + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: tier4_autoware_utils + +behavior_path_planner_avoidance: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance + +behavior_velocity_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: tier4_autoware_utils + +behavior_velocity_planner_intersection: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.intersection + +motion_obstacle_avoidance: + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: tier4_autoware_utils + +motion_velocity_smoother: + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: planning.scenario_planning.motion_velocity_smoother + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: tier4_autoware_utils + +# ============================================================ +# control +# ============================================================ + +lateral_controller: + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: control.trajectory_follower.controller_node_exe.lateral_controller + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils + +longitudinal_controller: + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: control.trajectory_follower.controller_node_exe.longitudinal_controller + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils + +vehicle_cmd_gate: + - node_name: /control/vehicle_cmd_gate + logger_name: control.vehicle_cmd_gate + - node_name: /control/vehicle_cmd_gate + logger_name: tier4_autoware_utils diff --git a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp index f513a7e04a248..4d9b81ffb86bf 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp +++ b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp @@ -48,24 +48,24 @@ class LoggingLevelConfigureRvizPlugin : public rviz_common::Panel QMap buttonGroups_; rclcpp::Node::SharedPtr raw_node_; - // logger_node_map_[target_name] = {container_name, logger_name} - std::map>> logger_node_map_; + // node_logger_map_[button_name] = {node_name, logger_name} + std::map>> node_logger_map_; - // client_map_[container_name] = service_client + // client_map_[node_name] = service_client std::unordered_map::SharedPtr> client_map_; - // button_map_[target_name][logging_level] = Q_button_pointer + // button_map_[button_name][logging_level] = Q_button_pointer std::unordered_map> button_map_; - QStringList getContainerList(); + QStringList getNodeList(); int getMaxModuleNameWidth(QLabel * containerLabel); void setLoggerNodeMap(); - void attachLoggingComponent(); private Q_SLOTS: void onButtonClick(QPushButton * button, const QString & name, const QString & level); - void updateButtonColors(const QString & target_module_name, QPushButton * active_button); + void updateButtonColors( + const QString & target_module_name, QPushButton * active_button, const QString & level); void changeLogLevel(const QString & container, const QString & level); }; diff --git a/common/tier4_logging_level_configure_rviz_plugin/package.xml b/common/tier4_logging_level_configure_rviz_plugin/package.xml index 4ba5bb1579e13..7849e6049a077 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/package.xml +++ b/common/tier4_logging_level_configure_rviz_plugin/package.xml @@ -21,6 +21,7 @@ rviz_common rviz_default_plugins rviz_rendering + yaml-cpp ament_lint_auto autoware_lint_common diff --git a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp index e645ff682d50e..b15c0f0f735fa 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp +++ b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp @@ -12,7 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "yaml-cpp/yaml.h" + #include +#include +#include #include #include @@ -26,59 +30,28 @@ LoggingLevelConfigureRvizPlugin::LoggingLevelConfigureRvizPlugin(QWidget * paren { } -// Calculate the maximum width among all target_module_name. -int LoggingLevelConfigureRvizPlugin::getMaxModuleNameWidth(QLabel * containerLabel) -{ - int max_width = 0; - QFontMetrics metrics(containerLabel->font()); - for (const auto & item : logger_node_map_) { - const auto & target_module_name = item.first; - int width = metrics.horizontalAdvance(target_module_name); - if (width > max_width) { - max_width = width; - } - } - return max_width; -} - -// create container list in logger_node_map_ without -QStringList LoggingLevelConfigureRvizPlugin::getContainerList() -{ - QStringList containers; - for (const auto & item : logger_node_map_) { - const auto & container_logger_vec = item.second; - for (const auto & container_logger_pair : container_logger_vec) { - if (!containers.contains(container_logger_pair.first)) { - containers.append(container_logger_pair.first); - } - } - } - return containers; -} - void LoggingLevelConfigureRvizPlugin::onInitialize() { - setLoggerNodeMap(); + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - attachLoggingComponent(); + setLoggerNodeMap(); QVBoxLayout * layout = new QVBoxLayout; QStringList levels = {"DEBUG", "INFO", "WARN", "ERROR", "FATAL"}; - QStringList loaded_container; constexpr int height = 20; - for (const auto & item : logger_node_map_) { - const auto & target_module_name = item.first; + for (const auto & item : node_logger_map_) { + const auto & target_node_name = item.first; QHBoxLayout * hLayout = new QHBoxLayout; - // Create a QLabel to display the container name. - QLabel * containerLabel = new QLabel(target_module_name); - containerLabel->setFixedHeight(height); // Set fixed height for the button - containerLabel->setFixedWidth(getMaxModuleNameWidth(containerLabel)); + // Create a QLabel to display the node name. + QLabel * label = new QLabel(target_node_name); + label->setFixedHeight(height); // Set fixed height for the button + label->setFixedWidth(getMaxModuleNameWidth(label)); - hLayout->addWidget(containerLabel); // Add the QLabel to the hLayout. + hLayout->addWidget(label); // Add the QLabel to the hLayout. QButtonGroup * group = new QButtonGroup(this); for (const QString & level : levels) { @@ -86,42 +59,66 @@ void LoggingLevelConfigureRvizPlugin::onInitialize() btn->setFixedHeight(height); // Set fixed height for the button hLayout->addWidget(btn); // Add each QPushButton to the hLayout. group->addButton(btn); - button_map_[target_module_name][level] = btn; - connect(btn, &QPushButton::clicked, this, [this, btn, target_module_name, level]() { - this->onButtonClick(btn, target_module_name, level); + button_map_[target_node_name][level] = btn; + connect(btn, &QPushButton::clicked, this, [this, btn, target_node_name, level]() { + this->onButtonClick(btn, target_node_name, level); }); } // Set the "INFO" button as checked by default and change its color. - updateButtonColors(target_module_name, button_map_[target_module_name]["INFO"]); + updateButtonColors(target_node_name, button_map_[target_node_name]["INFO"], "INFO"); - buttonGroups_[target_module_name] = group; + buttonGroups_[target_node_name] = group; layout->addLayout(hLayout); } - setLayout(layout); + // Create a QWidget to hold the layout. + QWidget * containerWidget = new QWidget; + containerWidget->setLayout(layout); + + // Create a QScrollArea to make the layout scrollable. + QScrollArea * scrollArea = new QScrollArea; + scrollArea->setWidget(containerWidget); + scrollArea->setWidgetResizable(true); + + // Set the QScrollArea as the layout of the main widget. + QVBoxLayout * mainLayout = new QVBoxLayout; + mainLayout->addWidget(scrollArea); + setLayout(mainLayout); // set up service clients - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - const auto & containers = getContainerList(); - for (const QString & container : containers) { + const auto & nodes = getNodeList(); + for (const QString & node : nodes) { const auto client = raw_node_->create_client( - container.toStdString() + "/config_logger"); - client_map_[container] = client; + node.toStdString() + "/config_logger"); + client_map_[node] = client; } } -void LoggingLevelConfigureRvizPlugin::attachLoggingComponent() +// Calculate the maximum width among all target_module_name. +int LoggingLevelConfigureRvizPlugin::getMaxModuleNameWidth(QLabel * label) { - const auto & containers = getContainerList(); - for (const auto & container_name : containers) { - // Load the component for each container - QString command = "ros2 component load --node-namespace " + container_name + - " --node-name logging_configure " + container_name + - " logging_demo logging_demo::LoggerConfig"; - int result = system(qPrintable(command)); - std::cerr << "load logger in " << container_name.toStdString() << ": result = " << result - << std::endl; + int max_width = 0; + QFontMetrics metrics(label->font()); + for (const auto & item : node_logger_map_) { + const auto & target_module_name = item.first; + max_width = std::max(metrics.horizontalAdvance(target_module_name), max_width); } + return max_width; +} + +// create node list in node_logger_map_ without +QStringList LoggingLevelConfigureRvizPlugin::getNodeList() +{ + QStringList nodes; + for (const auto & item : node_logger_map_) { + const auto & node_logger_vec = item.second; + for (const auto & node_logger_pair : node_logger_vec) { + if (!nodes.contains(node_logger_pair.first)) { + nodes.append(node_logger_pair.first); + } + } + } + return nodes; } // Modify the signature of the onButtonClick function: @@ -135,40 +132,48 @@ void LoggingLevelConfigureRvizPlugin::onButtonClick( << std::string(future.get()->success ? "success!" : "failed...") << std::endl; }; - for (const auto & container_logger_map : logger_node_map_[target_module_name]) { - const auto container_node = container_logger_map.first; - const auto logger_name = container_logger_map.second; + for (const auto & node_logger_map : node_logger_map_[target_module_name]) { + const auto node_name = node_logger_map.first; + const auto logger_name = node_logger_map.second; const auto req = std::make_shared(); req->logger_name = logger_name.toStdString(); req->level = level.toStdString(); std::cerr << "logger level of " << req->logger_name << " is set to " << req->level << std::endl; - client_map_[container_node]->async_send_request(req, callback); + client_map_[node_name]->async_send_request(req, callback); } updateButtonColors( - target_module_name, button); // Modify updateButtonColors to accept QPushButton only. + target_module_name, button, level); // Modify updateButtonColors to accept QPushButton only. } } void LoggingLevelConfigureRvizPlugin::updateButtonColors( - const QString & target_module_name, QPushButton * active_button) + const QString & target_module_name, QPushButton * active_button, const QString & level) { - const QString LIGHT_GREEN = "rgb(181, 255, 20)"; - const QString LIGHT_GRAY = "rgb(211, 211, 211)"; + std::unordered_map colorMap = { + {"DEBUG", "rgb(181, 255, 20)"}, /* green */ + {"INFO", "rgb(200, 255, 255)"}, /* light blue */ + {"WARN", "rgb(255, 255, 0)"}, /* yellow */ + {"ERROR", "rgb(255, 0, 0)"}, /* red */ + {"FATAL", "rgb(139, 0, 0)"}, /* dark red */ + {"OFF", "rgb(211, 211, 211)"} /* gray */ + }; + const QString LIGHT_GRAY_TEXT = "rgb(180, 180, 180)"; + const QString color = colorMap.count(level) ? colorMap[level] : colorMap["OFF"]; + for (const auto & button : button_map_[target_module_name]) { if (button.second == active_button) { - button.second->setStyleSheet("background-color: " + LIGHT_GREEN + "; color: black"); + button.second->setStyleSheet("background-color: " + color + "; color: black"); } else { button.second->setStyleSheet( - "background-color: " + LIGHT_GRAY + "; color: " + LIGHT_GRAY_TEXT); + "background-color: " + colorMap["OFF"] + "; color: " + LIGHT_GRAY_TEXT); } } } - void LoggingLevelConfigureRvizPlugin::save(rviz_common::Config config) const { Panel::save(config); @@ -181,68 +186,25 @@ void LoggingLevelConfigureRvizPlugin::load(const rviz_common::Config & config) void LoggingLevelConfigureRvizPlugin::setLoggerNodeMap() { - // =============================================================================================== - // ====================================== Planning =============================================== - // =============================================================================================== - - QString behavior_planning_container = - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_planning_container"; - QString motion_planning_container = - "/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container"; - - // behavior_path_planner (all) - logger_node_map_["behavior_path_planner"] = { - {behavior_planning_container, - "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner"}, - {behavior_planning_container, "tier4_autoware_utils"}}; - - // behavior_path_planner: avoidance - logger_node_map_["behavior_path_planner: avoidance"] = { - {behavior_planning_container, - "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner." - "avoidance"}}; - - // behavior_velocity_planner (all) - logger_node_map_["behavior_velocity_planner"] = { - {behavior_planning_container, - "planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner"}, - {behavior_planning_container, "tier4_autoware_utils"}}; - - // behavior_velocity_planner: intersection - logger_node_map_["behavior_velocity_planner: intersection"] = { - {behavior_planning_container, - "planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner." - "intersection"}}; - - // obstacle_avoidance_planner - logger_node_map_["motion: obstacle_avoidance"] = { - {motion_planning_container, - "planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner"}, - {motion_planning_container, "tier4_autoware_utils"}}; - - // motion_velocity_smoother - QString container = "/planning/scenario_planning/motion_velocity_smoother_container"; - logger_node_map_["motion: velocity_smoother"] = { - {container, "planning.scenario_planning.motion_velocity_smoother"}, - {container, "tier4_autoware_utils"}}; - - // =============================================================================================== - // ======================================= Control =============================================== - // =============================================================================================== - - QString control_container = "/control/control_container"; - - // lateral_controller - logger_node_map_["lateral_controller"] = { - {control_container, "control.trajectory_follower.controller_node_exe.lateral_controller"}, - {control_container, "tier4_autoware_utils"}, - }; - - // longitudinal_controller - logger_node_map_["longitudinal_controller"] = { - {control_container, "control.trajectory_follower.controller_node_exe.longitudinal_controller"}, - {control_container, "tier4_autoware_utils"}, - }; + const std::string package_share_directory = + ament_index_cpp::get_package_share_directory("tier4_logging_level_configure_rviz_plugin"); + const std::string default_config_path = package_share_directory + "/config/logger_config.yaml"; + + const auto filename = + raw_node_->declare_parameter("config_filename", default_config_path); + RCLCPP_INFO(raw_node_->get_logger(), "load config file: %s", filename.c_str()); + + YAML::Node config = YAML::LoadFile(filename); + + for (YAML::const_iterator it = config.begin(); it != config.end(); ++it) { + const auto key = QString::fromStdString(it->first.as()); + const YAML::Node values = it->second; + for (size_t i = 0; i < values.size(); i++) { + const auto node_name = QString::fromStdString(values[i]["node_name"].as()); + const auto logger_name = QString::fromStdString(values[i]["logger_name"].as()); + node_logger_map_[key].push_back({node_name, logger_name}); + } + } } } // namespace rviz_plugin From e7bc187a9f4f86cbb7712e5fa9086a70d5b4652f Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 3 Oct 2023 06:57:08 +0900 Subject: [PATCH 55/97] fix(map_based_prediction): filter by distance for opposite lanes as well (#5195) * fix(map_based_prediction): filter by distance for opposite lanes as well Signed-off-by: kminoda * update Signed-off-by: kminoda --------- Signed-off-by: kminoda --- .../map_based_prediction/map_based_prediction_node.hpp | 3 +-- .../map_based_prediction/src/map_based_prediction_node.cpp | 7 +++---- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 8ef6d628aa2f6..3d01ab96e9b62 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -176,8 +176,7 @@ class MapBasedPredictionNode : public rclcpp::Node LaneletsData getCurrentLanelets(const TrackedObject & object); bool checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const bool check_distance = true); + const std::pair & lanelet, const TrackedObject & object); float calculateLocalLikelihood( const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const; void updateObjectData(TrackedObject & object); diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 6a414c26b8806..58356f4f96ed6 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -1251,7 +1251,7 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob for (const auto & lanelet : surrounding_opposite_lanelets) { // Check if the close lanelets meet the necessary condition for start lanelets // except for distance checking - if (!checkCloseLaneletCondition(lanelet, object, false)) { + if (!checkCloseLaneletCondition(lanelet, object)) { continue; } @@ -1271,8 +1271,7 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob } bool MapBasedPredictionNode::checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const bool check_distance) + const std::pair & lanelet, const TrackedObject & object) { // Step1. If we only have one point in the centerline, we will ignore the lanelet if (lanelet.second.centerline().size() <= 1) { @@ -1307,7 +1306,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; const bool is_yaw_reversed = M_PI - delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta && object_vel < 0.0; - if (check_distance && dist_threshold_for_searching_lanelet_ < lanelet.first) { + if (dist_threshold_for_searching_lanelet_ < lanelet.first) { return false; } if (!is_yaw_reversed && delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta) { From a6af44eaf534a3c96e4bdb3b483f3eb99548027a Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 3 Oct 2023 06:57:21 +0900 Subject: [PATCH 56/97] feat(localization_error_monitor): update parameter (#5201) Signed-off-by: kminoda --- .../config/localization_error_monitor.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/localization/localization_error_monitor/config/localization_error_monitor.param.yaml b/localization/localization_error_monitor/config/localization_error_monitor.param.yaml index 2aa28014ead41..def5c80164642 100644 --- a/localization/localization_error_monitor/config/localization_error_monitor.param.yaml +++ b/localization/localization_error_monitor/config/localization_error_monitor.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: scale: 3.0 - error_ellipse_size: 1.0 - warn_ellipse_size: 0.8 + error_ellipse_size: 1.5 + warn_ellipse_size: 1.2 error_ellipse_size_lateral_direction: 0.3 warn_ellipse_size_lateral_direction: 0.25 From ccf9235e7a5537ed83aea451216ac963487cbd36 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 3 Oct 2023 08:35:39 +0900 Subject: [PATCH 57/97] fix(avoidance): add debug info (#5205) Signed-off-by: Fumiya Watanabe --- .../src/scene_module/avoidance/avoidance_module.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 9117efcc447da..36908bd87de23 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2662,6 +2662,7 @@ void AvoidanceModule::updateDebugMarker( addObjects(data.other_objects, std::string("OutOfTargetArea")); addObjects(data.other_objects, std::string("NotNeedAvoidance")); addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); + addObjects(data.other_objects, std::string("TooNearToGoal")); } // shift line pre-process From 6e49e8297c915a0c68aaae7663d1ad89d4863221 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 3 Oct 2023 11:05:22 +0900 Subject: [PATCH 58/97] fix(simulator, controller): fix inverse pitch calculation (#5199) Signed-off-by: Mamoru Sobue Signed-off-by: Takamasa Horibe Co-authored-by: Takamasa Horibe --- control/pid_longitudinal_controller/README.md | 4 + .../media/slope_definition.drawio.svg | 165 +++++++++ .../src/pid_longitudinal_controller.cpp | 4 +- simulator/simple_planning_simulator/README.md | 6 + .../media/pitch-calculation.drawio.svg | 329 ++++++++++++++++++ .../simple_planning_simulator_core.cpp | 2 +- 6 files changed, 508 insertions(+), 2 deletions(-) create mode 100644 control/pid_longitudinal_controller/media/slope_definition.drawio.svg create mode 100644 simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 418b3f7d27220..9d25322c793e0 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -75,6 +75,10 @@ For instance, if the vehicle is attempting to start with an acceleration of `1.0 A suitable example of a vehicle system for the slope compensation function is one in which the output acceleration from the longitudinal_controller is converted into target accel/brake pedal input without any feedbacks. In this case, the output acceleration is just used as a feedforward term to calculate the target pedal, and hence the issue mentioned above does not arise. +Note: The angle of the slope is defined as positive for an uphill slope, while the pitch angle of the ego pose is defined as negative when facing upward. They have an opposite definition. + +![slope_definition](./media/slope_definition.drawio.svg) + #### PID control For deviations that cannot be handled by FeedForward control, such as model errors, PID control is used to construct a feedback system. diff --git a/control/pid_longitudinal_controller/media/slope_definition.drawio.svg b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg new file mode 100644 index 0000000000000..7f0aa677629a3 --- /dev/null +++ b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Slope is defined as positive for an uphill +
+
+
+ + Slope is defined as positive for an uphi... + + + + + + + + + + + + + + + +
+
+
+ (Pitch of ego is defined as negative when facing upward) +
+
+
+ + (Pitch of ego is defined as negative when facing upwar... + + + + + + +
+
+
+ x +
+
+
+ + x + + + + + + +
+
+
+ y +
+
+
+ + z + + + + + + +
+
+
+ z +
+
+
+ + y + + + + + + +
+
+
+ d_z +
+
+
+ + d_z + + + + + + +
+
+
+ d_xy +
+
+
+ + d_xy + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 644effce50824..b80874a8a2e57 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -457,7 +457,9 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData current_pose, m_trajectory, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch - const double raw_pitch = longitudinal_utils::getPitchByPose(current_pose.orientation); + // NOTE: getPitchByTraj() calculates the pitch angle as defined in + // ../media/slope_definition.drawio.svg while getPitchByPose() is not, so `raw_pitch` is reversed + const double raw_pitch = (-1.0) * longitudinal_utils::getPitchByPose(current_pose.orientation); const double traj_pitch = longitudinal_utils::getPitchByTraj(m_trajectory, control_data.nearest_idx, m_wheel_base); control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index d70aca794dc07..ceecacfe8cd8c 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -89,6 +89,12 @@ _Note_: The steering/velocity/acceleration dynamics is modeled by a first order Since the vehicle outputs `odom`->`base_link` tf, this simulator outputs the tf with the same frame_id configuration. In the simple_planning_simulator.launch.py, the node that outputs the `map`->`odom` tf, that usually estimated by the localization module (e.g. NDT), will be launched as well. Since the tf output by this simulator module is an ideal value, `odom`->`map` will always be 0. +### (Caveat) Pitch calculation + +Ego vehicle pitch angle is calculated in the following manner. + +![pitch calculation](./media/pitch-calculation.drawio.svg) + ## Error detection and handling The only validation on inputs being done is testing for a valid vehicle model type. diff --git a/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg b/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg new file mode 100644 index 0000000000000..d022b29d2a730 --- /dev/null +++ b/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg @@ -0,0 +1,329 @@ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + forward
$diff_xy > 0
 + +
+
+
+ + forward... + + + + + +
+
+
+ + + ego +
+ direction +
+ + +
+
+
+ + ego... + + + + + +
+
+
+ + reverse
$diff_xy < 0
 + +
+
+
+ + reverse... + + + + + +
+
+
+ + + lane +
+ direction +
+ + +
+
+
+ + lane... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $diff_z > 0
 + +
+
+
+ + $diff_z > 0 + + + + + +
+
+
+ + $diff_z < 0
 + +
+
+
+ + $diff_z < 0 + + + + + +
+
+
+ + pitch < 0
 + +
+
+
+ + pitch < 0 + + + + + +
+
+
+ + pitch > 0
 + +
+
+
+ + pitch > 0 + + + + + +
+
+
+ + pitch < 0
 + +
+
+
+ + pitch < 0 + + + + + +
+
+
+ + pitch > 0
 + +
+
+
+ + pitch > 0 + + + + + + + + Text is not SVG - cannot display + + + diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 8c2688928b4c9..208fd80d57a08 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -313,7 +313,7 @@ double SimplePlanningSimulator::calculate_ego_pitch() const std::cos(ego_yaw_against_lanelet); const bool reverse_sign = std::cos(ego_yaw_against_lanelet) < 0.0; const double ego_pitch_angle = - reverse_sign ? std::atan2(-diff_z, -diff_xy) : std::atan2(diff_z, diff_xy); + reverse_sign ? -std::atan2(-diff_z, -diff_xy) : -std::atan2(diff_z, diff_xy); return ego_pitch_angle; } From 8ab910ba8dd3ff4fd09d149d6857b629b3cf9b0b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 3 Oct 2023 12:11:48 +0900 Subject: [PATCH 59/97] feat(crosswalk): dyanmic timeout for no intention to walk decision (#5188) * feat(crosswalk): dyanmic timeout for no intention to walk decision Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/crosswalk.param.yaml | 4 +- .../src/manager.cpp | 6 ++- .../src/scene_crosswalk.cpp | 2 +- .../src/scene_crosswalk.hpp | 54 ++++++++++++++++--- 4 files changed, 55 insertions(+), 11 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index b1e5bcb32bd18..e7accc14096e0 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -54,7 +54,9 @@ ## param for yielding disable_stop_for_yield_cancel: true # for the crosswalk where there is a traffic signal disable_yield_for_new_stopped_object: true # for the crosswalk where there is a traffic signal - timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + # if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order + timeout_map_for_no_intention_to_walk: [3.0, 0.0] # [s] timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not # param for target object filtering diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 304b4bdf6f5e7..94e87d0174193 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -104,8 +104,10 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".pass_judge.disable_stop_for_yield_cancel"); cp.disable_yield_for_new_stopped_object = getOrDeclareParameter(node, ns + ".pass_judge.disable_yield_for_new_stopped_object"); - cp.timeout_no_intention_to_walk = - getOrDeclareParameter(node, ns + ".pass_judge.timeout_no_intention_to_walk"); + cp.distance_map_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.distance_map_for_no_intention_to_walk"); + cp.timeout_map_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.timeout_map_for_no_intention_to_walk"); cp.timeout_ego_stop_for_yield = getOrDeclareParameter(node, ns + ".pass_judge.timeout_ego_stop_for_yield"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index b99bd522b81f8..62acd191d8697 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -928,7 +928,7 @@ void CrosswalkModule::updateObjectState( getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); object_info_manager_.update( obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, - has_traffic_light, collision_point, planner_param_); + has_traffic_light, collision_point, planner_param_, crosswalk_.polygon2d().basicPolygon()); if (collision_point) { const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 667298cbb4c58..063ea4f83cd45 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -30,12 +30,15 @@ #include #include +#include +#include #include #include #include #include #include +#include #include #include #include @@ -45,6 +48,7 @@ namespace behavior_velocity_planner { +namespace bg = boost::geometry; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -71,6 +75,33 @@ double interpolateEgoPassMargin( } return y_vec.back(); } + +double InterpolateMap( + const std::vector & key_map, const std::vector & value_map, const double query) +{ + // If the speed is out of range of the key_map, apply zero-order hold. + if (query <= key_map.front()) { + return value_map.front(); + } + if (query >= key_map.back()) { + return value_map.back(); + } + + // Apply linear interpolation + for (size_t i = 0; i < key_map.size() - 1; ++i) { + if (key_map.at(i) <= query && query <= key_map.at(i + 1)) { + auto ratio = (query - key_map.at(i)) / std::max(key_map.at(i + 1) - key_map.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + const auto interp = value_map.at(i) + ratio * (value_map.at(i + 1) - value_map.at(i)); + return interp; + } + } + + std::cerr << "Crosswalk's InterpolateMap interpolation logic is broken." + "Please check the code." + << std::endl; + return value_map.back(); +} } // namespace class CrosswalkModule : public SceneModuleInterface @@ -111,7 +142,8 @@ class CrosswalkModule : public SceneModuleInterface double min_object_velocity; bool disable_stop_for_yield_cancel; bool disable_yield_for_new_stopped_object; - double timeout_no_intention_to_walk; + std::vector distance_map_for_no_intention_to_walk; + std::vector timeout_map_for_no_intention_to_walk; double timeout_ego_stop_for_yield; // param for input data double traffic_light_state_timeout; @@ -132,9 +164,10 @@ class CrosswalkModule : public SceneModuleInterface std::optional collision_point{}; void transitState( - const rclcpp::Time & now, const double vel, const bool is_ego_yielding, - const bool has_traffic_light, const std::optional & collision_point, - const PlannerParam & planner_param) + const rclcpp::Time & now, const geometry_msgs::msg::Point & position, const double vel, + const bool is_ego_yielding, const bool has_traffic_light, + const std::optional & collision_point, const PlannerParam & planner_param, + const lanelet::BasicPolygon2d & crosswalk_polygon) { const bool is_stopped = vel < planner_param.stop_object_velocity; @@ -147,8 +180,13 @@ class CrosswalkModule : public SceneModuleInterface if (!time_to_start_stopped) { time_to_start_stopped = now; } + const double distance_to_crosswalk = + bg::distance(crosswalk_polygon, lanelet::BasicPoint2d(position.x, position.y)); + const double timeout_no_intention_to_walk = InterpolateMap( + planner_param.distance_map_for_no_intention_to_walk, + planner_param.timeout_map_for_no_intention_to_walk, distance_to_crosswalk); const bool intent_to_cross = - (now - *time_to_start_stopped).seconds() < planner_param.timeout_no_intention_to_walk; + (now - *time_to_start_stopped).seconds() < timeout_no_intention_to_walk; if ( (is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) && !intent_to_cross) { @@ -203,7 +241,8 @@ class CrosswalkModule : public SceneModuleInterface void update( const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel, const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light, - const std::optional & collision_point, const PlannerParam & planner_param) + const std::optional & collision_point, const PlannerParam & planner_param, + const lanelet::BasicPolygon2d & crosswalk_polygon) { // update current uuids current_uuids_.push_back(uuid); @@ -221,7 +260,8 @@ class CrosswalkModule : public SceneModuleInterface // update object state objects.at(uuid).transitState( - now, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param); + now, position, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param, + crosswalk_polygon); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; } From 8c3812603173b68ec8fc3c5b7b300e170fa2bea6 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 3 Oct 2023 12:24:25 +0900 Subject: [PATCH 60/97] fix(out_of_lane): improve stop decision (#5207) Signed-off-by: Takayuki Murooka --- .../CMakeLists.txt | 8 +- .../README.md | 15 ++-- .../config/out_of_lane.param.yaml | 3 +- .../src/calculate_slowdown_points.hpp | 87 ++++++++++++++++++ .../src/decisions.cpp | 20 ++--- .../src/filter_predicted_objects.hpp | 10 +-- .../src/footprint.cpp | 6 +- .../src/lanelets_selection.cpp | 3 + .../src/manager.cpp | 2 +- .../src/scene_out_of_lane.cpp | 90 +++++-------------- .../src/scene_out_of_lane.hpp | 9 -- .../src/types.hpp | 4 +- 12 files changed, 136 insertions(+), 121 deletions(-) create mode 100644 planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt index 633d5fdd613af..8ce1a334c4b35 100644 --- a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt @@ -6,13 +6,7 @@ autoware_package() pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp - src/decisions.cpp - src/footprint.cpp - src/lanelets_selection.cpp - src/manager.cpp - src/overlapping_range.cpp - src/scene_out_of_lane.cpp + DIRECTORY src ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_out_of_lane_module/README.md b/planning/behavior_velocity_out_of_lane_module/README.md index 885c0dc2859ab..99396eb0edf22 100644 --- a/planning/behavior_velocity_out_of_lane_module/README.md +++ b/planning/behavior_velocity_out_of_lane_module/README.md @@ -149,14 +149,13 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | | `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) | -| Parameter /action | Type | Description | -| ----------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | -| `strict` | bool | [-] if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego; if false, ego stops just before entering a lane but may then be overlapping another lane | -| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | -| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | -| `slowdown.velocity` | double | [m/s] slow down velocity | -| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | +| Parameter /action | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- | +| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | +| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | +| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | +| `slowdown.velocity` | double | [m] slow down velocity | +| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | | Parameter /ego | Type | Description | | -------------------- | ------ | ---------------------------------------------------- | diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml index 510dc86ef651d..c74c5b3d87c1d 100644 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -24,8 +24,7 @@ action: # action to insert in the path if an object causes a conflict at an overlap skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed - strict: true # if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego - # if false, ego stops just before entering a lane but may then be overlapping another lane. + precision: 0.1 # [m] precision when inserting a stop pose in the path distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane slowdown: distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap diff --git a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp new file mode 100644 index 0000000000000..5d22f01222919 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -0,0 +1,87 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CALCULATE_SLOWDOWN_POINTS_HPP_ +#define CALCULATE_SLOWDOWN_POINTS_HPP_ + +#include "footprint.hpp" +#include "types.hpp" + +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner::out_of_lane +{ + +bool can_decelerate( + const EgoData & ego_data, const PathPointWithLaneId & point, const double target_vel) +{ + if (ego_data.velocity < 0.5) return true; + const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( + ego_data.path.points, ego_data.pose.position, point.point.pose.position); + const auto acc_to_target_vel = + (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); + return acc_to_target_vel < std::abs(ego_data.max_decel); +} + +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint, + const PlannerParam & params) +{ + const auto from_arc_length = + motion_utils::calcSignedArcLength(ego_data.path.points, 0, ego_data.first_path_idx); + const auto to_arc_length = + motion_utils::calcSignedArcLength(ego_data.path.points, 0, decision.target_path_idx); + PathPointWithLaneId interpolated_point; + for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { + // TODO(Maxime): binary search + interpolated_point.point.pose = motion_utils::calcInterpolatedPose(ego_data.path.points, l); + const auto respect_decel_limit = + !params.skip_if_over_max_decel || + can_decelerate(ego_data, interpolated_point, decision.velocity); + if ( + respect_decel_limit && !boost::geometry::overlaps( + project_to_pose(footprint, interpolated_point.point.pose), + decision.lane_to_avoid.polygon2d().basicPolygon())) + return interpolated_point; + } + return std::nullopt; +} + +/// @brief calculate the slowdown point to insert in the path +/// @param ego_data ego data (path, velocity, etc) +/// @param decisions decision (before which point to stop, what lane to avoid entering, etc) +/// @param params parameters +/// @return optional slowdown point to insert in the path +std::optional calculate_slowdown_point( + const EgoData & ego_data, const std::vector & decisions, PlannerParam params) +{ + params.extra_front_offset += params.dist_buffer; + const auto base_footprint = make_base_footprint(params); + + // search for the first slowdown decision for which a stop point can be inserted + for (const auto & decision : decisions) { + const auto last_in_lane_pose = + calculate_last_in_lane_pose(ego_data, decision, base_footprint, params); + if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; + } + return std::nullopt; +} +} // namespace behavior_velocity_planner::out_of_lane +#endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index da46c1355e735..78d9b73c86a17 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -35,7 +35,7 @@ namespace behavior_velocity_planner::out_of_lane double distance_along_path(const EgoData & ego_data, const size_t target_idx) { return motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, ego_data.first_path_idx + target_idx); + ego_data.path.points, ego_data.pose.position, ego_data.first_path_idx + target_idx); } double time_along_path(const EgoData & ego_data, const size_t target_idx, const double min_velocity) @@ -43,7 +43,7 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx, const const auto dist = distance_along_path(ego_data, target_idx); const auto v = std::max( std::max(ego_data.velocity, min_velocity), - ego_data.path->points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * + ego_data.path.points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * 0.5); return dist / v; } @@ -126,11 +126,9 @@ std::optional> object_time_to_range( const auto same_driving_direction_as_ego = enter_time < exit_time; if (same_driving_direction_as_ego) { - RCLCPP_DEBUG(logger, " / SAME DIR \\\n"); worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; } else { - RCLCPP_DEBUG(logger, " / OPPOSITE DIR \\\n"); worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; } @@ -212,8 +210,11 @@ std::optional> object_time_to_range( bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params) { - return std::min(range_times.object.enter_time, range_times.object.exit_time) < - params.time_threshold; + const auto enter_within_threshold = + range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold; + const auto exit_within_threshold = + range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold; + return enter_within_threshold || exit_within_threshold; } bool intervals_condition( @@ -357,11 +358,10 @@ std::optional calculate_decision( { std::optional decision; if (should_not_enter(range, inputs, params, logger)) { - const auto stop_before_range = params.strict ? find_most_preceding_range(range, inputs) : range; decision.emplace(); - decision->target_path_idx = inputs.ego_data.first_path_idx + - stop_before_range.entering_path_idx; // add offset from curr pose - decision->lane_to_avoid = stop_before_range.lane; + decision->target_path_idx = + inputs.ego_data.first_path_idx + range.entering_path_idx; // add offset from curr pose + decision->lane_to_avoid = range.lane; const auto ego_dist_to_range = distance_along_path(inputs.ego_data, range.entering_path_idx); set_decision_velocity(decision, ego_dist_to_range, params); } diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index f2555f31a4d5e..13bad0d047922 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -23,9 +23,6 @@ namespace behavior_velocity_planner::out_of_lane { -using motion_utils::calcLateralOffset; -using tier4_autoware_utils::calcDistance2d; - /// @brief filter predicted objects and their predicted paths /// @param [in] objects predicted objects to filter /// @param [in] ego_data ego data @@ -47,13 +44,8 @@ autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( auto filtered_object = object; const auto is_invalid_predicted_path = [&](const auto & predicted_path) { const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; - const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); - - // Note: lateral offset can not be calculated for path with less than 2 unique points const auto lat_offset_to_current_ego = - unique_path_points.size() > 1 - ? std::abs(calcLateralOffset(unique_path_points, ego_data.pose.position)) - : calcDistance2d(unique_path_points.front(), ego_data.pose.position); + std::abs(motion_utils::calcLateralOffset(predicted_path.path, ego_data.pose.position)); const auto is_crossing_ego = lat_offset_to_current_ego <= object.shape.dimensions.y / 2.0 + std::max( diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp index 9ad7437901309..31cc035703a7b 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp @@ -57,9 +57,9 @@ std::vector calculate_path_footprints( { const auto base_footprint = make_base_footprint(params); std::vector path_footprints; - path_footprints.reserve(ego_data.path->points.size()); - for (auto i = ego_data.first_path_idx; i < ego_data.path->points.size(); ++i) { - const auto & path_pose = ego_data.path->points[i].point.pose; + path_footprints.reserve(ego_data.path.points.size()); + for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size(); ++i) { + const auto & path_pose = ego_data.path.points[i].point.pose; const auto angle = tf2::getYaw(path_pose.orientation); const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index 7f3b69f40840b..c5dedf23785ab 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -20,6 +20,7 @@ #include #include +#include namespace behavior_velocity_planner::out_of_lane { @@ -68,6 +69,8 @@ lanelet::ConstLanelets calculate_other_lanelets( return false; }; for (const auto & ll : lanelets_within_range) { + if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") + continue; const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id()); const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); if (!is_path_lanelet && !is_ignored_lanelet && !is_overlapped_by_path_lanelets(ll.second)) diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 5765363024aed..2422c6fe84e56 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -56,7 +56,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.skip_if_over_max_decel = getOrDeclareParameter(node, ns + ".action.skip_if_over_max_decel"); - pp.strict = getOrDeclareParameter(node, ns + ".action.strict"); + pp.precision = getOrDeclareParameter(node, ns + ".action.precision"); pp.dist_buffer = getOrDeclareParameter(node, ns + ".action.distance_buffer"); pp.slow_velocity = getOrDeclareParameter(node, ns + ".action.slowdown.velocity"); pp.slow_dist_threshold = diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 013643b6bd6d4..1487d9e63c8b0 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -14,6 +14,7 @@ #include "scene_out_of_lane.hpp" +#include "calculate_slowdown_points.hpp" #include "debug.hpp" #include "decisions.hpp" #include "filter_predicted_objects.hpp" @@ -31,6 +32,8 @@ #include #include +#include + #include #include @@ -62,9 +65,9 @@ bool OutOfLaneModule::modifyPathVelocity( stopwatch.tic(); EgoData ego_data; ego_data.pose = planner_data_->current_odometry->pose; - ego_data.path = path; + ego_data.path.points = path->points; ego_data.first_path_idx = - motion_utils::findNearestSegmentIndex(path->points, ego_data.pose.position); + motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); ego_data.velocity = planner_data_->current_velocity->twist.linear.x; ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold; stopwatch.tic("calculate_path_footprints"); @@ -73,7 +76,7 @@ bool OutOfLaneModule::modifyPathVelocity( const auto calculate_path_footprints_us = stopwatch.toc("calculate_path_footprints"); // Calculate lanelets to ignore and consider const auto path_lanelets = planning_utils::getLaneletsOnPath( - *path, planner_data_->route_handler_->getLaneletMapPtr(), + ego_data.path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); const auto ignored_lanelets = calculate_ignored_lanelets(ego_data, path_lanelets, *planner_data_->route_handler_, params_); @@ -113,22 +116,27 @@ bool OutOfLaneModule::modifyPathVelocity( auto decisions = calculate_decisions(inputs, params_, logger_); const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); stopwatch.tic("calc_slowdown_points"); - const auto points_to_insert = calculate_slowdown_points(ego_data, decisions, params_); - debug_data_.slowdowns = points_to_insert; + const auto point_to_insert = calculate_slowdown_point(ego_data, decisions, params_); const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); stopwatch.tic("insert_slowdown_points"); - for (const auto & point : points_to_insert) { - auto path_idx = point.slowdown.target_path_idx; - planning_utils::insertVelocity(*ego_data.path, point.point, point.slowdown.velocity, path_idx); - if (point.slowdown.velocity == 0.0) { + debug_data_.slowdowns.clear(); + if (point_to_insert) { + debug_data_.slowdowns = {*point_to_insert}; + auto path_idx = motion_utils::findNearestSegmentIndex( + path->points, point_to_insert->point.point.pose.position) + + 1; + planning_utils::insertVelocity( + *path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx, + params_.precision); + if (point_to_insert->slowdown.velocity == 0.0) { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = point.point.point.pose; + stop_factor.stop_pose = point_to_insert->point.point.pose; stop_factor.dist_to_stop_pose = motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, point.point.point.pose.position); + ego_data.path.points, ego_data.pose.position, point_to_insert->point.point.pose.position); planning_utils::appendStopReason(stop_factor, stop_reason); } velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, point.point.point.pose, + path->points, planner_data_->current_odometry->pose, point_to_insert->point.point.pose, VelocityFactor::UNKNOWN); } const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); @@ -181,62 +189,4 @@ motion_utils::VirtualWalls OutOfLaneModule::createVirtualWalls() return virtual_walls; } -std::vector calculate_slowdown_points( - const EgoData & ego_data, const std::vector & decisions, PlannerParam params) -{ - std::vector to_insert; - params.extra_front_offset += params.dist_buffer; - const auto base_footprint = make_base_footprint(params); - - const auto can_decel = [&](const auto dist_ahead_of_ego, const auto target_vel) { - const auto acc_to_target_vel = - (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); - return acc_to_target_vel < std::abs(ego_data.max_decel); - }; - const auto insert_decision = [&](const auto & path_point, const auto & decision) -> bool { - const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, path_point.point.pose.position); - if (!params.skip_if_over_max_decel || can_decel(dist_ahead_of_ego, decision.velocity)) { - to_insert.push_back({decision, path_point}); - return true; - } - return false; - }; - const auto insert_interpolated_decision = - [&](const auto & path_point, const auto & decision) -> bool { - auto interpolated_point = path_point; - const auto & path_pose = path_point.point.pose; - const auto & prev_path_pose = ego_data.path->points[decision.target_path_idx - 1].point.pose; - constexpr auto precision = 0.1; - for (auto ratio = precision; ratio <= 1.0; ratio += precision) { - interpolated_point.point.pose = - tier4_autoware_utils::calcInterpolatedPose(path_pose, prev_path_pose, ratio, false); - const auto is_overlap = boost::geometry::overlaps( - project_to_pose(base_footprint, interpolated_point.point.pose), - decision.lane_to_avoid.polygon2d().basicPolygon()); - if (!is_overlap) { - return insert_decision(path_point, decision); - } - } - return false; - }; - for (const auto & decision : decisions) { - const auto & path_point = ego_data.path->points[decision.target_path_idx]; - const auto decision_is_at_beginning_of_path = - decision.target_path_idx == ego_data.first_path_idx; - bool inserted = false; - if (decision_is_at_beginning_of_path) { - inserted = insert_decision(path_point, decision); - } else { - inserted = insert_interpolated_decision(path_point, decision); - // if no valid point found, fallback to using the previous index (known to not overlap) - if (!inserted) - inserted = insert_decision(ego_data.path->points[decision.target_path_idx], decision); - } - // only insert the first (i.e., lowest arc length) decision - if (inserted) break; - } - return to_insert; -} - } // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index 1a64c11a3c921..cdf8eef8f277b 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -57,15 +57,6 @@ class OutOfLaneModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; - -/// @brief calculate points along the path where we want ego to slowdown/stop -/// @param ego_data ego data (path, velocity, etc) -/// @param decisions decisions (before which point to stop, what lane to avoid entering, etc) -/// @param params parameters -/// @return precise points to insert in the path -std::vector calculate_slowdown_points( - const EgoData & ego_data, const std::vector & decisions, PlannerParam params); - } // namespace behavior_velocity_planner::out_of_lane #endif // SCENE_OUT_OF_LANE_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index b81fa09884819..64d88f15e6966 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -52,11 +52,11 @@ struct PlannerParam double overlap_min_dist; // [m] min distance inside another lane to consider an overlap // action to insert in the path if an object causes a conflict at an overlap bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel - bool strict; // if true stop before entering *any* other lane, not only the lane to avoid double dist_buffer; double slow_velocity; double slow_dist_threshold; double stop_dist_threshold; + double precision; // [m] precision when inserting a stop pose in the path // ego dimensions used to create its polygon footprint double front_offset; // [m] front offset (from vehicle info) double rear_offset; // [m] rear offset (from vehicle info) @@ -135,7 +135,7 @@ struct OtherLane /// @brief data related to the ego vehicle struct EgoData { - autoware_auto_planning_msgs::msg::PathWithLaneId * path{}; + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; size_t first_path_idx{}; double velocity{}; // [m/s] double max_decel{}; // [m/s²] From 58a513efabcce9b93d1bc2ee4639d7b15b852038 Mon Sep 17 00:00:00 2001 From: shulanbushangshu <102840938+shulanbushangshu@users.noreply.github.com> Date: Tue, 3 Oct 2023 11:33:33 +0800 Subject: [PATCH 61/97] feat: add common interface specs test (#5034) * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * style(pre-commit): autofix * fix copyright Signed-off-by: Takagi, Isamu --------- Signed-off-by: jack.song Signed-off-by: Takagi, Isamu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Takagi, Isamu --- .../component_interface_specs/CMakeLists.txt | 34 ++++++++++ common/component_interface_specs/package.xml | 17 ++++- .../test/gtest_main.cpp | 21 ++++++ .../test/test_control.cpp | 46 +++++++++++++ .../test/test_localization.cpp | 46 +++++++++++++ .../test/test_map.cpp | 28 ++++++++ .../test/test_perception.cpp | 28 ++++++++ .../test/test_planning.cpp | 64 +++++++++++++++++++ .../test/test_system.cpp | 37 +++++++++++ .../test/test_vehicle.cpp | 64 +++++++++++++++++++ 10 files changed, 384 insertions(+), 1 deletion(-) create mode 100644 common/component_interface_specs/test/gtest_main.cpp create mode 100644 common/component_interface_specs/test/test_control.cpp create mode 100644 common/component_interface_specs/test/test_localization.cpp create mode 100644 common/component_interface_specs/test/test_map.cpp create mode 100644 common/component_interface_specs/test/test_perception.cpp create mode 100644 common/component_interface_specs/test/test_planning.cpp create mode 100644 common/component_interface_specs/test/test_system.cpp create mode 100644 common/component_interface_specs/test/test_vehicle.cpp diff --git a/common/component_interface_specs/CMakeLists.txt b/common/component_interface_specs/CMakeLists.txt index 31c723600183f..b519a0ddce86a 100644 --- a/common/component_interface_specs/CMakeLists.txt +++ b/common/component_interface_specs/CMakeLists.txt @@ -4,4 +4,38 @@ project(component_interface_specs) find_package(autoware_cmake REQUIRED) autoware_package() +include_directories( + include + SYSTEM + ${rclcpp_INCLUDE_DIRS} + ${rosidl_runtime_cpp_INCLUDE_DIRS} + ${rcl_INCLUDE_DIRS} + ${autoware_adapi_v1_msgs_INCLUDE_DIRS} + ${autoware_auto_planning_msgs_INCLUDE_DIRS} + ${autoware_planning_msgs_INCLUDE_DIRS} + ${autoware_auto_vehicle_msgs_INCLUDE_DIRS} + ${tier4_control_msgs_INCLUDE_DIRS} + ${nav_msgs_INCLUDE_DIRS} + ${tier4_system_msgs_INCLUDE_DIRS} + ${tier4_vehicle_msgs_INCLUDE_DIRS} + ${autoware_auto_perception_msgs_INCLUDE_DIRS} + ${tier4_map_msgs_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_component_interface_specs + test/gtest_main.cpp + test/test_planning.cpp + test/test_control.cpp + test/test_localization.cpp + test/test_system.cpp + test/test_map.cpp + test/test_perception.cpp + test/test_vehicle.cpp + ) + target_include_directories(test_component_interface_specs + PRIVATE include + ) +endif() + ament_auto_package() diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 7ebc3f0d1bea9..93f77c651869d 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -11,13 +11,28 @@ Apache License 2.0 ament_cmake_auto + ament_cmake_core + ament_cmake_export_dependencies + ament_cmake_test autoware_cmake + ament_cmake_core + ament_cmake_test + autoware_adapi_v1_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_planning_msgs + nav_msgs + rcl + rclcpp + rosidl_runtime_cpp + tier4_control_msgs tier4_map_msgs + tier4_system_msgs tier4_vehicle_msgs - + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/component_interface_specs/test/gtest_main.cpp b/common/component_interface_specs/test/gtest_main.cpp new file mode 100644 index 0000000000000..81d9d5345270d --- /dev/null +++ b/common/component_interface_specs/test/gtest_main.cpp @@ -0,0 +1,21 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/component_interface_specs/test/test_control.cpp b/common/component_interface_specs/test/test_control.cpp new file mode 100644 index 0000000000000..366641eacbd23 --- /dev/null +++ b/common/component_interface_specs/test/test_control.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/control.hpp" +#include "gtest/gtest.h" + +TEST(control, interface) +{ + { + using control_interface::IsPaused; + IsPaused is_paused; + size_t depth = 1; + EXPECT_EQ(is_paused.depth, depth); + EXPECT_EQ(is_paused.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_paused.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using control_interface::IsStartRequested; + IsStartRequested is_start_requested; + size_t depth = 1; + EXPECT_EQ(is_start_requested.depth, depth); + EXPECT_EQ(is_start_requested.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_start_requested.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using control_interface::IsStopped; + IsStopped is_stopped; + size_t depth = 1; + EXPECT_EQ(is_stopped.depth, depth); + EXPECT_EQ(is_stopped.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_stopped.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/common/component_interface_specs/test/test_localization.cpp b/common/component_interface_specs/test/test_localization.cpp new file mode 100644 index 0000000000000..31d8e139bd69a --- /dev/null +++ b/common/component_interface_specs/test/test_localization.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/localization.hpp" +#include "gtest/gtest.h" + +TEST(localization, interface) +{ + { + using localization_interface::InitializationState; + InitializationState initialization_state; + size_t depth = 1; + EXPECT_EQ(initialization_state.depth, depth); + EXPECT_EQ(initialization_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(initialization_state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using localization_interface::KinematicState; + KinematicState kinematic_state; + size_t depth = 1; + EXPECT_EQ(kinematic_state.depth, depth); + EXPECT_EQ(kinematic_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(kinematic_state.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using localization_interface::Acceleration; + Acceleration acceleration; + size_t depth = 1; + EXPECT_EQ(acceleration.depth, depth); + EXPECT_EQ(acceleration.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(acceleration.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_specs/test/test_map.cpp b/common/component_interface_specs/test/test_map.cpp new file mode 100644 index 0000000000000..a65f2cb7120d1 --- /dev/null +++ b/common/component_interface_specs/test/test_map.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/map.hpp" +#include "gtest/gtest.h" + +TEST(map, interface) +{ + { + using map_interface::MapProjectorInfo; + MapProjectorInfo map_projector; + size_t depth = 1; + EXPECT_EQ(map_projector.depth, depth); + EXPECT_EQ(map_projector.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(map_projector.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/common/component_interface_specs/test/test_perception.cpp b/common/component_interface_specs/test/test_perception.cpp new file mode 100644 index 0000000000000..ec80c06bb119a --- /dev/null +++ b/common/component_interface_specs/test/test_perception.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/perception.hpp" +#include "gtest/gtest.h" + +TEST(perception, interface) +{ + { + using perception_interface::ObjectRecognition; + ObjectRecognition object_recognition; + size_t depth = 1; + EXPECT_EQ(object_recognition.depth, depth); + EXPECT_EQ(object_recognition.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(object_recognition.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_specs/test/test_planning.cpp b/common/component_interface_specs/test/test_planning.cpp new file mode 100644 index 0000000000000..8c504cb119854 --- /dev/null +++ b/common/component_interface_specs/test/test_planning.cpp @@ -0,0 +1,64 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/planning.hpp" +#include "gtest/gtest.h" + +TEST(planning, interface) +{ + { + using planning_interface::RouteState; + RouteState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::Route; + Route route; + size_t depth = 1; + EXPECT_EQ(route.depth, depth); + EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::NormalRoute; + NormalRoute route; + size_t depth = 1; + EXPECT_EQ(route.depth, depth); + EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::MrmRoute; + MrmRoute route; + size_t depth = 1; + EXPECT_EQ(route.depth, depth); + EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::Trajectory; + Trajectory trajectory; + size_t depth = 1; + EXPECT_EQ(trajectory.depth, depth); + EXPECT_EQ(trajectory.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(trajectory.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_specs/test/test_system.cpp b/common/component_interface_specs/test/test_system.cpp new file mode 100644 index 0000000000000..416d2effad766 --- /dev/null +++ b/common/component_interface_specs/test/test_system.cpp @@ -0,0 +1,37 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/system.hpp" +#include "gtest/gtest.h" + +TEST(system, interface) +{ + { + using system_interface::MrmState; + MrmState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using system_interface::OperationModeState; + OperationModeState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/common/component_interface_specs/test/test_vehicle.cpp b/common/component_interface_specs/test/test_vehicle.cpp new file mode 100644 index 0000000000000..1f2041b6cb79e --- /dev/null +++ b/common/component_interface_specs/test/test_vehicle.cpp @@ -0,0 +1,64 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/vehicle.hpp" +#include "gtest/gtest.h" + +TEST(vehicle, interface) +{ + { + using vehicle_interface::SteeringStatus; + SteeringStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::GearStatus; + GearStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::TurnIndicatorStatus; + TurnIndicatorStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::HazardLightStatus; + HazardLightStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::EnergyStatus; + EnergyStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} From 81ede03f749d46fb3eed3c9e96ce9d993acaf8c3 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 3 Oct 2023 12:52:18 +0900 Subject: [PATCH 62/97] fix(utils): fix bug in drivable area expansion (#5198) * fix(avoidance): fix invalid drivable area bound Signed-off-by: satoshi-ota * refactor(avoidance): split huge process to three functions Signed-off-by: satoshi-ota * fix(utils): improve sharp bound remove logic Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_planner/utils/utils.hpp | 9 + .../behavior_path_planner/src/utils/utils.cpp | 347 +++++++++++------- 2 files changed, 230 insertions(+), 126 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 3d81c584b7c01..c5e5aa3e50191 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -231,6 +231,15 @@ std::vector calcBound( const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool is_left); +std::vector getBoundWithHatchedRoadMarkings( + const std::vector & original_bound, + const std::shared_ptr & route_handler); + +std::vector getBoundWithIntersectionAreas( + const std::vector & original_bound, + const std::shared_ptr & route_handler, + const std::vector & drivable_lanes, const bool is_left); + boost::optional getOverlappedLaneletId(const std::vector & lanes); std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 1b14741db4a56..7305405b50410 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1552,31 +1552,10 @@ void generateDrivableArea( } } -// calculate bounds from drivable lanes and hatched road markings -std::vector calcBound( - const std::shared_ptr route_handler, - const std::vector & drivable_lanes, - const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left) +std::vector getBoundWithHatchedRoadMarkings( + const std::vector & original_bound, + const std::shared_ptr & route_handler) { - // a function to convert drivable lanes to points without duplicated points - const auto convert_to_points = [&](const std::vector & drivable_lanes) { - constexpr double overlap_threshold = 0.01; - - std::vector points; - for (const auto & drivable_lane : drivable_lanes) { - const auto bound = - is_left ? drivable_lane.left_lane.leftBound3d() : drivable_lane.right_lane.rightBound3d(); - for (const auto & point : bound) { - if ( - points.empty() || - overlap_threshold < (points.back().basicPoint2d() - point.basicPoint2d()).norm()) { - points.push_back(point); - } - } - } - return points; - }; // a function to get polygon with a designated point id const auto get_corresponding_polygon_index = [&](const auto & polygon, const auto & target_point_id) { @@ -1589,18 +1568,89 @@ std::vector calcBound( // This means calculation has some errors. return polygon.size() - 1; }; + const auto mod = [&](const int a, const int b) { return (a + b) % b; // NOTE: consider negative value }; + + std::vector expanded_bound{}; + + std::optional current_polygon{std::nullopt}; + std::vector current_polygon_border_indices; + // expand drivable area by hatched road markings. + for (size_t bound_point_idx = 0; bound_point_idx < original_bound.size(); ++bound_point_idx) { + const auto & bound_point = original_bound[bound_point_idx]; + const auto polygon = getPolygonByPoint(route_handler, bound_point, "hatched_road_markings"); + + bool will_close_polygon{false}; + if (!current_polygon) { + if (!polygon) { + expanded_bound.push_back(bound_point); + } else { + // There is a new additional polygon to expand + current_polygon = polygon; + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, bound_point.id())); + } + } else { + if (!polygon) { + will_close_polygon = true; + } else { + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, bound_point.id())); + } + } + + if (bound_point_idx == original_bound.size() - 1 && current_polygon) { + // If drivable lanes ends earlier than polygon, close the polygon + will_close_polygon = true; + } + + if (will_close_polygon) { + // The current additional polygon ends to expand + if (current_polygon_border_indices.size() == 1) { + expanded_bound.push_back((*current_polygon)[current_polygon_border_indices.front()]); + } else { + const size_t current_polygon_points_num = current_polygon->size(); + const bool is_polygon_opposite_direction = [&]() { + const size_t modulo_diff = mod( + static_cast(current_polygon_border_indices[1]) - + static_cast(current_polygon_border_indices[0]), + current_polygon_points_num); + return modulo_diff == 1; + }(); + + const int target_points_num = + current_polygon_points_num - current_polygon_border_indices.size() + 1; + for (int poly_idx = 0; poly_idx <= target_points_num; ++poly_idx) { + const int target_poly_idx = current_polygon_border_indices.front() + + poly_idx * (is_polygon_opposite_direction ? -1 : 1); + expanded_bound.push_back( + (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)]); + } + } + current_polygon = std::nullopt; + current_polygon_border_indices.clear(); + } + } + + return expanded_bound; +} + +std::vector getBoundWithIntersectionAreas( + const std::vector & original_bound, + const std::shared_ptr & route_handler, + const std::vector & drivable_lanes, const bool is_left) +{ const auto extract_bound_from_polygon = [](const auto & polygon, const auto start_idx, const auto end_idx, const auto clockwise) { - std::vector ret; + std::vector ret{}; for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { - ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[i])); + ret.push_back(polygon[i]); if (i + 1 == polygon.size() && clockwise) { if (end_idx == 0) { - ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[end_idx])); + ret.push_back(polygon[end_idx]); return ret; } i = 0; @@ -1609,7 +1659,7 @@ std::vector calcBound( if (i == 0 && !clockwise) { if (end_idx == polygon.size() - 1) { - ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[end_idx])); + ret.push_back(polygon[end_idx]); return ret; } i = polygon.size() - 1; @@ -1617,34 +1667,30 @@ std::vector calcBound( } } + ret.push_back(polygon[end_idx]); + return ret; }; - // If no need to expand with polygons, return here. - std::vector output_points; - const auto bound_points = convert_to_points(drivable_lanes); - if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { - for (const auto & point : bound_points) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); - } - return motion_utils::removeOverlapPoints(output_points); - } + std::vector expanded_bound = original_bound; - std::optional current_polygon{std::nullopt}; - std::vector current_polygon_border_indices; + // expand drivable area by using intersection area. for (const auto & drivable_lane : drivable_lanes) { - // extract target lane and bound. - const auto bound_lane = is_left ? drivable_lane.left_lane : drivable_lane.right_lane; - const auto bound = is_left ? bound_lane.leftBound3d() : bound_lane.rightBound3d(); + const auto edge_lanelet = is_left ? drivable_lane.left_lane : drivable_lane.right_lane; + const auto lanelet_bound = is_left ? edge_lanelet.leftBound3d() : edge_lanelet.rightBound3d(); - if (bound.size() < 2) { + if (lanelet_bound.size() < 2) { continue; } - // expand drivable area by intersection areas. - const std::string id = bound_lane.attributeOr("intersection_area", "else"); - const auto use_intersection_area = enable_expanding_intersection_areas && id != "else"; - if (use_intersection_area) { + const std::string id = edge_lanelet.attributeOr("intersection_area", "else"); + if (id == "else") { + continue; + } + + // Step1. extract intersection partial bound. + std::vector intersection_bound{}; + { const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); @@ -1652,96 +1698,137 @@ std::vector calcBound( boost::geometry::is_valid(lanelet::utils::to2D(polygon.basicPolygon())); const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; - const auto start_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { - return p.id() == bound.front().id(); - }); + const auto intersection_bound_itr_init = std::find_if( + polygon.begin(), polygon.end(), + [&lanelet_bound](const auto & p) { return p.id() == lanelet_bound.front().id(); }); - const auto end_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { - return p.id() == bound.back().id(); - }); + const auto intersection_bound_itr_last = std::find_if( + polygon.begin(), polygon.end(), + [&lanelet_bound](const auto & p) { return p.id() == lanelet_bound.back().id(); }); - if (start_itr == polygon.end() || end_itr == polygon.end()) { + if ( + intersection_bound_itr_init == polygon.end() || + intersection_bound_itr_last == polygon.end()) { continue; } // extract line strings between start_idx and end_idx. - const size_t start_idx = std::distance(polygon.begin(), start_itr); - const size_t end_idx = std::distance(polygon.begin(), end_itr); - for (const auto & point : - extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration)) { - output_points.push_back(point); - } + const size_t start_idx = std::distance(polygon.begin(), intersection_bound_itr_init); + const size_t end_idx = std::distance(polygon.begin(), intersection_bound_itr_last); - continue; + intersection_bound = + extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration); } - if (!enable_expanding_hatched_road_markings) { - for (const auto & point : bound) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); - } + // Step2. check shared bound point. + const auto shared_point_itr_init = + std::find_if(expanded_bound.begin(), expanded_bound.end(), [&](const auto & p) { + return std::any_of( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & point) { return point.id() == p.id(); }); + }); + + const auto shared_point_itr_last = + std::find_if(expanded_bound.rbegin(), expanded_bound.rend(), [&](const auto & p) { + return std::any_of( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & point) { return point.id() == p.id(); }); + }); + + if ( + shared_point_itr_init == expanded_bound.end() || + shared_point_itr_last == expanded_bound.rend()) { continue; } - // expand drivable area by hatched road markings. - for (size_t bound_point_idx = 0; bound_point_idx < bound.size(); ++bound_point_idx) { - const auto & bound_point = bound[bound_point_idx]; - const auto polygon = getPolygonByPoint(route_handler, bound_point, "hatched_road_markings"); - - bool will_close_polygon{false}; - if (!current_polygon) { - if (!polygon) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(bound_point)); - } else { - // There is a new additional polygon to expand - current_polygon = polygon; - current_polygon_border_indices.push_back( - get_corresponding_polygon_index(*current_polygon, bound_point.id())); - } - } else { - if (!polygon) { - will_close_polygon = true; - } else { - current_polygon_border_indices.push_back( - get_corresponding_polygon_index(*current_polygon, bound_point.id())); - } - } + // Step3. overwrite duplicate drivable bound by intersection bound. + { + const auto trim_point_itr_init = std::find_if( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & p) { return p.id() == shared_point_itr_init->id(); }); - if (bound_point_idx == bound_points.size() - 1 && current_polygon) { - // If drivable lanes ends earlier than polygon, close the polygon - will_close_polygon = true; + const auto trim_point_itr_last = std::find_if( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & p) { return p.id() == shared_point_itr_last->id(); }); + + if ( + trim_point_itr_init == intersection_bound.end() || + trim_point_itr_last == intersection_bound.end()) { + continue; } - if (will_close_polygon) { - // The current additional polygon ends to expand - if (current_polygon_border_indices.size() == 1) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( - (*current_polygon)[current_polygon_border_indices.front()])); - } else { - const size_t current_polygon_points_num = current_polygon->size(); - const bool is_polygon_opposite_direction = [&]() { - const size_t modulo_diff = mod( - static_cast(current_polygon_border_indices[1]) - - static_cast(current_polygon_border_indices[0]), - current_polygon_points_num); - return modulo_diff == 1; - }(); + std::vector tmp_bound{}; - const int target_points_num = - current_polygon_points_num - current_polygon_border_indices.size() + 1; - for (int poly_idx = 0; poly_idx <= target_points_num; ++poly_idx) { - const int target_poly_idx = current_polygon_border_indices.front() + - poly_idx * (is_polygon_opposite_direction ? -1 : 1); - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( - (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)])); - } + tmp_bound.insert(tmp_bound.end(), expanded_bound.begin(), shared_point_itr_init); + tmp_bound.insert(tmp_bound.end(), trim_point_itr_init, trim_point_itr_last); + tmp_bound.insert( + tmp_bound.end(), std::next(shared_point_itr_last).base(), expanded_bound.end()); + + expanded_bound = tmp_bound; + } + } + + return expanded_bound; +} + +// calculate bounds from drivable lanes and hatched road markings +std::vector calcBound( + const std::shared_ptr route_handler, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool is_left) +{ + // a function to convert drivable lanes to points without duplicated points + const auto convert_to_points = [&](const std::vector & drivable_lanes) { + constexpr double overlap_threshold = 0.01; + + std::vector points; + for (const auto & drivable_lane : drivable_lanes) { + const auto bound = + is_left ? drivable_lane.left_lane.leftBound3d() : drivable_lane.right_lane.rightBound3d(); + for (const auto & point : bound) { + if ( + points.empty() || + overlap_threshold < (points.back().basicPoint2d() - point.basicPoint2d()).norm()) { + points.push_back(point); } - current_polygon = std::nullopt; - current_polygon_border_indices.clear(); } } + return points; + }; + + const auto to_ros_point = [](const std::vector & bound) { + std::vector ret{}; + std::for_each(bound.begin(), bound.end(), [&](const auto & p) { + ret.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return ret; + }; + + // Step1. create drivable bound from drivable lanes. + std::vector bound_points = convert_to_points(drivable_lanes); + + // Step2. if there is no drivable area defined by polygon, return original drivable bound. + if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { + return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + } + + // Step3. if there are hatched road markings, expand drivable bound with the polygon. + if (enable_expanding_hatched_road_markings) { + bound_points = getBoundWithHatchedRoadMarkings(bound_points, route_handler); + } + + if (!enable_expanding_intersection_areas) { + return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); } - return motion_utils::removeOverlapPoints(output_points); + // Step4. if there are intersection areas, expand drivable bound with the polygon. + { + bound_points = + getBoundWithIntersectionAreas(bound_points, route_handler, drivable_lanes, is_left); + } + + return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); } void makeBoundLongitudinallyMonotonic( @@ -1915,19 +2002,27 @@ void makeBoundLongitudinallyMonotonic( return bound; } - std::vector ret; + std::vector ret = bound; + auto itr = std::next(ret.begin()); + while (std::next(itr) != ret.end()) { + const auto p1 = *std::prev(itr); + const auto p2 = *itr; + const auto p3 = *std::next(itr); - for (size_t i = 0; i < bound.size(); i++) { - const auto & p_new = bound.at(i); - const auto duplicated_points_itr = std::find_if( - ret.begin(), ret.end(), - [&p_new](const auto & p) { return calcDistance2d(p, p_new) < 0.1; }); + const std::vector vec_1to2 = {p2.x - p1.x, p2.y - p1.y, p2.z - p1.z}; + const std::vector vec_3to2 = {p2.x - p3.x, p2.y - p3.y, p2.z - p3.z}; + const auto product = + std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); - if (duplicated_points_itr != ret.end() && std::next(duplicated_points_itr, 2) == ret.end()) { - ret.erase(duplicated_points_itr, ret.end()); - } + const auto dist_1to2 = tier4_autoware_utils::calcDistance3d(p1, p2); + const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2); - ret.push_back(p_new); + constexpr double epsilon = 1e-3; + if (std::cos(M_PI_4) < product / dist_1to2 / dist_3to2 + epsilon) { + itr = ret.erase(itr); + } else { + itr++; + } } return ret; From 1c2c59d0ab81400b692aa8d9c3b595be0222b9ab Mon Sep 17 00:00:00 2001 From: shulanbushangshu <102840938+shulanbushangshu@users.noreply.github.com> Date: Tue, 3 Oct 2023 13:16:21 +0800 Subject: [PATCH 63/97] feat: add common interface utils test (#5173) * add interface utils test Signed-off-by: jack.song * style(pre-commit): autofix --------- Signed-off-by: jack.song Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../component_interface_utils/CMakeLists.txt | 20 +++++ common/component_interface_utils/package.xml | 1 + .../test/test_component_interface_utils.cpp | 76 +++++++++++++++++++ 3 files changed, 97 insertions(+) mode change 100644 => 100755 common/component_interface_utils/package.xml create mode 100644 common/component_interface_utils/test/test_component_interface_utils.cpp diff --git a/common/component_interface_utils/CMakeLists.txt b/common/component_interface_utils/CMakeLists.txt index d4f6343e381c7..435d5535e0a37 100644 --- a/common/component_interface_utils/CMakeLists.txt +++ b/common/component_interface_utils/CMakeLists.txt @@ -3,4 +3,24 @@ project(component_interface_utils) find_package(autoware_cmake REQUIRED) autoware_package() + +include_directories( + include + SYSTEM + ${rclcpp_INCLUDE_DIRS} + ${rosidl_runtime_cpp_INCLUDE_DIRS} + ${rcl_INCLUDE_DIRS} + ${autoware_adapi_v1_msgs_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_component_interface_utils + test/test_component_interface_utils.cpp + ) + + target_include_directories(test_component_interface_utils + PRIVATE include + ) +endif() + ament_auto_package() diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml old mode 100644 new mode 100755 index a1803a35cc1c9..dc82fda3f5c64 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -19,6 +19,7 @@ rclcpp rclcpp_components + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/component_interface_utils/test/test_component_interface_utils.cpp b/common/component_interface_utils/test/test_component_interface_utils.cpp new file mode 100644 index 0000000000000..3c41f4a85b4f9 --- /dev/null +++ b/common/component_interface_utils/test/test_component_interface_utils.cpp @@ -0,0 +1,76 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_utils/rclcpp/exceptions.hpp" +#include "component_interface_utils/specs.hpp" +#include "component_interface_utils/status.hpp" +#include "gtest/gtest.h" + +TEST(interface, utils) +{ + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + + ResponseStatusCode code = 10; + const std::string message = "test_exception"; + ServiceException service(code, message); + ResponseStatus code_back; + code_back = service.status(); + EXPECT_EQ(code_back.code, code); + EXPECT_EQ(code_back.message, message); + } + + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + + ResponseStatusCode code = 10; + const std::string message = "test_exception"; + ServiceException service(code, message); + ResponseStatus code_set; + service.set(code_set); + EXPECT_EQ(code_set.code, code); + EXPECT_EQ(code_set.message, message); + } + + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + using component_interface_utils::status::copy; + + class status_test + { + public: + status_test(ResponseStatusCode code, const std::string & message, bool success = false) + { + status.code = code; + status.message = message; + status.success = success; + } + ResponseStatus status; + }; + + const status_test status_in(10, "test_exception", true); + auto status_copy = std::make_shared(100, "test_exception_copy", false); + copy(&status_in, status_copy); + + EXPECT_EQ(status_in.status.code, status_copy->status.code); + EXPECT_EQ(status_in.status.message, status_copy->status.message); + EXPECT_EQ(status_in.status.success, status_copy->status.success); + } +} From 9aa2e5a8fdabe62c86dfec5d56de128f5e6ab6aa Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Tue, 3 Oct 2023 17:56:46 +0900 Subject: [PATCH 64/97] feat(goal_planner): secure lateral distance for path planning with corresponding to the centrifugal force (#5196) * add feature to plan the path with considering latral overshoot Signed-off-by: Yuki Takagi * separate a function in "safety_check" in order to that "goal_planner_module" can handle the ego_polygons Signed-off-by: Yuki Takagi * add visualization codes fix the handling miss of sign of value Signed-off-by: Yuki Takagi * refactor codes based on comments in the PR Signed-off-by: Yuki Takagi --------- Signed-off-by: Yuki Takagi --- .../goal_planner/goal_planner_module.hpp | 1 + .../path_safety_checker/safety_check.hpp | 14 +++-- .../goal_planner/goal_planner_module.cpp | 63 ++++++++++++------- .../path_safety_checker/safety_check.cpp | 40 +++++++++--- 4 files changed, 82 insertions(+), 36 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index eb6ea5996cc6f..a82cff16756f8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -106,6 +106,7 @@ struct FreespacePlannerDebugData struct GoalPlannerDebugData { FreespacePlannerDebugData freespace_planner{}; + std::vector ego_polygons_expanded{}; }; class GoalPlannerModule : public SceneModuleInterface diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 9cc4afe22bdce..4983060aa1c0a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -110,6 +110,10 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, const double hysteresis_factor, CollisionCheckDebug & debug); +std::vector generatePolygonsWithStoppingAndInertialMargin( + const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear, + const double width, const double maximum_deceleration, const double max_extra_stopping_margin); + /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. @@ -133,14 +137,12 @@ std::vector getCollidedPolygons( const double hysteresis_factor, CollisionCheckDebug & debug); /** - * @brief Check collision between ego path footprints with extra longitudinal stopping margin and - * objects. + * @brief Check collision between ego polygons with margin and objects. * @return Has collision or not */ -bool checkCollisionWithExtraStoppingMargin( - const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, - const double base_to_front, const double base_to_rear, const double width, - const double maximum_deceleration, const double margin, const double max_stopping_margin); +bool checkCollisionWithMargin( + const std::vector & ego_polygons, const PredictedObjects & dynamic_objects, + const double collision_check_margin); } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 574a1a38fc66a..ce88760212d90 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1296,28 +1296,32 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const } } - if (parameters_->use_object_recognition) { - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); - const auto [pull_over_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); - const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_over_lane_objects, parameters_->th_moving_object_velocity); - const auto common_parameters = planner_data_->parameters; - const double base_link2front = common_parameters.base_link2front; - const double base_link2rear = common_parameters.base_link2rear; - const double vehicle_width = common_parameters.vehicle_width; - if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin( - path, pull_over_lane_stop_objects, base_link2front, base_link2rear, vehicle_width, - parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin, - parameters_->object_recognition_collision_check_max_extra_stopping_margin)) { - return true; - } + if (!parameters_->use_object_recognition) { + return false; } - return false; + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); + const auto [pull_over_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_->th_moving_object_velocity); + const auto common_parameters = planner_data_->parameters; + const double base_link2front = common_parameters.base_link2front; + const double base_link2rear = common_parameters.base_link2rear; + const double vehicle_width = common_parameters.vehicle_width; + + const auto ego_polygons_expanded = + utils::path_safety_checker::generatePolygonsWithStoppingAndInertialMargin( + path, base_link2front, base_link2rear, vehicle_width, parameters_->maximum_deceleration, + parameters_->object_recognition_collision_check_max_extra_stopping_margin); + debug_data_.ego_polygons_expanded = ego_polygons_expanded; + + return utils::path_safety_checker::checkCollisionWithMargin( + ego_polygons_expanded, pull_over_lane_stop_objects, + parameters_->object_recognition_collision_check_margin); } bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const @@ -1686,6 +1690,24 @@ void GoalPlannerModule::setDebugData() add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } + + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST, + tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); + + for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) { + for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) { + const auto & current_point = ego_polygon.outer().at(ep_idx); + const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size()); + + marker.points.push_back( + tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + } + } + debug_marker_.markers.push_back(marker); } // safety check if (parameters_->safety_check_params.enable_safety_check) { @@ -1695,7 +1717,6 @@ void GoalPlannerModule::setDebugData() add(createPredictedPathMarkerArray( ego_predicted_path, vehicle_info_, "ego_predicted_path_goal_planner", 0, 0.0, 0.5, 0.9)); } - if (goal_planner_data_.filtered_objects.objects.size() > 0) { add(createObjectsMarkerArray( goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 4e671f9f4b599..b44a3f841035d 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -364,27 +364,49 @@ std::vector getCollidedPolygons( return collided_polygons; } -bool checkCollisionWithExtraStoppingMargin( - const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, - const double base_to_front, const double base_to_rear, const double width, - const double maximum_deceleration, const double collision_check_margin, - const double max_extra_stopping_margin) + +std::vector generatePolygonsWithStoppingAndInertialMargin( + const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear, + const double width, const double maximum_deceleration, const double max_extra_stopping_margin) { - for (const auto & p : ego_path.points) { + std::vector polygons; + const auto curvatures = motion_utils::calcCurvature(ego_path.points); + + for (size_t i = 0; i < ego_path.points.size(); ++i) { + const auto p = ego_path.points.at(i); + const double extra_stopping_margin = std::min( std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, max_extra_stopping_margin); + double extra_lateral_margin = (-1) * curvatures[i] * p.point.longitudinal_velocity_mps * + std::abs(p.point.longitudinal_velocity_mps); + extra_lateral_margin = + std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin); + + const auto lateral_offset_pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); const auto ego_polygon = tier4_autoware_utils::toFootprint( - p.point.pose, base_to_front + extra_stopping_margin, base_to_rear, width); + lateral_offset_pose, base_to_front + extra_stopping_margin, base_to_rear, + width + std::abs(extra_lateral_margin)); + polygons.push_back(ego_polygon); + } + return polygons; +} +bool checkCollisionWithMargin( + const std::vector & ego_polygons, const PredictedObjects & dynamic_objects, + const double collision_check_margin) +{ + for (const auto & ego_polygon : ego_polygons) { for (const auto & object : dynamic_objects.objects) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); const double distance = boost::geometry::distance(obj_polygon, ego_polygon); - if (distance < collision_check_margin) return true; + if (distance < collision_check_margin) { + return true; + } } } - return false; } } // namespace behavior_path_planner::utils::path_safety_checker From 68544c73bffca2b5acbaab299303f64653b6be8d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Tue, 3 Oct 2023 12:13:21 +0300 Subject: [PATCH 65/97] ci(build-and-test-differential): remove sync for the build-and-test-differential workflows (#5200) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- .github/sync-files.yaml | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 4a1f20cada359..f9cb3065f12a7 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -42,20 +42,6 @@ - \"\" - -cuda include:" {source} - - source: .github/workflows/build-and-test-differential.yaml - pre-commands: | - sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} - - sd -s 'container: ${{ matrix.container }}' 'container: ${{ matrix.container }}${{ matrix.container-suffix }}' {source} - sd -- \ - " include:" \ - " container-suffix: - - \"\" - - -cuda - include:" {source} - - sd "^ container: ghcr.io/autowarefoundation/autoware-universe:(\w+)-latest\$" \ - " container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {source} - source: .github/workflows/build-and-test-differential-self-hosted.yaml pre-commands: | sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} From ac06a5c2e05bc166478d1ff5e0d730471089a41c Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Tue, 3 Oct 2023 09:24:59 +0000 Subject: [PATCH 66/97] chore: sync files (#4372) Signed-off-by: GitHub Co-authored-by: xmfcx --- mkdocs.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/mkdocs.yaml b/mkdocs.yaml index 2ca8b3c86ad43..a72529fbe959a 100644 --- a/mkdocs.yaml +++ b/mkdocs.yaml @@ -55,8 +55,7 @@ plugins: regex: - ^(?!(.*/)?assets/).*\.(?!(.*\.)?md|(.*\.)?svg|(.*\.)?png|(.*\.)?gif|(.*\.)?jpg).*$ - ^(.*/)?[^.]*$ - - macros: - module_name: mkdocs_macros + - macros - mkdocs-video - same-dir - search From 8a0ff1cfaa2c5a8afd47a280bd67db435f799146 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Tue, 3 Oct 2023 19:01:42 +0900 Subject: [PATCH 67/97] revert: "chore: sync files (#4372)" (#5213) Signed-off-by: Ryohsuke Mitsudome --- mkdocs.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mkdocs.yaml b/mkdocs.yaml index a72529fbe959a..2ca8b3c86ad43 100644 --- a/mkdocs.yaml +++ b/mkdocs.yaml @@ -55,7 +55,8 @@ plugins: regex: - ^(?!(.*/)?assets/).*\.(?!(.*\.)?md|(.*\.)?svg|(.*\.)?png|(.*\.)?gif|(.*\.)?jpg).*$ - ^(.*/)?[^.]*$ - - macros + - macros: + module_name: mkdocs_macros - mkdocs-video - same-dir - search From 6155f1ff63b95492f5d62250bda03334efe416c7 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 4 Oct 2023 09:49:56 +0900 Subject: [PATCH 68/97] feat(intersection): aggressively peek into attention area if traffic light does not exist (#5192) Signed-off-by: Mamoru Sobue --- .../README.md | 93 +- .../config/intersection.param.yaml | 3 + .../docs/data-structure.drawio.svg | 771 ++++++++++++++++ .../intersection-attention-ll-rr.drawio.svg | 134 ++- .../intersection-attention-lr-rl.drawio.svg | 832 ++++++++++++++---- ...intersection-attention-straight.drawio.svg | 152 +++- .../docs/intersection-attention.drawio.svg | 348 ++++++-- .../docs/occlusion-without-tl.drawio.svg | 518 +++++++++++ .../docs/stuck-vehicle.drawio.svg | 42 +- .../package.xml | 4 - .../src/debug.cpp | 12 +- .../src/manager.cpp | 20 +- .../src/scene_intersection.cpp | 337 ++++--- .../src/scene_intersection.hpp | 45 +- .../src/util.cpp | 2 +- .../src/util.hpp | 8 - .../src/util_type.hpp | 1 + 17 files changed, 2803 insertions(+), 519 deletions(-) create mode 100644 planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg create mode 100644 planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index e8a823a7e790e..16459bb64074b 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -24,7 +24,7 @@ This module is activated when the path contains the lanes with `turn_direction` ### Attention area -The `Attention Area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority. +The `attention area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `common.attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority(`yield lane`). `Intersection Area`, which is supposed to be defined on the HDMap, is an area converting the entire intersection. @@ -54,10 +54,10 @@ For [stuck vehicle detection](#stuck-vehicle-detection) and [collision detection Objects that satisfy all of the following conditions are considered as target objects (possible collision objects): -- The center of mass of the object is **within a certain distance** from the attention lane (threshold = `attention_area_margin`) . +- The center of mass of the object is **within a certain distance** from the attention lane (threshold = `common.attention_area_margin`) . - (Optional condition) The center of gravity is in the **intersection area**. - To deal with objects that is in the area not covered by the lanelets in the intersection. -- The posture of object is **the same direction as the attention lane** (threshold = `attention_area_angle_threshold`). +- The posture of object is **the same direction as the attention lane** (threshold = `common.attention_area_angle_threshold`). - Not being **in the adjacent lanes of the ego vehicle**. #### Stuck Vehicle Detection @@ -68,31 +68,31 @@ If there is any object on the path in inside the intersection and at the exit of #### Collision detection -The following process is performed for the targets objects to determine whether the ego vehicle can pass the intersection safely. If it is judged that the ego vehicle cannot pass through the intersection with enough _margin_, it will insert a stopline on the _path_. +The following process is performed for the targets objects to determine whether the ego vehicle can pass the intersection safely. If it is judged that the ego vehicle cannot pass through the intersection with enough margin, this module inserts a stopline on the path. 1. calculate the time interval that the ego vehicle is in the intersection. This time is set as $t_s$ ~ $t_e$ -2. extract the predicted path of the target object whose confidence is greater than `min_predicted_path_confidence`. +2. extract the predicted path of the target object whose confidence is greater than `collision_detection.min_predicted_path_confidence`. 3. detect collision between the extracted predicted path and ego's predicted path in the following process. 1. obtain the passing area of the ego vehicle $A_{ego}$ in $t_s$ ~ $t_e$. - 2. calculate the passing area of the target object $A_{target}$ at $t_s$ - `collision_start_margin_time` ~ $t_e$ + `collision_end_margin_time` for each predicted path (\*1). + 2. calculate the passing area of the target object $A_{target}$ at $t_s$ - `collision_detection.collision_start_margin_time` ~ $t_e$ + `collision_detection.collision_end_margin_time` for each predicted path (\*1). 3. check if $A_{ego}$ and $A_{target}$ polygons are overlapped (has collision). 4. when a collision is detected, the module inserts a stopline. 5. If ego is over the `pass_judge_line`, collision checking is not processed to avoid sudden braking and/or unnecessary stop in the middle of the intersection. -The parameters `collision_start_margin_time` and `collision_end_margin_time` can be interpreted as follows: +The parameters `collision_detection.collision_start_margin_time` and `collision_detection.collision_end_margin_time` can be interpreted as follows: -- If the ego vehicle was to pass through the intersection earlier than the target object, collision would be detected if the time difference between the two was less than `collision_start_margin_time`. -- If the ego vehicle was to pass through the intersection later than the target object, collision would be detected if the time difference between the two was less than `collision_end_margin_time`. +- If the ego vehicle was to pass through the intersection earlier than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_start_margin_time`. +- If the ego vehicle was to pass through the intersection later than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_end_margin_time`. -If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless _safe_ judgement continues for a certain period to prevent the chattering of decisions. +If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless safe judgement continues for a certain period `collision_detection.state_transit_margin` to prevent the chattering of decisions. #### Stop Line Automatic Generation -If a stopline is associated with the intersection lane, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention lane is defined as the position of the stop line for the vehicle front. +If a stopline is associated with the intersection lane on the map, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`common.path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention area is defined as the position of the stop line for the vehicle front. #### Pass Judge Line -To avoid a rapid braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) are needed to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position. +To avoid sudden braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) is required to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position. ### Occlusion detection @@ -104,6 +104,66 @@ The occlusion is detected as the common area of occlusion attention area(which i If the nearest occlusion cell value is below the threshold, occlusion is detected. It is expected that the occlusion gets cleared as the vehicle approaches the occlusion peeking stop line. +In there are no traffic lights associated with the lane, the ego vehicle will make a brief stop at the _default stop line_ and the position where the vehicle heading touches the attention area for the first time(which is denoted as _first attention stop line_). After stopping at the _first attention area stop line_ this module inserts `occlusion.absence_traffic_light.creep_velocity` velocity between ego and the position which is `occlusion.absence_traffic_light.maximum_peeking_distance` ahead of _first attention area stop line_ while occlusion is not cleared. If collision is detected, ego will instantly stop. Once the occlusion is cleared or ego passed `occlusion.absence_traffic_light.maximum_peeking_distance` this module does not detect collision and occlusion because ego vehicle is already inside the intersection. + +![occlusion_detection](./docs/occlusion-without-tl.drawio.svg) + +### Data Structure + +#### `IntersectionLanelets` + +```plantuml +@startuml +entity IntersectionLanelets { + * conflicting lanes/area + -- + * first conflicting area + The conflicting lane area which the path intersects first + -- + * attention lanes/area + -- + * first attention lane area + The attention lane area which the path intersects first + -- + * occlusion attention lanes/area + Part of attention lanes/area for occlusion detection + -- + * is_priortized: bool + If ego vehicle has priority in current traffic light context +} +@enduml +``` + +#### `IntersectionStopLines` + +Each stop lines are generated from interpolated path points to obtain precise positions. + +```plantuml +@startuml +entity IntersectionStopLines { + * closest_idx: size_t + closest path point index for ego vehicle + -- + * stuck_stop_line: size_t + stop line index on stuck vehicle detection + -- + * default_stop_line: size_t + If defined on the map, its index on the path. Otherwise generated before first_attention_stop_line + -- + * first_attention_stop_line + The index of the first path point which is inside the attention area + -- + * occlusion_peeking_stop_line + The index of the path point for the peeking limit position + -- + * pass_judge_line + The index of the path point which is before first_attention_stop_line/occlusion_peeking_stop_line by braking distance +} +@enduml +``` + +![data structure](./docs/data-structure.drawio.svg) + ### Module Parameters | Parameter | Type | Description | @@ -115,7 +175,6 @@ If the nearest occlusion cell value is below the threshold, occlusion is detecte | `common.intersection_velocity` | double | [m/s] velocity profile for pass judge calculation | | `common.intersection_max_accel` | double | [m/s^2] acceleration profile for pass judge calculation | | `common.stop_overshoot_margin` | double | [m] margin for the overshoot from stopline | -| `common.path_interpolation_ds` | double | [m] path interpolation interval | | `stuck_vehicle.stuck_vehicle_detect_dist` | double | [m] length toward from the exit of intersection for stuck vehicle detection | | `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threshold for stuck vehicle detection | | `collision_detection.state_transit_margin_time` | double | [m] time margin to change state | @@ -128,7 +187,13 @@ If the nearest occlusion cell value is below the threshold, occlusion is detecte | `occlusion.peeking_offset` | double | [m] the offset of the front of the vehicle into the attention area for peeking to occlusion | | `occlusion.min_vehicle_brake_for_rss` | double | [m/s] assumed minimum brake of the vehicle running from behind the occlusion | | `occlusion.max_vehicle_velocity_for_rss` | double | [m/s] assumed maximum velocity of the vehicle running from behind the occlusion | -| `occlusion.denoise_kernel` | double | [m] the window size of morphology process for clearing noisy occlusion | + +#### For developers only + +| Parameter | Type | Description | +| ------------------------------ | ------ | ---------------------------------------------------------------------- | +| `common.path_interpolation_ds` | double | [m] path interpolation interval | +| `occlusion.denoise_kernel` | double | [m] the window size of morphology process for clearing noisy occlusion | ### How to turn parameters diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 4042adeb3623e..5e74e837bf186 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -54,6 +54,9 @@ ignore_parked_vehicle_speed_threshold: 0.8333 # == 3.0km/h stop_release_margin_time: 1.5 # [s] temporal_stop_before_attention_area: false + absence_traffic_light: + creep_velocity: 1.388 # [m/s] + maximum_peeking_distance: 6.0 # [m] enable_rtc: intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg new file mode 100644 index 0000000000000..fb512902ef856 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg @@ -0,0 +1,771 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + closest_idx + +
+
+
+ + closest_idx + + + + + + + + +
+
+
+ + stuck_stop_line + +
+
+
+ + stuck_stop_line + + + + + + + + + + + +
+
+
+ + default_stop_line + +
+
+
+ + default_stop_line + + + + + + + + +
+
+
+ + first_attention_stop_line + +
+
+
+ + first_attention_stop_line + + + + + + + + +
+
+
+ + + occlusion_peeking +
+ _stop_line + + +
+
+
+ + occlusion_peeking... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + next + + +
+
+
+ + next + + + + + + + + +
+
+
+ + + prev + + +
+
+
+ + prev + + + + + + + + +
+
+
+ + + ego_or_entry2exit + + +
+
+
+ + ego_or_entry2exit + + + + + + + + +
+
+
+ + + entry2ego + + +
+
+
+ + entry2ego + + + + + + + +
+
+
+ + IntersectionStopLines + +
+
+
+ + IntersectionStopLines + + + + + +
+
+
+ + PathLanelets + +
+
+
+ + PathLanelets + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg index d4d709eee6b99..51e926fe266d6 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg @@ -8,12 +8,48 @@ width="3723px" height="2401px" viewBox="-0.5 -0.5 3723 2401" - content="<mxfile host="Electron" modified="2023-06-06T10:28:31.375Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="M_P_fA_OZlKNZHzEOQxb" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + content="<mxfile host="Electron" modified="2023-10-03T02:44:21.616Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="wkqIuFIE5UyEHdVd8sjR" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > + + + + + + + + + - + @@ -51,7 +87,7 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - + yield lane - - + + @@ -749,10 +785,10 @@ - - + + - + - attention area + attention lane - attention area + attention lane @@ -1141,12 +1177,12 @@

- attention area + attention lane @@ -1226,9 +1262,9 @@ stroke-miterlimit="10" pointer-events="none" /> - + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg index 2d590b9fd51ca..25a7c6b519f96 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg @@ -5,10 +5,10 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="3729px" + width="3727px" height="2651px" - viewBox="-0.5 -0.5 3729 2651" - content="<mxfile host="Electron" modified="2023-06-06T10:27:24.636Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="EZYouzMJcjefpLDW8VVp" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + viewBox="-0.5 -0.5 3727 2651" + content="<mxfile host="Electron" modified="2023-10-03T03:32:56.314Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="aYRnTHIqVcSkLQHKGcXP" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > @@ -25,13 +25,13 @@ transform="rotate(-130,954.7,670.3)" pointer-events="none" /> - - - + + + - + - - - + + + @@ -50,8 +50,7 @@ - - + @@ -88,9 +87,9 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - + - + - + - + - - - + + + - + - + - + - - - - - + + + + + - + - + - - - + + + - + - + - + - - + + - + - + - + - + @@ -436,8 +435,8 @@ ego lane - - + + @@ -474,16 +473,16 @@
- attention area + attention lane
- attention area + attention lane - - + + @@ -548,12 +547,12 @@ transform="rotate(225,2890.14,695.64)" pointer-events="none" /> - + - - + + - attention area + attention lane - attention area + attention lane - - + + - - - + + + - - + + - + @@ -753,13 +752,13 @@ transform="rotate(-130,954.7,1832.3)" pointer-events="none" /> - - - + + + - + - - - + + + @@ -779,7 +778,7 @@ - + - + - + - + - + - + - + - - - + + + - - - - - - - - + - - + + - - + + - + - + - - - + + + - + - + - + - - + + - + - + - + - + - + @@ -1150,8 +1118,8 @@ ego lane - - + + @@ -1186,16 +1154,16 @@
- attention area + attention lane
- attention area + attention lane - - + + @@ -1224,12 +1192,12 @@ transform="rotate(225,2890.14,1857.64)" pointer-events="none" /> - + - - + + - attention area + attention lane - attention area + attention lane - - - - - + + + + + - - + + - + @@ -1376,6 +1344,494 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg index d33674a257d9b..640eba618fa49 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg @@ -5,41 +5,42 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="3389px" - height="1411px" - viewBox="-0.5 -0.5 3389 1411" - content="<mxfile host="Electron" modified="2023-06-06T10:31:30.247Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="U1JUqxOsfTr30viw8iHe" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + width="3390px" + height="1412px" + viewBox="-0.5 -0.5 3390 1412" + content="<mxfile host="Electron" modified="2023-10-03T04:48:10.725Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="02kGXKyoYgRU_RdR3ghD" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > + - + + - - + - + - + @@ -390,7 +391,7 @@ - + - + - + - + - + - + - - - + + + - + attention area - - - - + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg index 3e9826fd84ad6..57552f586e63b 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg @@ -6,9 +6,9 @@ xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="3707px" - height="2193px" - viewBox="-0.5 -0.5 3707 2193" - content="<mxfile host="Electron" modified="2023-06-06T08:17:01.045Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="0BpzzxhIQKLXKpqq79kr" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + height="2195px" + viewBox="-0.5 -0.5 3707 2195" + content="<mxfile host="Electron" modified="2023-10-03T05:15:42.124Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="WBrLRHcQvF5FOJ4jcZAE" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">7V1bd+M2kv41Pmf2QTy4Xx7bfcl9JiedTif7kiNbsluJ2vLI6nT3/voFJYEiAQgEKYCEbCvZnViiQBH11YeqQqHqAr/8+OWb9fT+w0+r2Xx5gcDsywV+dYEQwoDiAqn/Kt/8unsTIsBgIXZv3q4Xs/3bhzfeLv5vvn8T7N/9tJjNHxoXblar5WZx33zzenV3N7/eNN6brterz83LblbL5l3vp7dz642319Olflc/RPn++8Vs82H3vkD88P6388XtB31vyOTuk49TffH+WR4+TGerz7W38OsL/HK9Wm12//Xxy8v5spxFPTO/vfyGfP3vu9mvL2b3b968fPXH/377ZrIb7E2Xr1SPtp7fbXoPffntX3fk6k9y+Zlfvpfz3ybo328mYv9s/0yXn/ZTtn/YzVc9h+vVp7vZvBwFXuDLzx8Wm/nb++l1+elnBR713ofNx+X+4+X0ar68nF7/fbv92svVcrXeDoPfbF/qkofNevX3vPYJ2L7UJzeru80eQ5ju/65d94rLy911i+VSv3+3ulPXX06Xi9s79ee1mqK5ev8ycM72c/vPfL2Zf6khaT+H38xXH+eb9Vd1yf7TCSQIk4LuvrdXjQmHYD/S5wPIsJ7dDzWAIf3mdA/t2+oWB+Gp/9jLr4MsteieZRkuSwYpMCWpuWsQSX78ZTX95urtHz/8yT7PFr8SxjcPEynbJVnKpJTXerWZbharcr4mEpw+WRUXT6/0rUDLJFLAaSEPL9GcUcoxVB8zCrEiVwSxNb0UkAJhjCjDmHIuqT3ZBMhC0MMYACeaeyHa576ctIVaZH4sFeTn1cNiL4Cr1Waz+qhkoC94sUfyZmWollpL7svBPn65Ldff4mr6sLgulNCUjtxdv14u1QI53+qXeuvF3e32zqAQDHEBMScAYqn+T10xv5sdPlfYxYwreUjOGSWyVKb1tV73QMG4oXF1TawjiQBLuffKuVJ0sdiUsqVutPkRHaybgjRR5NBKKKANFMhlKmQEaOWBX0E7vwYIYnQxQN4uBySwLQeWjBw7mSzdxXC5/cfFrCGCUOo8W8wPS93+qmTymRjyUUuVvXpxMaR8YLt8StIqrfvDBNXk0RSeZWFUarL7RJMbsWUx/7LY/L4ftPzvP7YcSPd/vfqyv8P2j6/6jzs1B7/X/6h9q/zz8LXtX4fvzd4syol61UMdH1af1tfzAOpRq8Ht3DeivnA+a/hFNnwaq6+NDv3eer5UOvBP08dyQWZ/h59XC/XEFToJRQ10YtIcYffg+y/VfRdzHNAcRxluzYF2E2MNtAVw9dQnYDrAtK5h+no5fVDreWJYgwIx1IA2EjwNuNUgP8/XCzWNpT2eBuIiEOKa/zOBuHIHG9CEoifGpTGOqSupIY7bIX79af1P5TwOy+GgE4cPwcW54E/buxo3tB/8IOSNcUwz+gj8FASmX2uX3ZcXPPjUxflzj/4q1LwctVxuPASWhqrsfm5UxREBiuNw1s3Yh60Vrk8M7bhRfuK3e5WzcZ7c7yfAcA4oY/uFuQ5+Rmz0M1lID9ZPc9RomtW6TlPBIgqkH42iTFgFF4gQIjigjDJO9eKg1aonx1CG63GipmrjYU06DdQQtS3V7DetszWVq3uIUIwUfCPAH3wjtkZCQFwee0KVZLx9tmPHTiaY6UB07crdy6XBlePfT4MrQAX77gTYsRQ2uGQC4p2DkWVfB6WjiFp9DA3XTNgYAmzYNT0J2GAHc5jElAtB8L7G4DRKJG1sUhjxBsokK1mWYgmAIlk7yEalQ/6YkUKI6ltcGGtcNC2GIMAKfVTbFs4wbextiwNig0kdswIf9qlMIGFe2+gSgOz9mRF3NaC24YeMp48vJoiUnJgtG0scRNmsTgcmVfwcggD35Vw3OHpIalISM8dEMIEBVxwKmmJDkBaUHjhW2jJEWBR67R9IhgHexSkBtEqGiQJo2gxTax43TDFSvRG0FbL9rGfwuEJLa1CuUppW0666MhPbjgPfekHKDLb6gtHP7vPfBCr1aK5KA5uFAc5htNBzB80ZciswDdTJGUEdEuBFYXAgG3lvI0wLLAjrXcPc/kcVzidNGqWGIJGfP/yiBJPEBrooIDlTBSRYNmHZNKRMVPYMMjC/mptrTbIYhDNJNnizZvAIBGPKdaxNTWPWJlw5xwU5xCf2wKpn+cjG1yF2BBKxYNGzKP/93Wr2hfzzYUEu362/+5v/ya++ncCnFo5wBqDVBLt4csblVdhWkRfCXUISTWA0d+0gB8WQYQg3YBJHIZzycTi9I4hnwlDBGeMEM6bsfVHtoWr5AJd8EHPIJ4ZD6xZP4piEW31EFvIpQ0UuYtXLppAu8XDHMp5OPInDDWlzLrsJrm6feakkE6OLe9EDedHXzPKjkloDx7Os3NM+pBffAZAt3rgPapkgqLOkw11l78jYBmck99j/RIf7xnKJ3YBN7xF3gWmsnW8foOseq3edzwT53QEaCn01pFepcEL6nP786vL3P8D9j8vJh3e/bNYvrz7KEMf0FANLMo5fsDADq7x2ykIMrGjHMKWxFzdhXNp5S0SvMQmSY96hf14yPlvBW0L/pr9/+Iku3qcWyvGEz8rGrYlLBtm88UQCScFMqQhKC2xvsDGdvF4XDIeFT+NDBXPF7pffff/7jzOymixevf/++//wVxOMnrJklMdhHsvbSsaRTaAkU0DH1mck4TifqVv6rW37NRdL3HkyM1m8lEWMMTDF1HcLr5Q7MkbjkBkBvEj2metm6qfjqEaZU4SnOrKPGTtEoHjYYZAZvyoldsg+mpgUOwEO6NPADoPQ2D5QytvTUFajUWRuRkBmsFg87FDErZ9O0mOnmy8Ygp3Z9OFDFeuw90rflP+cMcSYJqMDoSiXivcGmT0eS7c16J7iAGv/qfCHg8Bxb9FSZLoRCdce+2Zq7SnoABSCA7Lb2+JJDdIo8fB2/93VevNhdbu6my5fH969nC+vtmPpnVKLafYgbbpCjweVDBs7KuGopEJYoyFjtHiotH86ZqD7LzS+dDKOnZGoxGVuXOnHMtFBrVguN8eWKLjj8BYEEBTUkXfBUWFaTNFiVIkjIecpLl2NLz9xxXdSHruhaVboY4wY43SwRaC0Rwtj/R706gRAyD7po470m5FLpt+pR/r1pKSI9K/Ez/9Mv75/dfvy9b9f/Rf+NpncBuUfjZUYKC1vvtouqbMZK8/B2LOmWK4AGEqk/hWY6/pg8WcwdilZA9dX02sxcy4wCBNCZ07Y+mXdjltrfoF+QUf6FS1nOtX0hpRVqzIeW6Z2+nC/q7Z8s/hSisOcawDE1L39AQDlwLH832xfSWRQ7QUXhFFCy4oGWNnqTTdPFhAiYtWWqOfHokKAMr2OEwShDmY302Pdl8SXZYDNFkeWe/vBtgqApC4xVuV7k4mR4QIiCSmAlEConPMzFmNIwCitGFGt2vKw2ghJQcskYUGFUk0E4XnI0WWR0dHVkW1fOcgRNcTIYAEFqhY9aO8vZyRG0ilwcSYGSFUBhBWSVEYcJM1NEmqYeMAWFOGFdOQBUJBIGnRccpzNb6aflpsMVAoaDg+VBcMHnWIoZ52iAbkc+hTOzXL+ZR+3uOyWN6mfnRcISAzU01EqoPJOdyPpNgKFBMoCVEYD4hIwyXcH0Wc6FgIVWUFlU1ClAIpShaDdpZxJ2EM9RCEROLyatFyW2++5HQNJ15FTp40H0MTQx1KHKBhtA9MXR6gfevAF7esJvr7rMsH5BCo1tTYHOeqH7XI0K4+0MmkGwnOILfmM5zqefbsadTz7rssHz1ySQuoOJrXcCF4QTgx3qAfAncNzBfwCmBk+iYHeLVtioJLofYB+cj2BUKB7M5nrSPdemBHUETFxCPtniXBoohqYu/uJER2wBfCM6Hbqdpw18l2XEZ4BLBizuJWKguODwdw/j9I5vloaUAGS1T52Aj1RfWzXJuQjsVF8R7Zabe7cgF6i2QAhx72Jm1u5TRyBYYm726GBYfB8hsStMxNamRvmVR55UkaEmuc3J4zI/kSNkDkaJQMfpg8Ii4yUjqC0G5mzI5G/wWNYbSIevTaR22UMSFR4VLWJXCl81Ka50BK8fsB26Cxo9H/g9bJhlfgHKUnkxklAKCdJZuW4YjHaciDUKpdkpYjcYkl8KPuUetXBxZEjCWtbYcxUIse5eeBYfJMVIzr1KNWo4c+OomqPY+YVtcGGrY7LnMraym024Qk1mszDqhD5x01sPgVkAoxSUNhfisiLtFwA1FHQwfVYeHPcFlxGOpljPg2Pe7zGLdAA83O4HaJokRQveNu3cfKKkXREY/jmu6E8ZFSWxPELdTzysyicWmWKdPJgn3iwORpXMBgWAfHLbTxuBHCMzfIE1vmhLukK3BoND4yAp34ayVJoHdwcqO6YWyqJz+BmLxXzjBilDqnoSRlKKiS95fbY9hm86G6tRqkvzIb+XfsM0DhQd9I+A2HGaKlTHnLeZ5BlR0YBuPq3KnR2MJU4qX2K7KCXerdgorpAF8xrFsFrXMKMPZ54ZSRHKKkQ3tHNFq0fKuF9B2RZQfIwvUbaGqSg4Lz62MHwmMqCO2LLPFVsOYThx9sa4gILyhmhBAEMibU1hDgq++RxLDHlkppbQ0gEIkcR5tjQgYAWBBCAJcBUQLM4FASkoEDqj6GwT5BTNYJu09pcC3jBeCr8BNBpdEXftbkeVVwTI7BBCSwotPVZsoIk0md3MvapEY5H0vHKK+b2hD6Hvea9MBN7jVJWNxFE09eGUBYCHZafnjnZtGmHGDchjQ+pNIgn9VmEgPDOGPsgWhcKBGhDH6CQfo1Qf5i9RPtqSa/dmGCd0YTYqjMkrxAXFdIDZ8xxFJ3BvpsQ5L1JpK0fSry/Qbh+w7HnIdQ3FnVOWtI9JtStylSee0z9MoO1UsMUSh26nZWZUmPkwyejUZSa+3WA9dDpaFEOlFwdHlvCu3ezoD1wl1uCMMda5IedFgEOJ+n6VJKzClU7R4wH7ZefXr+7//x68oG+/OvPn7+8ff3+fy8nroAFq+oeNCDO/vtppT+YPGzP1L9QF2B6/2ULFf25+q/b/f9uB7rSb7xbX03vylDeevXwsF5NZw9bCSoBKkispzc3SmcQWG7Ft/+ueqYra7x1wDv6jX/9OL/ZTD5M72aHm/zP8e8F3s85L1fT679vt2o+ud4pdjk9i7vFZrGtrHt8hk6YanBdccjhTcyuxfzqxr7h/HZVzvBUmeaHB97dvfpF049lDOHu6mEbSpiWMrpfL1brkoIQWCmnvpzJD3Pz0o6TmvsU7retjqO6esyvi/lytp/Wh+PzGk2pGrN+q1jtTgvkmBb5NGZHjau7Us57Lfm0vpvMFuv59TZCdBB/0fJwLjkfufThfssE7fMAQsmlG/zUe7ufoN82VvPN/MumuX5P99HYpeKTCztI+3Exm21Laq/n6tfvdynKJXTvR6hR6eUFfVWO9Gmz2j2hI8dh7zY76tN0W4s7lbulsJmzPeHEEX2ryls3Tl3ICLE358qUuGOsMxTacQv7oCTqhvOHTVIpUWIXJXbErWHV2zvBtrZTUPFbX3SbxGyMRJeEzFkPNxCJlMZoHJrVtqIVmLdvpn563GRWJ3a6tTx43NgxHQyiS3v1wo45GgxMDOuFHeunRw5SObHTMRH6EYOHQGEmgpxCPFRayRLpiIda4BmEeILKIT9nrtYEJZBhJDJIcP+GTY7xmK5rPFDsI6iM8hMhEGv5J33juFudNkeDZtn4mARi/3QRu2GTGz8RSlg9N2zqhEohe2dLYkmt0UQyVNo/ne27IHb6hcaXEuE4oHBVlGbI/oPq9ebIozraSMrCytMVBBSuLFFXdd1I/X/cwkocFzk7YSFYWGnQAisJuoTlSuJNKazn81EdKZ8jmwJ7l5YiVWWJ2mgD25hncD5qWIUFwCqOhF2Hc1wlJxJGMQdIeelevTHvPX6vMVHf4/demA/3SGruyMMTnCAJzNEQSJa24oZ0t8j8M6RbTK52SNPcIC0YLaQ060bXIjgkSlnplvtwSGnK+tJOYeBuEb7nSur+fdhW8OeGfWVVWFtaZoPmDu2eLYeeQ5EsG98N6G7hyufjwy0meTud55aFSDAxD7YTfbKtj3ckrWPy2BgtMaRD2gM9+8cN/5iaMuPmxm4HBFijqZU6WQ0hNwKeIySdEwXMbr4M9d+Fk8QebWAEPEdIrAiJpeOOqqkEOdrxpYyQ0AB38qkJygxlMeionjp0KEsHLZ4NxRM9nz3g210fkdki4TIU+zcecRiKhA7rzLP00dnHdgLLu9i2Qzo334diMyuNMcZPOYFFoLRsH9eIiaEtAth6V0LJ3pM9cW3rUVWJcEBrrRi4OYOUkYL4WjlQ6EBJit4NzskOKbDy6Hs3lGVkwwq+BJlTflQH21OkGV/DQ/ZvcGMlIN42VqOAIQUDcatksBZC7AYBbsGM0MEhvBzakR4OQ0psYlS7Q8jeYK+KJg0iMh4hVbIp0vKgoG7sbW9AYktYdbnGKjDRKBZxqErDgWhYhqXGdK1K0xEbrYafq/SEl/UyMfyQ0WoOmiWQgjtMGCXZ05VScitAQI5l6lJK2KE2R1Wh4BA11aFcodsUQv1l1Vc6VGvCzWpNiHXXi7aqS0NpSV5hYdbENu+pIxAaymYGliPlR5vdEbihcQlynPk4lfxCVK6mH8zQD9RDP2KFzobSpLw6dxiapPzdSKo08HojAwyufKIKFHGrefe2LmKEnpAIVoOYdBbNvMUBq/ujDytMBggrVLDu0CbSqBqt/x4vsBDSNye2/+raRxtfOM1tBSRt0STrDemWTEA2wmgtO6MHFnook5EEB7V3NlpgQac/5WvXjbNL5BV4q/Wm9aDVetMXZmK9IWCQvVmpPbidIG0OhOCw5pvo5q5ETF5OhuxGrCwXnIcmN+O8sptNnGPQE+fMGGdgL0UEbOvnEBU7JYeljKTRJswRQS1Ad0fS+sS+EnB+Xge3EDGoum+EGBn9ZbDp+8dqGguN+6AB4l8ywENNs2dW6lbtwt3Lq5/jnieHdlleO6dPGb6yoAMf/ZcRjII+a/cIW2vdBNzKbRr9rdwm8+I254F4hbF+BKdGY9aZqKqw9VCByRFc7LMkIkhMUTHhCF2NQkSjtYfotIXZjXL6MGNsmgo1wWRebndJLGalDQ7ICTQFrdFkMppyd0AJsJdGamyp7BFYgENzO4MlhBSFlFVvO2RzBqegkKL6vi7+XAeGwOqS2sHwCKHwy2//uiNXf5LLz/zyvZz/NkH/PkxL91n2S+3kaYYFxGqiBeZqAWaQATyfODahzKl09HlOMJXOnjEh5VBPWVjZS/F6e3CutTBUdWVr+0AvJsIXS6qkxahkQJbtdLnhX6mVtODUpxBYkEK/PUif0JDqoyP2CaUX5hYhCuoG6oLIhNjNQEfAiNEQFNFm33ajISjSJ3rGbggaUqI0tlIfO3M3tMiUIOrLnGESBEgMCwcdp+sVGqDT0WKZlSw6NEuE0miWKIdLL2oLXfoQ09pWTSOjta2a1qZMzGalZw2A63rvOpC57QporVudw5rYfxfQvEuioGfZrNvzK6B0/oq0TQ4DuPVEL7a7mg6xW9zW5PA0ZUTnqYwKeU2ANvHJ/FoSqoyEeO8Ce+liLIXQjmCqXIqhtEGvdAeNKPNWemy4xVaN0J6+ekHLRDUgkg3QGrWROQIx1iloqIZxF+xfDeOpxv3b//w+f/Hd++9/+O5vePlu9e3tPz852yRGNMODm1F1ygxLZpjryJ6mLYEKV4ESAAvpcJgUlZqFMPsY4U45pV/Sz7NvcShv+cBfpy3fdZmwllSEQWAVFTK6jSgQFxQC/SnjPfsKUIbLypZ6HNzpJolJa4xjsieTVn2T7a5sgpEyKGTIizgL/BeODGQMcWHmmUQjsYCth+FJrKvD3ytlqj+JZUI6ClIFqMX6jR7PBBlbssFOvH9cjrmCI6heRvmMeDzjFEHAbrA+auTYQh//9BGjxCwtw4Wd2K6uK6hjlwCC0vyBEql/BeZmp6Noe2Wdyq/BznR+Nb0WM2c+hFrGCJ0FicqLj2BeRk232MHKO1lUeIcOiqawiFFGxvlEIY5ytb3SIobpw71Cq/rjZvGlFJ2r968lEmVycOBIarnZvgYUldm9T8cQ6mF4VAiASvOHIKjsA9c5Pfcl8bebQ1pGJhXcJZDUJbgdjscTnK7xkqncQmq5R5GbEsWbN0cTw9BeSJlpnXCcjs1IeCFOeVKlY9tXBnIj4Jzk1sl/PGuDQx9p0OY0seRClIHtqLdr9sWNN/khTmA6pZnNb6aflpscVKa57Z+5ygRk6OusnJvl/Mveh7/s5s7rZ+cFAhID9XSUCogl242kc2MLCQij5VYrl4BJzncfa2cfwgJCAiFVHouiSCFoZylm4nTTZhFK0dPHNoaB0BgnsRft7BXItjp41UAQ+++n1S73vPZft/v/rXR26whrtT1cuE16VwpsfXd+u1JfWU4VGexHUU+xG6g5uHr76vDeKevBssxsu5xe/327/VrDBCtfrqWhfG0/qWeBU5unqsZUDqqzQR4raZ8Bq+0r1pkO9SPy2BFGiHFE3g2s9Nn6oVKprusTHmzGKt1sFZp+79XATGhN/R4omAGnaj3snlXPODCcdYapMVpqjnPFsOJzHL8BwMVxXxfz5ewcWI6/AeCsWE5SO146MMulb3pQScVmohY5Vd/sw3uxWS63ZgUM2n2ouGGhd2A5Yuz0lk05jHSDxCyHXFHGbiw33WyUtEv3ymKrEWnpQDdZ0hI0++ZwgB20lKqilBsLIZHLJD1YA2ip+mYGtITyyh5RcEJWoy3Zt7BqucdokZyAwxpfyBXPazem9jsTRw2xio+uV3c3y8X1ZnF3u2etBzdtHTG8DK1Qer9p6oEzq8RBMdP9gahrhcwyl9M6KfVxMZstj8XNmtp2hCb1zQP1KyXpoWZMuTwq69rLcXXdSEZ6AVvVWWcEEGY176aOWR07IwAF2Lx5RugrhDyenAA8+t5yqqSA7sI6r6yAoE7hZ5kVcLLk8k4LCOqIfb5pASdLL+/NMhywv3yeeQHdBXdWiQEh1c4fi9mRX2ZASPXwc8wM6KE055QaEFJa/DGkBqC8wsuPIzcAR0uxn4zkUXPArLqa4JgPZ6NhKI8aZ5Vkf0xYfoyER49Mn9pRXnBkn/qsEu0Ti8tc7hwFb/JZ7sjo0ZAuPvWwksN5S+78cu2HFV+Vm5up/AYLisRwqxOLzuwZn7nossq4TywaYeyH6mcd0bUmZ5R0n1pxmpHE3BXnieTd47wS76GRIQF7FtEwx6mOjA3kXpOA2IyjWea+2IUtu+QH1pkwU1MIs6Nfx51pOIgzTQKiFgMuZ8ek5YdEO2N+bepUPt4zCQhmRLPkARDTWhZaXxc6lZCqUmbNjRaa9cY0GT380cmJTiw7fk6io+PHP7o40KnVTpyT6EZPB+nkQA8rOp615AJCH4/A4NCf2rk5QzvMdNwUjm4O87CKkjfFBQQ6HoO/TPI60UmbFkzviu7+YRI7yzR4L3pwv5hIpUAECMDVvwpPzT4wEw4pKwCvHDKbPzlxCL+sb4xoVXxPRFBKZ1MvFmB3pOmMF1KuM6j5nR8w4RvTWBbb6MQuQMGMMp2QkEIcBCKpnUdQVovmDnc7RvcPt/ACTI+8OvpwxDERHEtMuQxq7+MCkKO7z/j4MZr9YGNphkCUNKDL8lJiG7Wpm/24MTRGwV7McmQAhcnC2HalSuuBw/0QqND+5CCazhN3SRiv86UiCkAumi2C5EXXFkEdMdHaFFMza2tTTL1+ZmLsQWW41mwRYtRMEKRuioC+B94hM+7SuAn23yRxq18REAOL1hErgtr0LXGt1IY11AZ1Vppe/bESqBDOS4WoD9yEGujup0GY+G5CTRUK0qDO3bF6/IZYvbHcqpu+bMEJCpt7r6xwxcTnqZhY+vDKYBTFJH6lYH0UM1oQJPiE+/BBEM6MpDcmHaFiqOaPOjoK+BIFzP4l0WxqZ10w53Se5IYcO2EUPsvmBA6TXeGes/EP/weJwi/wdo9wlHwK54921nUyF8WI+RQveuZTDCKWKqGiIIwSSjlhmDHjRIksIEQ1ksllU8Qt3tH3fVuzLYaULMMFRBJSACmBiuzEGUs2IDg7cjLGoDqr7DOqph0KKpQCI6Oz9nmJdvyz6AkMkuNKWahlT5sWEIOmpUfd5towCQJu8eR+WH1I4UFcUHFQO9bcnqSyYBhV1o2jbHROavdETrPTvE6zE1JIfiBm2rS2OC0krqxjoFNQujfiNW6Cu9wlteMd4PXklKqv1lGz7mPVBDADb9w5xyzAzwmsNtAp56qS7bn55+5Z7JTOMZRl0ls4FSg6FKdsrm/ZnY8IydnI1J8fUIxQB5rOInk7JIciPyd+QHGys5LmuAdyezruQyonPydpBjiDZ7QiHjVXPFQ6fso+C/DccvPIBxXRGaXxs4BU7Efhhe+lkIkXbh4i7ZvHD1vOAyR2pVkHVzrPonIUGkkBSiMcyeBjF5XjAf50pmVdWGcCzb+oHB99ny9ZUbnu4jqronJ8MId98KJyJ0su76JyfDC3fJyicieLL/MCS3x0RzxZUbnuojuvonJ8fK97ONFkWFSO5+5wD6g46KwU54l41ywz7/qRFJXjrpzmRrs9uxFo+cHkYSvyF+oCTO+/XPiaIFdN9d6tr6Zlr9Dr9erhYb2azh628lLiQmCznt7cKBAisNwKy9OT72od8I5+41+/lMNNPkzvZoe7/M/xLwbe0DkxV1UHvsm+SWE5P4u7xWYxXXqn6IS5dvdDZNdifuVoLj2/XV20tZaefiz5/O7qYUvr01JI9+vFal0ebkFgpZi0nMkPc/PSjpOa+xRKxvGUeWBdPWa9Y/fD8XmNplWNWb9VJHanBXJMjXwqs2PC1V0p572WfFrfTQ7BtIP4i079yL2XPtxvqaB9HkAou3SDn3pv9xOCG3vqhp3L+U35ydF2neu5+vX78GC5du6P1alR6eUFfVWO9Gmz2j1hp46h9koZqz8npdLsz0movQvEnP05ZTKb1BX67dahOqR1bPm66UmVGbS7Ll/bT7Jsd02gzl+pgt9Yh1nq7a6xI/KdrN2189zoc4/h8+4xTKBRu2DCgbAd62F7DOsfkJjCjpkqdcMkaxIrH+AF64CjsUlMotFJ7Hij9FBoTTcbNVc7E8+AyIhYOMg4TywI01QC0IEFV12vdFgICXyfVggh1MyorqsKj1yE1z9oFllwR4d8gqyXMvCalJlEkbbUYtT/Y5j0zNQoT9VLZoxGzMhj4oCSCDlLdRoUq8XChksoZfQBZ2QoatMgHygisxQlE6xnWFONRplhjjFm9rVMDcWQPY3ToFgB6ryhmFfdFgUeDE3wSL1D1QOKwtzSYdysjxoPih9/WU2/uXr7xw9/ss+zxa+E8c3DJPis0ml2S5/sNGxuFDMBeCGl1IUpXQVoSYFZVbmSOEwdDlnBxKEsTqpqlCHNJ06pRsleiteXby7sftHRalF68RKeqEGhEol5Gl7XIOO44OggUIdXDGgBHaf5ktWnDOk8kVcl2qDisy64TCCOWn42DmAgxgWoHaA1WAACUlBwgIxeuseuPhvS9iK2vu+z+XMRYCUhWpcQkLYTisWgKp3eCa2k0zS3iC2InKrv+eTdWnxPA761+J6+MBMjTlCDUJhwwrWrOSeNaiKKmrzjJq4eG5Q3dULB2COI71RnuVA2WBPRULZCWv1llX+N4qmcpA0kr/gNA82TjYyyJhj7YVwNWwACIAdMOc1AE6vOoaZq3eVKv6AkXP3/YfEeUAMxWrXkE8Bfq3osGnCFonux8N61l1vqJftU4dz4vsQsp0xQobxDwZlwYJYjxsqSbRLxniFObKTOQgL6sH/XysfUKI+iPFrXbWMVO3biAp240Ng6ZAPSF2zIBGcTyDE2dmAop0ZkMTw4xJC5n2OPFglF6qcDsyWuutkA2Ak51/JUsUOlMU4H7Ehkjma5V9Gw47iZztpKip1uXt0jxo6DKaiZ+9YBO9zMKaDI3G2Jhx37ZlQov4mkh0+37cAQ+MymDx8q89I+PPem/OeMUSaIzSn9GcoezcJs4q0PGICAkfY+mCS0kJJRiEt6NbacKOei3AfhTFJwqJBXBwKEBabV15EjGEoQLBCqxsAxAqPuaQ4IRIwXUGeIC4i58mmVyQyxFV2HQM0MBVxyziiRZqidcTtS6+wTRkIDtTbY/OAN7wnGGpDgvLmnjnmBMEaUYUy5ANhRZFFnpzaKQHCZCjcB58dit3MbXUgQmVKyhFTTWVc7t0SZfu7nC4i5jNdyUxHBbDE/5DYczcCLJLuJUHxBMBFMYMAFR814MCLbvU6P8JAcVHgBG9QJOopF3xJRK6ERRC47UnaJfzljyh1hUo+KeXUlE3uOybp9oUWvt+VbbIPgcLHvJhALF5kNZPyFNCNJ3VzPpQon7/T1wG59c8OL3XaQ8/MBOSy7JNeMaNhz3w9C310E894kkiPtfVDufM60IeHR+t95lpcuu4N9N1aiax4/T80jSrmUO0UELRtulBuCTVsIKCsXUYK5kFxSKXpm89Im7o1Sy1CouxwWGJGuyZ1bCYILBA4eX+AYNR2LSdmAyjJIISCOAiIl2XgAcZJRigOW5egexS6LynFc5EgVLD1GP6ewgkW33m3HRXFoJqELwsQXSyI+r7N28Dz3tZE6yqeVcjVWM6FcVhBCJCi3gDkUghjhHVqA+qunRV/GJRFFGHIoBWekyz0S0y1G+dJtlWZ+oFsB1WRhgBjGgiCbeymQtYAcl7qEXLPMJ3AMEl/7A3ZpH30El0aODVZwDY4vIQmLWlq0JM34Eubu+O54EVwdQx5yMQ+ODqYTEwU+MSFRyumgs9BWfSxcS32qOCDuVOAwqsk1jpzq+lLJgVaL2XiCSH8qcKiYbBLDTIu41WnGoTFZfWEmFhwWSDnNTCrzTflnUDZthtKZpaLsbIiRWvJM1g5OVGwMygrMDjrAZFieRzSTLWCL71gQNuOY0dnAn+QFf9p0WSBrWrdmvYBQxBNgjAv940aKyRJj4efOu6bNrgzYnx1epQqOGlqFMPHrlSPxPbrO5HXQnGADsjKSKhhRAb8DmZr9842PMtHcUWlMmnLeAfLnXxHlvNedMYfbhYVsbH9G6J7izpLN+jzzuaZf4fAgrjv9Sh91d2dfEVfbl2GddxIQ8s08/aq7kMz0q+5SQoO6jAQNL6ShErC6S2/CcMFqCVjGkRZlgjUTTuyWL0hnow8jvVNPv5ylt98rVUWDodWQ07TV6vzoC3Ox+ABxWQza94cG8fQz/zD13QSbN0nWT88tkPRVGvo4KyUlNLwVwZN5K8Egz2vnURLfomnjqh94GfcuzX5zOjV2A2LoY2QT9opiDUHPmSBXgckHKob8jNghD9Zzl9Lo9ClIpHgUBH2e9dgjCe9g3BzshOkRztlJe7I0YCdGt3Z1JBI1bWijEmPtkxm7YpSFuDfpz5vprJcqxsGYvTHJiKPzV8p0MJIogOnNO6oENmzekZ7bTKgRS+UKq8UUEiwglbBZvhf3rQSMSFmbTflmaghRVqgwUohIISmxar8NtYx37esR2hrn18kuQIbAX5/udO+az+o3lo08enTG+deP85ujraMufEXSM+weY1bMOZH+fHlYVPvyFc1J5Dg9yxxqBUmqMICzcuBzN5mz6ibDEDUXUCId0cFUjRjcwAqIDj614vteDcxk6VVwwkZptQnFOgG0T60FbtIeSVfx3D3DcXuCKMFP86ClvHuCMDPGp+x67YI3Dh4n6gnixkL6QF+oVDKnpbySEspVznADJpQiAyYdaMlM+FHgRMPG7Wj8IkDdMJCNaCkBop62bFeD6738EOXpdRo6WnUpChnz33mAJCx6aiHeJ4OxEwreETWab+h01e/a7jxIKTwakEj7NDDWRjYnFMZTNjPwYixZlby2Ow9WMq9bOPa5Zh6RsIWWTmA8/9BDV9Nz9rTKMnb6S3nlc/C0qy9JGfYCLoNIKgQBSYujtbPi1hbb1nJQPg9inCBgp6OR7d6n+hArQ/ZwOqeRRkxkIWg1Bje3qKPNrZbkk84jjn0I+ADY8OOlTJYp+/skR7PpSPMQMNafjpdGzAI0MvczwD2khJWnA2qVOVBDSkrXSzFZSlsPyaEh84h12muOyd4nlnHsILuGDvmEg13LWjrhZJcm3O9U5MiFGkFo2FUrQ2sKsb4wE3eD0cbCgJoGGgfOlaFzCjERnuWn5SaJXRB9gj+3LMxBazomgHleXjUmrD8Cg2EufVYWRL1w3vkEMej+pEnjP6zbdknGR/QTlXUMV77QYyqZKR9hPkiWVR9jaB9hvkWGjbrIKKO+XQlGizEg6t3v4rJb0TFHthJVvouo1S9KVXKQP7V4g6vBb/x4Q+d2zMhoUZ9dhIEnjjCc1Gg9oVx00pqOKRh1xcauZsUThxRO7IedTjATu2cHtkMIZNAQAu8WQhimAzYoENeVX/qfrUxu0jk6AHsXrFaTTmtGJibdIFXH1FrZMExg0zShzeo2A5885kPGDXroSR8HZ7It7NNUCYJEa8jBEWDrGYcI1ptQVyg3vUlVrow2VaVpftF+qnKGxcv4qbmZ0RUx2+oYKZQts7hDooJoFPqUDRLap1JgtHSrfCMNHFuZ4PtshsozvLCDC6R+khtihztSFk4TnuBONIsYBVjEz+GFi27eEu3uxQqHs5RTdAEF2Ia5Rxd6iEU2tVs5CW1yGbbwGUpctNwllmESFjoIK1hnqlJnw8jm1AMPT8dwo6F2m6ahVrtNX5iL3WbUGzfKnPXfIeLNcY3KZkNvCqFu5y8Gi691rV3WF/v9a56FawAO1YC8ivojA6kyjgJgU7HGxX/AwZCziaANweOZgJPoWmPa4cWwYPgAI9a3yQRvxneb1bQxDis03j1oZTwNcD7N0TUFN78uun0bhjxy0oiZcgsdavhcSeasKsmQHCvJQJioAeSjKyVT6WAmDL89ERuxlgwZv5YM1AUin4vJDMxM2Ir5jl5MBsL0qQ2Po5pMpTYZMZPuK3w4tdy/nAzh1ByNyWSJBPdv//P7/MV377//4bu/4eW71be3//w0ybuT4pG45Gyxnl/vR7tbrUvJp6xIxQBWhjEBWAJMBWx6sRNG+La1jv7YcfiKg8LVTgeqYflxDIaSjFOuWTVm7JnvdJovDE/iI5+y1OM5vusyYS3IyxxpAKot4+a+NKecF/Twad90KS4BLQjsd5fELBcQ63zqLEekwAokFcsZBjgjFObHclk1C3wsLMcDWY7nxXISlpW4D6+m4c+ZYqcILCcphgUB/e6SmOUCmhM+sxygfpYD+bGcM0b6THMn0pwMpDmZGc0hiLw0x6LQHFMmo4/mfHeJR3Nf4V+//vD5y9uPn6/mP/6yevHn5Qs+CYjr5kdzR6gqFM/evEYs6pRmMBqXtIXRCFCWnSvbRjkNEXLUnDJ0hUNHZ7QeKQTdU3VsRjOYpR/F+TSlTnHHrxuN0QhQbiMtGYcRKIyuPpAqp1IA/SnjRsHK8C4ulNaDOMw4NNR2n8ScFhARfua0A6fBHDktfY34Z06rNKWV03C+nIbwMJzWdp/EnHb2Wwuf5w+bpDTHqLLlj9McwzhDmstyZ+HR0Zxjq+H4dWPRnGTMQ3MY8gb99GQ5uj3IWd0Ed7lHYoY7+22FsRmOUpYhw2W5q/DoGM6xzXD8ukwZTpL0DOe/R2KGO/stheQMRyhvuKrIoDgO2rJDxqC4PLcUHh3HOfYYjl83FschCRreapN/EBExOG57lP4oybXcJPWGwlnuKNRZbj5NzHJ0V9j1KMsxSHJkOfTMckNsMwTvM4x7rBFwdJznMGYxeA4yzjzGnP8mqXnuLHcZsuI5inN0WOHz1sMgPBe69wBH3Xzw8xyBUXxWP8+13CQ1z7l2Hp4PW6XetGUQGOl0XMGkcDQJQ7pEfOzzVm44pN+xqCbcPhIVKqoj+XEM+wnwaBmLhDQ46p7DFmfGcWMOe2+hQkaJiVponuhKzVeufYRnvkrNVwQyC0c6lXE8skq/+fC0yGrU7YMSZNyiFwYK3aGgz2l2s0eG4itjtN58pf5cr0piOVxeFoH8aTWbl1f8Pw==</diagram></mxfile>" > @@ -23,7 +23,7 @@ pointer-events="none" /> - + - + - - + + - + - + - - - + + + @@ -139,7 +139,7 @@ - + @@ -176,9 +176,9 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - + - + - + - + - - - + + + - + - + - + - - + + - - + + - + - + - - - + + + - + - + - + + + + + + + @@ -589,16 +613,16 @@
- attention area + attention lane
- attention area + attention lane - - + + @@ -654,7 +678,7 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - - + + - attention area + attention lane - attention area + attention lane - - + + @@ -875,7 +899,7 @@ stroke-miterlimit="10" pointer-events="none" /> - + @@ -990,10 +1014,10 @@ stroke-miterlimit="10" pointer-events="none" /> - - + + - - + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg b/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg new file mode 100644 index 0000000000000..2fc22c8a4a401 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg @@ -0,0 +1,518 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + default stop +
+ line + +
+ + +
+
+
+ + default stop... + + + + + + + + +
+
+
+ + occlusion + +
+
+
+ + occlusion + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + first attention +
+ stop line + +
+ + +
+
+
+ + first attentio... + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg index 8802dc786b617..c29cb7ade21cd 100644 --- a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg @@ -8,7 +8,7 @@ width="2842px" height="1201px" viewBox="-0.5 -0.5 2842 1201" - content="<mxfile host="Electron" modified="2023-08-28T08:00:31.600Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="CbgId97oWJwBUVWni98L" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + content="<mxfile host="Electron" modified="2023-10-03T03:33:38.127Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="TqIhwSEqRvc68Imy-9XT" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > @@ -18,7 +18,7 @@ - + @@ -57,9 +57,9 @@ stroke-miterlimit="10" pointer-events="all" /> - + - + - - - - + + + + - - - + + + - + - +
- +
autoware_perception_msgs behavior_velocity_planner_common geometry_msgs - grid_map_core interpolation lanelet2_extension libopencv-dev - magic_enum motion_utils nav_msgs pluginlib @@ -41,8 +39,6 @@ vehicle_info_util visualization_msgs - grid_map_rviz_plugin - ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index fbb82d6ace175..c875611b16003 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -271,29 +271,37 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() { - // TODO(Mamoru Sobue): collision stop pose depends on before/after occlusion clearance motion_utils::VirtualWalls virtual_walls; motion_utils::VirtualWall wall; - wall.style = motion_utils::VirtualWallType::stop; if (debug_data_.collision_stop_wall_pose) { + wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection"; wall.ns = "intersection" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.collision_stop_wall_pose.value(); virtual_walls.push_back(wall); } if (debug_data_.occlusion_first_stop_wall_pose) { + wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection"; wall.ns = "intersection_occlusion_first_stop" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.occlusion_first_stop_wall_pose.value(); virtual_walls.push_back(wall); } if (debug_data_.occlusion_stop_wall_pose) { + wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection_occlusion"; wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.occlusion_stop_wall_pose.value(); virtual_walls.push_back(wall); } + if (debug_data_.absence_traffic_light_creep_wall) { + wall.style = motion_utils::VirtualWallType::slowdown; + wall.text = "intersection_occlusion"; + wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_"; + wall.pose = debug_data_.absence_traffic_light_creep_wall.value(); + virtual_walls.push_back(wall); + } return virtual_walls; } diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index e02a1e4220030..d42c9b3ca2aa2 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -136,6 +136,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.stop_release_margin_time"); ip.occlusion.temporal_stop_before_attention_area = getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); + ip.occlusion.absence_traffic_light.creep_velocity = + getOrDeclareParameter(node, ns + ".occlusion.absence_traffic_light.creep_velocity"); + ip.occlusion.absence_traffic_light.maximum_peeking_distance = getOrDeclareParameter( + node, ns + ".occlusion.absence_traffic_light.maximum_peeking_distance"); } void IntersectionModuleManager::launchNewModules( @@ -165,13 +169,21 @@ void IntersectionModuleManager::launchNewModules( continue; } - const auto associative_ids = - planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); const std::string location = ll.attributeOr("location", "else"); const bool is_private_area = (location.compare("private") == 0); + const auto associative_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); + bool has_traffic_light = false; + if (const auto tl_reg_elems = ll.regulatoryElementsAs(); + tl_reg_elems.size() != 0) { + const auto tl_reg_elem = tl_reg_elems.front(); + const auto stop_line_opt = tl_reg_elem->stopLine(); + if (!!stop_line_opt) has_traffic_light = true; + } const auto new_module = std::make_shared( - module_id, lane_id, planner_data_, intersection_param_, associative_ids, is_private_area, - enable_occlusion_detection, node_, logger_.get_child("intersection_module"), clock_); + module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, + has_traffic_light, enable_occlusion_detection, is_private_area, node_, + logger_.get_child("intersection_module"), clock_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ const UUID uuid = getUUID(new_module->getModuleId()); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 89bb65f403095..89cdeaae56723 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -17,7 +17,6 @@ #include "util.hpp" #include -#include #include #include #include @@ -65,14 +64,18 @@ static bool isTargetCollisionVehicleType( } IntersectionModule::IntersectionModule( - const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, + const int64_t module_id, const int64_t lane_id, + [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const bool is_private_area, const bool enable_occlusion_detection, rclcpp::Node & node, + const std::string & turn_direction, const bool has_traffic_light, + const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), node_(node), lane_id_(lane_id), associative_ids_(associative_ids), + turn_direction_(turn_direction), + has_traffic_light_(has_traffic_light), enable_occlusion_detection_(enable_occlusion_detection), occlusion_attention_divisions_(std::nullopt), is_private_area_(is_private_area), @@ -81,11 +84,10 @@ IntersectionModule::IntersectionModule( velocity_factor_.init(VelocityFactor::INTERSECTION); planner_param_ = planner_param; - const auto & assigned_lanelet = - planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); - turn_direction_ = assigned_lanelet.attributeOr("turn_direction", "else"); - collision_state_machine_.setMarginTime( - planner_param_.collision_detection.state_transit_margin_time); + { + collision_state_machine_.setMarginTime( + planner_param_.collision_detection.state_transit_margin_time); + } { before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); before_creep_state_machine_.setState(StateMachine::State::STOP); @@ -226,6 +228,23 @@ void prepareRTCByDecisionResult( return; } +template <> +void prepareRTCByDecisionResult( + const IntersectionModule::OccludedAbsenceTrafficLight & result, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) +{ + RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedAbsenceTrafficLight"); + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.closest_idx; + *default_safety = !result.collision_detected; + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + *occlusion_safety = result.is_actually_occlusion_cleared; + *occlusion_distance = 0; + return; +} + template <> void prepareRTCByDecisionResult( const IntersectionModule::OccludedCollisionStop & result, @@ -570,6 +589,59 @@ void reactRTCApprovalByDecisionResult( return; } +template <> +void reactRTCApprovalByDecisionResult( + const bool rtc_default_approved, const bool rtc_occlusion_approved, + 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) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("reactRTCApprovalByDecisionResult"), + "OccludedAbsenceTrafficLight, approval = (default: %d, occlusion: %d)", rtc_default_approved, + rtc_occlusion_approved); + if (!rtc_default_approved) { + const auto stop_line_idx = decision_result.closest_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->collision_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(decision_result.closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + if (!rtc_occlusion_approved && decision_result.temporal_stop_before_attention_required) { + const auto stop_line_idx = decision_result.first_attention_area_stop_line_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->occlusion_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(decision_result.closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + if (!rtc_occlusion_approved && !decision_result.temporal_stop_before_attention_required) { + const auto closest_idx = decision_result.closest_idx; + const auto peeking_limit_line = decision_result.peeking_limit_line_idx; + for (auto i = closest_idx; i <= peeking_limit_line; ++i) { + planning_utils::setVelocityFromIndex( + i, planner_param.occlusion.absence_traffic_light.creep_velocity, path); + } + debug_data->absence_traffic_light_creep_wall = + planning_utils::getAheadPose(closest_idx, baselink2front, *path); + } + return; +} + template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, @@ -675,7 +747,8 @@ void reactRTCApproval( static std::string formatDecisionResult(const IntersectionModule::DecisionResult & decision_result) { if (std::holds_alternative(decision_result)) { - return "Indecisive"; + const auto indecisive = std::get(decision_result); + return "Indecisive because " + indecisive.error; } if (std::holds_alternative(decision_result)) { return "Safe"; @@ -695,6 +768,9 @@ static std::string formatDecisionResult(const IntersectionModule::DecisionResult if (std::holds_alternative(decision_result)) { return "OccludedCollisionStop"; } + if (std::holds_alternative(decision_result)) { + return "OccludedAbsenceTrafficLight"; + } if (std::holds_alternative(decision_result)) { return "TrafficLightArrowSolidOn"; } @@ -747,15 +823,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { - RCLCPP_DEBUG(logger_, "splineInterpolate failed"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"splineInterpolate failed"}; } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { - RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{ + "Path has no interval on intersection lane " + std::to_string(lane_id_)}; } + // 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); @@ -766,30 +842,40 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_param_.occlusion.occlusion_attention_area_length, planner_param_.common.consider_wrong_direction_vehicle); } + auto & intersection_lanelets = intersection_lanelets_.value(); + + // 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); const bool is_prioritized = traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; - intersection_lanelets_.value().update(is_prioritized, interpolated_path_info); + intersection_lanelets.update(is_prioritized, interpolated_path_info); - const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting(); - const auto & first_conflicting_area_opt = intersection_lanelets_.value().first_conflicting_area(); + // this is abnormal + const auto & conflicting_lanelets = intersection_lanelets.conflicting(); + const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area(); if (conflicting_lanelets.empty() || !first_conflicting_area_opt) { - RCLCPP_DEBUG(logger_, "conflicting area is empty"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"conflicting area is empty"}; } const auto first_conflicting_area = first_conflicting_area_opt.value(); - const auto & first_attention_area_opt = intersection_lanelets_.value().first_attention_area(); + // 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 double peeking_offset = + has_traffic_light_ ? planner_param_.occlusion.peeking_offset + : planner_param_.occlusion.absence_traffic_light.maximum_peeking_distance; const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin, - planner_param_.occlusion.peeking_offset, path); + peeking_offset, path); if (!intersection_stop_lines_opt) { - RCLCPP_DEBUG(logger_, "failed to generate intersection_stop_lines"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"}; } const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); const auto @@ -797,38 +883,49 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] = intersection_stop_lines; - const auto & conflicting_area = intersection_lanelets_.value().conflicting_area(); + // see the doc for struct PathLanelets + 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_.value().attention_area(), - closest_idx, planner_data_->vehicle_info_.vehicle_width_m); + conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx, + planner_data_->vehicle_info_.vehicle_width_m); if (!path_lanelets_opt.has_value()) { - RCLCPP_DEBUG(logger_, "failed to generate PathLanelets"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"failed to generate PathLanelets"}; } const auto path_lanelets = path_lanelets_opt.value(); - const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); + // utility functions + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path->points, closest_idx, index); + }; + auto stoppedForDuration = + [&](const size_t pos, const double duration, StateMachine & state_machine) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); + const bool over_stopline = (dist_stopline < 0.0); + const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + state_machine.setState(StateMachine::State::GO); + } else if (is_stopped_duration && approached_dist_stopline) { + state_machine.setState(StateMachine::State::GO); + } + return state_machine.getState() == StateMachine::State::GO; + }; + // 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); if (stuck_detected && stuck_stop_line_idx_opt) { auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); - const double dist_stopline = motion_utils::calcSignedArcLength( - path->points, path->points.at(closest_idx).point.pose.position, - path->points.at(stuck_stop_line_idx).point.pose.position); - const bool approached_stop_line = - (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); - const bool is_stopped = planner_data_->isVehicleStopped(); - if (is_stopped && approached_stop_line) { - stuck_private_area_timeout_.setStateWithMarginTime( - StateMachine::State::GO, logger_.get_child("stuck_private_area_timeout"), *clock_); - } - const bool timeout = - (is_private_area_ && stuck_private_area_timeout_.getState() == StateMachine::State::GO); + const bool stopped_at_stuck_line = stoppedForDuration( + stuck_stop_line_idx, planner_param_.stuck_vehicle.timeout_private_area, + stuck_private_area_timeout_); + const bool timeout = (is_private_area_ && stopped_at_stuck_line); if (!timeout) { if ( default_stop_line_idx_opt && - motion_utils::calcSignedArcLength(path->points, stuck_stop_line_idx, closest_idx) > - planner_param_.common.stop_overshoot_margin) { + fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { stuck_stop_line_idx = default_stop_line_idx_opt.value(); } return IntersectionModule::StuckStop{ @@ -836,15 +933,16 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } + // if attention area is empty, collision/occlusion detection is impossible if (!first_attention_area_opt) { - RCLCPP_DEBUG(logger_, "attention area is empty"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"attention area is empty"}; } const auto first_attention_area = first_attention_area_opt.value(); + // if attention area is not null but default stop line is not available, ego/backward-path has + // already passed the stop line if (!default_stop_line_idx_opt) { - RCLCPP_DEBUG(logger_, "default stop line is null"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"default stop line is null"}; } const auto default_stop_line_idx = default_stop_line_idx_opt.value(); @@ -873,25 +971,25 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // is_go_out_: previous RTC approval // activated_: current RTC approval is_permanent_go_ = true; - RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"}; } + // occlusion stop line is generated from the intersection of ego footprint along the path with the + // attention area, so if this is null, eog has already passed the intersection if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) { - RCLCPP_DEBUG(logger_, "occlusion stop line is null"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"occlusion stop line is null"}; } const auto collision_stop_line_idx = is_over_default_stop_line ? closest_idx : default_stop_line_idx; const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value(); const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); - const auto & attention_lanelets = intersection_lanelets_.value().attention(); - const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent(); - const auto & occlusion_attention_lanelets = intersection_lanelets_.value().occlusion_attention(); - const auto & occlusion_attention_area = intersection_lanelets_.value().occlusion_attention_area(); - debug_data_.attention_area = intersection_lanelets_.value().attention_area(); - debug_data_.adjacent_area = intersection_lanelets_.value().adjacent_area(); + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); + const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); // get intersection area const auto intersection_area = planner_param_.common.use_intersection_area @@ -902,17 +1000,17 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } - // calculate dynamic collision around detection area - const double time_delay = (is_go_out_ || is_prioritized) - ? 0.0 - : (planner_param_.collision_detection.state_transit_margin_time - - collision_state_machine_.getDuration()); + // calculate dynamic collision around attention area + const double time_to_restart = (is_go_out_ || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.state_transit_margin_time - + collision_state_machine_.getDuration()); const auto target_objects = filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); const bool has_collision = checkCollision( *path, target_objects, path_lanelets, closest_idx, - std::min(occlusion_stop_line_idx, path->points.size() - 1), time_delay, + std::min(occlusion_stop_line_idx, path->points.size() - 1), time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, @@ -959,74 +1057,63 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool is_occlusion_cleared_with_margin = (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); - // check safety + // distinguish if ego detected occlusion or RTC detects occlusion const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); - if ( - occlusion_stop_state_machine_.getState() == StateMachine::State::STOP || - ext_occlusion_requested) { - const double dist_default_stopline = motion_utils::calcSignedArcLength( - path->points, path->points.at(closest_idx).point.pose.position, - path->points.at(default_stop_line_idx).point.pose.position); - const bool approached_default_stop_line = - (std::fabs(dist_default_stopline) < planner_param_.common.stop_overshoot_margin); - const bool over_default_stop_line = (dist_default_stopline < 0.0); - const bool is_stopped_at_default = - planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time); - if (over_default_stop_line) { - before_creep_state_machine_.setState(StateMachine::State::GO); - } - if (before_creep_state_machine_.getState() == StateMachine::State::GO) { - const double dist_first_attention_stopline = motion_utils::calcSignedArcLength( - path->points, path->points.at(closest_idx).point.pose.position, - path->points.at(first_attention_stop_line_idx).point.pose.position); - const bool approached_first_attention_stop_line = - (std::fabs(dist_first_attention_stopline) < planner_param_.common.stop_overshoot_margin); - const bool over_first_attention_stop_line = (dist_first_attention_stopline < 0.0); - const bool is_stopped_at_first_attention = - planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time); - if (planner_param_.occlusion.temporal_stop_before_attention_area) { - if (over_first_attention_stop_line) { - temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO); - } - if (is_stopped_at_first_attention && approached_first_attention_stop_line) { - temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO); - } - } - const bool temporal_stop_before_attention_required = - planner_param_.occlusion.temporal_stop_before_attention_area && - temporal_stop_before_attention_state_machine_.getState() == StateMachine::State::STOP; - if (has_collision_with_margin) { - return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stop_line_idx, - first_attention_stop_line_idx, - occlusion_stop_line_idx}; - } else { - return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stop_line_idx, - first_attention_stop_line_idx, - occlusion_stop_line_idx}; + const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); + // Safe + if (!is_occlusion_state && !has_collision_with_margin) { + return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + // Only collision + if (!is_occlusion_state && has_collision_with_margin) { + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + // Occluded + const bool stopped_at_default_line = stoppedForDuration( + default_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, + before_creep_state_machine_); + if (stopped_at_default_line) { + // in this case ego will temporarily stop before entering attention area + const bool temporal_stop_before_attention_required = + (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) + ? !stoppedForDuration( + first_attention_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, + temporal_stop_before_attention_state_machine_) + : false; + if (!has_traffic_light_) { + if (fromEgoDist(first_attention_stop_line_idx) <= -peeking_offset) { + return IntersectionModule::Indecisive{ + "already passed maximum peeking line in the absence of traffic light"}; } + return IntersectionModule::OccludedAbsenceTrafficLight{ + is_occlusion_cleared_with_margin, has_collision_with_margin, + temporal_stop_before_attention_required, closest_idx, + first_attention_stop_line_idx, occlusion_stop_line_idx}; + } + if (has_collision_with_margin) { + return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stop_line_idx, + first_attention_stop_line_idx, + occlusion_stop_line_idx}; } else { - if (is_stopped_at_default && approached_default_stop_line) { - // start waiting at the first stop line - before_creep_state_machine_.setState(StateMachine::State::GO); - } - const auto occlusion_stop_line = planner_param_.occlusion.temporal_stop_before_attention_area - ? first_attention_stop_line_idx - : occlusion_stop_line_idx; - return IntersectionModule::FirstWaitBeforeOcclusion{ - is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, occlusion_stop_line}; + return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stop_line_idx, + first_attention_stop_line_idx, + occlusion_stop_line_idx}; } - } else if (has_collision_with_margin) { - return IntersectionModule::NonOccludedCollisionStop{ - closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } else { + const auto occlusion_stop_line = + (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) + ? first_attention_stop_line_idx + : occlusion_stop_line_idx; + return IntersectionModule::FirstWaitBeforeOcclusion{ + is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, occlusion_stop_line}; } - - return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } bool IntersectionModule::checkStuckVehicle( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 550b23aa17606..7bd923ed6198c 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -18,9 +18,7 @@ #include "util_type.hpp" #include -#include #include -#include #include #include @@ -116,10 +114,18 @@ class IntersectionModule : public SceneModuleInterface double ignore_parked_vehicle_speed_threshold; double stop_release_margin_time; bool temporal_stop_before_attention_area; + struct AbsenceTrafficLight + { + double creep_velocity; + double maximum_peeking_distance; + } absence_traffic_light; } occlusion; }; - using Indecisive = std::monostate; + struct Indecisive + { + std::string error; + }; struct StuckStop { size_t closest_idx{0}; @@ -159,6 +165,15 @@ class IntersectionModule : public SceneModuleInterface size_t first_attention_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; }; + struct OccludedAbsenceTrafficLight + { + bool is_actually_occlusion_cleared{false}; + bool collision_detected{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t first_attention_area_stop_line_idx{0}; + size_t peeking_limit_line_idx{0}; + }; struct Safe { // NOTE: if RTC is disapproved status, default stop lines are still needed. @@ -174,20 +189,22 @@ class IntersectionModule : public SceneModuleInterface size_t occlusion_stop_line_idx{0}; }; using DecisionResult = std::variant< - Indecisive, // internal process error, or over the pass judge line - StuckStop, // detected 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 - Safe, // judge as safe - TrafficLightArrowSolidOn // only detect vehicles violating traffic rules + Indecisive, // internal process error, or over the pass judge line + StuckStop, // detected 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 + TrafficLightArrowSolidOn // only detect vehicles violating traffic rules >; IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const bool is_private_area, const bool enable_occlusion_detection, rclcpp::Node & node, + const std::string & turn_direction, const bool has_traffic_light, + const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); /** @@ -211,7 +228,8 @@ class IntersectionModule : public SceneModuleInterface rclcpp::Node & node_; const int64_t lane_id_; const std::set associative_ids_; - std::string turn_direction_; + const std::string turn_direction_; + const bool has_traffic_light_; bool is_go_out_ = false; bool is_permanent_go_ = false; @@ -230,6 +248,7 @@ class IntersectionModule : public SceneModuleInterface StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; StateMachine temporal_stop_before_attention_state_machine_; + // NOTE: uuid_ is base member // for stuck vehicle detection diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 37dc83cbdf761..f381925b13208 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -16,10 +16,10 @@ #include "util_type.hpp" +#include #include #include #include -#include #include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 7ba894bd9d32a..6acc1d60443bf 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -20,14 +20,6 @@ #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 7e5bff89485ed..5c188f4aebebf 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -51,6 +51,7 @@ struct DebugData 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}; }; struct InterpolatedPathInfo From a9bb25adda3f33adefaf5cccbc3402307a0f2e52 Mon Sep 17 00:00:00 2001 From: Felix F Xu <84662027+felixf4xu@users.noreply.github.com> Date: Wed, 4 Oct 2023 12:18:37 +0800 Subject: [PATCH 69/97] build(lidar_apollo_segmentation_tvm): fix compiling error tier4_autoware_utils (#4516) * fix compiling error tier4_autoware_utils:: Signed-off-by: Felix F Xu * style(pre-commit): autofix --------- Signed-off-by: Felix F Xu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> From 26e3be8569945733f2e76ba59a7077ccb0c29fab Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Wed, 4 Oct 2023 14:14:34 +0900 Subject: [PATCH 70/97] fix(obstacle_cruise_planner): fix coordinate transformation bag (#5180) * fix coordinate transformation. linear.x denotes longtidional velocity, not the x velocity in the world coordinate Signed-off-by: Yuki Takagi * declare Eigen type Signed-off-by: Yuki Takagi --------- Signed-off-by: Yuki Takagi --- planning/obstacle_cruise_planner/src/node.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index b5e0718f20a49..7de6bb2c966f6 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -100,14 +100,15 @@ std::pair projectObstacleVelocityToTrajectory( const std::vector & traj_points, const Obstacle & obstacle) { const size_t object_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); - - const double object_vel_norm = std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); - const double object_vel_yaw = std::atan2(obstacle.twist.linear.y, obstacle.twist.linear.x); const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); - return std::make_pair( - object_vel_norm * std::cos(object_vel_yaw - traj_yaw), - object_vel_norm * std::sin(object_vel_yaw - traj_yaw)); + const double obstacle_yaw = tf2::getYaw(obstacle.pose.orientation); + + const Eigen::Rotation2Dd R_ego_to_obstacle(obstacle_yaw - traj_yaw); + const Eigen::Vector2d obstacle_velocity(obstacle.twist.linear.x, obstacle.twist.linear.y); + const Eigen::Vector2d projected_velocity = R_ego_to_obstacle * obstacle_velocity; + + return std::make_pair(projected_velocity[0], projected_velocity[1]); } double calcObstacleMaxLength(const Shape & shape) From 9cf1f7cc4f8255dce34273eb8aba4ec59a1797a7 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Wed, 4 Oct 2023 15:14:03 +0900 Subject: [PATCH 71/97] perf(ndt_scan_matcher): changed default `initial_estimate_particles_num` to 200 (#5220) Changed initial_estimate_particles_num to 200 Signed-off-by: Shintaro Sakoda --- .../ndt_scan_matcher/config/ndt_scan_matcher.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 718bc6ca3b3a3..cf41a4cf55d0b 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -41,7 +41,7 @@ converged_param_nearest_voxel_transformation_likelihood: 2.3 # The number of particles to estimate initial pose - initial_estimate_particles_num: 100 + initial_estimate_particles_num: 200 # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] lidar_topic_timeout_sec: 1.0 From bc6806390ffbe07b91d23e2a7ceea641280aa03a Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Wed, 4 Oct 2023 15:53:57 +0900 Subject: [PATCH 72/97] feat(yabloc_image_processing): support both of raw and compressed image input (#5209) * add raw image subscriber Signed-off-by: Kento Yabuuchi * update README Signed-off-by: Kento Yabuuchi * improve format and variable names Signed-off-by: Kento Yabuuchi --------- Signed-off-by: Kento Yabuuchi --- .../pose_twist_estimator/yabloc.launch.xml | 2 +- .../yabloc/yabloc_image_processing/README.md | 13 +++-- .../launch/image_processing.launch.xml | 3 +- .../src/undistort/undistort_node.cpp | 53 ++++++++++++++----- 4 files changed, 53 insertions(+), 18 deletions(-) diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml index a9b0b93947054..8f0191d6156fe 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/README.md b/localization/yabloc/yabloc_image_processing/README.md index eb9ae658c39a7..85e1d408a1b4b 100644 --- a/localization/yabloc/yabloc_image_processing/README.md +++ b/localization/yabloc/yabloc_image_processing/README.md @@ -108,10 +108,15 @@ This node performs image resizing and undistortion at the same time. #### Input -| Name | Type | Description | -| ---------------------------- | ----------------------------------- | -------------------- | -| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | raw compressed image | -| `input/image_raw/compressed` | `sensor_msgs::msg::CompressedImage` | raw sensor info | +| Name | Type | Description | +| ---------------------------- | ----------------------------------- | ----------------------- | +| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | camera info | +| `input/image_raw` | `sensor_msgs::msg::Image` | raw camera image | +| `input/image_raw/compressed` | `sensor_msgs::msg::CompressedImage` | compressed camera image | + +This node subscribes to both compressed image and raw image topics. +If raw image is subscribed to even once, compressed image will no longer be subscribed to. +This is to avoid redundant decompression within Autoware. #### Output diff --git a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml index fd55a8163123a..aa6fc10ee6667 100644 --- a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml @@ -1,5 +1,5 @@ - + @@ -9,6 +9,7 @@ + diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 7412db28e1550..7fc9ad785dbe2 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -50,9 +50,12 @@ class UndistortNode : public rclcpp::Node } auto on_image = std::bind(&UndistortNode::on_image, this, _1); + auto on_compressed_image = std::bind(&UndistortNode::on_compressed_image, this, _1); auto on_info = std::bind(&UndistortNode::on_info, this, _1); - sub_image_ = - create_subscription("~/input/image_raw", qos, std::move(on_image)); + sub_image_ = create_subscription("~/input/image_raw", qos, std::move(on_image)); + sub_compressed_image_ = create_subscription( + "~/input/image_raw/compressed", qos, std::move(on_compressed_image)); + sub_info_ = create_subscription("~/input/camera_info", qos, std::move(on_info)); pub_info_ = create_publisher("~/output/resized_info", 10); @@ -63,7 +66,8 @@ class UndistortNode : public rclcpp::Node const int OUTPUT_WIDTH; const std::string OVERRIDE_FRAME_ID; - rclcpp::Subscription::SharedPtr sub_image_; + rclcpp::Subscription::SharedPtr sub_image_; + rclcpp::Subscription::SharedPtr sub_compressed_image_; rclcpp::Subscription::SharedPtr sub_info_; rclcpp::Publisher::SharedPtr pub_image_; rclcpp::Publisher::SharedPtr pub_info_; @@ -99,14 +103,8 @@ class UndistortNode : public rclcpp::Node scaled_info_->height = new_size.height; } - void on_image(const CompressedImage & msg) + void remap_and_publish(const cv::Mat & image, const std_msgs::msg::Header & header) { - if (!info_.has_value()) return; - if (undistort_map_x.empty()) make_remap_lut(); - - tier4_autoware_utils::StopWatch stop_watch; - cv::Mat image = common::decompress_to_cv_mat(msg); - cv::Mat undistorted_image; cv::remap(image, undistorted_image, undistort_map_x, undistort_map_y, cv::INTER_LINEAR); @@ -120,16 +118,47 @@ class UndistortNode : public rclcpp::Node // Publish Image { cv_bridge::CvImage bridge; - bridge.header.stamp = msg.header.stamp; + bridge.header.stamp = header.stamp; if (OVERRIDE_FRAME_ID != "") bridge.header.frame_id = OVERRIDE_FRAME_ID; else - bridge.header.frame_id = msg.header.frame_id; + bridge.header.frame_id = header.frame_id; bridge.encoding = "bgr8"; bridge.image = undistorted_image; pub_image_->publish(*bridge.toImageMsg()); } + } + void on_image(const Image & msg) + { + if (!info_.has_value()) { + return; + } + if (undistort_map_x.empty()) { + make_remap_lut(); + } + + // To remove redundant decompression, deactivate compressed image subscriber + if (sub_compressed_image_) { + sub_compressed_image_.reset(); + } + + tier4_autoware_utils::StopWatch stop_watch; + remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); + RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); + } + + void on_compressed_image(const CompressedImage & msg) + { + if (!info_.has_value()) { + return; + } + if (undistort_map_x.empty()) { + make_remap_lut(); + } + + tier4_autoware_utils::StopWatch stop_watch; + remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); } From 5fed746365fd647c61eae400fbddc304088b9d4c Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Wed, 4 Oct 2023 16:47:58 +0900 Subject: [PATCH 73/97] chore: fix spell-check errors in perception packages (#5215) fix: cspell error Signed-off-by: Shunsuke Miura --- .../src/lidar_apollo_segmentation_tvm.cpp | 1 + perception/multi_object_tracker/models.md | 2 +- perception/traffic_light_classifier/README.md | 4 +++- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index 905d70235e33e..e6c95f202106d 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -22,6 +22,7 @@ #include #include +// cspell: ignore bcnn using model_zoo::perception::lidar_obstacle_detection::baidu_cnn::onnx_bcnn::config; namespace autoware diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index c83038ffa7b59..f74967943be32 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -2,7 +2,7 @@ ## Tracking model - + ### CTRV model [1] diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md index 7df0c5466695b..2aecf66f8fb7b 100644 --- a/perception/traffic_light_classifier/README.md +++ b/perception/traffic_light_classifier/README.md @@ -148,6 +148,8 @@ DATASET_ROOT #### Prerequisites + + **Step 1.** Download and install Miniconda from the [official website](https://mmpretrain.readthedocs.io/en/latest/get_started.html). **Step 2.** Create a conda virtual environment and activate it @@ -295,7 +297,7 @@ python tools/train.py configs/mobilenet_v2/mobilenet-v2_8xb32_custom.py Training logs and weights will be saved in the `work_dirs/mobilenet-v2_8xb32_custom` folder. -### Convert PyTorh model to ONNX model +### Convert PyTorch model to ONNX model #### Install mmdeploy From 36c17b338e390a7f40e1f0ee216c95e9354675f3 Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Wed, 4 Oct 2023 20:33:34 +0900 Subject: [PATCH 74/97] refactor(heatmap_visualizer): add JSON Schema and remove default value definition (#4901) * refactor: add JSON Schema and remove default value defined in `declare_parameter()` Signed-off-by: ktro2828 * refactor: add configuration file and update launcher to load this Signed-off-by: ktro2828 * docs: update the docuemnt of core parameters Signed-off-by: ktro2828 * feat: add config into install to share Signed-off-by: ktro2828 --------- Signed-off-by: ktro2828 --- perception/heatmap_visualizer/CMakeLists.txt | 1 + perception/heatmap_visualizer/README.md | 17 ++-- .../config/heatmap_visualizer.param.yaml | 33 ++++++++ .../launch/heatmap_visualizer.launch.xml | 14 +--- .../schema/heatmap_visualizer.schema.json | 81 +++++++++++++++++++ .../src/heatmap_visualizer_node.cpp | 14 ++-- 6 files changed, 133 insertions(+), 27 deletions(-) create mode 100644 perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml create mode 100644 perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json diff --git a/perception/heatmap_visualizer/CMakeLists.txt b/perception/heatmap_visualizer/CMakeLists.txt index ed995491f2af6..75fa02a1cdcf7 100644 --- a/perception/heatmap_visualizer/CMakeLists.txt +++ b/perception/heatmap_visualizer/CMakeLists.txt @@ -16,4 +16,5 @@ rclcpp_components_register_node(heatmap_visualizer_component ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/heatmap_visualizer/README.md b/perception/heatmap_visualizer/README.md index dd8b6f2a7f2b2..747a158b3d487 100644 --- a/perception/heatmap_visualizer/README.md +++ b/perception/heatmap_visualizer/README.md @@ -44,14 +44,15 @@ When publishing, firstly these values are normalized to [0, 1] using maximum and ### Core Parameters -| Name | Type | Default Value | Description | -| ----------------------------- | ------ | ------------- | ------------------------------------------------- | -| `frame_count` | int | `50` | The number of frames to publish heatmap | -| `map_frame` | string | `base_link` | the frame of heatmap to be respected | -| `map_length` | float | `200.0` | the length of map in meter | -| `map_resolution` | float | `0.8` | the resolution of map | -| `use_confidence` | bool | `false` | the flag if use confidence score as heatmap value | -| `rename_car_to_truck_and_bus` | bool | `true` | the flag if rename car to truck or bus | +| Name | Type | Default Value | Description | +| --------------------- | ------------- | ------------------------------------------------------------------------------------- | ----------------------------------------------- | +| `publish_frame_count` | int | `50` | The number of frames to publish heatmap | +| `heatmap_frame_id` | string | `base_link` | The frame ID of heatmap to be respected | +| `heatmap_length` | float | `200.0` | A length of map in meter | +| `heatmap_resolution` | float | `0.8` | A resolution of map | +| `use_confidence` | bool | `false` | A flag if use confidence score as heatmap value | +| `class_names` | array | `["UNKNOWN", "CAR", "TRUCK", "BUS", "TRAILER", "BICYCLE", "MOTORBIKE", "PEDESTRIAN"]` | An array of class names to be published | +| `rename_to_car` | bool | `true` | A flag if rename car like vehicle to car | ## Assumptions / Known limits diff --git a/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml b/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml new file mode 100644 index 0000000000000..2da863ea5e37a --- /dev/null +++ b/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml @@ -0,0 +1,33 @@ +# Copyright 2023 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +/**: + ros__parameters: + publish_frame_count: 50 + heatmap_frame_id: "base_link" + heatmap_length: 200.0 + heatmap_resolution: 0.8 + use_confidence: false + class_names: + [ + "UNKNOWN", + "CAR", + "TRUCK", + "BUS", + "TRAILER", + "BICYCLE", + "MOTORBIKE", + "PEDESTRIAN", + ] + rename_to_car: false diff --git a/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml b/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml index e87ae4e63a4d1..7251d776b6110 100644 --- a/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml +++ b/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml @@ -2,20 +2,10 @@ - - - - - - + - - - - - - + diff --git a/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json b/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json new file mode 100644 index 0000000000000..cb67ae383c35a --- /dev/null +++ b/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json @@ -0,0 +1,81 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Heatmap Visualizer Node", + "type": "object", + "definitions": { + "heatmap_visualizer": { + "type": "object", + "properties": { + "publish_frame_count": { + "type": "integer", + "description": "The number of frames to be published at once.", + "default": 50, + "minimum": 1 + }, + "heatmap_frame_id": { + "type": "string", + "description": "A frame ID in which visualized heatmap is with respect to.", + "default": "base_link" + }, + "heatmap_length": { + "type": "number", + "description": "A size length of heatmap [m].", + "default": 200.0, + "exclusiveMinimum": 0.0 + }, + "heatmap_resolution": { + "type": "number", + "description": "A cell resolution of heatmap [m].", + "default": 0.8, + "exclusiveMinimum": 0.0 + }, + "use_confidence": { + "type": "boolean", + "description": "Indicates whether to use object existence probability as score. It this parameter is false, 1.0 is set to score.", + "default": false + }, + "class_names": { + "type": "array", + "description": "An array of object class names.", + "default": [ + "UNKNOWN", + "CAR", + "TRUCK", + "BUS", + "TRAILER", + "BICYCLE", + "MOTORBIKE", + "PEDESTRIAN" + ], + "uniqueItems": true + }, + "rename_to_car": { + "type": "boolean", + "description": "Indicates whether to rename car like vehicle into car.", + "default": false + } + }, + "required": [ + "publish_frame_count", + "heatmap_frame_id", + "heatmap_length", + "heatmap_resolution", + "use_confidence", + "class_names", + "rename_to_car" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/heatmap_visualizer" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp b/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp index ad57d8638065f..d9337e0c9f61d 100644 --- a/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp +++ b/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp @@ -27,13 +27,13 @@ namespace heatmap_visualizer HeatmapVisualizerNode::HeatmapVisualizerNode(const rclcpp::NodeOptions & node_options) : Node("heatmap_visualizer", node_options), frame_count_(0) { - total_frame_ = declare_parameter("frame_count", 50); - map_frame_ = declare_parameter("map_frame", "base_link"); - map_length_ = declare_parameter("map_length", 200.0); - map_resolution_ = declare_parameter("map_resolution", 0.8); - use_confidence_ = declare_parameter("use_confidence", false); - class_names_ = declare_parameter("class_names", class_names_); - rename_car_to_truck_and_bus_ = declare_parameter("rename_car_to_truck_and_bus", false); + total_frame_ = static_cast(declare_parameter("publish_frame_count")); + map_frame_ = declare_parameter("heatmap_frame_id"); + map_length_ = static_cast(declare_parameter("heatmap_length")); + map_resolution_ = static_cast(declare_parameter("heatmap_resolution")); + use_confidence_ = declare_parameter("use_confidence"); + class_names_ = declare_parameter>("class_names"); + rename_car_to_truck_and_bus_ = declare_parameter("rename_to_car"); width_ = static_cast(map_length_ / map_resolution_); height_ = static_cast(map_length_ / map_resolution_); From 8770e1e7ff25d951b539b7d48f4e30024aa15cd8 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 4 Oct 2023 21:17:07 +0900 Subject: [PATCH 75/97] feat(intersection): ignore high curvature lane occlusion (#5223) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 2 + .../src/debug.cpp | 8 ++++ .../src/manager.cpp | 4 ++ .../src/scene_intersection.cpp | 5 ++- .../src/scene_intersection.hpp | 2 + .../src/util.cpp | 43 ++++++++++++++++++- .../src/util.hpp | 3 +- .../src/util_type.hpp | 1 + 8 files changed, 64 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 5e74e837bf186..ccd3612203af5 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -57,6 +57,8 @@ absence_traffic_light: creep_velocity: 1.388 # [m/s] maximum_peeking_distance: 6.0 # [m] + attention_lane_crop_curvature_threshold: 0.25 + attention_lane_curvature_calculation_ds: 0.5 enable_rtc: intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index c875611b16003..9ff638cb61876 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -173,6 +173,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array); } + if (debug_data_.occlusion_attention_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + debug_data_.occlusion_attention_area.value(), "occlusion_attention_area", lane_id_, 0.917, + 0.568, 0.596), + &debug_marker_array); + } + if (debug_data_.adjacent_area) { appendMarkerArray( createLaneletPolygonsMarkerArray( diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index d42c9b3ca2aa2..6f47dfbbe31fb 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -140,6 +140,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.absence_traffic_light.creep_velocity"); ip.occlusion.absence_traffic_light.maximum_peeking_distance = getOrDeclareParameter( node, ns + ".occlusion.absence_traffic_light.maximum_peeking_distance"); + ip.occlusion.attention_lane_crop_curvature_threshold = + getOrDeclareParameter(node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); + ip.occlusion.attention_lane_curvature_calculation_ds = + getOrDeclareParameter(node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 89cdeaae56723..10f1ed73bf435 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -989,6 +989,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.occlusion_attention_area = occlusion_attention_area; debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); // get intersection area @@ -1027,7 +1028,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (!occlusion_attention_divisions_) { occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution); + planner_data_->occupancy_grid->info.resolution, + planner_param_.occlusion.attention_lane_crop_curvature_threshold, + planner_param_.occlusion.attention_lane_curvature_calculation_ds); } const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 7bd923ed6198c..e75c57ffb3fc7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -119,6 +119,8 @@ class IntersectionModule : public SceneModuleInterface double creep_velocity; double maximum_peeking_distance; } absence_traffic_light; + double attention_lane_crop_curvature_threshold; + double attention_lane_curvature_calculation_ds; } occlusion; }; diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index f381925b13208..2f3fcc6f6fde0 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -42,6 +43,22 @@ #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; @@ -809,10 +826,26 @@ TrafficPrioritizedLevel getTrafficPrioritizedLevel( 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, - [[maybe_unused]] const lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const double resolution) + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, + const double curvature_threshold, const double curvature_calculation_ds) { using lanelet::utils::getCenterlineWithOffset; using lanelet::utils::to2D; @@ -824,6 +857,12 @@ std::vector generateDetectionLaneDivisions( if (turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0) { continue; } + 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); } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 6acc1d60443bf..a75545353fb7a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -109,7 +109,8 @@ TrafficPrioritizedLevel getTrafficPrioritizedLevel( std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, + const double curvature_threshold, const double curvature_calculation_ds); std::optional generateInterpolatedPath( const int lane_id, const std::set & associative_lane_ids, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 5c188f4aebebf..aed81d771e480 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -39,6 +39,7 @@ struct DebugData 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 intersection_area{std::nullopt}; std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; From 98004cedb5059b863f6c964cbacf6812736bd52b Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Wed, 4 Oct 2023 17:06:57 +0200 Subject: [PATCH 76/97] build(yabloc_common): add missing libgoogle-glog-dev dependency (#5225) * build(yabloc_common): add missing libgoogle-glog-dev dependency Signed-off-by: Esteve Fernandez * style(pre-commit): autofix --------- Signed-off-by: Esteve Fernandez Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/yabloc/yabloc_common/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index f3f46952dec13..d7e94ebefa24b 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -16,6 +16,7 @@ geometry_msgs lanelet2_core lanelet2_extension + libgoogle-glog-dev pcl_conversions rclcpp sensor_msgs From 8649d930353151e4fc656e5f2b8a67f2de35f535 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 5 Oct 2023 09:14:48 +0900 Subject: [PATCH 77/97] refactor(safety_check): support lambda function in target filtering logic (#5210) * feat(safety_check): make it possible to set various conditions Signed-off-by: satoshi-ota * feat(utils): add util func Signed-off-by: satoshi-ota * refactor(start_planner): use common function Signed-off-by: satoshi-ota * refactor(avoidance): use common function Signed-off-by: satoshi-ota * refactor(goal_planner): use common function Signed-off-by: satoshi-ota * refactor(ABLC): use common function Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../path_safety_checker/objects_filtering.hpp | 6 +- .../behavior_path_planner/utils/utils.hpp | 2 + .../goal_planner/goal_planner_module.cpp | 8 ++- .../lane_change/avoidance_by_lane_change.cpp | 8 ++- .../start_planner/start_planner_module.cpp | 8 ++- .../src/utils/avoidance/utils.cpp | 65 ++++++++++--------- .../src/utils/goal_planner/goal_searcher.cpp | 7 +- .../path_safety_checker/objects_filtering.cpp | 26 ++------ .../start_planner/geometric_pull_out.cpp | 8 ++- .../utils/start_planner/shift_pull_out.cpp | 9 ++- .../behavior_path_planner/src/utils/utils.cpp | 13 ++++ 11 files changed, 102 insertions(+), 58 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index e1256fd774027..4d5b9052a4681 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -117,14 +117,16 @@ void filterObjectsByClass( * lanelet. */ std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition); /** * @brief Separate the objects into two part based on whether the object is within lanelet. * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. */ std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition); /** * @brief Get the predicted path from an object. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index c5e5aa3e50191..3e03a09d3adf8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -336,6 +336,8 @@ std::optional getSignedDistanceFromBoundary( // misc +Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet); + std::vector getTargetLaneletPolygons( const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index ce88760212d90..4de8720a4a52d 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -21,6 +21,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -1300,12 +1301,17 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const return false; } + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length); const auto [pull_over_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); + *(planner_data_->dynamic_object), pull_over_lanes, condition); const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_over_lane_objects, parameters_->th_moving_object_velocity); const auto common_parameters = planner_data_->parameters; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 6d06b26cd1e17..ce49501b78d08 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -140,9 +141,14 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( { const auto p = std::dynamic_pointer_cast(avoidance_parameters_); + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto [object_within_target_lane, object_outside_target_lane] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, data.current_lanelets); + *planner_data_->dynamic_object, data.current_lanelets, condition); // Assume that the maximum allocation for data.other object is the sum of // objects_within_target_lane and object_outside_target_lane. The maximum allocation for diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 092b91499252c..f19bcf30e7771 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -770,9 +771,14 @@ std::vector StartPlannerModule::searchPullOutStartPoses( std::vector pull_out_start_pose{}; // filter pull out lanes stop objects + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, status_.pull_out_lanes); + *planner_data_->dynamic_object, status_.pull_out_lanes, condition); const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index b2b4ecd2ea42a..86886e4414838 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -564,25 +564,6 @@ double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const return v * std::cos(calcYawDeviation(p_ref, p_target)); } -bool isCentroidWithinLanelets( - const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets) -{ - if (target_lanelets.empty()) { - return false; - } - - const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; - lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); - - for (const auto & llt : target_lanelets) { - if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { - return true; - } - } - - return false; -} - lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset) @@ -1591,14 +1572,26 @@ std::vector getSafetyCheckTargetObjects( std::vector target_objects; - const auto append_target_objects = [&](const auto & check_lanes, const auto & objects) { - std::for_each(objects.begin(), objects.end(), [&](const auto & object) { - if (isCentroidWithinLanelets(object.object, check_lanes)) { - target_objects.push_back(utils::avoidance::transform(object.object, p)); - } + const auto append = [&](const auto & objects) { + std::for_each(objects.objects.begin(), objects.objects.end(), [&](const auto & object) { + target_objects.push_back(utils::avoidance::transform(object, p)); }); }; + const auto to_predicted_objects = [&p](const auto & objects) { + PredictedObjects ret{}; + std::for_each(objects.begin(), objects.end(), [&p, &ret](const auto & object) { + ret.objects.push_back(object.object); + }); + return ret; + }; + + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); + }; + const auto unavoidable_objects = [&data]() { ObjectDataArray ret; std::for_each(data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { @@ -1614,11 +1607,15 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = getAdjacentLane(planner_data, p, true); if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, condition); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, condition); + append(targets); } } @@ -1627,11 +1624,15 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = getAdjacentLane(planner_data, p, false); if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, condition); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, condition); + append(targets); } } @@ -1640,11 +1641,15 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = data.current_lanelets; if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, condition); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, condition); + append(targets); } } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 11173154f6b3a..bd374c12427bc 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -266,13 +266,18 @@ void GoalSearcher::countObjectsToAvoid( void GoalSearcher::update(GoalCandidates & goal_candidates) const { + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); const auto [pull_over_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_over_lanes); + utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_over_lanes, condition); if (parameters_.prioritize_goals_before_objects) { countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index ff54ce2d2dcc4..e63d35f7bc925 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -128,7 +128,8 @@ void filterObjectsByClass( } std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition) { if (target_lanelets.empty()) { return {}; @@ -138,25 +139,9 @@ std::pair, std::vector> separateObjectIndicesByLanel std::vector other_indices; for (size_t i = 0; i < objects.objects.size(); i++) { - // create object polygon - const auto & obj = objects.objects.at(i); - // create object polygon - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); bool is_filtered_object = false; for (const auto & llt : target_lanelets) { - // create lanelet polygon - const auto polygon2d = llt.polygon2d().basicPolygon(); - if (polygon2d.empty()) { - // no lanelet polygon - continue; - } - Polygon2d lanelet_polygon; - for (const auto & lanelet_point : polygon2d) { - lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); - } - lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); - // check the object does not intersect the lanelet - if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { + if (condition(objects.objects.at(i), llt)) { target_indices.push_back(i); is_filtered_object = true; break; @@ -172,13 +157,14 @@ std::pair, std::vector> separateObjectIndicesByLanel } std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition) { PredictedObjects target_objects; PredictedObjects other_objects; const auto [target_indices, other_indices] = - separateObjectIndicesByLanelets(objects, target_lanelets); + separateObjectIndicesByLanelets(objects, target_lanelets, condition); target_objects.objects.reserve(target_indices.size()); other_objects.objects.reserve(other_indices.size()); diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index bc071e2a2a288..f4bfb4d681440 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -62,10 +63,15 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; 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::separateObjectsByLanelets(stop_objects, pull_out_lanes, condition); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index b82157cc17e41..02c6d41bdbf08 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -64,8 +65,14 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P } // extract stop objects in pull out lane for collision check + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(*dynamic_objects, pull_out_lanes); + utils::path_safety_checker::separateObjectsByLanelets( + *dynamic_objects, pull_out_lanes, condition); const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_.th_moving_object_velocity); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 7305405b50410..03063b5a9e2fe 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2675,6 +2675,19 @@ double getArcLengthToTargetLanelet( std::min(arc_front.length - arc_pose.length, arc_back.length - arc_pose.length), 0.0); } +Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet) +{ + Polygon2d polygon; + for (const auto & p : lanelet.polygon2d().basicPolygon()) { + polygon.outer().emplace_back(p.x(), p.y()); + } + polygon.outer().push_back(polygon.outer().front()); + + return tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); +} + std::vector getTargetLaneletPolygons( const lanelet::PolygonLayer & map_polygons, lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type) From c763eda68ede1ccc5b50290cb61f5775061aa212 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Thu, 5 Oct 2023 11:32:43 +0900 Subject: [PATCH 78/97] fix(detected_object_validation): change the points_num of the validator to be set class by class (#5177) * fix: add param for each object class Signed-off-by: badai-nguyen * fix: add missing classes param Signed-off-by: badai-nguyen * fix: launch file Signed-off-by: badai-nguyen * fix: typo Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- ...mera_lidar_fusion_based_detection.launch.xml | 1 + ...idar_radar_fusion_based_detection.launch.xml | 1 + .../detection/lidar_based_detection.launch.xml | 1 + .../launch/perception.launch.xml | 1 + ...stacle_pointcloud_based_validator.param.yaml | 13 +++++++++++++ .../obstacle_pointcloud_based_validator.hpp | 8 ++++---- ...stacle_pointcloud_based_validator.launch.xml | 2 ++ .../src/obstacle_pointcloud_based_validator.cpp | 17 ++++++++++++----- 8 files changed, 35 insertions(+), 9 deletions(-) create mode 100644 perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 178e193dc2d99..51359d8d8f0e2 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -314,6 +314,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 38d141241c215..45cfb91bf25c1 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -348,6 +348,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 3b6b9ba652a24..c844db39f4e9d 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -115,6 +115,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index b0219376e9625..3d4ce1035e72c 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -17,6 +17,7 @@ + diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml new file mode 100644 index 0000000000000..745a0fd6591a9 --- /dev/null +++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + min_points_num: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10, 10, 10, 10, 10, 10, 10, 10] + + max_points_num: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10, 10, 10, 10, 10, 10, 10, 10] + + min_points_and_distance_ratio: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 270e5c7bdb7ff..5819302664907 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -32,14 +32,14 @@ #include #include - +#include namespace obstacle_pointcloud_based_validator { struct PointsNumThresholdParam { - size_t min_points_num; - size_t max_points_num; - float min_points_and_distance_ratio; + std::vector min_points_num; + std::vector max_points_num; + std::vector min_points_and_distance_ratio; }; class ObstaclePointCloudBasedValidator : public rclcpp::Node { diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index 8196e95f37957..799b605658345 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -3,11 +3,13 @@ + + diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index 00e53e9de9a9c..47640c9332e4d 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -92,10 +92,12 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); - points_num_threshold_param_.min_points_num = declare_parameter("min_points_num", 10); - points_num_threshold_param_.max_points_num = declare_parameter("max_points_num", 10); + points_num_threshold_param_.min_points_num = + declare_parameter>("min_points_num"); + points_num_threshold_param_.max_points_num = + declare_parameter>("max_points_num"); points_num_threshold_param_.min_points_and_distance_ratio = - declare_parameter("min_points_and_distance_ratio", 800.0); + declare_parameter>("min_points_and_distance_ratio"); const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); @@ -134,6 +136,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { const auto & transformed_object = transformed_objects.objects.at(i); + const auto object_label_id = transformed_object.classification.front().label; const auto & object = input_objects->objects.at(i); const auto & transformed_object_position = transformed_object.kinematics.pose_with_covariance.pose.position; @@ -155,13 +158,17 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); // Filter object that have few pointcloud in them. + // TODO(badai-nguyen) add 3d validator option const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud); const auto object_distance = std::hypot(transformed_object_position.x, transformed_object_position.y); size_t min_pointcloud_num = std::clamp( static_cast( - points_num_threshold_param_.min_points_and_distance_ratio / object_distance + 0.5f), - points_num_threshold_param_.min_points_num, points_num_threshold_param_.max_points_num); + points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / + object_distance + + 0.5f), + static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), + static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); if (num) { (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) : removed_objects.objects.push_back(object); From 02fda2f44217bdd57690302460f16ff3b9c4dcbe Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 5 Oct 2023 13:16:32 +0900 Subject: [PATCH 79/97] refactor(safety_check): use common function in safety check library (#5217) refactor(avoidance): use common function Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 10 ++- .../utils/avoidance/avoidance_module_data.hpp | 6 +- .../utils/avoidance/utils.hpp | 8 --- .../avoidance/avoidance_module.cpp | 14 ++-- .../src/scene_module/avoidance/manager.cpp | 22 ++++-- .../src/utils/avoidance/utils.cpp | 69 ++----------------- 6 files changed, 43 insertions(+), 86 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 589233cf5a441..df2dd806fe2c7 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -139,12 +139,16 @@ check_other_object: true # [-] # collision check parameters check_all_predicted_path: false # [-] - time_resolution: 0.5 # [s] - time_horizon_for_front_object: 10.0 # [s] - time_horizon_for_rear_object: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] hysteresis_factor_expand_rate: 2.0 # [-] hysteresis_factor_safe_count: 10 # [-] + # predicted path parameters + min_velocity: 1.38 # [m/s] + max_velocity: 50.0 # [m/s] + time_resolution: 0.5 # [s] + time_horizon_for_front_object: 10.0 # [s] + time_horizon_for_rear_object: 10.0 # [s] + delay_until_departure: 0.0 # [s] # rss parameters expected_front_deceleration: -1.0 # [m/ss] expected_rear_deceleration: -1.0 # [m/ss] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 634ac69bae12c..6173ecb15a5c2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -183,9 +183,6 @@ struct AvoidanceParameters // parameters for collision check. bool check_all_predicted_path{false}; - double time_horizon_for_front_object{0.0}; - double time_horizon_for_rear_object{0.0}; - double safety_check_time_resolution{0.0}; // find adjacent lane vehicles double safety_check_backward_distance{0.0}; @@ -295,6 +292,9 @@ struct AvoidanceParameters // parameters depend on object class std::unordered_map object_parameters; + // ego predicted path params. + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params{}; + // rss parameters utils::path_safety_checker::RSSparams rss_params{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 44d5d6f6b4e56..c0bd621cc86b4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -87,11 +87,6 @@ std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width); -std::vector convertToPredictedPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_object_front, const bool limit_to_max_velocity, - const std::shared_ptr & parameters); - double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); bool isCentroidWithinLanelets( @@ -162,9 +157,6 @@ AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine AvoidLineArray combineRawShiftLinesWithUniqueCheck( const AvoidLineArray & base_lines, const AvoidLineArray & added_lines); -ExtendedPredictedObject transform( - const PredictedObject & object, const std::shared_ptr & parameters); - std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 36908bd87de23..a8dd0b794245e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1839,10 +1839,16 @@ bool AvoidanceModule::isSafePath( } const bool limit_to_max_velocity = false; - const auto ego_predicted_path_for_front_object = utils::avoidance::convertToPredictedPath( - shifted_path.path, planner_data_, true, limit_to_max_velocity, parameters_); - const auto ego_predicted_path_for_rear_object = utils::avoidance::convertToPredictedPath( - shifted_path.path, planner_data_, false, limit_to_max_velocity, parameters_); + const auto ego_predicted_path_params = + std::make_shared( + parameters_->ego_predicted_path_params); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points); + const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + true, limit_to_max_velocity); + const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + false, limit_to_max_velocity); const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); const auto is_right_shift = [&]() -> std::optional { diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 912f867fd5701..8c65323e5a123 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -142,11 +142,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.check_other_object = getOrDeclareParameter(*node, ns + "check_other_object"); p.check_all_predicted_path = getOrDeclareParameter(*node, ns + "check_all_predicted_path"); - p.time_horizon_for_front_object = - getOrDeclareParameter(*node, ns + "time_horizon_for_front_object"); - p.time_horizon_for_rear_object = - getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); - p.safety_check_time_resolution = getOrDeclareParameter(*node, ns + "time_resolution"); p.safety_check_backward_distance = getOrDeclareParameter(*node, ns + "safety_check_backward_distance"); p.hysteresis_factor_expand_rate = @@ -155,6 +150,23 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); } + // safety check predicted path params + { + std::string ns = "avoidance.safety_check."; + p.ego_predicted_path_params.min_velocity = + getOrDeclareParameter(*node, ns + "min_velocity"); + p.ego_predicted_path_params.max_velocity = + getOrDeclareParameter(*node, ns + "max_velocity"); + p.ego_predicted_path_params.acceleration = + getOrDeclareParameter(*node, "avoidance.constraints.longitudinal.max_acceleration"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); + p.ego_predicted_path_params.time_resolution = + getOrDeclareParameter(*node, ns + "time_resolution"); + p.ego_predicted_path_params.delay_until_departure = + getOrDeclareParameter(*node, ns + "delay_until_departure"); + } + // safety check rss params { std::string ns = "avoidance.safety_check."; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 86886e4414838..d7da362402197 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -1457,68 +1457,6 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( return combined; } -std::vector convertToPredictedPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_object_front, const bool limit_to_max_velocity, - const std::shared_ptr & parameters) -{ - const auto & vehicle_pose = planner_data->self_odometry->pose.pose; - const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); - const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); - - auto ego_predicted_path_params = - std::make_shared(); - - ego_predicted_path_params->min_velocity = parameters->min_slow_down_speed; - ego_predicted_path_params->acceleration = parameters->max_acceleration; - ego_predicted_path_params->max_velocity = std::numeric_limits::infinity(); - ego_predicted_path_params->time_horizon_for_front_object = - parameters->time_horizon_for_front_object; - ego_predicted_path_params->time_horizon_for_rear_object = - parameters->time_horizon_for_rear_object; - ego_predicted_path_params->time_resolution = parameters->safety_check_time_resolution; - ego_predicted_path_params->delay_until_departure = 0.0; - - return behavior_path_planner::utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, path.points, vehicle_pose, initial_velocity, ego_seg_idx, - is_object_front, limit_to_max_velocity); -} - -ExtendedPredictedObject transform( - const PredictedObject & object, const std::shared_ptr & parameters) -{ - ExtendedPredictedObject extended_object; - extended_object.uuid = object.object_id; - extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; - extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; - extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; - extended_object.shape = object.shape; - - const auto & obj_velocity_norm = std::hypot( - extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); - const auto & time_horizon = - std::max(parameters->time_horizon_for_front_object, parameters->time_horizon_for_rear_object); - const auto & time_resolution = parameters->safety_check_time_resolution; - - extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); - for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { - const auto & path = object.kinematics.predicted_paths.at(i); - extended_object.predicted_paths.at(i).confidence = path.confidence; - - // create path - for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { - const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); - if (obj_pose) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); - extended_object.predicted_paths.at(i).path.emplace_back( - t, *obj_pose, obj_velocity_norm, obj_polygon); - } - } - } - - return extended_object; -} - lanelet::ConstLanelets getAdjacentLane( const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift) @@ -1572,9 +1510,14 @@ std::vector getSafetyCheckTargetObjects( std::vector target_objects; + const auto time_horizon = std::max( + parameters->ego_predicted_path_params.time_horizon_for_front_object, + parameters->ego_predicted_path_params.time_horizon_for_rear_object); + const auto append = [&](const auto & objects) { std::for_each(objects.objects.begin(), objects.objects.end(), [&](const auto & object) { - target_objects.push_back(utils::avoidance::transform(object, p)); + target_objects.push_back(utils::path_safety_checker::transform( + object, time_horizon, parameters->ego_predicted_path_params.time_resolution)); }); }; From dd275bf94e78e5c18d43a2228759812c267d2184 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 5 Oct 2023 14:43:08 +0900 Subject: [PATCH 80/97] fix(lane_change): current lane obj treated as target lane obj (#5214) Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/normal.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index d8fa9cb5630e9..c75970e6fcc2d 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -683,6 +683,16 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); std::vector> target_backward_polygons; for (const auto & target_backward_lane : target_backward_lanes) { + // Check to see is target_backward_lane is in current_lanes + // Without this check, current lane object might be treated as target lane object + const auto is_current_lane = [&](const lanelet::ConstLanelet & current_lane) { + return current_lane.id() == target_backward_lane.id(); + }; + + if (std::any_of(current_lanes.begin(), current_lanes.end(), is_current_lane)) { + continue; + } + lanelet::ConstLanelets lanelet{target_backward_lane}; auto lane_polygon = utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits::max()); From 877b04b133175fc39444a9400cd83ae8f22de5b4 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 5 Oct 2023 15:51:15 +0900 Subject: [PATCH 81/97] fix(blind_spot): fix runtime crash (#5227) Signed-off-by: Mamoru Sobue --- planning/behavior_velocity_blind_spot_module/src/debug.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index f95ed73c2a184..7da494bfd5231 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -19,6 +19,8 @@ #include #include +#include + #include #include From 8e971bcc8ef480e2791e436d7eb979f82d324e54 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 5 Oct 2023 17:22:20 +0900 Subject: [PATCH 82/97] fix(behavior_velocity_crosswalk_module): stop at stop line associated with crosswalk (#5231) don't consider margin when stop line is found Signed-off-by: kyoichi-sugahara --- .../behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 62acd191d8697..fe96985743b94 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -299,8 +299,7 @@ std::optional> CrosswalkModule::get const auto p_stop_lines = getLinestringIntersects( ego_path, lanelet::utils::to2D(stop_line).basicLineString(), ego_pos, 2); if (!p_stop_lines.empty()) { - return std::make_pair( - p_stop_lines.front(), -planner_param_.stop_distance_from_object - base_link2front); + return std::make_pair(p_stop_lines.front(), -base_link2front); } } From b0310e769c9184e70a3b2eecd616cd1eb066fd18 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 5 Oct 2023 18:49:30 +0900 Subject: [PATCH 83/97] refactor(safety_check): define filtering function in safety check library (#5228) Signed-off-by: satoshi-ota --- .../path_safety_checker/objects_filtering.hpp | 18 ++++++++++++++ .../goal_planner/goal_planner_module.cpp | 8 ++----- .../lane_change/avoidance_by_lane_change.cpp | 8 ++----- .../start_planner/start_planner_module.cpp | 8 ++----- .../src/utils/avoidance/utils.cpp | 24 +++++++++---------- .../src/utils/goal_planner/goal_searcher.cpp | 8 ++----- .../path_safety_checker/objects_filtering.cpp | 14 +++++++++++ .../start_planner/geometric_pull_out.cpp | 8 ++----- .../utils/start_planner/shift_pull_out.cpp | 7 +----- 9 files changed, 55 insertions(+), 48 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index 4d5b9052a4681..15b15bac40b51 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -38,6 +38,24 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +/** + * @brief Filters objects based on object centroid position. + * + * @param objects The predicted objects to filter. + * @param lanelet + * @return result. + */ +bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); + +/** + * @brief Filters objects based on object polygon overlapping with lanelet. + * + * @param objects The predicted objects to filter. + * @param lanelet + * @return result. + */ +bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); + /** * @brief Filters objects based on various criteria. * diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 4de8720a4a52d..9c2544cf60cec 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1301,17 +1301,13 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const return false; } - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length); const auto [pull_over_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes, condition); + *(planner_data_->dynamic_object), pull_over_lanes, + utils::path_safety_checker::isPolygonOverlapLanelet); const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_over_lane_objects, parameters_->th_moving_object_velocity); const auto common_parameters = planner_data_->parameters; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index ce49501b78d08..1f305e3988828 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -141,14 +141,10 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( { const auto p = std::dynamic_pointer_cast(avoidance_parameters_); - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto [object_within_target_lane, object_outside_target_lane] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, data.current_lanelets, condition); + *planner_data_->dynamic_object, data.current_lanelets, + utils::path_safety_checker::isPolygonOverlapLanelet); // Assume that the maximum allocation for data.other object is the sum of // objects_within_target_lane and object_outside_target_lane. The maximum allocation for diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index f19bcf30e7771..c828a747856d8 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -771,14 +771,10 @@ std::vector StartPlannerModule::searchPullOutStartPoses( std::vector pull_out_start_pose{}; // filter pull out lanes stop objects - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, status_.pull_out_lanes, condition); + *planner_data_->dynamic_object, status_.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); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index d7da362402197..0e7eb87feed9b 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -1529,12 +1529,6 @@ std::vector getSafetyCheckTargetObjects( return ret; }; - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; - lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); - return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); - }; - const auto unavoidable_objects = [&data]() { ObjectDataArray ret; std::for_each(data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { @@ -1551,13 +1545,15 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, condition); + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, condition); + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } } @@ -1568,13 +1564,15 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, condition); + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, condition); + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } } @@ -1585,13 +1583,15 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, condition); + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, condition); + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index bd374c12427bc..12044980ebd81 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -266,18 +266,14 @@ void GoalSearcher::countObjectsToAvoid( void GoalSearcher::update(GoalCandidates & goal_candidates) const { - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); const auto [pull_over_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_over_lanes, condition); + utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_over_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); if (parameters_.prioritize_goals_before_objects) { countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index e63d35f7bc925..dfc9f76b25e33 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -24,6 +24,20 @@ namespace behavior_path_planner::utils::path_safety_checker { +bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +{ + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); +} + +bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +{ + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); +} + PredictedObjects filterObjects( const std::shared_ptr & objects, const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index f4bfb4d681440..3cf42dbb5da26 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -63,15 +63,11 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; 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, condition); + 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, diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 02c6d41bdbf08..a430e18c330e1 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -65,14 +65,9 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P } // extract stop objects in pull out lane for collision check - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *dynamic_objects, pull_out_lanes, condition); + *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); From e12817df60b2579b1be513950d463b6652c4e717 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Fri, 6 Oct 2023 09:03:28 +0900 Subject: [PATCH 84/97] fix(tier4_perception_launch): fix faraway detection to reduce calculation cost (#5233) * fix(tier4_perception_launch): fix node order in radar_based_detection.launch Signed-off-by: scepter914 * fix comment out unused node Signed-off-by: scepter914 --------- Signed-off-by: scepter914 --- .../detection/radar_based_detection.launch.xml | 16 ++++++++-------- .../tracking/tracking.launch.xml | 2 ++ 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml index 52c8aedff8ef9..5b5646a061ac7 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml @@ -20,14 +20,8 @@ - - - - - - - + @@ -40,8 +34,14 @@ + + + + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 1ba1e8de9d42e..77de1e5995ee0 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -15,6 +15,7 @@ + From 6dfb197f95771421a7fc9acbdafe2028856ce789 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 6 Oct 2023 13:53:46 +0900 Subject: [PATCH 85/97] feat(obstacle_cruise_planner): obstacle type dependent slow down for obstacle cruise planner (#5208) * Add slow down parameters dependant on obstacle type Signed-off-by: Daniel Sanchez * divide obstacle parameters based on obstacle type Signed-off-by: Daniel Sanchez * preserve obstacle type for velocity calculation Signed-off-by: Daniel Sanchez * added pre-commit changes Signed-off-by: Daniel Sanchez * change sample config params Signed-off-by: Daniel Sanchez * Update Readme Signed-off-by: Daniel Sanchez * Update README Signed-off-by: Daniel Sanchez * small refactor for cleaner code Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez Signed-off-by: Daniel Sanchez --- planning/obstacle_cruise_planner/README.md | 21 +++-- .../config/obstacle_cruise_planner.param.yaml | 17 +++- .../common_structs.hpp | 8 +- .../planner_interface.hpp | 80 +++++++++++++++---- planning/obstacle_cruise_planner/src/node.cpp | 6 +- .../src/planner_interface.cpp | 7 +- 6 files changed, 101 insertions(+), 38 deletions(-) diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 314f9dcc29393..260f302791079 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -230,14 +230,19 @@ $$ ### Slow down planning -| Parameter | Type | Description | -| ---------------------------- | ------ | ------------------------------------------------------------------- | -| `slow_down.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m] | -| `slow_down.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m] | -| `slow_down.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m] | -| `slow_down.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m] | - -The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. +| Parameter | Type | Description | +| ----------------------------------------------- | -------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `slow_down.labels` | vector(string) | A vector of labels for customizing obstacle-label-based slow down behavior. Each label represents an obstacle type that will be treated differently when applying slow down. The possible labels are ("default" (Mandatory), "unknown","car","truck","bus","trailer","motorcycle","bicycle" or "pedestrian") | +| `slow_down.default.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `slow_down.default.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `slow_down.default.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `slow_down.default.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `(optional) slow_down."label".min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | +| `(optional) slow_down."label".max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | +| `(optional) slow_down."label".min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | +| `(optional) slow_down."label".max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | + +The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. The parameters can be customized depending on the obstacle type (see `slow_down.labels`), making it possible to adjust the slow down behavior depending if the obstacle is a pedestrian, bicycle, car, etc. The closest point on the obstacle to the ego's trajectory is calculated. Then, the slow down velocity is calculated by linear interpolation with the distance between the point and trajectory as follows. diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 73bc4a83249a4..123d0dda93b7a 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -179,10 +179,19 @@ slow_down: # parameters to calculate slow down velocity by linear interpolation - min_lat_margin: 0.2 - max_lat_margin: 1.0 - min_ego_velocity: 2.0 - max_ego_velocity: 8.0 + labels: + - "default" + - "pedestrian" + default: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 2.0 + max_ego_velocity: 8.0 + pedestrian: + min_lat_margin: 0.5 + max_lat_margin: 1.5 + min_ego_velocity: 1.0 + max_ego_velocity: 2.0 time_margin_on_target_velocity: 1.5 # [s] diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index e958074971f2a..c6cb73a680e5a 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -137,19 +137,21 @@ struct SlowDownObstacle : public TargetObstacleInterface { SlowDownObstacle( const std::string & arg_uuid, const rclcpp::Time & arg_stamp, - const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity, - const double arg_lat_velocity, const double arg_precise_lat_dist, + const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose, + const double arg_lon_velocity, const double arg_lat_velocity, const double arg_precise_lat_dist, const geometry_msgs::msg::Point & arg_front_collision_point, const geometry_msgs::msg::Point & arg_back_collision_point) : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), precise_lat_dist(arg_precise_lat_dist), front_collision_point(arg_front_collision_point), - back_collision_point(arg_back_collision_point) + back_collision_point(arg_back_collision_point), + classification(object_classification) { } double precise_lat_dist; // for efficient calculation geometry_msgs::msg::Point front_collision_point; geometry_msgs::msg::Point back_collision_point; + ObjectClassification classification; }; struct LongitudinalInfo diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 9a5a6521a6494..d8c31fb35df9b 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -221,12 +222,42 @@ class PlannerInterface struct SlowDownParam { + std::vector obstacle_labels{"default"}; + std::unordered_map types_map; + struct ObstacleSpecificParams + { + double max_lat_margin; + double min_lat_margin; + double max_ego_velocity; + double min_ego_velocity; + }; explicit SlowDownParam(rclcpp::Node & node) { - max_lat_margin = node.declare_parameter("slow_down.max_lat_margin"); - min_lat_margin = node.declare_parameter("slow_down.min_lat_margin"); - max_ego_velocity = node.declare_parameter("slow_down.max_ego_velocity"); - min_ego_velocity = node.declare_parameter("slow_down.min_ego_velocity"); + types_map = {{ObjectClassification::UNKNOWN, "unknown"}, + {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, + {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, + {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, + {ObjectClassification::PEDESTRIAN, "pedestrian"}}; + obstacle_labels = + node.declare_parameter>("slow_down.labels", obstacle_labels); + // obstacle label dependant parameters + for (const auto & label : obstacle_labels) { + ObstacleSpecificParams params; + params.max_lat_margin = + node.declare_parameter("slow_down." + label + ".max_lat_margin"); + params.min_lat_margin = + node.declare_parameter("slow_down." + label + ".min_lat_margin"); + params.max_ego_velocity = + node.declare_parameter("slow_down." + label + ".max_ego_velocity"); + params.min_ego_velocity = + node.declare_parameter("slow_down." + label + ".min_ego_velocity"); + obstacle_to_param_struct_map.emplace(std::make_pair(label, params)); + } + + // common parameters time_margin_on_target_velocity = node.declare_parameter("slow_down.time_margin_on_target_velocity"); lpf_gain_slow_down_vel = node.declare_parameter("slow_down.lpf_gain_slow_down_vel"); @@ -235,16 +266,35 @@ class PlannerInterface node.declare_parameter("slow_down.lpf_gain_dist_to_slow_down"); } + ObstacleSpecificParams getObstacleParamByLabel(const ObjectClassification & label_id) const + { + const std::string label = types_map.at(label_id.label); + if (obstacle_to_param_struct_map.count(label) > 0) { + return obstacle_to_param_struct_map.at(label); + } + return obstacle_to_param_struct_map.at("default"); + } + void onParam(const std::vector & parameters) { - tier4_autoware_utils::updateParam( - parameters, "slow_down.max_lat_margin", max_lat_margin); - tier4_autoware_utils::updateParam( - parameters, "slow_down.min_lat_margin", min_lat_margin); - tier4_autoware_utils::updateParam( - parameters, "slow_down.max_ego_velocity", max_ego_velocity); - tier4_autoware_utils::updateParam( - parameters, "slow_down.min_ego_velocity", min_ego_velocity); + // obstacle type dependant parameters + for (const auto & label : obstacle_labels) { + auto & param_by_obstacle_label = obstacle_to_param_struct_map[label]; + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".max_lat_margin", + param_by_obstacle_label.max_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".min_lat_margin", + param_by_obstacle_label.min_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".max_ego_velocity", + param_by_obstacle_label.max_ego_velocity); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".min_ego_velocity", + param_by_obstacle_label.min_ego_velocity); + } + + // common parameters tier4_autoware_utils::updateParam( parameters, "slow_down.time_margin_on_target_velocity", time_margin_on_target_velocity); tier4_autoware_utils::updateParam( @@ -255,10 +305,8 @@ class PlannerInterface parameters, "slow_down.lpf_gain_dist_to_slow_down", lpf_gain_dist_to_slow_down); } - double max_lat_margin; - double min_lat_margin; - double max_ego_velocity; - double min_ego_velocity; + std::unordered_map obstacle_to_param_struct_map; + double time_margin_on_target_velocity; double lpf_gain_slow_down_vel; double lpf_gain_lat_dist; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 7de6bb2c966f6..d50054da46237 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1175,9 +1175,9 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, - tangent_vel, normal_vel, precise_lat_dist, - front_collision_point, back_collision_point}; + return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, + obstacle.pose, tangent_vel, normal_vel, + precise_lat_dist, front_collision_point, back_collision_point}; } void ObstacleCruisePlannerNode::checkConsistency( diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 287df10dcebb5..76469364cfb39 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -665,7 +665,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( double PlannerInterface::calculateSlowDownVelocity( const SlowDownObstacle & obstacle, const std::optional & prev_output) const { - const auto & p = slow_down_param_; + const auto & p = slow_down_param_.getObstacleParamByLabel(obstacle.classification); const double stable_precise_lat_dist = [&]() { if (prev_output) { @@ -691,7 +691,6 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( const SlowDownObstacle & obstacle, const std::optional & prev_output, const double dist_to_ego) const { - const auto & p = slow_down_param_; const double abs_ego_offset = planner_data.is_driving_forward ? std::abs(vehicle_info_.max_longitudinal_offset_m) : std::abs(vehicle_info_.min_longitudinal_offset_m); @@ -728,8 +727,8 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( // calculate distance during deceleration, slow down preparation, and slow down const double min_slow_down_prepare_dist = 3.0; - const double slow_down_prepare_dist = - std::max(min_slow_down_prepare_dist, slow_down_vel * p.time_margin_on_target_velocity); + const double slow_down_prepare_dist = std::max( + min_slow_down_prepare_dist, slow_down_vel * slow_down_param_.time_margin_on_target_velocity); const double deceleration_dist = offset_dist_to_collision + dist_to_front_collision - abs_ego_offset - dist_to_ego - slow_down_prepare_dist; const double slow_down_dist = From 950805fda122ac31f9784e841ac5106b201bc1ca Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 6 Oct 2023 13:54:15 +0900 Subject: [PATCH 86/97] feat(intersection)!: disable the exception behavior in the private areas (#5229) * feat: make configurable to disable the exception treat of stuck obstacle in the private areas feat: change behavior in the private areas Signed-off-by: Yuki Takagi * delete the coment outed lines Signed-off-by: Yuki Takagi * change "enabled" to "enable" Signed-off-by: Yuki Takagi * fix setting Signed-off-by: Yuki Takagi * delete unused variables Signed-off-by: Yuki Takagi --------- Signed-off-by: Yuki Takagi --- .../config/intersection.param.yaml | 1 + .../src/manager.cpp | 2 ++ .../src/scene_intersection.cpp | 11 ++--------- .../src/scene_intersection.hpp | 2 +- 4 files changed, 6 insertions(+), 10 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index ccd3612203af5..e210d2fb21b88 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -19,6 +19,7 @@ # enable_front_car_decel_prediction: false # By default this feature is disabled # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area + enable_private_area_stuck_disregard: false #In the intersections which labeled as "private area", the obstacle vehicles judged as "stuck" are neglected if this param is set as true. collision_detection: state_transit_margin_time: 1.0 diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 6f47dfbbe31fb..3c96a171516bd 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -77,6 +77,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) */ ip.stuck_vehicle.timeout_private_area = getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); + ip.stuck_vehicle.enable_private_area_stuck_disregard = + getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard"); ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 10f1ed73bf435..b30a57f9e8dad 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -96,10 +96,6 @@ IntersectionModule::IntersectionModule( occlusion_stop_state_machine_.setMarginTime(planner_param_.occlusion.stop_release_margin_time); occlusion_stop_state_machine_.setState(StateMachine::State::GO); } - { - stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); - stuck_private_area_timeout_.setState(StateMachine::State::STOP); - } { temporal_stop_before_attention_state_machine_.setMarginTime( planner_param_.occlusion.before_creep_stop_time); @@ -918,16 +914,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); if (stuck_detected && stuck_stop_line_idx_opt) { auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); - const bool stopped_at_stuck_line = stoppedForDuration( - stuck_stop_line_idx, planner_param_.stuck_vehicle.timeout_private_area, - stuck_private_area_timeout_); - const bool timeout = (is_private_area_ && stopped_at_stuck_line); - if (!timeout) { + if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { if ( default_stop_line_idx_opt && fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { stuck_stop_line_idx = default_stop_line_idx_opt.value(); } + } else { return IntersectionModule::StuckStop{ closest_idx, stuck_stop_line_idx, occlusion_peeking_stop_line_idx_opt}; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index e75c57ffb3fc7..b5cf3ec04cdc3 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -70,6 +70,7 @@ class IntersectionModule : public SceneModuleInterface bool enable_front_car_decel_prediction; //! flag for using above feature */ double timeout_private_area; + bool enable_private_area_stuck_disregard; } stuck_vehicle; struct CollisionDetection { @@ -255,7 +256,6 @@ class IntersectionModule : public SceneModuleInterface // for stuck vehicle detection const bool is_private_area_; - StateMachine stuck_private_area_timeout_; // for RTC const UUID occlusion_uuid_; From 057d3d4259f69e0c83e7897a954ddac3f4818fcc Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 6 Oct 2023 20:16:42 +0900 Subject: [PATCH 87/97] feat(lane_change): separate execution and cancel safety check param (#5246) Signed-off-by: Zulfaqar Azmi --- .../config/lane_change/lane_change.param.yaml | 26 ++++--- .../lane_change/lane_change_module_data.hpp | 1 + .../src/scene_module/lane_change/manager.cpp | 70 +++++++++++++------ 3 files changed, 66 insertions(+), 31 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index fda7390abdb9e..e7f3b51bd2d26 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -28,15 +28,23 @@ # safety check safety_check: - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.8 + allow_loose_check_for_cancel: true + execution: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + cancel: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -2.0 + rear_vehicle_reaction_time: 1.5 + rear_vehicle_safety_time_margin: 0.8 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 2.5 + longitudinal_velocity_delta_time: 0.6 # lane expansion for object filtering lane_expansion: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 0ba35635a9143..24a507d1f8695 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -82,6 +82,7 @@ struct LaneChangeParameters utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; // safety check + bool allow_loose_check_for_cancel{true}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 9c45f79d00263..e33a6c4b05ee3 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -92,35 +92,41 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.stop_time_threshold = getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); + // safety check + p.allow_loose_check_for_cancel = + getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_distance_min_threshold")); + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_velocity_delta_time")); - p.rss_params.front_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.expected_front_deceleration")); - p.rss_params.rear_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.expected_rear_deceleration")); - p.rss_params.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); - p.rss_params.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); - p.rss_params.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); + *node, parameter("safety_check.execution.longitudinal_velocity_delta_time")); + p.rss_params.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_front_deceleration")); + p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_rear_deceleration")); + p.rss_params.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.execution.rear_vehicle_reaction_time")); + p.rss_params.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.execution.rear_vehicle_safety_time_margin")); + p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.lateral_distance_max_threshold")); p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_distance_min_threshold")); + *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_velocity_delta_time")); + *node, parameter("safety_check.cancel.longitudinal_velocity_delta_time")); p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.expected_front_deceleration_for_abort")); + *node, parameter("safety_check.cancel.expected_front_deceleration")); p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.expected_rear_deceleration_for_abort")); - p.rss_params_for_abort.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); - p.rss_params_for_abort.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); - p.rss_params_for_abort.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); + *node, parameter("safety_check.cancel.expected_rear_deceleration")); + p.rss_params_for_abort.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.cancel.rear_vehicle_reaction_time")); + p.rss_params_for_abort.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin")); + p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.cancel.lateral_distance_max_threshold")); // target object { @@ -165,6 +171,26 @@ LaneChangeModuleManager::LaneChangeModuleManager( exit(EXIT_FAILURE); } + // validation of safety check parameters + // if loosely check is not allowed, lane change module will keep on chattering and canceling, and + // false positive situation might occur + if (!p.allow_loose_check_for_cancel) { + if ( + p.rss_params.front_vehicle_deceleration > p.rss_params_for_abort.front_vehicle_deceleration || + p.rss_params.rear_vehicle_deceleration > p.rss_params_for_abort.rear_vehicle_deceleration || + p.rss_params.rear_vehicle_reaction_time > p.rss_params_for_abort.rear_vehicle_reaction_time || + p.rss_params.rear_vehicle_safety_time_margin > + p.rss_params_for_abort.rear_vehicle_safety_time_margin || + p.rss_params.lateral_distance_max_threshold > + p.rss_params_for_abort.lateral_distance_max_threshold || + p.rss_params.longitudinal_distance_min_threshold > + p.rss_params_for_abort.longitudinal_distance_min_threshold || + p.rss_params.longitudinal_velocity_delta_time > + p.rss_params_for_abort.longitudinal_velocity_delta_time) { + RCLCPP_FATAL_STREAM(logger_, "abort parameter might be loose... Terminating the program..."); + exit(EXIT_FAILURE); + } + } if (p.cancel.delta_time < 1.0) { RCLCPP_WARN_STREAM( logger_, "cancel.delta_time: " << p.cancel.delta_time From 0c48944c343cd74a2acb26cf428f113eb536a7f6 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Sat, 7 Oct 2023 01:26:59 +0900 Subject: [PATCH 88/97] feat(intersection): yield initially on green light (#5234) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 4 + .../src/manager.cpp | 8 ++ .../src/scene_intersection.cpp | 76 ++++++++++++++++++- .../src/scene_intersection.hpp | 29 ++++--- 4 files changed, 103 insertions(+), 14 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index e210d2fb21b88..972f572890d16 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -37,6 +37,10 @@ keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity + yield_on_green_traffic_light: + distance_to_assigned_lanelet_start: 5.0 + duration: 2.0 + range: 30.0 # [m] occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 3c96a171516bd..4137090a5b6ae 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -109,6 +109,14 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".collision_detection.use_upstream_velocity"); ip.collision_detection.minimum_upstream_velocity = getOrDeclareParameter(node, ns + ".collision_detection.minimum_upstream_velocity"); + ip.collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start = + getOrDeclareParameter( + node, + ns + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start"); + ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); + ip.collision_detection.yield_on_green_traffic_light.range = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.range"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index b30a57f9e8dad..75bb4e861a60f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -30,14 +30,15 @@ #include #include +#include #include #include #include +#include #include #include #include - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -807,6 +808,36 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } +static bool isGreenSolidOn( + 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 false; + } + 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 false; + } + const auto & tl_info = tl_info_it->second; + for (auto && tl_light : tl_info.signal.elements) { + if ( + tl_light.color == TrafficSignalElement::GREEN && + tl_light.shape == TrafficSignalElement::CIRCLE) { + return true; + } + } + return false; +} + IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { @@ -994,13 +1025,52 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } + const auto target_objects = + filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); + + // If there are any vehicles on the attention area when ego entered the intersection on green + // light, do pseudo collision detection because the vehicles are very slow and no collisions may + // be detected. check if ego vehicle entered assigned lanelet + const bool is_green_solid_on = + isGreenSolidOn(assigned_lanelet, planner_data_->traffic_light_id_map); + if (is_green_solid_on) { + if (!initial_green_light_observed_time_) { + const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path->points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } + } + if (initial_green_light_observed_time_) { + const auto now = clock_->now(); + const bool exist_close_vehicles = std::any_of( + target_objects.objects.begin(), target_objects.objects.end(), [&](const auto & object) { + return tier4_autoware_utils::calcDistance3d( + object.kinematics.initial_pose_with_covariance.pose, current_pose) < + planner_param_.collision_detection.yield_on_green_traffic_light.range; + }); + if ( + exist_close_vehicles && + rclcpp::Duration((now - initial_green_light_observed_time_.value())).seconds() < + planner_param_.collision_detection.yield_on_green_traffic_light.duration) { + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + } + } + // calculate dynamic collision around attention area const double time_to_restart = (is_go_out_ || is_prioritized) ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - collision_state_machine_.getDuration()); - const auto target_objects = - filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); const bool has_collision = checkCollision( *path, target_objects, path_lanelets, closest_idx, diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index b5cf3ec04cdc3..de7d97a50c133 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -96,6 +96,12 @@ class IntersectionModule : public SceneModuleInterface double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr bool use_upstream_velocity; double minimum_upstream_velocity; + struct YieldOnGreeTrafficLight + { + double distance_to_assigned_lanelet_start; + double duration; + double range; + } yield_on_green_traffic_light; } collision_detection; struct Occlusion { @@ -234,36 +240,37 @@ class IntersectionModule : public SceneModuleInterface const std::string turn_direction_; const bool has_traffic_light_; - bool is_go_out_ = false; - bool is_permanent_go_ = false; - DecisionResult prev_decision_result_; + bool is_go_out_{false}; + bool is_permanent_go_{false}; + DecisionResult prev_decision_result_{Indecisive{""}}; // Parameter PlannerParam planner_param_; - std::optional intersection_lanelets_; + std::optional intersection_lanelets_{std::nullopt}; // for occlusion detection const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_; - // OcclusionState prev_occlusion_state_ = OcclusionState::NONE; + std::optional> occlusion_attention_divisions_{std::nullopt}; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; StateMachine temporal_stop_before_attention_state_machine_; - // NOTE: uuid_ is base member + // 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 stuck vehicle detection const bool is_private_area_; // for RTC const UUID occlusion_uuid_; - bool occlusion_safety_ = true; - double occlusion_stop_distance_; - bool occlusion_activated_ = true; + 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; + bool occlusion_first_stop_required_{false}; void initializeRTCStatus(); void prepareRTCStatus( From 83821d4818cf83ea3bcb14511fc64482859ed7f1 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Sat, 7 Oct 2023 17:20:47 +0900 Subject: [PATCH 89/97] feat(rtc_auto_mode_manager): eliminate rtc auto mode manager (#5235) * change namespace of auto_mode Signed-off-by: kyoichi-sugahara * delete RTC auto mode manager package Signed-off-by: kyoichi-sugahara * delete rtc_replayer.param Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * fix typo Signed-off-by: kyoichi-sugahara * fix typo Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../launch/planning.launch.xml | 2 - .../scenario_planning/lane_driving.launch.xml | 5 - launch/tier4_planning_launch/package.xml | 1 - .../config/scene_module_manager.param.yaml | 20 ++-- .../config/blind_spot.param.yaml | 2 +- .../config/crosswalk.param.yaml | 2 +- .../config/detection_area.param.yaml | 2 +- .../config/intersection.param.yaml | 4 +- .../config/no_stopping_area.param.yaml | 2 +- .../config/traffic_light.param.yaml | 2 +- planning/rtc_auto_mode_manager/CMakeLists.txt | 29 ----- planning/rtc_auto_mode_manager/README.md | 47 -------- .../config/rtc_auto_mode_manager.param.yaml | 31 ----- .../include/rtc_auto_mode_manager/node.hpp | 44 ------- .../rtc_auto_mode_manager_interface.hpp | 53 --------- .../launch/rtc_auto_mode_manager.launch.xml | 7 -- planning/rtc_auto_mode_manager/package.xml | 28 ----- planning/rtc_auto_mode_manager/src/node.cpp | 51 -------- .../src/rtc_auto_mode_manager_interface.cpp | 109 ------------------ .../include/rtc_interface/rtc_interface.hpp | 2 +- .../launch/rtc_replayer.launch.xml | 6 +- 21 files changed, 19 insertions(+), 430 deletions(-) delete mode 100644 planning/rtc_auto_mode_manager/CMakeLists.txt delete mode 100644 planning/rtc_auto_mode_manager/README.md delete mode 100644 planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml delete mode 100644 planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp delete mode 100644 planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp delete mode 100644 planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml delete mode 100644 planning/rtc_auto_mode_manager/package.xml delete mode 100644 planning/rtc_auto_mode_manager/src/node.cpp delete mode 100644 planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 7e78c8842f7df..6ace3f423d1bc 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -15,8 +15,6 @@ - - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 24a33b9d3f3df..5f17256df6b5c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -14,11 +14,6 @@ - - - - - diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 39b95286bb6cc..5988d34cded88 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -68,7 +68,6 @@ obstacle_stop_planner planning_evaluator planning_validator - rtc_auto_mode_manager scenario_selector surround_obstacle_checker diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index 9895d9b5e473f..5797704e8a0ca 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -5,7 +5,7 @@ ros__parameters: external_request_lane_change_left: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -14,7 +14,7 @@ external_request_lane_change_right: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -23,7 +23,7 @@ lane_change_left: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -32,7 +32,7 @@ lane_change_right: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -41,7 +41,7 @@ start_planner: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -50,7 +50,7 @@ side_shift: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -59,7 +59,7 @@ goal_planner: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: true @@ -68,7 +68,7 @@ avoidance: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -77,7 +77,7 @@ avoidance_by_lc: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -86,7 +86,7 @@ dynamic_avoidance: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true keep_last: false diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml index 3aa8c82a5e556..f9ddd3ce61099 100644 --- a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml @@ -8,4 +8,4 @@ max_future_movement_time: 10.0 # [second] threshold_yaw_diff: 0.523 # [rad] adjacent_extend_width: 1.5 # [m] - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index e7accc14096e0..a4f9e9d7ce23d 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -5,7 +5,7 @@ show_processing_time: false # [-] whether to show processing time # param for input data traffic_light_state_timeout: 3.0 # [s] timeout threshold for traffic light signal - enable_rtc: true # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. + enable_rtc: false # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. # param for stop position stop_position: diff --git a/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml b/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml index b5d075a4d6493..62b5f2458336f 100644 --- a/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml +++ b/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml @@ -8,4 +8,4 @@ state_clear_time: 2.0 hold_stop_margin_distance: 0.0 distance_to_judge_over_stop_line: 0.5 - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 972f572890d16..30fefcaeee035 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -66,8 +66,8 @@ attention_lane_curvature_calculation_ds: 0.5 enable_rtc: - intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval - intersection_to_occlusion: true + intersection: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + intersection_to_occlusion: false merge_from_private: stop_line_margin: 3.0 diff --git a/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml b/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml index f550188d4f2c1..c84848f8cc31d 100644 --- a/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml +++ b/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml @@ -8,4 +8,4 @@ stop_line_margin: 1.0 # [m] margin to auto-gen stop line at no stopping area detection_area_length: 200.0 # [m] used to create detection area polygon stuck_vehicle_front_margin: 6.0 # [m] obstacle stop max distance(5.0m) - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index e8e0357daa4a1..a837ae1b46b9b 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -5,4 +5,4 @@ tl_state_timeout: 1.0 yellow_lamp_period: 2.75 enable_pass_judge: true - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/rtc_auto_mode_manager/CMakeLists.txt b/planning/rtc_auto_mode_manager/CMakeLists.txt deleted file mode 100644 index 52c0c68384a57..0000000000000 --- a/planning/rtc_auto_mode_manager/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(rtc_auto_mode_manager) - -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/rtc_auto_mode_manager_interface.cpp - src/node.cpp -) - -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "rtc_auto_mode_manager::RTCAutoModeManagerNode" - EXECUTABLE ${PROJECT_NAME}_node -) - -ament_auto_package( - INSTALL_TO_SHARE - config - launch -) diff --git a/planning/rtc_auto_mode_manager/README.md b/planning/rtc_auto_mode_manager/README.md deleted file mode 100644 index 0e2614baaf1c8..0000000000000 --- a/planning/rtc_auto_mode_manager/README.md +++ /dev/null @@ -1,47 +0,0 @@ -# RTC Auto Mode Manager - -## Purpose - -RTC Auto Mode Manager is a node to approve request to cooperate from behavior planning modules automatically. - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ------------------------------- | --------------------------- | ------------------------------------------ | -| `/planning/enable_auto_mode/**` | tier4_rtc_msgs/srv/AutoMode | Service to enable auto mode for the module | - -### Output - -| Name | Type | Description | -| ---------------------------------------- | --------------------------- | ------------------------------------------ | -| `/planning/enable_auto_mode/internal/**` | tier4_rtc_msgs/srv/AutoMode | Service to enable auto mode for the module | - -## Parameters - -| Name | Type | Description | -| :-------------------- | :--------------- | :----------------------------------------------- | -| `module_list` | List of `string` | Module names managing in `rtc_auto_mode_manager` | -| `default_enable_list` | List of `string` | Module names enabled auto mode at initialization | - -## Inner-workings / Algorithms - -```plantuml - -start -:Read parameters; -:Send enable auto mode service to the module listed in `default_enable_list`; -repeat - if (Enable auto mode command received?) then (yes) - :Send enable auto mode command to rtc_interface; - else (no) - endif -repeat while (Is node running?) is (yes) not (no) -end - -``` - -## Assumptions / Known limits - -## Future extensions / Unimplemented parts diff --git a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml b/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml deleted file mode 100644 index 0aa3cbd49e8e9..0000000000000 --- a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml +++ /dev/null @@ -1,31 +0,0 @@ -/**: - ros__parameters: - module_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "goal_planner" - - "start_planner" - - "intersection_occlusion" - - default_enable_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "goal_planner" - - "start_planner" - - "intersection_occlusion" diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp deleted file mode 100644 index d36974508d2df..0000000000000 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RTC_AUTO_MODE_MANAGER__NODE_HPP_ -#define RTC_AUTO_MODE_MANAGER__NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp" - -#include "tier4_rtc_msgs/msg/auto_mode_status_array.hpp" - -#include -#include -#include - -namespace rtc_auto_mode_manager -{ -using tier4_rtc_msgs::msg::AutoModeStatusArray; -class RTCAutoModeManagerNode : public rclcpp::Node -{ -public: - explicit RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_options); - -private: - rclcpp::TimerBase::SharedPtr timer_; - AutoModeStatusArray auto_mode_statuses_; - rclcpp::Publisher::SharedPtr statuses_pub_; - std::vector> managers_; -}; - -} // namespace rtc_auto_mode_manager - -#endif // RTC_AUTO_MODE_MANAGER__NODE_HPP_ diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp deleted file mode 100644 index 098852c397161..0000000000000 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ -#define RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "tier4_rtc_msgs/msg/auto_mode_status.hpp" -#include "tier4_rtc_msgs/msg/module.hpp" -#include "tier4_rtc_msgs/srv/auto_mode.hpp" - -#include -#include -#include - -namespace rtc_auto_mode_manager -{ -using tier4_rtc_msgs::msg::AutoModeStatus; -using tier4_rtc_msgs::msg::Module; -using tier4_rtc_msgs::srv::AutoMode; - -class RTCAutoModeManagerInterface -{ -public: - RTCAutoModeManagerInterface( - rclcpp::Node * node, const std::string & module_name, const bool default_enable); - AutoModeStatus getAutoModeStatus() const { return auto_mode_status_; } - -private: - void onEnableService( - const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response); - AutoMode::Request createRequest(const AutoMode::Request::SharedPtr request) const; - AutoModeStatus auto_mode_status_; - rclcpp::Client::SharedPtr enable_cli_; - rclcpp::Service::SharedPtr enable_srv_; - - std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; -}; -} // namespace rtc_auto_mode_manager - -#endif // RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ diff --git a/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml b/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml deleted file mode 100644 index 85d4b3446fb89..0000000000000 --- a/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/planning/rtc_auto_mode_manager/package.xml b/planning/rtc_auto_mode_manager/package.xml deleted file mode 100644 index e9c16aac1bb91..0000000000000 --- a/planning/rtc_auto_mode_manager/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - rtc_auto_mode_manager - 0.1.0 - The rtc_auto_mode_manager package - - Fumiya Watanabe - Taiki Tanaka - - Apache License 2.0 - - Fumiya Watanabe - - ament_cmake_auto - autoware_cmake - - rclcpp - rclcpp_components - tier4_rtc_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/planning/rtc_auto_mode_manager/src/node.cpp b/planning/rtc_auto_mode_manager/src/node.cpp deleted file mode 100644 index 6092d2741d909..0000000000000 --- a/planning/rtc_auto_mode_manager/src/node.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "rtc_auto_mode_manager/node.hpp" - -#include - -namespace rtc_auto_mode_manager -{ - -RTCAutoModeManagerNode::RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_options) -: Node("rtc_auto_mode_manager_node", node_options) -{ - const std::vector module_list = - declare_parameter>("module_list"); - const std::vector default_enable_list = - declare_parameter>("default_enable_list"); - - for (const auto & module_name : module_list) { - const bool enabled = - std::count(default_enable_list.begin(), default_enable_list.end(), module_name) != 0; - managers_.push_back(std::make_shared(this, module_name, enabled)); - } - statuses_pub_ = create_publisher("output/auto_mode_statuses", 1); - using std::chrono_literals::operator""ms; - timer_ = rclcpp::create_timer(this, get_clock(), 100ms, [this] { - AutoModeStatusArray auto_mode_statuses; - for (const auto & m : managers_) { - auto_mode_statuses.stamp = get_clock()->now(); - auto_mode_statuses.statuses.emplace_back(m->getAutoModeStatus()); - } - statuses_pub_->publish(auto_mode_statuses); - auto_mode_statuses.statuses.clear(); - }); -} - -} // namespace rtc_auto_mode_manager - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(rtc_auto_mode_manager::RTCAutoModeManagerNode) diff --git a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp deleted file mode 100644 index e28957cee161b..0000000000000 --- a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp" - -namespace rtc_auto_mode_manager -{ -Module getModuleType(const std::string & module_name) -{ - Module module; - if (module_name == "blind_spot") { - module.type = Module::BLIND_SPOT; - } else if (module_name == "crosswalk") { - module.type = Module::CROSSWALK; - } else if (module_name == "detection_area") { - module.type = Module::DETECTION_AREA; - } else if (module_name == "intersection") { - module.type = Module::INTERSECTION; - } else if (module_name == "no_stopping_area") { - module.type = Module::NO_STOPPING_AREA; - } else if (module_name == "occlusion_spot") { - module.type = Module::OCCLUSION_SPOT; - } else if (module_name == "traffic_light") { - module.type = Module::TRAFFIC_LIGHT; - } else if (module_name == "virtual_traffic_light") { - module.type = Module::TRAFFIC_LIGHT; - } else if (module_name == "external_request_lane_change_left") { - module.type = Module::EXT_REQUEST_LANE_CHANGE_LEFT; - } else if (module_name == "external_request_lane_change_right") { - module.type = Module::EXT_REQUEST_LANE_CHANGE_RIGHT; - } else if (module_name == "lane_change_left") { - module.type = Module::LANE_CHANGE_LEFT; - } else if (module_name == "lane_change_right") { - module.type = Module::LANE_CHANGE_RIGHT; - } else if (module_name == "avoidance_by_lane_change_left") { - module.type = Module::AVOIDANCE_BY_LC_LEFT; - } else if (module_name == "avoidance_by_lane_change_right") { - module.type = Module::AVOIDANCE_BY_LC_RIGHT; - } else if (module_name == "avoidance_left") { - module.type = Module::AVOIDANCE_LEFT; - } else if (module_name == "avoidance_right") { - module.type = Module::AVOIDANCE_RIGHT; - } else if (module_name == "goal_planner") { - module.type = Module::GOAL_PLANNER; - } else if (module_name == "start_planner") { - module.type = Module::START_PLANNER; - } else if (module_name == "intersection_occlusion") { - module.type = Module::INTERSECTION_OCCLUSION; - } else { - module.type = Module::NONE; - } - return module; -} - -RTCAutoModeManagerInterface::RTCAutoModeManagerInterface( - rclcpp::Node * node, const std::string & module_name, const bool default_enable) -{ - using std::chrono_literals::operator""s; - using std::placeholders::_1; - using std::placeholders::_2; - - // Service client - enable_cli_ = - node->create_client(enable_auto_mode_namespace_ + "/internal/" + module_name); - - while (!enable_cli_->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for service."); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO_STREAM(node->get_logger(), "Waiting for service... [" << module_name << "]"); - } - - // Service - enable_srv_ = node->create_service( - enable_auto_mode_namespace_ + "/" + module_name, - std::bind(&RTCAutoModeManagerInterface::onEnableService, this, _1, _2)); - - auto_mode_status_.module = getModuleType(module_name); - // Send enable auto mode request - if (default_enable) { - auto_mode_status_.is_auto_mode = true; - AutoMode::Request::SharedPtr request = std::make_shared(); - request->enable = true; - enable_cli_->async_send_request(request); - } -} - -void RTCAutoModeManagerInterface::onEnableService( - const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) -{ - auto_mode_status_.is_auto_mode = request->enable; - enable_cli_->async_send_request(request); - response->success = true; -} - -} // namespace rtc_auto_mode_manager diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 4929d0f8e27f3..d6e083421c0b7 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -85,7 +85,7 @@ class RTCInterface std::string cooperate_status_namespace_ = "/planning/cooperate_status"; std::string cooperate_commands_namespace_ = "/planning/cooperate_commands"; - std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode/internal"; + std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; mutable std::mutex mutex_; }; diff --git a/planning/rtc_replayer/launch/rtc_replayer.launch.xml b/planning/rtc_replayer/launch/rtc_replayer.launch.xml index 9b832b369c77c..70d7baf2dce4a 100644 --- a/planning/rtc_replayer/launch/rtc_replayer.launch.xml +++ b/planning/rtc_replayer/launch/rtc_replayer.launch.xml @@ -1,7 +1,3 @@ - - - - - + From c508aa71c19d09f4198e24cd93c39191c9dacca6 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Mon, 9 Oct 2023 11:29:00 +0900 Subject: [PATCH 90/97] fix(image_projection_based_fusion): add iou_x use in long range for roi_cluster_fusion (#5148) * fix: add iou_x for long range obj Signed-off-by: badai-nguyen * fix: add launch file param Signed-off-by: badai-nguyen * chore: fix unexpect calc iou in long range Signed-off-by: badai-nguyen * fix: multi iou usable Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * docs: update readme Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- ...ra_lidar_fusion_based_detection.launch.xml | 6 +- ...ar_radar_fusion_based_detection.launch.xml | 6 +- .../detection/detection.launch.xml | 6 +- .../launch/perception.launch.xml | 6 +- .../docs/roi-cluster-fusion.md | 26 ++++---- .../roi_cluster_fusion/node.hpp | 18 +++--- .../launch/roi_cluster_fusion.launch.xml | 13 ++-- .../src/roi_cluster_fusion/node.cpp | 59 ++++++++++++------- 8 files changed, 86 insertions(+), 54 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 51359d8d8f0e2..67e123ea5b018 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -15,7 +15,8 @@ - + + @@ -169,7 +170,8 @@ - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 45cfb91bf25c1..fbe5f21c041b6 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -15,7 +15,8 @@ - + + @@ -190,7 +191,8 @@ - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 77a3e345ec5e8..1fcf29606891b 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -35,7 +35,8 @@ - + + @@ -64,7 +65,8 @@ - + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 3d4ce1035e72c..bbb7ebb2d8d25 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -93,7 +93,8 @@ - + + @@ -169,7 +170,8 @@ - + + diff --git a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md index 01cdc3756866b..03eaab2a3c6ca 100644 --- a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md @@ -32,18 +32,20 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a ### Core Parameters -| Name | Type | Description | -| --------------------------- | ----- | -------------------------------------------------------------------------------- | -| `use_iou_x` | bool | calculate IoU only along x-axis | -| `use_iou_y` | bool | calculate IoU only along y-axis | -| `use_iou` | bool | calculate IoU both along x-axis and y-axis | -| `use_cluster_semantic_type` | bool | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion | -| `only_allow_inside_cluster` | bool | if `true`, the only clusters contained inside RoIs by a detector | -| `roi_scale_factor` | float | the scale factor for offset of detector RoIs if `only_allow_inside_cluster=true` | -| `iou_threshold` | float | the IoU threshold to overwrite a label of clusters with a label of roi | -| `unknown_iou_threshold` | float | the IoU threshold to fuse cluster with unknown label of roi | -| `rois_number` | int | the number of input rois | -| `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | +| Name | Type | Description | +| --------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `fusion_distance` | double | If the detected object's distance to frame_id is less than the threshold, the fusion will be processed | +| `trust_object_distance` | double | if the detected object's distance is less than the `trust_object_distance`, `trust_object_iou_mode` will be used, otherwise `non_trust_object_iou_mode` will be used | +| `trust_object_iou_mode` | string | select mode from 3 options {`iou`, `iou_x`, `iou_y`} to calculate IoU in range of [`0`, `trust_distance`].
 `iou`: IoU along x-axis and y-axis
 `iou_x`: IoU along x-axis
 `iou_y`: IoU along y-axis | +| `non_trust_object_iou_mode` | string | the IOU mode using in range of [`trust_distance`, `fusion_distance`] if `trust_distance` < `fusion_distance` | +| `use_cluster_semantic_type` | bool | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion | +| `only_allow_inside_cluster` | bool | if `true`, the only clusters contained inside RoIs by a detector | +| `roi_scale_factor` | double | the scale factor for offset of detector RoIs if `only_allow_inside_cluster=true` | +| `iou_threshold` | double | the IoU threshold to overwrite a label of clusters with a label of roi | +| `unknown_iou_threshold` | double | the IoU threshold to fuse cluster with unknown label of roi | +| `remove_unknown` | bool | if `true`, remove all `UNKNOWN` labeled objects from output | +| `rois_number` | int | the number of input rois | +| `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | ## Assumptions / Known limits diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 70a7866e79b87..e54710ad477da 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -17,10 +17,12 @@ #include "image_projection_based_fusion/fusion_node.hpp" +#include #include - +#include namespace image_projection_based_fusion { +const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; class RoiClusterFusionNode : public FusionNode @@ -38,9 +40,7 @@ class RoiClusterFusionNode const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) override; - bool use_iou_x_{false}; - bool use_iou_y_{false}; - bool use_iou_{false}; + std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; bool only_allow_inside_cluster_{false}; double roi_scale_factor_{1.1}; @@ -49,10 +49,14 @@ class RoiClusterFusionNode const float min_roi_existence_prob_ = 0.1; // keep small value to lessen affect on merger object stage bool remove_unknown_; - double trust_distance_; - - bool filter_by_distance(const DetectedObjectWithFeature & obj); + double fusion_distance_; + double trust_object_distance_; + std::string non_trust_object_iou_mode_{"iou_x"}; + bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold); bool out_of_scope(const DetectedObjectWithFeature & obj); + double cal_iou_by_mode( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode); // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 2d99c25bb68f7..60f6f943b8cda 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -1,5 +1,5 @@ - + @@ -20,7 +20,8 @@ - + + @@ -45,9 +46,8 @@ - - - + + @@ -55,7 +55,8 @@ - + + diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 316d29dae1301..4a46c8aace696 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -36,16 +36,16 @@ namespace image_projection_based_fusion RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_cluster_fusion", options) { - use_iou_x_ = declare_parameter("use_iou_x"); - use_iou_y_ = declare_parameter("use_iou_y"); - use_iou_ = declare_parameter("use_iou"); + trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); + non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type"); only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster"); roi_scale_factor_ = declare_parameter("roi_scale_factor"); iou_threshold_ = declare_parameter("iou_threshold"); unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold"); remove_unknown_ = declare_parameter("remove_unknown"); - trust_distance_ = declare_parameter("trust_distance"); + fusion_distance_ = declare_parameter("fusion_distance"); + trust_object_distance_ = declare_parameter("trust_object_distance"); } void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) @@ -111,7 +111,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } - if (filter_by_distance(input_cluster_msg.feature_objects.at(i))) { + if (is_far_enough(input_cluster_msg.feature_objects.at(i), fusion_distance_)) { continue; } @@ -175,25 +175,23 @@ void RoiClusterFusionNode::fuseOnSingleImage( bool is_roi_label_known = feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; for (const auto & cluster_map : m_cluster_roi) { - double iou(0.0), iou_x(0.0), iou_y(0.0); - if (use_iou_) { - iou = calcIoU(cluster_map.second, feature_obj.feature.roi); - } - // use for unknown roi to improve small objects like traffic cone detect - // TODO(badai-nguyen): add option to disable roi_cluster mode - if (use_iou_x_ || !is_roi_label_known) { - iou_x = calcIoUX(cluster_map.second, feature_obj.feature.roi); - } - if (use_iou_y_) { - iou_y = calcIoUY(cluster_map.second, feature_obj.feature.roi); + double iou(0.0); + bool is_use_non_trust_object_iou_mode = is_far_enough( + input_cluster_msg.feature_objects.at(cluster_map.first), trust_object_distance_); + if (is_use_non_trust_object_iou_mode || is_roi_label_known) { + iou = + cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, non_trust_object_iou_mode_); + } else { + iou = cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, trust_object_iou_mode_); } + const bool passed_inside_cluster_gate = only_allow_inside_cluster_ ? is_inside(feature_obj.feature.roi, cluster_map.second, roi_scale_factor_) : true; - if (max_iou < iou + iou_x + iou_y && passed_inside_cluster_gate) { + if (max_iou < iou && passed_inside_cluster_gate) { index = cluster_map.first; - max_iou = iou + iou_x + iou_y; + max_iou = iou; associated = true; } } @@ -274,11 +272,30 @@ bool RoiClusterFusionNode::out_of_scope(const DetectedObjectWithFeature & obj) return is_out; } -bool RoiClusterFusionNode::filter_by_distance(const DetectedObjectWithFeature & obj) +bool RoiClusterFusionNode::is_far_enough( + const DetectedObjectWithFeature & obj, const double distance_threshold) { const auto & position = obj.object.kinematics.pose_with_covariance.pose.position; - const auto square_distance = position.x * position.x + position.y + position.y; - return square_distance > trust_distance_ * trust_distance_; + return position.x * position.x + position.y * position.y > + distance_threshold * distance_threshold; +} + +double RoiClusterFusionNode::cal_iou_by_mode( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode) +{ + switch (IOU_MODE_MAP.at(iou_mode)) { + case 0 /* use iou mode */: + return calcIoU(roi_1, roi_2); + + case 1 /* use iou_x mode */: + return calcIoUX(roi_1, roi_2); + + case 2 /* use iou_y mode */: + return calcIoUY(roi_1, roi_2); + default: + return 0.0; + } } } // namespace image_projection_based_fusion From 9ddb07ae06c269d8cf007bab8fe402c24de54223 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 10 Oct 2023 14:06:17 +0900 Subject: [PATCH 91/97] fix(vehicle_cmd_gate): fix filtering condition (#5250) Signed-off-by: kminoda --- control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index e2ee0ec65eec0..e681bc22cd24b 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -555,9 +555,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont // set prev value for both to keep consistency over switching: // Actual steer, vel, acc should be considered in manual mode to prevent sudden motion when // switching from manual to autonomous - const auto in_autonomous = - (mode.mode == OperationModeState::AUTONOMOUS && mode.is_autoware_control_enabled); - auto prev_values = in_autonomous ? out : current_status_cmd; + auto prev_values = mode.is_autoware_control_enabled ? out : current_status_cmd; if (ego_is_stopped) { prev_values.longitudinal = out.longitudinal; From adb4872de9bedac1ff7440b135e741d5da2336ae Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 10 Oct 2023 18:01:13 +0900 Subject: [PATCH 92/97] fix(lane_change): add guard to neigibor lanelets (#5245) Signed-off-by: kosuke55 --- .../src/scene_module/lane_change/normal.cpp | 3 +++ planning/behavior_path_planner/src/utils/lane_change/utils.cpp | 3 +++ 2 files changed, 6 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index c75970e6fcc2d..71ba8c51ec6ad 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -946,6 +946,9 @@ bool NormalLaneChange::getLaneChangePaths( const auto target_neighbor_preferred_lane_poly_2d = utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + if (target_neighbor_preferred_lane_poly_2d.empty()) { + return false; + } const auto target_objects = getTargetObjects(current_lanes, target_lanes); debug_filtered_objects_ = target_objects; diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index df609dce48512..038cf9488345f 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -236,6 +236,9 @@ lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( { const auto target_neighbor_lanelets = utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type); + if (target_neighbor_lanelets.empty()) { + return {}; + } const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength( target_neighbor_lanelets, 0, std::numeric_limits::max()); From b36bc09b982f3f7b95c7c5164fa436b5b313ca00 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 10 Oct 2023 18:31:52 +0900 Subject: [PATCH 93/97] chore(default_ad_api_helpers): update readme topic (#5258) * chore(default_ad_api_helpers): update readme topic Signed-off-by: kminoda * style(pre-commit): autofix * update readme Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- system/default_ad_api_helpers/ad_api_adaptors/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/default_ad_api_helpers/ad_api_adaptors/README.md index 121befb68c88a..2e299fd7a2e51 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md @@ -21,9 +21,11 @@ The clear API is called automatically before setting the route. | Interface | Local Name | Global Name | Description | | ------------ | ------------------ | ------------------------------------- | -------------------------------------------------- | +| Subscription | - | /api/routing/state | The state of the routing API. | | Subscription | ~/input/fixed_goal | /planning/mission_planning/goal | The goal pose of route. Disable goal modification. | | Subscription | ~/input/rough_goal | /rviz/routing/rough_goal | The goal pose of route. Enable goal modification. | | Subscription | ~/input/reroute | /rviz/routing/reroute | The goal pose of reroute. | | Subscription | ~/input/waypoint | /planning/mission_planning/checkpoint | The waypoint pose of route. | | Client | - | /api/routing/clear_route | The route clear API. | | Client | - | /api/routing/set_route_points | The route points set API. | +| Client | - | /api/routing/change_route_points | The route points change API. | From 45339f63a05434cceaabbb2fa2d162b96e11d275 Mon Sep 17 00:00:00 2001 From: cyn-liu <104069308+cyn-liu@users.noreply.github.com> Date: Tue, 10 Oct 2023 18:16:58 +0800 Subject: [PATCH 94/97] feat(autoware_auto_msgs_adapter): add map adapter (#4700) * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * style(pre-commit): autofix --------- Signed-off-by: Cynthia Liu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../autoware_auto_msgs_adapter/CMakeLists.txt | 1 + .../config/adapter_map.param.yaml | 5 + .../autoware_auto_msgs_adapter.launch.xml | 4 + system/autoware_auto_msgs_adapter/package.xml | 4 +- .../autoware_auto_msgs_adapter.schema.json | 1 + .../src/autoware_auto_msgs_adapter_core.cpp | 5 + .../src/include/adapter_map.hpp | 59 ++++++++++ .../autoware_auto_msgs_adapter_core.hpp | 1 + .../test/test_msg_had_map_bin.cpp | 104 ++++++++++++++++++ 9 files changed, 182 insertions(+), 2 deletions(-) create mode 100644 system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml create mode 100644 system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp create mode 100644 system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp diff --git a/system/autoware_auto_msgs_adapter/CMakeLists.txt b/system/autoware_auto_msgs_adapter/CMakeLists.txt index ff8dcf6cedbb3..8b1d31ff2d01b 100644 --- a/system/autoware_auto_msgs_adapter/CMakeLists.txt +++ b/system/autoware_auto_msgs_adapter/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${NODE_NAME} src/include/adapter_base.hpp src/include/adapter_base_interface.hpp src/include/adapter_control.hpp + src/include/adapter_map.hpp src/include/adapter_perception.hpp src/include/autoware_auto_msgs_adapter_core.hpp src/autoware_auto_msgs_adapter_core.cpp) diff --git a/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml b/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml new file mode 100644 index 0000000000000..dcaa49e089b44 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + msg_type_target: "autoware_auto_mapping_msgs/msg/HADMapBin" + topic_name_source: "/map/vector_map" + topic_name_target: "/map/vector_map_auto" diff --git a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml index f914fe6e824c4..d80c1bd8cbedf 100755 --- a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml +++ b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml @@ -1,11 +1,15 @@ + + + + diff --git a/system/autoware_auto_msgs_adapter/package.xml b/system/autoware_auto_msgs_adapter/package.xml index de19032e1aee9..99a94fa043565 100644 --- a/system/autoware_auto_msgs_adapter/package.xml +++ b/system/autoware_auto_msgs_adapter/package.xml @@ -10,8 +10,6 @@ Apache License 2.0 - M. Fatih Cırıt - ament_cmake_auto autoware_cmake @@ -22,8 +20,10 @@ autoware_lint_common autoware_auto_control_msgs + autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_control_msgs + autoware_map_msgs autoware_perception_msgs rclcpp rclcpp_components diff --git a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json index 35e3aef1511cc..f6aa87f774528 100644 --- a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json +++ b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json @@ -11,6 +11,7 @@ "description": "Target message type", "enum": [ "autoware_auto_control_msgs/msg/AckermannControlCommand", + "autoware_auto_mapping_msgs/msg/HADMapBin", "autoware_auto_perception_msgs/msg/PredictedObjects" ], "default": "autoware_auto_control_msgs/msg/AckermannControlCommand" diff --git a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp index 0f65571aba07b..15e3c90d227dd 100644 --- a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp +++ b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp @@ -57,6 +57,11 @@ MapStringAdapter AutowareAutoMsgsAdapterNode::create_adapter_map( return std::static_pointer_cast( std::make_shared(*this, topic_name_source, topic_name_target)); }}, + {"autoware_auto_mapping_msgs/msg/HADMapBin", + [&] { + return std::static_pointer_cast( + std::make_shared(*this, topic_name_source, topic_name_target)); + }}, {"autoware_auto_perception_msgs/msg/PredictedObjects", [&] { return std::static_pointer_cast( diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp new file mode 100644 index 0000000000000..8150b7d7dba08 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef ADAPTER_MAP_HPP_ +#define ADAPTER_MAP_HPP_ + +#include "adapter_base.hpp" + +#include + +#include +#include + +#include + +namespace autoware_auto_msgs_adapter +{ +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; + +class AdapterMap : public autoware_auto_msgs_adapter::AdapterBase +{ +public: + AdapterMap( + rclcpp::Node & node, const std::string & topic_name_source, + const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) + : AdapterBase(node, topic_name_source, topic_name_target, qos) + { + RCLCPP_DEBUG( + node.get_logger(), "AdapterMap is initialized to convert: %s -> %s", + topic_name_source.c_str(), topic_name_target.c_str()); + } + +protected: + HADMapBin convert(const LaneletMapBin & msg_source) override + { + autoware_auto_mapping_msgs::msg::HADMapBin msg_auto; + msg_auto.header = msg_source.header; + msg_auto.format_version = msg_source.version_map_format; + msg_auto.map_version = msg_source.version_map; + msg_auto.map_format = 0; + msg_auto.data = msg_source.data; + + return msg_auto; + } +}; +} // namespace autoware_auto_msgs_adapter + +#endif // ADAPTER_MAP_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp index f595359dcc1e7..f0e28f48aacd8 100644 --- a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp +++ b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp @@ -15,6 +15,7 @@ #define AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ #include "adapter_control.hpp" +#include "adapter_map.hpp" #include "adapter_perception.hpp" #include diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp new file mode 100644 index 0000000000000..b312bd144f6f3 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp @@ -0,0 +1,104 @@ +// Copyright 2023 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include + +autoware_map_msgs::msg::LaneletMapBin generate_map_msg() +{ + // generate deterministic random int + std::mt19937 gen(0); + std::uniform_int_distribution<> dis_int(0, 1000000); + auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; + + autoware_map_msgs::msg::LaneletMapBin msg_map; + msg_map.header.stamp = rclcpp::Time(rand_int()); + msg_map.header.frame_id = "test_frame"; + + msg_map.version_map_format = "1.1.1"; + msg_map.version_map = "1.0.0"; + msg_map.name_map = "florence-prato-city-center"; + msg_map.data.push_back(rand_int()); + + return msg_map; +} + +TEST(AutowareAutoMsgsAdapter, TestHADMapBin) // NOLINT for gtest +{ + const std::string msg_type_target = "autoware_auto_mapping_msgs/msg/HADMapBin"; + const std::string topic_name_source = "topic_name_source"; + const std::string topic_name_target = "topic_name_target"; + + std::cout << "Creating the adapter node..." << std::endl; + + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("msg_type_target", msg_type_target); + node_options.append_parameter_override("topic_name_source", topic_name_source); + node_options.append_parameter_override("topic_name_target", topic_name_target); + + using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; + AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; + node_adapter = std::make_shared(node_options); + + std::cout << "Creating the subscriber node..." << std::endl; + + auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); + + bool test_completed = false; + + const auto msg_map = generate_map_msg(); + auto sub = node_subscriber->create_subscription( + topic_name_target, rclcpp::QoS{1}, + [&msg_map, &test_completed](const autoware_auto_mapping_msgs::msg::HADMapBin::SharedPtr msg) { + EXPECT_EQ(msg->header.stamp, msg_map.header.stamp); + EXPECT_EQ(msg->header.frame_id, msg_map.header.frame_id); + + EXPECT_EQ(msg->map_format, 0); + EXPECT_EQ(msg->format_version, msg_map.version_map_format); + EXPECT_EQ(msg->map_version, msg_map.version_map); + EXPECT_EQ(msg->data, msg_map.data); + + test_completed = true; + }); + + std::cout << "Creating the publisher node..." << std::endl; + + auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); + auto pub = node_publisher->create_publisher( + topic_name_source, rclcpp::QoS{1}); + pub->publish(msg_map); + + auto start_time = std::chrono::system_clock::now(); + auto max_test_dur = std::chrono::seconds(5); + auto timed_out = false; + + while (rclcpp::ok() && !test_completed) { + rclcpp::spin_some(node_subscriber); + rclcpp::spin_some(node_adapter); + rclcpp::spin_some(node_publisher); + rclcpp::sleep_for(std::chrono::milliseconds(50)); + if (std::chrono::system_clock::now() - start_time > max_test_dur) { + timed_out = true; + break; + } + } + + EXPECT_TRUE(test_completed); + EXPECT_FALSE(timed_out); + + // rclcpp::shutdown(); +} From 82d9840bc16dd380e2509e4fdac6d0be041f1796 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 11 Oct 2023 09:05:36 +0900 Subject: [PATCH 95/97] refactor(behavior_path_planner): separate processing time function (#5256) Signed-off-by: Takamasa Horibe --- .../include/behavior_path_planner/planner_manager.hpp | 5 +++++ .../src/behavior_path_planner_node.cpp | 1 + planning/behavior_path_planner/src/planner_manager.cpp | 10 ++++++++-- 3 files changed, 14 insertions(+), 2 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 ee5771c98217e..72c32039da627 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 @@ -242,6 +242,11 @@ class PlannerManager */ void print() const; + /** + * @brief publish processing time of each module. + */ + void publishProcessingTime() const; + /** * @brief visit each module and get debug information. */ 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 6e37bd0ec4b2c..755817aa22ae7 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -579,6 +579,7 @@ void BehaviorPathPlannerNode::run() lk_pd.unlock(); // release planner_data_ planner_manager_->print(); + planner_manager_->publishProcessingTime(); planner_manager_->publishMarker(); planner_manager_->publishVirtualWall(); lk_manager.unlock(); // release planner_manager_ diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 5963ddb778874..24967608775f3 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -886,13 +886,19 @@ void PlannerManager::print() const string_stream << std::right << "[" << std::setw(max_string_num + 1) << std::left << t.first << ":" << std::setw(4) << std::right << t.second << "ms]\n" << std::setw(21); - std::string name = std::string("processing_time/") + t.first; - debug_publisher_ptr_->publish(name, t.second); } RCLCPP_INFO_STREAM(logger_, string_stream.str()); } +void PlannerManager::publishProcessingTime() const +{ + for (const auto & t : processing_time_) { + std::string name = std::string("processing_time/") + t.first; + debug_publisher_ptr_->publish(name, t.second); + } +} + std::shared_ptr PlannerManager::getDebugMsg() { debug_msg_ptr_ = std::make_shared(); From 2aa9d1cd68b81df7b154195e7243f3e872256b8e Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 11 Oct 2023 09:05:54 +0900 Subject: [PATCH 96/97] chore(obstacle_velocity_limiter): publish processing time as float (#5257) Signed-off-by: Takamasa Horibe --- planning/obstacle_velocity_limiter/README.md | 2 +- .../obstacle_velocity_limiter_node.hpp | 4 ++-- .../src/obstacle_velocity_limiter_node.cpp | 6 ++++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/planning/obstacle_velocity_limiter/README.md b/planning/obstacle_velocity_limiter/README.md index d670f07d26e9d..36aa35bb31156 100644 --- a/planning/obstacle_velocity_limiter/README.md +++ b/planning/obstacle_velocity_limiter/README.md @@ -161,7 +161,7 @@ For example a value of `1` means all trajectory points will be evaluated while a | ------------------------------- | ---------------------------------------- | -------------------------------------------------------- | | `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Trajectory with adjusted velocities | | `~/output/debug_markers` | `visualization_msgs/MarkerArray` | Debug markers (envelopes, obstacle polygons) | -| `~/output/runtime_microseconds` | `std_msgs/Int64` | Time taken to calculate the trajectory (in microseconds) | +| `~/output/runtime_microseconds` | `tier4_debug_msgs/Float64` | Time taken to calculate the trajectory (in microseconds) | ## Parameters diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index a2ff72d0da562..cbdd390183d61 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include #include @@ -56,7 +56,7 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node pub_trajectory_; //!< @brief publisher for output trajectory rclcpp::Publisher::SharedPtr pub_debug_markers_; //!< @brief publisher for debug markers - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_runtime_; //!< @brief publisher for callback runtime rclcpp::Subscription::SharedPtr sub_trajectory_; //!< @brief subscriber for reference trajectory diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index f4601979b9b8a..086d76e9ef0fe 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -68,7 +68,8 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio pub_trajectory_ = create_publisher("~/output/trajectory", 1); pub_debug_markers_ = create_publisher("~/output/debug_markers", 1); - pub_runtime_ = create_publisher("~/output/runtime_microseconds", 1); + pub_runtime_ = + create_publisher("~/output/runtime_microseconds", 1); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); @@ -223,7 +224,8 @@ void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr const auto t_end = std::chrono::system_clock::now(); const auto runtime = std::chrono::duration_cast(t_end - t_start); - pub_runtime_->publish(std_msgs::msg::Int64().set__data(runtime.count())); + pub_runtime_->publish(tier4_debug_msgs::msg::Float64Stamped().set__stamp(now()).set__data( + static_cast(runtime.count()))); if (pub_debug_markers_->get_subscription_count() > 0) { const auto safe_projected_linestrings = From 3a8ff49eb0da121c6c0a28546f0853d5853f57d4 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Wed, 11 Oct 2023 10:09:48 +0900 Subject: [PATCH 97/97] fix(map_projection_loader): fix sample config parameter in readme (#5262) fix readme Signed-off-by: Kento Yabuuchi --- map/map_projection_loader/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 0b0c41821d036..94b142da3dcf9 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -25,7 +25,7 @@ sample-map-rosbag ```yaml # map_projector_info.yaml -type: "local" +projector_type: local ``` ### Using MGRS