From dbc85806112192734c3942751656034d28d18cf1 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Mon, 28 Feb 2022 14:06:29 +0900 Subject: [PATCH 01/25] fix(tier4_simulator_launch, tier4_vehicle_launch)!: fix launch args (#443) Signed-off-by: Kenji Miyake --- .../tier4_simulator_launch/launch/simulator.launch.xml | 10 ++++------ launch/tier4_vehicle_launch/launch/vehicle.launch.xml | 4 ++-- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 0bec3d13b6a35..6d60482391216 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -1,12 +1,12 @@ + + - - @@ -17,7 +17,7 @@ - + @@ -26,15 +26,13 @@ - - + - diff --git a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml index f696e6b33d73d..b33eac296abc9 100644 --- a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml +++ b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml @@ -1,9 +1,9 @@ + - @@ -17,7 +17,7 @@ - + From 37c3020c5eae4cb5c35f3461f1e4e79fc9858f1b Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Mon, 28 Feb 2022 15:09:36 +0900 Subject: [PATCH 02/25] fix: remove curand dependency (#449) Signed-off-by: wep21 --- perception/lidar_apollo_instance_segmentation/CMakeLists.txt | 1 - perception/lidar_centerpoint/CMakeLists.txt | 1 - perception/tensorrt_yolo/CMakeLists.txt | 1 - perception/traffic_light_classifier/CMakeLists.txt | 1 - perception/traffic_light_ssd_fine_detector/CMakeLists.txt | 1 - 5 files changed, 5 deletions(-) diff --git a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt index 41f3bd75bc722..ea18545da4970 100644 --- a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt @@ -123,7 +123,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${NVINFER_PLUGIN} ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} - ${CUDA_curand_LIBRARY} ${CUDNN_LIBRARY} ${PCL_LIBRARIES} ) diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index c1c99076a4b47..99b23bf4bc3ac 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -130,7 +130,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND TORCH_AVAIL) ${NVINFER_PLUGIN} ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} - ${CUDA_curand_LIBRARY} ${TORCH_LIBRARIES} ) diff --git a/perception/tensorrt_yolo/CMakeLists.txt b/perception/tensorrt_yolo/CMakeLists.txt index c5ad50afde187..e93c6fcef104d 100755 --- a/perception/tensorrt_yolo/CMakeLists.txt +++ b/perception/tensorrt_yolo/CMakeLists.txt @@ -217,7 +217,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${NVINFER_PLUGIN} ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} - ${CUDA_curand_LIBRARY} ${CUDNN_LIBRARY} mish_plugin yolo_layer_plugin diff --git a/perception/traffic_light_classifier/CMakeLists.txt b/perception/traffic_light_classifier/CMakeLists.txt index 4e591fa478c39..2f07b8d328cee 100644 --- a/perception/traffic_light_classifier/CMakeLists.txt +++ b/perception/traffic_light_classifier/CMakeLists.txt @@ -132,7 +132,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${NVINFER_PLUGIN} ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} - ${CUDA_curand_LIBRARY} ${CUDNN_LIBRARY} ${Boost_LIBRARIES} ) diff --git a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt index 7fc6ad12675f2..b20d06f91f5a5 100644 --- a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt @@ -140,7 +140,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ${NVINFER_PLUGIN} ${CUDA_LIBRARIES} ${CUBLAS_LIBRARIES} - ${CUDA_curand_LIBRARY} ${CUDNN_LIBRARY} ) From 4707dcb68ab2166ec4fa707b0ba9decca575f719 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Mon, 28 Feb 2022 16:09:36 +0900 Subject: [PATCH 03/25] refactor(behavior_velocity): refactor occlusion spot module (#400) * chore(behavior_velocity): private road to occlusion spot Signed-off-by: tanaka3 Signed-off-by: taikitanaka * chore(behavior_velocity): refactor occlusion spot module Signed-off-by: tanaka3 Signed-off-by: taikitanaka * chore(behavior_velocity): unite to general Signed-off-by: tanaka3 Signed-off-by: taikitanaka * chore(behavior_velocity): default to predicted object Signed-off-by: taikitanaka * chore(behavior_velocity): change register and unregister timing Signed-off-by: taikitanaka * chore(behavior_velocity): add include and remove unused Signed-off-by: taikitanaka * fix(behavior_velocity): fix after margin point velocity Signed-off-by: taikitanaka * refactor(behavior_velocity): refactor conditions for quantize Signed-off-by: taikitanaka * chore(occupancy_grid_map): update angle increment to pandar64 level Signed-off-by: taikitanaka --- .../behavior_velocity_planner/CMakeLists.txt | 2 +- .../config/occlusion_spot.param.yaml | 3 +- .../scene_module/occlusion_spot/manager.hpp | 4 +- .../occlusion_spot/occlusion_spot_utils.hpp | 2 + ...vate_road.hpp => scene_occlusion_spot.hpp} | 12 +-- .../scene_occlusion_spot_in_public_road.hpp | 2 +- .../src/scene_module/occlusion_spot/debug.cpp | 4 +- .../occlusion_spot/grid_utils.cpp | 10 ++- .../scene_module/occlusion_spot/manager.cpp | 87 +++++-------------- .../risk_predictive_braking.cpp | 2 +- ...vate_road.cpp => scene_occlusion_spot.cpp} | 15 +--- .../scene_occlusion_spot_in_public_road.cpp | 6 -- .../laserscan_to_occupancy_grid_map.launch.py | 2 +- 13 files changed, 49 insertions(+), 102 deletions(-) rename planning/behavior_velocity_planner/include/scene_module/occlusion_spot/{scene_occlusion_spot_in_private_road.hpp => scene_occlusion_spot.hpp} (87%) rename planning/behavior_velocity_planner/src/scene_module/occlusion_spot/{scene_occlusion_spot_in_private_road.cpp => scene_occlusion_spot.cpp} (87%) diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index c589624e56f61..927793f713433 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -224,7 +224,7 @@ ament_auto_add_library(scene_module_occlusion_spot SHARED src/scene_module/occlusion_spot/manager.cpp src/scene_module/occlusion_spot/debug.cpp src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp - src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp + src/scene_module/occlusion_spot/scene_occlusion_spot.cpp src/scene_module/occlusion_spot/occlusion_spot_utils.cpp src/scene_module/occlusion_spot/risk_predictive_braking.cpp ) diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index 7ccda0468bac6..a70d94e7e3e54 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: occlusion_spot: + method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object" debug: false # [-] whether to publish debug markers. Note Default should be false for performance pedestrian_vel: 1.0 # [m/s] assume pedestrian is dashing from occlusion at this velocity threshold: @@ -18,7 +19,7 @@ safe_margin: 1.0 # [m] maximum safety distance for any error detection_area: min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle. - slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path. + slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path. max_lateral_distance: 4.0 # [m] buffer around the ego path used to build the detection area. grid: free_space_max: 40 # [-] maximum value of a free space cell in the occupancy grid diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp index 68945d2ded3b0..6464226c788b0 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include @@ -45,7 +45,7 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface const char * getModuleName() override { return "occlusion_spot"; } private: - enum class ModuleID { PRIVATE, PUBLIC }; + enum class ModuleID { OCCUPANCY, OBJECT }; using PlannerParam = occlusion_spot_utils::PlannerParam; PlannerParam planner_param_; diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index dc6349dd12793..3a3f13d709338 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -65,6 +65,7 @@ using DetectionAreaIdx = boost::optional>; namespace occlusion_spot_utils { enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN }; +enum METHOD { OCCUPANCY_GRID, PREDICTED_OBJECT }; struct DetectionArea { @@ -96,6 +97,7 @@ struct LatLon struct PlannerParam { + METHOD method; bool debug; // [-] // parameters in yaml double detection_area_length; // [m] diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp similarity index 87% rename from planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp rename to planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp index fed53518eb1ca..147c1e4396201 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_ +#ifndef SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ +#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ #include #include @@ -37,7 +37,7 @@ namespace behavior_velocity_planner { -class OcclusionSpotInPrivateModule : public SceneModuleInterface +class OcclusionSpotModule : public SceneModuleInterface { using PlannerParam = occlusion_spot_utils::PlannerParam; @@ -45,7 +45,7 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface struct DebugData { double z; - std::string road_type = "private"; + std::string road_type = "occupancy"; std::vector detection_areas; std::vector possible_collisions; std::vector occlusion_points; @@ -53,7 +53,7 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface PathWithLaneId interp_path; }; - OcclusionSpotInPrivateModule( + OcclusionSpotModule( const int64_t module_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, @@ -82,4 +82,4 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_ +#endif // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp index 869084e7ed410..073d3bb6aea86 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp @@ -43,7 +43,7 @@ class OcclusionSpotInPublicModule : public SceneModuleInterface public: struct DebugData { - std::string road_type = "public"; + std::string road_type = "object"; std::vector detection_areas; std::vector parked_vehicle_point; std::vector possible_collisions; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index 4cae4d7b92a80..595d97d766a94 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include #include @@ -269,7 +269,7 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMar return debug_marker_array; } -visualization_msgs::msg::MarkerArray OcclusionSpotInPrivateModule::createDebugMarkerArray() +visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray() { const auto current_time = this->clock_->now(); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp index 6e7d31e4b5705..8e8b408082e78 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp @@ -16,6 +16,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -181,13 +182,14 @@ void toQuantizedImage( for (int y = height - 1; y >= 0; y--) { const int idx = (height - 1 - y) + (width - 1 - x) * height; int8_t intensity = occupancy_grid.data.at(idx); - if (0 <= intensity && intensity < param.free_space_max) { + if (0 <= intensity && intensity <= param.free_space_max) { intensity = grid_utils::occlusion_cost_value::FREE_SPACE; - } else if ( // NOLINT - intensity == occlusion_cost_value::NO_INFORMATION || intensity < param.occupied_min) { + } else if (param.free_space_max < intensity && intensity < param.occupied_min) { intensity = grid_utils::occlusion_cost_value::UNKNOWN; - } else { + } else if (param.occupied_min <= intensity) { intensity = grid_utils::occlusion_cost_value::OCCUPIED; + } else { + std::logic_error("behavior_velocity[occlusion_spot_grid]: invalid if clause"); } cv_image->at(y, x) = intensity; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp index 1ad0f5970c3f2..9c57cff00f35d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include @@ -25,50 +25,7 @@ namespace behavior_velocity_planner { -namespace -{ -std::vector getLaneletsOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) -{ - std::vector lanelets; - - for (const auto & p : path.points) { - const auto lane_id = p.lane_ids.at(0); - lanelets.push_back(lanelet_map->laneletLayer.get(lane_id)); - } - return lanelets; -} - -bool hasPublicRoadOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) -{ - for (const auto & ll : getLaneletsOnPath(path, lanelet_map)) { - // Is public load ? - const std::string location = ll.attributeOr("location", "else"); - if (location == "urban" || location == "public") { - return true; - } - } - return false; -} - -bool hasPrivateRoadOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) -{ - for (const auto & ll : getLaneletsOnPath(path, lanelet_map)) { - // Is private load ? - const std::string location = ll.attributeOr("location", "else"); - if (location == "private") { - return true; - } - } - return false; -} - -} // namespace +using occlusion_spot_utils::METHOD; OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -80,6 +37,14 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) // for crosswalk parameters auto & pp = planner_param_; // assume pedestrian coming out from occlusion spot with this velocity + const std::string method = node.declare_parameter(ns + ".method", "occupancy_grid"); + if (method == "occupancy_grid") { + pp.method = METHOD::OCCUPANCY_GRID; + } else if (method == "predicted_object") { + pp.method = METHOD::PREDICTED_OBJECT; + } else { + throw std::invalid_argument{"[behavior_velocity]: occlusion spot detection method is invalid"}; + } pp.debug = node.declare_parameter(ns + ".debug", false); pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel", 1.0); pp.detection_area_length = node.declare_parameter(ns + ".threshold.detection_area_length", 200.0); @@ -117,19 +82,20 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) void OcclusionSpotModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const int64_t private_road_module_id = static_cast(ModuleID::PRIVATE); - const int64_t public_road_module_id = static_cast(ModuleID::PUBLIC); - // private - if (!isModuleRegistered(private_road_module_id)) { - if (hasPrivateRoadOnPath(path, planner_data_->lanelet_map)) { - registerModule(std::make_shared( - private_road_module_id, planner_data_, planner_param_, - logger_.get_child("occlusion_spot_in_private_module"), clock_, pub_debug_occupancy_grid_)); + if (path.points.empty()) return; + const int64_t module_id = static_cast(ModuleID::OCCUPANCY); + const int64_t public_road_module_id = static_cast(ModuleID::OBJECT); + // general + if (!isModuleRegistered(module_id)) { + if (planner_param_.method == METHOD::OCCUPANCY_GRID) { + registerModule(std::make_shared( + module_id, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), + clock_, pub_debug_occupancy_grid_)); } } // public if (!isModuleRegistered(public_road_module_id)) { - if (hasPublicRoadOnPath(path, planner_data_->lanelet_map)) { + if (planner_param_.method == METHOD::PREDICTED_OBJECT) { registerModule(std::make_shared( public_road_module_id, planner_data_, planner_param_, logger_.get_child("occlusion_spot_in_public_module"), clock_)); @@ -141,17 +107,8 @@ std::function &)> OcclusionSpotModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const bool has_public_road = hasPublicRoadOnPath(path, planner_data_->lanelet_map); - const bool has_private_road = hasPrivateRoadOnPath(path, planner_data_->lanelet_map); - return [has_private_road, - has_public_road](const std::shared_ptr & scene_module) { - if (scene_module->getModuleId() == static_cast(ModuleID::PRIVATE)) { - return !has_private_road; - } - if (scene_module->getModuleId() == static_cast(ModuleID::PUBLIC)) { - return !has_public_road; - } - return true; + return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { + return false; }; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp index e2427b90ae055..6c5b9f10d218b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp @@ -47,7 +47,7 @@ void applySafeVelocityConsideringPossibleCollision( // min allowed velocity : min allowed velocity consider maximum allowed braking const double v_slow_down = - (l_obs < 0) + (l_obs < 0 && v0 <= v_safe) ? v_safe : planning_utils::calcDecelerationVelocityFromDistanceToTarget(j_min, a_min, a0, v0, l_obs); // compare safe velocity consider EBS, minimum allowed velocity and original velocity diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp similarity index 87% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp rename to planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 4ce2b34b60c60..26aaaaae02df0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include #include @@ -27,12 +27,11 @@ namespace behavior_velocity_planner { -using occlusion_spot_utils::ROAD_TYPE::PRIVATE; namespace bg = boost::geometry; namespace lg = lanelet::geometry; namespace utils = occlusion_spot_utils; -OcclusionSpotInPrivateModule::OcclusionSpotInPrivateModule( +OcclusionSpotModule::OcclusionSpotModule( const int64_t module_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, @@ -42,12 +41,11 @@ OcclusionSpotInPrivateModule::OcclusionSpotInPrivateModule( param_ = planner_param; } -bool OcclusionSpotInPrivateModule::modifyPathVelocity( +bool OcclusionSpotModule::modifyPathVelocity( autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { debug_data_ = DebugData(); - debug_data_.road_type = "private"; if (path->points.size() < 2) { return true; } @@ -83,9 +81,6 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( } // return if ego is final point of interpolated path if (closest_idx == static_cast(interp_path.points.size()) - 1) return true; - DetectionAreaIdx focus_area = - extractTargetRoadArcLength(lanelet_map_ptr, max_range, *path, PRIVATE); - if (!focus_area) return true; nav_msgs::msg::OccupancyGrid occ_grid = *occ_grid_ptr; grid_map::GridMap grid_map; grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid); @@ -94,16 +89,12 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( std::vector detection_area_polygons; utils::buildDetectionAreaPolygon( detection_area_polygons, interp_path, offset_from_start_to_ego, param_); - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 3000, "closest_idx : " << closest_idx); - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 3000, "offset_from_start_to_ego : " << offset_from_start_to_ego); std::vector possible_collisions; // Note: Don't consider offset from path start to ego here utils::createPossibleCollisionsInDetectionArea( detection_area_polygons, possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, debug_data_.occlusion_points); if (detection_area_polygons.empty()) return true; - utils::filterCollisionByRoadType(possible_collisions, focus_area); RCLCPP_DEBUG_STREAM_THROTTLE( logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size()); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp index 15b5029d39b3f..3a3d6013b49b6 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp @@ -30,7 +30,6 @@ namespace behavior_velocity_planner { using occlusion_spot_utils::PossibleCollisionInfo; -using occlusion_spot_utils::ROAD_TYPE::PUBLIC; namespace utils = occlusion_spot_utils; OcclusionSpotInPublicModule::OcclusionSpotInPublicModule( @@ -47,7 +46,6 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { debug_data_ = DebugData(); - debug_data_.road_type = "public"; if (path->points.size() < 2) { return true; } @@ -81,9 +79,6 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( } // return if ego is final point of interpolated path if (closest_idx == static_cast(interp_path.points.size()) - 1) return true; - DetectionAreaIdx focus_area = - extractTargetRoadArcLength(lanelet_map_ptr, param_.detection_area_length, *path, PUBLIC); - if (!focus_area) return true; std::vector obj = utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point); double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx); @@ -96,7 +91,6 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( std::vector possible_collisions = utils::generatePossibleCollisionBehindParkedVehicle( interp_path, param_, offset_from_start_to_ego, filtered_obj); - utils::filterCollisionByRoadType(possible_collisions, focus_area); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); // Note: Consider offset from path start to ego here utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego, 0.0); diff --git a/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py b/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py index fc7b9323ad183..9b550c359a9ab 100644 --- a/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py +++ b/sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py @@ -61,7 +61,7 @@ def add_launch_arg(name: str, default_value=None): "max_height": 2.0, "angle_min": -3.141592, # -M_PI "angle_max": 3.141592, # M_PI - "angle_increment": 0.00436332222, # 0.25*M_PI/180.0 + "angle_increment": 0.00349065850, # 0.20*M_PI/180.0 "scan_time": 0.0, "range_min": 1.0, "range_max": 200.0, From 4bef070152e38350302fb2eccdd79a0afdddd8d6 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 28 Feb 2022 16:31:09 +0900 Subject: [PATCH 04/25] chore(ad_service_state_monitor): fix node names in comments (#450) * chore(ad_service_state_monitor): fix typo * Update ad_service_state_monitor.planning_simulation.param.yaml Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --- .../config/ad_service_state_monitor.param.yaml | 2 +- .../ad_service_state_monitor.planning_simulation.param.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml index 03610e09f1f45..bfbb6beeaf054 100644 --- a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml +++ b/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml @@ -1,4 +1,4 @@ -# Autoware State Monitor Parameters +# AD Service State Monitor Parameters /**: ros__parameters: # modules_names: string array diff --git a/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml index 41928923a0dfd..a3b20c4c94d31 100644 --- a/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml +++ b/system/ad_service_state_monitor/config/ad_service_state_monitor.planning_simulation.param.yaml @@ -1,4 +1,4 @@ -# Autoware State Monitor: Planning Simulator Parameters +# AD Service State Monitor: Planning Simulator Parameters /**: ros__parameters: # modules_names: string array From e8931171c9b53b5812c4c043d09472c391562a91 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Mon, 28 Feb 2022 20:00:41 +0000 Subject: [PATCH 05/25] chore: sync files (#446) Signed-off-by: GitHub Co-authored-by: kenji-miyake --- CPPLINT.cfg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CPPLINT.cfg b/CPPLINT.cfg index 4dbbe0596b5db..1e2521f0b6442 100644 --- a/CPPLINT.cfg +++ b/CPPLINT.cfg @@ -9,5 +9,5 @@ filter=-whitespace/braces # we wrap open curly braces for namespaces, cl filter=-whitespace/indent # we don't indent keywords like public, protected and private with one space filter=-whitespace/parens # we allow closing parenthesis to be on the next line filter=-whitespace/semicolon # we allow the developer to decide about whitespace after a semicolon -filter=-build/header_guard # TODO(Kenji Miyake): Support ROS-style rule in cpplint or add auto-fix script in pre-commit +filter=-build/header_guard # we automatically fix the names of header guards using pre-commit filter=-build/include_order # we use the custom include order From fcfcceda3f0227b3393ced3935981f592d890b4e Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Tue, 1 Mar 2022 10:57:59 +0900 Subject: [PATCH 06/25] fix(dummy_perception): fix to use launch at perception launch (#454) * fix(dummy_perception): fix to use launch file in perception launch Signed-off-by: tanaka3 * fix(tier4_perception_launch): fix angle increment for occupancy grid Signed-off-by: tanaka3 --- .../launch/occupancy_grid_map/occupancy_grid_map.launch.py | 2 +- .../launch/dummy_perception_publisher.launch.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py b/launch/tier4_perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py index 57e172ffda42d..7e8dda8749251 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/occupancy_grid_map.launch.py @@ -59,7 +59,7 @@ def add_launch_arg(name: str, default_value=None): "max_height": 2.0, "angle_min": -3.141592, # -M_PI "angle_max": 3.141592, # M_PI - "angle_increment": 0.00436332222, # 0.25*M_PI/180.0 + "angle_increment": 0.00349065850, # 0.20*M_PI/180.0 "scan_time": 0.0, "range_min": 1.0, "range_max": 200.0, diff --git a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml index 12faac2799b13..24a68634f30bb 100644 --- a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml +++ b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml @@ -55,7 +55,7 @@ - + From 1d07c91d07685596808473e5d671cdf6b0b86d56 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara <81.s.kyo.19@gmail.com> Date: Tue, 1 Mar 2022 15:39:14 +0900 Subject: [PATCH 07/25] fix(behavior_path_planner): overwrite goal lane id when pull over function is on (#451) Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com> --- .../behavior_path_planner/src/behavior_path_planner_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 778e292f2310a..c368ca1011260 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -673,8 +673,8 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection( const PathWithLaneId & path) const { const auto goal = planner_data_->route_handler->getGoalPose(); - const auto goal_lane_id = planner_data_->route_handler->getGoalLaneId(); const auto is_approved = planner_data_->approval.is_approved.data; + auto goal_lane_id = planner_data_->route_handler->getGoalLaneId(); Pose refined_goal{}; { @@ -685,6 +685,7 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection( is_approved && planner_data_->route_handler->getPullOverTarget( planner_data_->route_handler->getShoulderLanelets(), &pull_over_lane)) { refined_goal = planner_data_->route_handler->getPullOverGoalPose(); + goal_lane_id = pull_over_lane.id(); } else if (planner_data_->route_handler->getGoalLanelet(&goal_lanelet)) { refined_goal = util::refineGoal(goal, goal_lanelet); } else { From 1d9e6eaf7394aba98583cb0935d98b9bb484629a Mon Sep 17 00:00:00 2001 From: Takayuki AKAMINE <38586589+takam5f2@users.noreply.github.com> Date: Tue, 1 Mar 2022 19:44:12 +0900 Subject: [PATCH 08/25] feat(hdd_monitor): add unit to value side as well as other metrics (#325) Signed-off-by: Takayuki AKAMINE Signed-off-by: Kenji Miyake --- system/system_monitor/src/hdd_monitor/hdd_monitor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp index b863a19547b39..719fdcf0b1d17 100644 --- a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp +++ b/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp @@ -268,9 +268,9 @@ void HDDMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) stat.add(fmt::format("HDD {}: status", hdd_index), usage_dict_.at(level)); stat.add(fmt::format("HDD {}: filesystem", hdd_index), list[0].c_str()); - stat.add(fmt::format("HDD {}: size (MB)", hdd_index), list[1].c_str()); - stat.add(fmt::format("HDD {}: used (MB)", hdd_index), list[2].c_str()); - stat.add(fmt::format("HDD {}: avail (MB)", hdd_index), list[3].c_str()); + stat.add(fmt::format("HDD {}: size", hdd_index), (list[1] + " MiB").c_str()); + stat.add(fmt::format("HDD {}: used", hdd_index), (list[2] + " MiB").c_str()); + stat.add(fmt::format("HDD {}: avail", hdd_index), (list[3] + " MiB").c_str()); stat.add(fmt::format("HDD {}: use", hdd_index), list[4].c_str()); std::string mounted_ = list[5]; if (list.size() > 6) { From 407c802ab0883a864b0bf00c3a52cfdaaf26081f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 1 Mar 2022 23:11:00 +0900 Subject: [PATCH 09/25] chore(map_based_prediction): improve readability (#453) * chore: add namespace Signed-off-by: satoshi-ota * fix: small bug Signed-off-by: satoshi-ota * chore: using alias(autoware_auto_perception_msgs) Signed-off-by: satoshi-ota * chore: using alias(autoware_auto_mapping_msgs) Signed-off-by: satoshi-ota --- .../map_based_prediction/CMakeLists.txt | 2 +- .../include/cubic_spline.hpp | 3 ++ .../include/map_based_prediction.hpp | 35 ++++++++----- .../include/map_based_prediction_ros.hpp | 40 ++++++++------ .../src/map_based_prediction.cpp | 35 ++++++------- .../src/map_based_prediction_ros.cpp | 52 +++++++++---------- 6 files changed, 90 insertions(+), 77 deletions(-) diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/map_based_prediction/CMakeLists.txt index 8c386f1bb2f44..86de43c839ce6 100644 --- a/perception/map_based_prediction/CMakeLists.txt +++ b/perception/map_based_prediction/CMakeLists.txt @@ -21,7 +21,7 @@ ament_auto_add_library(map_based_prediction_node SHARED ) rclcpp_components_register_node(map_based_prediction_node - PLUGIN "MapBasedPredictionROS" + PLUGIN "map_based_prediction::MapBasedPredictionROS" EXECUTABLE map_based_prediction ) diff --git a/perception/map_based_prediction/include/cubic_spline.hpp b/perception/map_based_prediction/include/cubic_spline.hpp index 7e0826ceb1197..d55f8ee9dde43 100644 --- a/perception/map_based_prediction/include/cubic_spline.hpp +++ b/perception/map_based_prediction/include/cubic_spline.hpp @@ -30,6 +30,8 @@ #include #include +namespace map_based_prediction +{ static std::vector vec_diff(const std::vector & input) { std::vector output; @@ -250,5 +252,6 @@ class Spline2D return out_s; } }; +} // namespace map_based_prediction #endif // CUBIC_SPLINE_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction.hpp b/perception/map_based_prediction/include/map_based_prediction.hpp index e297f51ae357d..19678f8c2a482 100644 --- a/perception/map_based_prediction/include/map_based_prediction.hpp +++ b/perception/map_based_prediction/include/map_based_prediction.hpp @@ -22,9 +22,20 @@ #include +namespace map_based_prediction +{ +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjectKinematics; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; +using autoware_auto_perception_msgs::msg::TrackedObjects; + struct DynamicObjectWithLanes { - autoware_auto_perception_msgs::msg::TrackedObject object; + TrackedObject object; std::vector> lanes; std::vector confidence; }; @@ -47,33 +58,29 @@ class MapBasedPrediction bool getPredictedPath( const double height, const double current_d_position, const double current_d_velocity, const double current_s_position, const double current_s_velocity, - const std_msgs::msg::Header & origin_header, Spline2D & spline2d, - autoware_auto_perception_msgs::msg::PredictedPath & path) const; + const std_msgs::msg::Header & origin_header, Spline2D & spline2d, PredictedPath & path) const; void getLinearPredictedPath( const geometry_msgs::msg::Pose & object_pose, const geometry_msgs::msg::Twist & object_twist, - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const; + PredictedPath & predicted_path) const; - void normalizeLikelihood( - autoware_auto_perception_msgs::msg::PredictedObjectKinematics & predicted_object_kinematics); + void normalizeLikelihood(PredictedObjectKinematics & predicted_object_kinematics); - autoware_auto_perception_msgs::msg::PredictedObjectKinematics convertToPredictedKinematics( - const autoware_auto_perception_msgs::msg::TrackedObjectKinematics & tracked_object); + PredictedObjectKinematics convertToPredictedKinematics( + const TrackedObjectKinematics & tracked_object); public: MapBasedPrediction( double interpolating_resolution, double time_horizon, double sampling_delta_time); bool doPrediction( - const DynamicObjectWithLanesArray & in_objects, - std::vector & out_objects); + const DynamicObjectWithLanesArray & in_objects, std::vector & out_objects); bool doLinearPrediction( - const autoware_auto_perception_msgs::msg::PredictedObjects & in_objects, - std::vector & out_objects); + const PredictedObjects & in_objects, std::vector & out_objects); - autoware_auto_perception_msgs::msg::PredictedObject convertToPredictedObject( - const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object); + PredictedObject convertToPredictedObject(const TrackedObject & tracked_object); }; +} // namespace map_based_prediction #endif // MAP_BASED_PREDICTION_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction_ros.hpp b/perception/map_based_prediction/include/map_based_prediction_ros.hpp index 5f477f0133f79..693a711cfa05e 100644 --- a/perception/map_based_prediction/include/map_based_prediction_ros.hpp +++ b/perception/map_based_prediction/include/map_based_prediction_ros.hpp @@ -62,6 +62,15 @@ class TrafficRules; } // namespace traffic_rules } // namespace lanelet +namespace map_based_prediction +{ +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + struct ObjectData { lanelet::ConstLanelets current_lanelets; @@ -95,9 +104,9 @@ class MapBasedPredictionROS : public rclcpp::Node double diff_dist_threshold_to_left_bound_; double diff_dist_threshold_to_right_bound_; - rclcpp::Subscription::SharedPtr sub_objects_; - rclcpp::Subscription::SharedPtr sub_map_; - rclcpp::Publisher::SharedPtr pub_objects_; + rclcpp::Subscription::SharedPtr sub_objects_; + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Publisher::SharedPtr pub_objects_; rclcpp::Publisher::SharedPtr pub_markers_; std::unordered_map> object_buffer_; @@ -113,32 +122,28 @@ class MapBasedPredictionROS : public rclcpp::Node bool getSelfPose(geometry_msgs::msg::Pose & self_pose, const std_msgs::msg::Header & header); bool getSelfPoseInMap(geometry_msgs::msg::Pose & self_pose); - double getObjectYaw(const autoware_auto_perception_msgs::msg::TrackedObject & object); + double getObjectYaw(const TrackedObject & object); double calculateLikelihood( - const std::vector & path, - const autoware_auto_perception_msgs::msg::TrackedObject & object); + const std::vector & path, const TrackedObject & object); void addValidPath( const lanelet::routing::LaneletPaths & candidate_paths, lanelet::routing::LaneletPaths & valid_paths); - void objectsCallback( - const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr in_objects); - void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects); + void mapCallback(const HADMapBin::ConstSharedPtr msg); bool getClosestLanelets( - const autoware_auto_perception_msgs::msg::TrackedObject & object, - const lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::ConstLanelets & closest_lanelets); + const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::ConstLanelets & closest_lanelets); bool checkCloseLaneletCondition( - const std::pair & lanelet, - const autoware_auto_perception_msgs::msg::TrackedObject & object, + const std::pair & lanelet, const TrackedObject & object, const lanelet::BasicPoint2d & search_point); void removeInvalidObject(const double current_time); bool updateObjectBuffer( - const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::TrackedObject & object, + const std_msgs::msg::Header & header, const TrackedObject & object, lanelet::ConstLanelets & current_lanelets); void updatePossibleLanelets( const std::string object_id, const lanelet::routing::LaneletPaths & paths); @@ -148,11 +153,12 @@ class MapBasedPredictionROS : public rclcpp::Node double calcLeftLateralOffset( const lanelet::ConstLineString2d & bound_line, const geometry_msgs::msg::Pose & search_pose); Maneuver detectLaneChange( - const autoware_auto_perception_msgs::msg::TrackedObject & object, - const lanelet::ConstLanelet & current_lanelet, const double current_time); + const TrackedObject & object, const lanelet::ConstLanelet & current_lanelet, + const double current_time); public: explicit MapBasedPredictionROS(const rclcpp::NodeOptions & node_options); }; +} // namespace map_based_prediction #endif // MAP_BASED_PREDICTION_ROS_HPP_ diff --git a/perception/map_based_prediction/src/map_based_prediction.cpp b/perception/map_based_prediction/src/map_based_prediction.cpp index e289bc0e8b6d0..7bd9f7498590d 100644 --- a/perception/map_based_prediction/src/map_based_prediction.cpp +++ b/perception/map_based_prediction/src/map_based_prediction.cpp @@ -22,6 +22,8 @@ #include #include +namespace map_based_prediction +{ MapBasedPrediction::MapBasedPrediction( double interpolating_resolution, double time_horizon, double sampling_delta_time) : interpolating_resolution_(interpolating_resolution), @@ -31,11 +33,10 @@ MapBasedPrediction::MapBasedPrediction( } bool MapBasedPrediction::doPrediction( - const DynamicObjectWithLanesArray & in_objects, - std::vector & out_objects) + const DynamicObjectWithLanesArray & in_objects, std::vector & out_objects) { for (auto & object_with_lanes : in_objects.objects) { - autoware_auto_perception_msgs::msg::PredictedObject tmp_object; + PredictedObject tmp_object; tmp_object = convertToPredictedObject(object_with_lanes.object); for (size_t path_id = 0; path_id < object_with_lanes.lanes.size(); ++path_id) { std::vector tmp_x; @@ -101,7 +102,7 @@ bool MapBasedPrediction::doPrediction( std::fabs(object_with_lanes.object.kinematics.twist_with_covariance.twist.linear.x); // Predict Path - autoware_auto_perception_msgs::msg::PredictedPath predicted_path; + PredictedPath predicted_path; getPredictedPath( object_point.z, current_d_position, current_d_velocity, current_s_position, current_s_velocity, in_objects.header, spline2d, predicted_path); @@ -135,15 +136,14 @@ bool MapBasedPrediction::doPrediction( } bool MapBasedPrediction::doLinearPrediction( - const autoware_auto_perception_msgs::msg::PredictedObjects & in_objects, - std::vector & out_objects) + const PredictedObjects & in_objects, std::vector & out_objects) { for (const auto & object : in_objects.objects) { - autoware_auto_perception_msgs::msg::PredictedPath path; + PredictedPath path; getLinearPredictedPath( object.kinematics.initial_pose_with_covariance.pose, object.kinematics.initial_twist_with_covariance.twist, path); - autoware_auto_perception_msgs::msg::PredictedObject tmp_object; + PredictedObject tmp_object; tmp_object = object; tmp_object.kinematics.predicted_paths.push_back(path); out_objects.push_back(tmp_object); @@ -152,10 +152,9 @@ bool MapBasedPrediction::doLinearPrediction( return true; } -autoware_auto_perception_msgs::msg::PredictedObject MapBasedPrediction::convertToPredictedObject( - const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object) +PredictedObject MapBasedPrediction::convertToPredictedObject(const TrackedObject & tracked_object) { - autoware_auto_perception_msgs::msg::PredictedObject output; + PredictedObject output; output.object_id = tracked_object.object_id; output.existence_probability = tracked_object.existence_probability; output.classification = tracked_object.classification; @@ -164,11 +163,10 @@ autoware_auto_perception_msgs::msg::PredictedObject MapBasedPrediction::convertT return output; } -autoware_auto_perception_msgs::msg::PredictedObjectKinematics -MapBasedPrediction::convertToPredictedKinematics( - const autoware_auto_perception_msgs::msg::TrackedObjectKinematics & tracked_object) +PredictedObjectKinematics MapBasedPrediction::convertToPredictedKinematics( + const TrackedObjectKinematics & tracked_object) { - autoware_auto_perception_msgs::msg::PredictedObjectKinematics output; + PredictedObjectKinematics output; output.initial_pose_with_covariance = tracked_object.pose_with_covariance; output.initial_twist_with_covariance = tracked_object.twist_with_covariance; output.initial_acceleration_with_covariance = tracked_object.acceleration_with_covariance; @@ -176,7 +174,7 @@ MapBasedPrediction::convertToPredictedKinematics( } void MapBasedPrediction::normalizeLikelihood( - autoware_auto_perception_msgs::msg::PredictedObjectKinematics & predicted_object_kinematics) + PredictedObjectKinematics & predicted_object_kinematics) { // might not be the smartest way double sum_confidence = 0.0; @@ -193,7 +191,7 @@ bool MapBasedPrediction::getPredictedPath( const double height, const double current_d_position, const double current_d_velocity, const double current_s_position, const double current_s_velocity, [[maybe_unused]] const std_msgs::msg::Header & origin_header, Spline2D & spline2d, - autoware_auto_perception_msgs::msg::PredictedPath & path) const + PredictedPath & path) const { // Quintic polynomial for d // A = np.array([[T**3, T**4, T**5], @@ -269,7 +267,7 @@ bool MapBasedPrediction::getPredictedPath( void MapBasedPrediction::getLinearPredictedPath( const geometry_msgs::msg::Pose & object_pose, const geometry_msgs::msg::Twist & object_twist, - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + PredictedPath & predicted_path) const { const double & sampling_delta_time = sampling_delta_time_; const double & time_horizon = time_horizon_; @@ -302,3 +300,4 @@ void MapBasedPrediction::getLinearPredictedPath( predicted_path.confidence = 1.0; predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_delta_time); } +} // namespace map_based_prediction diff --git a/perception/map_based_prediction/src/map_based_prediction_ros.cpp b/perception/map_based_prediction/src/map_based_prediction_ros.cpp index 4b77aabb818b6..19df2894e4087 100644 --- a/perception/map_based_prediction/src/map_based_prediction_ros.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_ros.cpp @@ -37,6 +37,8 @@ #include #include +namespace +{ std::string toHexString(const unique_identifier_msgs::msg::UUID & id) { std::stringstream ss; @@ -45,7 +47,9 @@ std::string toHexString(const unique_identifier_msgs::msg::UUID & id) } return ss.str(); } - +} // namespace +namespace map_based_prediction +{ MapBasedPredictionROS::MapBasedPredictionROS(const rclcpp::NodeOptions & node_options) : Node("map_based_prediction", node_options), interpolating_resolution_(0.5), @@ -79,21 +83,19 @@ MapBasedPredictionROS::MapBasedPredictionROS(const rclcpp::NodeOptions & node_op map_based_prediction_ = std::make_shared( interpolating_resolution_, prediction_time_horizon_, prediction_sampling_delta_time_); - sub_objects_ = this->create_subscription( + sub_objects_ = this->create_subscription( "/perception/object_recognition/tracking/objects", 1, std::bind(&MapBasedPredictionROS::objectsCallback, this, std::placeholders::_1)); - sub_map_ = this->create_subscription( + sub_map_ = this->create_subscription( "/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&MapBasedPredictionROS::mapCallback, this, std::placeholders::_1)); - pub_objects_ = this->create_publisher( - "objects", rclcpp::QoS{1}); + pub_objects_ = this->create_publisher("objects", rclcpp::QoS{1}); pub_markers_ = this->create_publisher( "objects_path_markers", rclcpp::QoS{1}); } -double MapBasedPredictionROS::getObjectYaw( - const autoware_auto_perception_msgs::msg::TrackedObject & object) +double MapBasedPredictionROS::getObjectYaw(const TrackedObject & object) { if (object.kinematics.orientation_availability) { return tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); @@ -117,8 +119,7 @@ double MapBasedPredictionROS::getObjectYaw( } double MapBasedPredictionROS::calculateLikelihood( - const std::vector & path, - const autoware_auto_perception_msgs::msg::TrackedObject & object) + const std::vector & path, const TrackedObject & object) { // We compute the confidence value based on the object current position and angle // Calculate path length @@ -157,8 +158,7 @@ double MapBasedPredictionROS::calculateLikelihood( } bool MapBasedPredictionROS::checkCloseLaneletCondition( - const std::pair & lanelet, - const autoware_auto_perception_msgs::msg::TrackedObject & object, + const std::pair & lanelet, const TrackedObject & object, const lanelet::BasicPoint2d & search_point) { // Step1. If we only have one point in the centerline, we will ignore the lanelet @@ -206,8 +206,8 @@ bool MapBasedPredictionROS::checkCloseLaneletCondition( } bool MapBasedPredictionROS::getClosestLanelets( - const autoware_auto_perception_msgs::msg::TrackedObject & object, - const lanelet::LaneletMapPtr & lanelet_map_ptr_, lanelet::ConstLanelets & closest_lanelets) + const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr_, + lanelet::ConstLanelets & closest_lanelets) { std::chrono::high_resolution_clock::time_point begin = std::chrono::high_resolution_clock::now(); @@ -304,11 +304,10 @@ void MapBasedPredictionROS::removeInvalidObject(const double current_time) } bool MapBasedPredictionROS::updateObjectBuffer( - const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::TrackedObject & object, + const std_msgs::msg::Header & header, const TrackedObject & object, lanelet::ConstLanelets & current_lanelets) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + using Label = ObjectClassification; // Ignore non-vehicle object if ( object.classification.front().label != Label::CAR && @@ -432,8 +431,8 @@ double MapBasedPredictionROS::calcLeftLateralOffset( } Maneuver MapBasedPredictionROS::detectLaneChange( - const autoware_auto_perception_msgs::msg::TrackedObject & object, - const lanelet::ConstLanelet & current_lanelet, const double current_time) + const TrackedObject & object, const lanelet::ConstLanelet & current_lanelet, + const double current_time) { // Step1. Check if we have the object in the buffer const std::string object_id = toHexString(object.object_id); @@ -530,8 +529,7 @@ Maneuver MapBasedPredictionROS::detectLaneChange( return Maneuver::LANE_FOLLOW; } -void MapBasedPredictionROS::objectsCallback( - const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr in_objects) +void MapBasedPredictionROS::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { debug_accumulated_time_ = 0.0; std::chrono::high_resolution_clock::time_point begin = std::chrono::high_resolution_clock::now(); @@ -570,7 +568,7 @@ void MapBasedPredictionROS::objectsCallback( ///////////////////////////////////////////////////////// ///////////////////// Prediction /////////////////////// /////////////////////////////////////////////////////// - autoware_auto_perception_msgs::msg::PredictedObjects objects_without_map; + PredictedObjects objects_without_map; objects_without_map.header = in_objects->header; DynamicObjectWithLanesArray prediction_input; prediction_input.header = in_objects->header; @@ -734,22 +732,21 @@ void MapBasedPredictionROS::objectsCallback( std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now(); std::chrono::nanoseconds time = std::chrono::duration_cast(end - begin); - std::vector out_objects_in_map; + std::vector out_objects_in_map; map_based_prediction_->doPrediction(prediction_input, out_objects_in_map); - autoware_auto_perception_msgs::msg::PredictedObjects output; + PredictedObjects output; output.header = in_objects->header; output.header.frame_id = "map"; output.objects = out_objects_in_map; - std::vector out_objects_without_map; + std::vector out_objects_without_map; map_based_prediction_->doLinearPrediction(objects_without_map, out_objects_without_map); output.objects.insert( output.objects.begin(), out_objects_without_map.begin(), out_objects_without_map.end()); pub_objects_->publish(output); } -void MapBasedPredictionROS::mapCallback( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +void MapBasedPredictionROS::mapCallback(const HADMapBin::ConstSharedPtr msg) { RCLCPP_INFO(get_logger(), "Start loading lanelet"); lanelet_map_ptr_ = std::make_shared(); @@ -757,6 +754,7 @@ void MapBasedPredictionROS::mapCallback( *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); RCLCPP_INFO(get_logger(), "Map is loaded"); } +} // namespace map_based_prediction #include -RCLCPP_COMPONENTS_REGISTER_NODE(MapBasedPredictionROS) +RCLCPP_COMPONENTS_REGISTER_NODE(map_based_prediction::MapBasedPredictionROS) From 9b605cb092c5e271aed68eb3a4bf05af1bfbc605 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Wed, 2 Mar 2022 10:33:54 +0900 Subject: [PATCH 10/25] fix(behavior_velocity): fix conflicting case of intersection and merge from private (#447) * fix(behavior_velocity): fix conflicting case of intersection and merge from private Signed-off-by: tanaka3 * refactor(behavior_velocity): refactor Param to double Signed-off-by: tanaka3 * fix(behavior_velocity): revert offset for wall pose Signed-off-by: tanaka3 --- .../scene_module/intersection/manager.hpp | 15 ++++++ .../scene_merge_from_private_road.hpp | 4 +- .../scene_module/intersection/util.hpp | 5 +- .../behavior_velocity_planner/src/node.cpp | 2 + .../src/scene_module/intersection/debug.cpp | 7 +-- .../src/scene_module/intersection/manager.cpp | 50 +++++++++++++++++-- .../intersection/scene_intersection.cpp | 8 +-- .../scene_merge_from_private_road.cpp | 8 +-- .../src/scene_module/intersection/util.cpp | 9 ++-- 9 files changed, 83 insertions(+), 25 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp index ca09b1cd7d422..2396bb16dc8f5 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp @@ -37,6 +37,21 @@ class IntersectionModuleManager : public SceneModuleManagerInterface private: IntersectionModule::PlannerParam intersection_param_; + + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class MergeFromPrivateModuleManager : public SceneModuleManagerInterface +{ +public: + explicit MergeFromPrivateModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "merge_from_private"; } + +private: MergeFromPrivateRoadModule::PlannerParam merge_from_private_area_param_; void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp index 407d9715c65b7..e38efd3f6bc9a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp @@ -99,7 +99,9 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface public: struct PlannerParam { - IntersectionModule::PlannerParam intersection_param; + double decel_velocity; + double detection_area_length; + double stop_line_margin; double stop_duration_sec; }; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 77342c1e6219a..dc647df54f533 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -52,7 +52,7 @@ bool hasDuplicatedPoint( */ bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const IntersectionModule::PlannerParam & planner_param, + const int lane_id, const double detection_area_length, std::vector * conflicting_lanelets_result, std::vector * objective_lanelets_result, const rclcpp::Logger logger); @@ -69,8 +69,7 @@ bool getObjectiveLanelets( */ bool generateStopLine( const int lane_id, const std::vector detection_areas, - const std::shared_ptr & planner_data, - const IntersectionModule::PlannerParam & planner_param, + const std::shared_ptr & planner_data, const double stop_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger); diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 0fba77d1c406a..06060d51d8c84 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -135,7 +135,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio planner_manager_.launchSceneModule(std::make_shared(*this)); } if (this->declare_parameter("launch_intersection", true)) { + // intersection module should be before merge from private to declare intersection parameters planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchSceneModule(std::make_shared(*this)); } if (this->declare_parameter("launch_blind_spot", true)) { planner_manager_.launchSceneModule(std::make_shared(*this)); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index d346856e8eadc..13e06e2dfe7cc 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -150,7 +150,8 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray( } visualization_msgs::msg::MarkerArray createVirtualStopWallMarkerArray( - const geometry_msgs::msg::Pose & pose, const int64_t lane_id, const std::string & stop_factor) + const geometry_msgs::msg::Pose & pose, const int64_t lane_id, const std::string & stop_factor, + const double offset_z = 0.0) { visualization_msgs::msg::MarkerArray msg; @@ -175,7 +176,7 @@ visualization_msgs::msg::MarkerArray createVirtualStopWallMarkerArray( marker_factor_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker_factor_text.action = visualization_msgs::msg::Marker::ADD; marker_factor_text.pose = pose; - marker_factor_text.pose.position.z += 2.0; + marker_factor_text.pose.position.z += 2.0 + offset_z; marker_factor_text.scale = createMarkerScale(0.0, 0.0, 1.0); marker_factor_text.color = createMarkerColor(1.0, 1.0, 1.0, 0.999); marker_factor_text.text = stop_factor; @@ -345,7 +346,7 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark appendMarkerArray( createVirtualStopWallMarkerArray( - debug_data_.virtual_wall_pose, lane_id_, "merge_from_private_road"), + debug_data_.virtual_wall_pose, lane_id_, "merge_from_private_road", -1.0), current_time, &debug_marker_array); } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 0d235d339af54..b9e88e71a1287 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -83,10 +83,18 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0); ip.collision_start_margin_time = node.declare_parameter(ns + ".collision_start_margin_time", 5.0); ip.collision_end_margin_time = node.declare_parameter(ns + ".collision_end_margin_time", 2.0); +} + +MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); auto & mp = merge_from_private_area_param_; mp.stop_duration_sec = node.declare_parameter(ns + ".merge_from_private_area.stop_duration_sec", 1.0); - mp.intersection_param = intersection_param_; + mp.decel_velocity = node.get_parameter("intersection.decel_velocity").as_double(); + mp.detection_area_length = node.get_parameter("intersection.detection_area_length").as_double(); + mp.stop_line_margin = node.get_parameter("intersection.stop_line_margin").as_double(); } void IntersectionModuleManager::launchNewModules( @@ -102,6 +110,32 @@ void IntersectionModuleManager::launchNewModules( continue; } + // Is intersection? + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + registerModule(std::make_shared( + module_id, lane_id, planner_data_, intersection_param_, + logger_.get_child("intersection_module"), clock_)); + } +} + +void MergeFromPrivateModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lanelets = getLaneletsOnPath(path, planner_data_->lanelet_map); + for (size_t i = 0; i < lanelets.size(); i++) { + const auto ll = lanelets.at(i); + const auto lane_id = ll.id(); + const auto module_id = lane_id; + + if (isModuleRegistered(module_id)) { + continue; + } + // Is intersection? const std::string turn_direction = ll.attributeOr("turn_direction", "else"); const auto is_intersection = @@ -121,10 +155,6 @@ void IntersectionModuleManager::launchNewModules( logger_.get_child("merge_from_private_road_module"), clock_)); } } - - registerModule(std::make_shared( - module_id, lane_id, planner_data_, intersection_param_, - logger_.get_child("intersection_module"), clock_)); } } @@ -138,4 +168,14 @@ IntersectionModuleManager::getModuleExpiredFunction( return lane_id_set.count(scene_module->getModuleId()) == 0; }; } +std::function &)> +MergeFromPrivateModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_id_set = getLaneIdSetOnPath(path); + + return [lane_id_set](const std::shared_ptr & scene_module) { + return lane_id_set.count(scene_module->getModuleId()) == 0; + }; +} } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 5ea098e885a56..786d44190af87 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -93,8 +93,8 @@ bool IntersectionModule::modifyPathVelocity( std::vector conflicting_area_lanelets; util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_, &conflicting_area_lanelets, - &detection_area_lanelets, logger_); + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, + &conflicting_area_lanelets, &detection_area_lanelets, logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); std::vector detection_areas = util::getPolygon3dFromLaneletsVec( @@ -115,8 +115,8 @@ bool IntersectionModule::modifyPathVelocity( int pass_judge_line_idx = -1; int first_idx_inside_lane = -1; if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_, path, *path, &stop_line_idx, - &pass_judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) { + lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, *path, + &stop_line_idx, &pass_judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); RCLCPP_DEBUG(logger_, "===== plan end ====="); return false; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index ceccb05d2659b..36dd26cef593c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -69,10 +69,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( std::vector conflicting_area_lanelets; util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.intersection_param, + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, &conflicting_area_lanelets, &detection_area_lanelets, logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( - conflicting_area_lanelets, planner_param_.intersection_param.detection_area_length); + conflicting_area_lanelets, planner_param_.detection_area_length); if (conflicting_areas.empty()) { RCLCPP_DEBUG(logger_, "no detection area. skip computation."); return true; @@ -86,7 +86,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( const auto private_path = extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m); if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_.intersection_param, path, + lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, private_path, &stop_line_idx, &judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); @@ -108,7 +108,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( /* set stop speed */ if (state_machine_.getState() == State::STOP) { constexpr double stop_vel = 0.0; - const double decel_vel = planner_param_.intersection_param.decel_velocity; + const double decel_vel = planner_param_.decel_velocity; double v = (has_traffic_light_ && turn_direction_ == "straight") ? decel_vel : stop_vel; util::setVelocityFrom(stop_line_idx, v, path); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 95946a6ba61be..6dbe52073ae53 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -162,8 +162,7 @@ int getFirstPointInsidePolygons( bool generateStopLine( const int lane_id, const std::vector detection_areas, - const std::shared_ptr & planner_data, - const IntersectionModule::PlannerParam & planner_param, + const std::shared_ptr & planner_data, const double stop_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger) @@ -178,7 +177,7 @@ bool generateStopLine( /* set parameters */ constexpr double interval = 0.2; - const int margin_idx_dist = std::ceil(planner_param.stop_line_margin / interval); + const int margin_idx_dist = std::ceil(stop_line_margin / interval); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval); @@ -322,7 +321,7 @@ bool getStopPoseIndexFromMap( bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const IntersectionModule::PlannerParam & planner_param, + const int lane_id, const double detection_area_length, std::vector * conflicting_lanelets_result, std::vector * objective_lanelets_result, const rclcpp::Logger logger) { @@ -379,7 +378,7 @@ bool getObjectiveLanelets( } // get possible lanelet path that reaches conflicting_lane longer than given length - const double length = planner_param.detection_area_length; + const double length = detection_area_length; std::vector objective_lanelets_sequences; for (const auto & ll : objective_lanelets) { // Preceding lanes does not include objective_lane so add them at the end From 4bff484f4b054d2e86e935baad713626ccdd458f Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 2 Mar 2022 14:39:53 +0900 Subject: [PATCH 11/25] refactor(scenario_planning.launch.xml): add parameter description (#464) Signed-off-by: Takamasa Horibe --- .../launch/scenario_planning/scenario_planning.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 8ceb7496ab828..8525313a03b38 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -24,7 +24,7 @@ - + From e5ae6ae54017c64d08ce5fd818fcebb48cbb0593 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 2 Mar 2022 14:42:04 +0900 Subject: [PATCH 12/25] feat(tier4_simulator_launch, dummy_perception_publisher): launch perception modules from simulator.launch.xml (#465) * feat(tier4_simulator_launch, dummy_perception_publisher): launch perception modules from simualtor.launch.xml Signed-off-by: Takayuki Murooka * remove perception launching dummy_perception_publisher.launch.xml Signed-off-by: Takayuki Murooka * remove unnecessary comment Signed-off-by: Takayuki Murooka --- .../launch/simulator.launch.xml | 43 ++++++++++++++++++- .../dummy_perception_publisher.launch.xml | 39 +---------------- 2 files changed, 43 insertions(+), 39 deletions(-) diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 6d60482391216..920d6e8a80cd0 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -4,7 +4,8 @@ - + + @@ -25,6 +26,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml index 24a68634f30bb..73385ef459233 100644 --- a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml +++ b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml @@ -5,7 +5,6 @@ - @@ -51,48 +50,12 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - - - - - From 6ded065e53e0e1acded361c132ab9ac86ec30f12 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Wed, 2 Mar 2022 17:10:45 +0900 Subject: [PATCH 13/25] refactor(behavior_velocity): move inline functions to cpp and remove unused (#462) Signed-off-by: tanaka3 --- .../occlusion_spot/occlusion_spot_utils.hpp | 58 ++------------- .../risk_predictive_braking.hpp | 72 ++----------------- .../occlusion_spot/occlusion_spot_utils.cpp | 61 ++++++++++++---- .../risk_predictive_braking.cpp | 61 ++++++++++++++++ .../occlusion_spot/scene_occlusion_spot.cpp | 2 +- .../scene_occlusion_spot_in_public_road.cpp | 2 +- 6 files changed, 120 insertions(+), 136 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 3a3f13d709338..3064cab1d23f7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -179,60 +179,14 @@ struct PossibleCollisionInfo lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path); // Note : consider offset_from_start_to_ego and safety margin for collision here -inline void handleCollisionOffset( - std::vector & possible_collisions, double offset, double margin) -{ - for (auto & pc : possible_collisions) { - pc.arc_lane_dist_at_collision.length -= offset; - pc.arc_lane_dist_at_collision.length -= margin; - } -} - -inline double offsetFromStartToEgo( - const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx) -{ - double offset_from_ego_to_closest = 0; - for (int i = 0; i < closest_idx; i++) { - const auto & curr_p = path.points.at(i).point.pose.position; - const auto & next_p = path.points.at(i + 1).point.pose.position; - offset_from_ego_to_closest += tier4_autoware_utils::calcDistance2d(curr_p, next_p); - } - const double offset_from_closest_to_target = - -planning_utils::transformRelCoordinate2D(ego_pose, path.points[closest_idx].point.pose) - .position.x; - return offset_from_ego_to_closest + offset_from_closest_to_target; -} - -inline void clipPathByLength( - const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length = 100.0) -{ - double length_sum = 0; - for (int i = 0; i < static_cast(path.points.size()) - 1; i++) { - length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (length_sum > max_length) return; - clipped.points.emplace_back(path.points.at(i)); - } -} - -inline bool isStuckVehicle(PredictedObject obj, const double min_vel) -{ - if ( - obj.classification.at(0).label == ObjectClassification::CAR || - obj.classification.at(0).label == ObjectClassification::TRUCK || - obj.classification.at(0).label == ObjectClassification::BUS) { - if (std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x) < min_vel) { - return true; - } - } - return false; -} -void filterCollisionByRoadType( - std::vector & possible_collisions, const DetectionAreaIdx road_type); +void handleCollisionOffset(std::vector & possible_collisions, double offset); +void clipPathByLength( + const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length = 100.0); +bool isStuckVehicle(PredictedObject obj, const double min_vel); +double offsetFromStartToEgo( + const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx); std::vector filterDynamicObjectByDetectionArea( std::vector & objs, const std::vector polys); -bool splineInterpolate( - const PathWithLaneId & input, const double interval, PathWithLaneId * output, - const rclcpp::Logger logger); std::vector getParkedVehicles( const PredictedObjects & dyn_objects, const PlannerParam & param, std::vector & debug_point); diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp index 59145de966dfe..9156d5d04ce07 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp @@ -34,87 +34,25 @@ int insertSafeVelocityToPath( const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param, PathWithLaneId * inout_path); -// @brief calculates the maximum velocity allowing to decelerate within the given distance -inline double calculateMinSlowDownVelocity( - const double v0, const double len, const double a_max, const double safe_vel) -{ - // if target velocity is inserted backward return current velocity as limit - if (len < 0) return safe_vel; - return std::sqrt(std::max(std::pow(v0, 2.0) - 2.0 * std::abs(a_max) * len, 0.0)); -} - /** * * @param: longitudinal_distance: longitudinal distance to collision * @param: param: planner param * @return lateral distance **/ -inline double calculateLateralDistanceFromTTC( - const double longitudinal_distance, const PlannerParam & param) -{ - const auto & v = param.v; - const auto & p = param; - double v_min = 1.0; - const double lateral_buffer = 0.5; - const double min_distance = p.half_vehicle_width + lateral_buffer; - const double max_distance = p.detection_area.max_lateral_distance; - if (longitudinal_distance <= 0) return min_distance; - if (v_min < param.v.min_allowed_velocity) v_min = param.v.min_allowed_velocity; - // use min velocity if ego velocity is below min allowed - const double v0 = (v.v_ego > v_min) ? v.v_ego : v_min; - // here is a part where ego t(ttc) can be replaced by calculation of velocity smoother or ? - double t = longitudinal_distance / v0; - double lateral_distance = t * param.pedestrian_vel + p.half_vehicle_width; - return std::min(max_distance, std::max(min_distance, lateral_distance)); -} +double calculateLateralDistanceFromTTC( + const double longitudinal_distance, const PlannerParam & param); /** * @param: v: ego velocity config * @param: ttc: time to collision * @return safe motion **/ -inline SafeMotion calculateSafeMotion(const Velocity & v, const double ttc) -{ - SafeMotion sm; - const double j_max = v.safety_ratio * v.max_stop_jerk; - const double a_max = v.safety_ratio * v.max_stop_accel; - const double t1 = v.delay_time; - double t2 = a_max / j_max; - double & v_safe = sm.safe_velocity; - double & stop_dist = sm.stop_dist; - if (ttc <= t1) { - // delay - v_safe = 0; - stop_dist = 0; - } else if (ttc <= t2 + t1) { - // delay + const jerk - t2 = ttc - t1; - v_safe = -0.5 * j_max * t2 * t2; - stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6; - } else { - const double t3 = ttc - t2 - t1; - // delay + const jerk + const accel - const double v2 = -0.5 * j_max * t2 * t2; - v_safe = v2 - a_max * t3; - stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6 + v2 * t3 - 0.5 * a_max * t3 * t3; - } - stop_dist += v.safe_margin; - return sm; -} +SafeMotion calculateSafeMotion(const Velocity & v, const double ttc); -inline double calculateInsertVelocity( +double calculateInsertVelocity( const double min_allowed_vel, const double safe_vel, const double min_vel, - const double original_vel) -{ - const double max_vel_noise = 0.05; - // ensure safe velocity doesn't exceed maximum allowed pbs deceleration - double cmp_safe_vel = std::max(min_allowed_vel + max_vel_noise, safe_vel); - // ensure safe path velocity is also above ego min velocity - cmp_safe_vel = std::max(cmp_safe_vel, min_vel); - // ensure we only lower the original velocity (and do not increase it) - cmp_safe_vel = std::min(cmp_safe_vel, original_vel); - return cmp_safe_vel; -} + const double original_vel); } // namespace occlusion_spot_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index 002306a91e7f6..8ba933fd2db40 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -146,6 +146,52 @@ void calcSlowDownPointsForPossibleCollision( } } +void handleCollisionOffset(std::vector & possible_collisions, double offset) +{ + for (auto & pc : possible_collisions) { + pc.arc_lane_dist_at_collision.length -= offset; + } +} + +void clipPathByLength( + const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length) +{ + double length_sum = 0; + for (int i = 0; i < static_cast(path.points.size()) - 1; i++) { + length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + if (length_sum > max_length) return; + clipped.points.emplace_back(path.points.at(i)); + } +} + +bool isStuckVehicle(PredictedObject obj, const double min_vel) +{ + if ( + obj.classification.at(0).label == ObjectClassification::CAR || + obj.classification.at(0).label == ObjectClassification::TRUCK || + obj.classification.at(0).label == ObjectClassification::BUS) { + if (std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x) < min_vel) { + return true; + } + } + return false; +} + +double offsetFromStartToEgo( + const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx) +{ + double offset_from_ego_to_closest = 0; + for (int i = 0; i < closest_idx; i++) { + const auto & curr_p = path.points.at(i).point.pose.position; + const auto & next_p = path.points.at(i + 1).point.pose.position; + offset_from_ego_to_closest += tier4_autoware_utils::calcDistance2d(curr_p, next_p); + } + const double offset_from_closest_to_target = + -planning_utils::transformRelCoordinate2D(ego_pose, path.points[closest_idx].point.pose) + .position.x; + return offset_from_ego_to_closest + offset_from_closest_to_target; +} + std::vector getParkedVehicles( const PredictedObjects & dyn_objects, const PlannerParam & param, std::vector & debug_point) @@ -337,21 +383,6 @@ std::vector filterDynamicObjectByDetectionArea( return filtered_obj; } -void filterCollisionByRoadType( - std::vector & possible_collisions, const DetectionAreaIdx area) -{ - std::pair focus_length = area.get(); - for (auto it = possible_collisions.begin(); it != possible_collisions.end();) { - const auto & pc_len = it->arc_lane_dist_at_collision.length; - if (focus_length.first < pc_len && pc_len < focus_length.second) { - it++; - } else { - // -----erase-----|start------target-------end|----erase--- - it = possible_collisions.erase(it); - } - } -} - void createPossibleCollisionsInDetectionArea( const std::vector & detection_area_polygons, std::vector & possible_collisions, const grid_map::GridMap & grid, diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp index 6c5b9f10d218b..cb7e56245a4c4 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp @@ -105,5 +105,66 @@ int insertSafeVelocityToPath( return 0; } +double calculateLateralDistanceFromTTC( + const double longitudinal_distance, const PlannerParam & param) +{ + const auto & v = param.v; + const auto & p = param; + double v_min = 1.0; + const double lateral_buffer = 0.5; + const double min_distance = p.half_vehicle_width + lateral_buffer; + const double max_distance = p.detection_area.max_lateral_distance; + if (longitudinal_distance <= 0) return min_distance; + if (v_min < param.v.min_allowed_velocity) v_min = param.v.min_allowed_velocity; + // use min velocity if ego velocity is below min allowed + const double v0 = (v.v_ego > v_min) ? v.v_ego : v_min; + // here is a part where ego t(ttc) can be replaced by calculation of velocity smoother or ? + double t = longitudinal_distance / v0; + double lateral_distance = t * param.pedestrian_vel + p.half_vehicle_width; + return std::min(max_distance, std::max(min_distance, lateral_distance)); +} + +SafeMotion calculateSafeMotion(const Velocity & v, const double ttc) +{ + SafeMotion sm; + const double j_max = v.safety_ratio * v.max_stop_jerk; + const double a_max = v.safety_ratio * v.max_stop_accel; + const double t1 = v.delay_time; + double t2 = a_max / j_max; + double & v_safe = sm.safe_velocity; + double & stop_dist = sm.stop_dist; + if (ttc <= t1) { + // delay + v_safe = 0; + stop_dist = 0; + } else if (ttc <= t2 + t1) { + // delay + const jerk + t2 = ttc - t1; + v_safe = -0.5 * j_max * t2 * t2; + stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6; + } else { + const double t3 = ttc - t2 - t1; + // delay + const jerk + const accel + const double v2 = -0.5 * j_max * t2 * t2; + v_safe = v2 - a_max * t3; + stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6 + v2 * t3 - 0.5 * a_max * t3 * t3; + } + stop_dist += v.safe_margin; + return sm; +} + +double calculateInsertVelocity( + const double min_allowed_vel, const double safe_vel, const double min_vel, + const double original_vel) +{ + const double max_vel_noise = 0.05; + // ensure safe velocity doesn't exceed maximum allowed pbs deceleration + double cmp_safe_vel = std::max(min_allowed_vel + max_vel_noise, safe_vel); + // ensure safe path velocity is also above ego min velocity + cmp_safe_vel = std::max(cmp_safe_vel, min_vel); + // ensure we only lower the original velocity (and do not increase it) + cmp_safe_vel = std::min(cmp_safe_vel, original_vel); + return cmp_safe_vel; +} } // namespace occlusion_spot_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 26aaaaae02df0..ad7d7da0f428a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -99,7 +99,7 @@ bool OcclusionSpotModule::modifyPathVelocity( logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size()); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); // Note: Consider offset from path start to ego here - utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego, 0.0); + utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego); // apply safe velocity using ebs and pbs deceleration utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); // these debug topics needs computation resource diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp index 3a3d6013b49b6..60e2d211e9ca6 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp @@ -93,7 +93,7 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( interp_path, param_, offset_from_start_to_ego, filtered_obj); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); // Note: Consider offset from path start to ego here - utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego, 0.0); + utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego); // apply safe velocity using ebs and pbs deceleration utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); if (param_.debug) { From 4c37f245eccebba8ac850a6437acd378a6333df0 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Wed, 2 Mar 2022 20:02:28 +0900 Subject: [PATCH 14/25] chore: sync files (#467) Signed-off-by: GitHub Co-authored-by: kenji-miyake --- .../workflows/build-and-test-differential-self-hosted.yaml | 2 +- .github/workflows/build-and-test-differential.yaml | 4 ++-- .github/workflows/build-and-test-self-hosted.yaml | 2 +- .github/workflows/build-and-test.yaml | 2 +- .github/workflows/check-build-depends.yaml | 2 +- .github/workflows/delete-closed-pr-docs.yaml | 2 +- .github/workflows/deploy-docs.yaml | 2 +- .github/workflows/pre-commit-optional.yaml | 2 +- .github/workflows/pre-commit.yaml | 2 +- .github/workflows/spell-check-differential.yaml | 2 +- 10 files changed, 11 insertions(+), 11 deletions(-) diff --git a/.github/workflows/build-and-test-differential-self-hosted.yaml b/.github/workflows/build-and-test-differential-self-hosted.yaml index 824ac368f936c..9b4b50c2a036d 100644 --- a/.github/workflows/build-and-test-differential-self-hosted.yaml +++ b/.github/workflows/build-and-test-differential-self-hosted.yaml @@ -21,7 +21,7 @@ jobs: container: ghcr.io/autowarefoundation/autoware-universe:latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 with: fetch-depth: 0 diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 6e8ebef574ed5..1ccc33765fc83 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -12,7 +12,7 @@ jobs: uses: styfle/cancel-workflow-action@0.9.1 - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 with: fetch-depth: 0 @@ -54,7 +54,7 @@ jobs: needs: build-and-test-differential steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 with: fetch-depth: 0 diff --git a/.github/workflows/build-and-test-self-hosted.yaml b/.github/workflows/build-and-test-self-hosted.yaml index c5a5ac4734bc5..dd7d4dee01f4b 100644 --- a/.github/workflows/build-and-test-self-hosted.yaml +++ b/.github/workflows/build-and-test-self-hosted.yaml @@ -11,7 +11,7 @@ jobs: container: ghcr.io/autowarefoundation/autoware-universe:latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 1e422cc380f11..975a8e991985f 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -13,7 +13,7 @@ jobs: container: ghcr.io/autowarefoundation/autoware-universe:latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml index d8f731787f4f0..26747fc8aefd8 100644 --- a/.github/workflows/check-build-depends.yaml +++ b/.github/workflows/check-build-depends.yaml @@ -14,7 +14,7 @@ jobs: uses: styfle/cancel-workflow-action@0.9.1 - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal diff --git a/.github/workflows/delete-closed-pr-docs.yaml b/.github/workflows/delete-closed-pr-docs.yaml index bb11e4dc75e78..abc983e3e368d 100644 --- a/.github/workflows/delete-closed-pr-docs.yaml +++ b/.github/workflows/delete-closed-pr-docs.yaml @@ -10,7 +10,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 with: fetch-depth: 0 diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index ce35cb8f76ce5..59b116e7731f8 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -30,7 +30,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 with: fetch-depth: 0 ref: ${{ github.event.pull_request.head.sha }} diff --git a/.github/workflows/pre-commit-optional.yaml b/.github/workflows/pre-commit-optional.yaml index 50b6778a25c0a..0ccc7e59cb98c 100644 --- a/.github/workflows/pre-commit-optional.yaml +++ b/.github/workflows/pre-commit-optional.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Run pre-commit uses: autowarefoundation/autoware-github-actions/pre-commit@tier4/proposal diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index 9eb501a19dac9..a1fc538110b82 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Run pre-commit uses: autowarefoundation/autoware-github-actions/pre-commit@tier4/proposal diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml index 1b5c52cb40ec4..8a7b7da4f8f4f 100644 --- a/.github/workflows/spell-check-differential.yaml +++ b/.github/workflows/spell-check-differential.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Run spell-check uses: autowarefoundation/autoware-github-actions/spell-check@tier4/proposal From 405c0ca58c6d3bedff52aa7b9b19b60d9f2f27f9 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 2 Mar 2022 11:13:43 +0000 Subject: [PATCH 15/25] chore(deps): bump actions/checkout from 2 to 3 (#466) * chore(deps): bump actions/checkout from 2 to 3 Bumps [actions/checkout](https://github.com/actions/checkout) from 2 to 3. - [Release notes](https://github.com/actions/checkout/releases) - [Changelog](https://github.com/actions/checkout/blob/main/CHANGELOG.md) - [Commits](https://github.com/actions/checkout/compare/v2...v3) --- updated-dependencies: - dependency-name: actions/checkout dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] * sync clang-tidy-pr-comments.yaml Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --- .github/sync-files.yaml | 1 + .github/workflows/clang-tidy-pr-comments.yaml | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index e4055a49f688e..f38ec151216a4 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -26,6 +26,7 @@ - source: .github/workflows/build-and-test-differential-self-hosted.yaml - source: .github/workflows/build-and-test-self-hosted.yaml - source: .github/workflows/check-build-depends.yaml + - source: .github/workflows/clang-tidy-pr-comments.yaml - repository: autowarefoundation/autoware-documentation files: diff --git a/.github/workflows/clang-tidy-pr-comments.yaml b/.github/workflows/clang-tidy-pr-comments.yaml index ea035a25f3000..dc53df49ac20c 100644 --- a/.github/workflows/clang-tidy-pr-comments.yaml +++ b/.github/workflows/clang-tidy-pr-comments.yaml @@ -13,7 +13,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Download analysis results run: | @@ -29,7 +29,7 @@ jobs: echo ::set-output name=pr-head-ref::"$(cat /tmp/clang-tidy-result/pr-head-ref.txt)" - name: Check out PR head - uses: actions/checkout@v2 + uses: actions/checkout@v3 with: repository: ${{ steps.set-variables.outputs.pr-head-repo }} ref: ${{ steps.set-variables.outputs.pr-head-ref }} From fbb7be74000ac2d024a7fde6c0d4e8e345c112cd Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 3 Mar 2022 04:18:17 +0900 Subject: [PATCH 16/25] feat(gyro_odometer): check message timestamp (#401) * feat(gyro_odometer): check message timestamp Signed-off-by: YamatoAndo * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/gyro_odometer/README.md | 7 ++-- .../gyro_odometer/gyro_odometer_core.hpp | 3 ++ .../launch/gyro_odometer.launch.xml | 2 + .../gyro_odometer/src/gyro_odometer_core.cpp | 41 +++++++++++++++---- 4 files changed, 42 insertions(+), 11 deletions(-) diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md index dc8718f76449b..ec50a113af4d9 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/gyro_odometer/README.md @@ -21,9 +21,10 @@ ## Parameters -| Parameter | Type | Description | -| -------------- | ------ | ----------------- | -| `output_frame` | String | output's frame id | +| Parameter | Type | Description | +| --------------------- | ------ | -------------------------------- | +| `output_frame` | String | output's frame id | +| `message_timeout_sec` | Double | delay tolerance time for message | ## Assumptions / Known limits diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index f95374a48a12d..b7dc9ea3414f4 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -54,7 +54,10 @@ class GyroOdometer : public rclcpp::Node tf2_ros::TransformListener tf_listener_; std::string output_frame_; + double message_timeout_sec_; + geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr_; + sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr_; }; #endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml index b3148aec12e55..9a0a78ad4bd39 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -7,6 +7,7 @@ + @@ -18,5 +19,6 @@ + diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index e4ab35dbace19..7dd1da5c0f0e7 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -24,7 +24,8 @@ GyroOdometer::GyroOdometer() : Node("gyro_odometer"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_), - output_frame_(declare_parameter("base_link", "base_link")) + output_frame_(declare_parameter("base_link", "base_link")), + message_timeout_sec_(declare_parameter("message_timeout_sec", 0.2)) { vehicle_twist_with_cov_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, @@ -47,25 +48,49 @@ void GyroOdometer::callbackTwistWithCovariance( { // TODO(YamatoAndo) trans from twist_with_cov_msg_ptr->header to base_frame twist_with_cov_msg_ptr_ = twist_with_cov_msg_ptr; + + if (!imu_msg_ptr_) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Imu msg is not subscribed"); + return; + } + const double imu_dt = std::abs((this->now() - imu_msg_ptr_->header.stamp).seconds()); + if (imu_dt > message_timeout_sec_) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, + "Imu msg is timeout. imu_dt: %lf[sec], tolerance %lf[sec]", imu_dt, message_timeout_sec_); + return; + } } void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { + imu_msg_ptr_ = imu_msg_ptr; + if (!twist_with_cov_msg_ptr_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Twist msg is not subscribed"); + return; + } + + const double twist_dt = std::abs((this->now() - twist_with_cov_msg_ptr_->header.stamp).seconds()); + if (twist_dt > message_timeout_sec_) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, + "Twist msg is timeout. twist_dt: %lf[sec], tolerance %lf[sec]", twist_dt, + message_timeout_sec_); return; } geometry_msgs::msg::TransformStamped::SharedPtr tf_base2imu_ptr = std::make_shared(); - getTransform(output_frame_, imu_msg_ptr->header.frame_id, tf_base2imu_ptr); + getTransform(output_frame_, imu_msg_ptr_->header.frame_id, tf_base2imu_ptr); geometry_msgs::msg::Vector3Stamped angular_velocity; - angular_velocity.header = imu_msg_ptr->header; - angular_velocity.vector = imu_msg_ptr->angular_velocity; + angular_velocity.header = imu_msg_ptr_->header; + angular_velocity.vector = imu_msg_ptr_->angular_velocity; geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; transformed_angular_velocity.header = tf_base2imu_ptr->header; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_base2imu_ptr); // clear imu yaw bias if vehicle is stopped @@ -77,14 +102,14 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m // TODO(YamatoAndo) move code geometry_msgs::msg::TwistStamped twist; - twist.header.stamp = imu_msg_ptr->header.stamp; + twist.header.stamp = imu_msg_ptr_->header.stamp; twist.header.frame_id = output_frame_; twist.twist.linear = twist_with_cov_msg_ptr_->twist.twist.linear; twist.twist.angular.z = transformed_angular_velocity.vector.z; // TODO(YamatoAndo) yaw_rate only twist_pub_->publish(twist); geometry_msgs::msg::TwistWithCovarianceStamped twist_with_covariance; - twist_with_covariance.header.stamp = imu_msg_ptr->header.stamp; + twist_with_covariance.header.stamp = imu_msg_ptr_->header.stamp; twist_with_covariance.header.frame_id = output_frame_; twist_with_covariance.twist.twist.linear = twist_with_cov_msg_ptr_->twist.twist.linear; twist_with_covariance.twist.twist.angular.z = @@ -98,7 +123,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m twist_with_covariance.twist.covariance[14] = 10000.0; twist_with_covariance.twist.covariance[21] = 10000.0; twist_with_covariance.twist.covariance[28] = 10000.0; - twist_with_covariance.twist.covariance[35] = imu_msg_ptr->angular_velocity_covariance[8]; + twist_with_covariance.twist.covariance[35] = imu_msg_ptr_->angular_velocity_covariance[8]; twist_with_covariance_pub_->publish(twist_with_covariance); } From 1bab84e75e6a6b750b5781212b04702b7646616f Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 3 Mar 2022 04:19:34 +0900 Subject: [PATCH 17/25] feat(ndt_scan_matcher): add tolerance of initial pose (#408) * feat(ndt_scan_matcher): add tolerance of initial pose Signed-off-by: Yamato Ando * move codes Signed-off-by: YamatoAndo * modify the default value Signed-off-by: YamatoAndo * change the variable names Signed-off-by: YamatoAndo * ci(pre-commit): autofix * fix typo Signed-off-by: YamatoAndo * add depend fmt Signed-off-by: YamatoAndo * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/ndt_scan_matcher.param.yaml | 6 ++ .../config/ndt_scan_matcher.param.yaml | 6 ++ .../ndt_scan_matcher_core.hpp | 9 +++ localization/ndt_scan_matcher/package.xml | 1 + .../src/ndt_scan_matcher_core.cpp | 60 ++++++++++++++++++- 5 files changed, 81 insertions(+), 1 deletion(-) diff --git a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml index ae5a39dd99ceb..fd21436b3909b 100644 --- a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml +++ b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml @@ -29,6 +29,12 @@ # The number of particles to estimate initial pose initial_estimate_particles_num: 100 + # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] + initial_pose_timeout_sec: 1.0 + + # Tolerance of distance difference between two initial poses used for linear interpolation. [m] + initial_pose_distance_tolerance_m: 10.0 + # neighborhood search method in OMP # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 omp_neighborhood_search_method: 0 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 9b044ec409934..7b62785f7f11a 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -30,6 +30,12 @@ # The number of particles to estimate initial pose initial_estimate_particles_num: 100 + # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] + initial_pose_timeout_sec: 1.0 + + # Tolerance of distance difference between two initial poses used for linear interpolation. [m] + initial_pose_distance_tolerance_m: 10.0 + # neighborhood search method in OMP # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 omp_neighborhood_search_method: 0 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 f4407819a0ae0..a74c5b47b51e7 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 @@ -113,6 +113,13 @@ class NDTScanMatcher : public rclcpp::Node const std::string & target_frame, const std::string & source_frame, const geometry_msgs::msg::TransformStamped::SharedPtr & transform_stamped_ptr); + bool validateTimeStampDifference( + const rclcpp::Time & target_time, const rclcpp::Time & reference_time, + const double time_tolerance_sec); + bool validatePositionDifference( + const geometry_msgs::msg::Point & target_point, + const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_); + void timerDiagnostic(); rclcpp::Subscription::SharedPtr initial_pose_sub_; @@ -154,6 +161,8 @@ class NDTScanMatcher : public rclcpp::Node std::string map_frame_; double converged_param_transform_probability_; int initial_estimate_particles_num_; + double initial_pose_timeout_sec_; + double initial_pose_distance_tolerance_m_; float inversion_vector_threshold_; float oscillation_threshold_; std::array output_pose_covariance_; diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index c83659f337958..9b6bb71d60426 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -8,6 +8,7 @@ ament_cmake_auto diagnostic_msgs + fmt geometry_msgs nav_msgs ndt 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 855b7c9750623..dca714376599a 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -104,6 +104,8 @@ NDTScanMatcher::NDTScanMatcher() map_frame_("map"), converged_param_transform_probability_(4.5), initial_estimate_particles_num_(100), + initial_pose_timeout_sec_(1.0), + initial_pose_distance_tolerance_m_(10.0), inversion_vector_threshold_(-0.9), oscillation_threshold_(10) { @@ -169,6 +171,12 @@ NDTScanMatcher::NDTScanMatcher() initial_estimate_particles_num_ = this->declare_parameter("initial_estimate_particles_num", initial_estimate_particles_num_); + initial_pose_timeout_sec_ = + this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_); + + initial_pose_distance_tolerance_m_ = this->declare_parameter( + "initial_pose_distance_tolerance_m", initial_pose_distance_tolerance_m_); + std::vector output_pose_covariance = this->declare_parameter>("output_pose_covariance"); for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { @@ -433,9 +441,27 @@ void NDTScanMatcher::callbackSensorPoints( initial_pose_msg_ptr_array_, sensor_ros_time, initial_pose_old_msg_ptr, initial_pose_new_msg_ptr); popOldPose(initial_pose_msg_ptr_array_, sensor_ros_time); - // TODO(Tier IV): check pose_timestamp - sensor_ros_time + + // check the time stamp + bool valid_old_timestamp = validateTimeStampDifference( + initial_pose_old_msg_ptr->header.stamp, sensor_ros_time, initial_pose_timeout_sec_); + bool valid_new_timestamp = validateTimeStampDifference( + initial_pose_new_msg_ptr->header.stamp, sensor_ros_time, initial_pose_timeout_sec_); + + // check the position jumping (ex. immediately after the initial pose estimation) + bool valid_new_to_old_distance = validatePositionDifference( + initial_pose_old_msg_ptr->pose.pose.position, initial_pose_new_msg_ptr->pose.pose.position, + initial_pose_distance_tolerance_m_); + + // must all validations are true + if (!(valid_old_timestamp && valid_new_timestamp && valid_new_to_old_distance)) { + RCLCPP_WARN(get_logger(), "Validation error."); + return; + } + const auto initial_pose_msg = interpolatePose(*initial_pose_old_msg_ptr, *initial_pose_new_msg_ptr, sensor_ros_time); + // enf of critical section for initial_pose_msg_ptr_array_ initial_pose_array_lock.unlock(); @@ -688,3 +714,35 @@ bool NDTScanMatcher::getTransform( } return true; } + +bool NDTScanMatcher::validateTimeStampDifference( + const rclcpp::Time & target_time, const rclcpp::Time & reference_time, + const double time_tolerance_sec) +{ + const double dt = std::abs((target_time - reference_time).seconds()); + if (dt > time_tolerance_sec) { + RCLCPP_WARN( + get_logger(), + "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " + "difference is %lf[sec] (the tolerance is %lf[sec]).", + reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); + return false; + } + return true; +} + +bool NDTScanMatcher::validatePositionDifference( + const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, + const double distance_tolerance_m_) +{ + double distance = norm(target_point, reference_point); + if (distance > distance_tolerance_m_) { + RCLCPP_WARN( + get_logger(), + "Validation error. The distance from reference position to target position is %lf[m] (the " + "tolerance is %lf[m]).", + distance, distance_tolerance_m_); + return false; + } + return true; +} From f03a7b851278df396e44b4effbb405a147cb9123 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Thu, 3 Mar 2022 01:20:09 +0000 Subject: [PATCH 18/25] chore: sync files (#469) Signed-off-by: GitHub Co-authored-by: kenji-miyake --- .../build-and-test-differential-self-hosted.yaml | 10 +++++----- .github/workflows/build-and-test-differential.yaml | 14 +++++++------- .github/workflows/build-and-test-self-hosted.yaml | 8 ++++---- .github/workflows/build-and-test.yaml | 8 ++++---- .github/workflows/check-build-depends.yaml | 6 +++--- .github/workflows/pre-commit-optional.yaml | 2 +- .github/workflows/pre-commit.yaml | 2 +- .github/workflows/semantic-pull-request.yaml | 2 +- .github/workflows/spell-check-differential.yaml | 2 +- 9 files changed, 27 insertions(+), 27 deletions(-) diff --git a/.github/workflows/build-and-test-differential-self-hosted.yaml b/.github/workflows/build-and-test-differential-self-hosted.yaml index 9b4b50c2a036d..a15ced5f3b0c8 100644 --- a/.github/workflows/build-and-test-differential-self-hosted.yaml +++ b/.github/workflows/build-and-test-differential-self-hosted.yaml @@ -10,7 +10,7 @@ on: jobs: prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@tier4/proposal + uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 with: label: ARM64 @@ -26,15 +26,15 @@ jobs: fetch-depth: 0 - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - name: Get modified packages id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@tier4/proposal + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - name: Build if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-build@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} @@ -42,7 +42,7 @@ jobs: - name: Test if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-test@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 1ccc33765fc83..4be5ce9620a6b 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -17,15 +17,15 @@ jobs: fetch-depth: 0 - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - name: Get modified packages id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@tier4/proposal + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - name: Build if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-build@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} @@ -34,7 +34,7 @@ jobs: - name: Test id: test if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-test@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} @@ -59,15 +59,15 @@ jobs: fetch-depth: 0 - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - name: Get modified packages id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@tier4/proposal + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - name: Run clang-tidy if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@tier4/proposal + uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} diff --git a/.github/workflows/build-and-test-self-hosted.yaml b/.github/workflows/build-and-test-self-hosted.yaml index dd7d4dee01f4b..5ae9846739dcb 100644 --- a/.github/workflows/build-and-test-self-hosted.yaml +++ b/.github/workflows/build-and-test-self-hosted.yaml @@ -14,15 +14,15 @@ jobs: uses: actions/checkout@v3 - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - name: Get self packages id: get-self-packages - uses: autowarefoundation/autoware-github-actions/get-self-packages@tier4/proposal + uses: autowarefoundation/autoware-github-actions/get-self-packages@v1 - name: Build if: ${{ steps.get-self-packages.outputs.self-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-build@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: rosdistro: galactic target-packages: ${{ steps.get-self-packages.outputs.self-packages }} @@ -30,7 +30,7 @@ jobs: - name: Test if: ${{ steps.get-self-packages.outputs.self-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-test@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: rosdistro: galactic target-packages: ${{ steps.get-self-packages.outputs.self-packages }} diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 975a8e991985f..61ec72be49316 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -16,15 +16,15 @@ jobs: uses: actions/checkout@v3 - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - name: Get self packages id: get-self-packages - uses: autowarefoundation/autoware-github-actions/get-self-packages@tier4/proposal + uses: autowarefoundation/autoware-github-actions/get-self-packages@v1 - name: Build if: ${{ steps.get-self-packages.outputs.self-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-build@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: rosdistro: galactic target-packages: ${{ steps.get-self-packages.outputs.self-packages }} @@ -33,7 +33,7 @@ jobs: - name: Test if: ${{ steps.get-self-packages.outputs.self-packages != '' }} id: test - uses: autowarefoundation/autoware-github-actions/colcon-test@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: rosdistro: galactic target-packages: ${{ steps.get-self-packages.outputs.self-packages }} diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml index 26747fc8aefd8..fbea900cde9a7 100644 --- a/.github/workflows/check-build-depends.yaml +++ b/.github/workflows/check-build-depends.yaml @@ -17,14 +17,14 @@ jobs: uses: actions/checkout@v3 - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@tier4/proposal + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - name: Get self packages id: get-self-packages - uses: autowarefoundation/autoware-github-actions/get-self-packages@tier4/proposal + uses: autowarefoundation/autoware-github-actions/get-self-packages@v1 - name: Build - uses: autowarefoundation/autoware-github-actions/colcon-build@tier4/proposal + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: rosdistro: galactic target-packages: ${{ steps.get-self-packages.outputs.self-packages }} diff --git a/.github/workflows/pre-commit-optional.yaml b/.github/workflows/pre-commit-optional.yaml index 0ccc7e59cb98c..93e05dc2c79e5 100644 --- a/.github/workflows/pre-commit-optional.yaml +++ b/.github/workflows/pre-commit-optional.yaml @@ -11,6 +11,6 @@ jobs: uses: actions/checkout@v3 - name: Run pre-commit - uses: autowarefoundation/autoware-github-actions/pre-commit@tier4/proposal + uses: autowarefoundation/autoware-github-actions/pre-commit@v1 with: pre-commit-config: .pre-commit-config-optional.yaml diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index a1fc538110b82..e1b72f706881c 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -11,6 +11,6 @@ jobs: uses: actions/checkout@v3 - name: Run pre-commit - uses: autowarefoundation/autoware-github-actions/pre-commit@tier4/proposal + uses: autowarefoundation/autoware-github-actions/pre-commit@v1 with: pre-commit-config: .pre-commit-config.yaml diff --git a/.github/workflows/semantic-pull-request.yaml b/.github/workflows/semantic-pull-request.yaml index 0edf1946330d3..71224c224ec0f 100644 --- a/.github/workflows/semantic-pull-request.yaml +++ b/.github/workflows/semantic-pull-request.yaml @@ -9,4 +9,4 @@ on: jobs: semantic-pull-request: - uses: autowarefoundation/autoware-github-actions/.github/workflows/semantic-pull-request.yaml@tier4/proposal + uses: autowarefoundation/autoware-github-actions/.github/workflows/semantic-pull-request.yaml@v1 diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml index 8a7b7da4f8f4f..eb18ccdba38d0 100644 --- a/.github/workflows/spell-check-differential.yaml +++ b/.github/workflows/spell-check-differential.yaml @@ -11,6 +11,6 @@ jobs: uses: actions/checkout@v3 - name: Run spell-check - uses: autowarefoundation/autoware-github-actions/spell-check@tier4/proposal + uses: autowarefoundation/autoware-github-actions/spell-check@v1 with: cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json From a3f0736e877020f7eae9dc1e9d33484aa46b1d79 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 3 Mar 2022 12:06:15 +0900 Subject: [PATCH 19/25] fix(route_handler): fix backward search logic (#445) * fix(route_handler): fix unnecessary backward search Signed-off-by: Fumiya Watanabe * fix(route_handler): fix logic of searching previous lanelets Signed-off-by: Fumiya Watanabe * fix(route_handler): remove unused variable Signed-off-by: Fumiya Watanabe * fix(route_handler): fix avoidance module Signed-off-by: Fumiya Watanabe --- .../avoidance/avoidance_module.cpp | 4 +- .../include/route_handler/route_handler.hpp | 4 +- planning/route_handler/src/route_handler.cpp | 89 +++++++++++++++---- 3 files changed, 74 insertions(+), 23 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 8a18c6450884c..1147fce939eec 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 @@ -1733,8 +1733,8 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c } // get previous lane, and return false if previous lane does not exist - lanelet::ConstLanelet prev_lane; - if (!route_handler->getPreviousLaneletWithinRoute(lane, &prev_lane)) { + lanelet::ConstLanelets prev_lanes; + if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) { return false; } diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 8eedd18786a4f..711709dfda9a7 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -90,8 +90,8 @@ class RouteHandler std::vector getLanesAfterGoal(const double vehicle_length) const; // for lanelet - bool getPreviousLaneletWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const; + bool getPreviousLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const; bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getLaneletsFromPoint(const lanelet::ConstPoint3d & point) const; diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index be4a14173f7e1..9619794208fc0 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -495,15 +495,39 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( lanelet::ConstLanelet current_lanelet = lanelet; double length = 0; - while (rclcpp::ok() && length < min_length) { - lanelet::ConstLanelet prev_lanelet; - if (!getPreviousLaneletWithinRoute(current_lanelet, &prev_lanelet)) { + lanelet::ConstLanelets candidate_lanelets; + if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) { + break; + } + + // If lanelet_sequence_backward with input lanelet contains all candidate lanelets, + // break the loop. + if (std::all_of( + candidate_lanelets.begin(), candidate_lanelets.end(), + [lanelet_sequence_backward, lanelet](auto & prev_llt) { + return std::any_of( + lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), + [prev_llt, lanelet](auto & llt) { + return (llt.id() == prev_llt.id() || lanelet.id() == prev_llt.id()); + }); + })) { + break; + } + + for (const auto & prev_lanelet : candidate_lanelets) { + if (std::any_of( + lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), + [prev_lanelet, lanelet](auto & llt) { + return (llt.id() == prev_lanelet.id() || lanelet.id() == prev_lanelet.id()); + })) { + continue; + } + lanelet_sequence_backward.push_back(prev_lanelet); + length += boost::geometry::length(prev_lanelet.centerline().basicLineString()); + current_lanelet = prev_lanelet; break; } - lanelet_sequence_backward.push_back(prev_lanelet); - length += boost::geometry::length(prev_lanelet.centerline().basicLineString()); - current_lanelet = prev_lanelet; } std::reverse(lanelet_sequence_backward.begin(), lanelet_sequence_backward.end()); @@ -744,20 +768,20 @@ lanelet::ConstLanelets RouteHandler::getNextLanelets(const lanelet::ConstLanelet return routing_graph_ptr_->following(lanelet); } -bool RouteHandler::getPreviousLaneletWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const +bool RouteHandler::getPreviousLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const { if (exists(start_lanelets_, lanelet)) { return false; } - lanelet::ConstLanelets previous_lanelets = routing_graph_ptr_->previous(lanelet); - for (const auto & llt : previous_lanelets) { + lanelet::ConstLanelets candidate_lanelets = routing_graph_ptr_->previous(lanelet); + prev_lanelets->clear(); + for (const auto & llt : candidate_lanelets) { if (exists(route_lanelets_, llt)) { - *prev_lanelet = llt; - return true; + prev_lanelets->push_back(llt); } } - return false; + return !(prev_lanelets->empty()); } lanelet::ConstLanelets RouteHandler::getLaneletsFromPoint(const lanelet::ConstPoint3d & point) const @@ -1359,15 +1383,42 @@ lanelet::ConstLanelets RouteHandler::getLaneSequence(const lanelet::ConstLanelet lanelet::ConstLanelets RouteHandler::getLaneSequenceUpTo( const lanelet::ConstLanelet & lanelet) const { - lanelet::ConstLanelets lane_sequence_backward; + lanelet::ConstLanelets lanelet_sequence_backward; if (!exists(route_lanelets_, lanelet)) { - return lane_sequence_backward; + return lanelet_sequence_backward; } lanelet::ConstLanelet current_lanelet = lanelet; while (rclcpp::ok()) { + lanelet::ConstLanelets candidate_lanelets; + if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) { + break; + } + + // If lanelet_sequence_backward with input lanelet contains all candidate lanelets, + // break the loop. + if (std::all_of( + candidate_lanelets.begin(), candidate_lanelets.end(), + [lanelet_sequence_backward, lanelet](auto & prev_llt) { + return std::any_of( + lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), + [prev_llt, lanelet](auto & llt) { + return (llt.id() == prev_llt.id() || lanelet.id() == prev_llt.id()); + }); + })) { + break; + } + lanelet::ConstLanelet prev_lanelet; - if (!getPreviousLaneletWithinRoute(current_lanelet, &prev_lanelet)) { + for (const auto & prev_llt : candidate_lanelets) { + if (std::any_of( + lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), + [prev_llt, lanelet](auto & llt) { + return (llt.id() == prev_llt.id() || lanelet.id() == prev_llt.id()); + })) { + continue; + } + prev_lanelet = prev_llt; break; } @@ -1376,12 +1427,12 @@ lanelet::ConstLanelets RouteHandler::getLaneSequenceUpTo( if (!isBijectiveConnection(prev_lanelet_section, current_lanelet_section)) { break; } - lane_sequence_backward.push_back(prev_lanelet); + lanelet_sequence_backward.push_back(prev_lanelet); current_lanelet = prev_lanelet; } - std::reverse(lane_sequence_backward.begin(), lane_sequence_backward.end()); - return lane_sequence_backward; + std::reverse(lanelet_sequence_backward.begin(), lanelet_sequence_backward.end()); + return lanelet_sequence_backward; } lanelet::ConstLanelets RouteHandler::getLaneSequenceAfter( From cf35622fd76bf90faf4d9a88e6a8daeb14b0ed60 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Thu, 3 Mar 2022 12:56:46 +0900 Subject: [PATCH 20/25] feat: adding switchable modes to LiDAR Visibility (#345) * feat: check segment in both directions Signed-off-by: badai-nguyen * feat: add free-space ROI setting and mode switching Check normal pcl surrounding weak_first_return pcl to get free space. Signed-off-by: badai-nguyen * feat: add ROI-xyz setting and mode switching Signed-off-by: badai-nguyen * feat: add ROI-azimuth setting and mode switching Signed-off-by: badai-nguyen * fix: frequency_image header Signed-off-by: badai-nguyen * fix: update frequency image issue Signed-off-by: badai-nguyen * fix: typo Signed-off-by: badai-nguyen * fix: typo Signed-off-by: badai-nguyen * ci(pre-commit): autofix * fix: typo Signed-off-by: badai-nguyen * fix: typo Signed-off-by: badai-nguyen * ci(pre-commit): autofix * fix: add normal pcl noise for visibility Signed-off-by: badai-nguyen * ci(pre-commit): autofix * fix: typo Signed-off-by: badai-nguyen * ci(pre-commit): autofix * docs: add ROI mode Signed-off-by: badai-nguyen * ci(pre-commit): autofix * fix: typo Signed-off-by: badai-nguyen * ci(pre-commit): autofix * fix: typo Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Signed-off-by: badai-nguyen * fix: typo Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Signed-off-by: badai-nguyen * fix: typo Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Signed-off-by: badai-nguyen * fix: typo Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Signed-off-by: badai-nguyen * docs: revise documentation Signed-off-by: badai-nguyen * fix: remove unconfirmed ROI mode Signed-off-by: badai-nguyen * fix: typo Signed-off-by: badai-nguyen 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> --- .../docs/dual-return-outlier-filter.md | 21 +++ ...filter-dual_return_ROI_setting_options.png | Bin 0 -> 87959 bytes ...tlier_filter-dual_return_detail.drawio.svg | 2 +- .../dual_return_outlier_filter_nodelet.hpp | 19 +++ .../dual_return_outlier_filter_nodelet.cpp | 125 +++++++++++++++--- 5 files changed, 144 insertions(+), 23 deletions(-) create mode 100644 sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_ROI_setting_options.png diff --git a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md index d2ddc3315256b..dca02739797bd 100644 --- a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -14,11 +14,22 @@ Therefore, in order to use this node, the sensor driver must publish custom data Another feature of this node is that it publishes visibility as a diagnostic topic. With this function, for example, in heavy rain, the sensing module can notify that the processing performance has reached its limit, which can lead to ensuring the safety of the vehicle. +In some complicated road scenes where normal objects also reflect the light in two stages, for instance plants, leaves, some plastic net etc, the visibility faces some drop in fine weather condition. To deal with that, optional settings of a region of interest (ROI) are added. + +1. `Fixed_xyz_ROI` mode: Visibility estimation based on the weak points in a fixed cuboid surrounding region of ego-vehicle, defined by x, y, z in base_link perspective. +2. `Fixed_azimuth_ROI` mode: Visibility estimation based on the weak points in a fixed surrounding region of ego-vehicle, defined by azimuth and distance of LiDAR perspective. + +When select 2 fixed ROI modes, due to the range of weak points is shrink, the sensitivity of visibility is decrease so that a trade of between `weak_first_local_noise_threshold` and `visibility_threshold` is needed. + ![outlier_filter-dual_return_overall](./image/outlier_filter-dual_return_overall.drawio.svg) The figure below describe how the node works. ![outlier_filter-dual_return_detail](./image/outlier_filter-dual_return_detail.drawio.svg) +The below picture shows the ROI options. + +![outlier_filter-dual_return_ROI_setting_options](./image/outlier_filter-dual_return_ROI_setting_options.png) + ## Inputs / Outputs This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). @@ -47,6 +58,16 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `general_distance_ratio` | double | Threshold for ring_outlier_filter | | `weak_first_local_noise_threshold` | int | The parameter for determining whether it is noise | | `visibility_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN | +| `roi_mode` | string | The name of ROI mode for switching | +| `min_azimuth_deg` | float | The left limit of azimuth for `Fixed_azimuth_ROI` mode | +| `max_azimuth_deg` | float | The right limit of azimuth for `Fixed_azimuth_ROI` mode | +| `max_distance` | float | The limit distance for for `Fixed_azimuth_ROI` mode | +| `x_max` | float | Maximum of x for `Fixed_xyz_ROI` mode | +| `x_min` | float | Minimum of x for `Fixed_xyz_ROI` mode | +| `y_max` | float | Maximum of y for `Fixed_xyz_ROI` mode | +| `y_min` | float | Minimum of y for `Fixed_xyz_ROI` mode | +| `z_max` | float | Maximum of z for `Fixed_xyz_ROI` mode | +| `z_min` | float | Minimum of z for `Fixed_xyz_ROI` mode | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_ROI_setting_options.png b/sensing/pointcloud_preprocessor/docs/image/outlier_filter-dual_return_ROI_setting_options.png new file mode 100644 index 0000000000000000000000000000000000000000..aad1eb144786f25466cd38a96a3eed6d9d031bf2 GIT binary patch literal 87959 zcmX6^1yEc~vxN=r?g{Sh8rn&81LxIMoA)z;L#(_1xn ztM~SF_c>=ks>(8`NJL0bP*A9HvXbghP|$xq#u5VDhh<0PndRdGV=bm61_jlWi2MSC z{g{)P%c?6uLHW`^K?R3HK|Ot#g8xE6d9p!49h*Wy31mS*;k)GhP!sz20?tf9MiT1% zzfVzbW$K3o(M49z{o`u#zhRWU8T)Yq?;)oo1%L1f0AT+t({BR&FkzSjmA#;#KIwk^ zoaK-4?ZXD$={fl2<7rTGl46?Q)=ppVGpWYX50L|>rliJ~^s>v5wdicbFdV#EayL0I zrl)f6y=Du(K0BYQIL{Vv-W;4xwVJl9I$+_D!H^@19@VB58)p9^o%%XN$Bnhwytaq+dfAIL+eICdmX()A}+TRffgTuCPpcqH&u?!pN#Dj z4AVZxSLeExNB8GrK8A5fGNf0_!*?uNC`B*5CW=1~` zo6p>3M)>3)Zyc02j!Xd!2*==~?^!<`K@$F0Q!=^JZ7pxA;C~O$MHEeY_uFR-ANYEr zoj;CD4!8*Hea{Sk-=ch}7luQBIfoweuLvMBtoo+1bn&qqNT>&<#hLF=8H$SbI_VmN zp%DoLiCX%U<`sbQ<@@(Zf|H+gUz0!9#@7p_6&V4&6peIAEY)YxtyW!h9@j}oAv9fd z!qabomQN;j)4dKsG{VW1H0NT^W3T<+mPiLEiN#VI3Kb*q<&<$QK|%$5QH&CJ>avp4 z6rRh6%VFwW5Jlj-Abal|UsT;|{Zj!37C9^+s^v|PR(2Pg-_3RT?!36q@Xh46>5nRF zox)EEZmCB!H>F;!SB7-Mj3xfeb~h1M&u_eDr*K$pN`vI9ruo(97QAuW43CRYsL(AysTEV(e7%^lBprItubmYWO)~FH6 zKW_k(R6qhu*btM(0#EC_@mN~;Z!X}8Dw0KWmC0`sHcgPns21H56JqzJD6>@ddd4xQ*1 z$o&L!H?}&*4vY)coK(fa&R;m}E0taCY(tbuC}2BVHjeXPV4M_j!?(6}8~Y}_T5R1D z>x9G{gb_YNjq(72kv-xiV554ldT5bGaL~=m*r`#6LXXLkA{PzKow<97yJ!pB%aCUd(%FfG$lb6xN_fXP) z-|SnbFj0Z)ZI+e($29b}OX#479ikOpF9HiV+;&~VufxkazxQ9gfA4QH9_rai{c4?! zXAPWvzg6kq%ix<$wnP1LVJ#EN+rTPmQ| ztt41wuSpUi#64$w3(h3y7LnBdAFS=1m20086Z1+#(6G+$&5{P(_!*%a9hF5MdhdoH za6{1T{Y!_*Yb5LW-FQBQDj@D4_hjYa>ABz=&-(+${f zkZm0%AH%!+h+*K)nC-rF` zrzlpDH*8Xq_ZX3vBMBD6!1$HfhN2ih|GO*ahi8-57?Hrm(w4yNd*i;Tu2DFj3p5k| zE9<__H`>5`0G{?tb34~q#L-K6&Zu9BugacjRal{7l}Bs|*(~ycBIg7WTxs6mchfAp z<{>p*kQX#w1TJQg{C>5NTyn~|{Az;Hm|}!Zua1wl?dOt_5s5x}1}Qpzl8`fS%o<;; zYsLnDcvDfn&J1O|IQDqBDuMXE4LHV&vt$#+mRmf>RhA?w94K2<-RahqP0(>DNl#k- zzZm7fgi2w{+1(iJczFE%f~u3-{X)*;;QX2**nQvG-6A8RglX{mYW1-2`f>X6-h$TK z@x>eZ+wQZon{AUAq`1=>1US=IE^9;qPqF0j;wk(9@O()0g92r7R_EW3nDb z8P`WS#R?@gPEDO{f@^prX)mKSc!RDLm7S|m*({l?cS!1c4lE-L(s%Vf6>fMV#n*se z4-ErGC4g_~EM6qP7UUr_NrJI1aJEc-UDO2n;^|oMh;_1ac64j;!P=BXtI;+*L?f=@ z>c<22!YF&56zH`7?Ue<(LUaOUt9j-=t`Th+7EOy}>R2K0DSY|Oy*{xg9p{ae<}-%A z`TY-9Di1%`b#xsW6q}`WE-uejOfGNXc{^Qi1f3kuuY760P)ex8ZIQMw;X0L*G2Yj* zM+&W!=?&6N6VmX9X>!V=ly0OPBoT@M0VvSXP~;M2NEqrR3^jKlX(dtR17DS>_t5m4Z|^3D!Y3*rYn2o_inKW`h9AqxIE-rwJelXs^#AO=A?~7WjkY< z*0Q3q`q?6mLslu+i#&sXHcjV;t7x#0s+!VIpsiD)Shee^x)}vJN8;zWIx{;+wQ6=E`--ODBt_HZ|cen z)8dl1JWS<##CJS#0s~w#66ahdl-c55SBfzv_%SbLsbWHEt#H+XT1!Aq4u!yIm)y(sb1fAbT%^+N>0Y;a`Kj8R7D-oQ#t;sbKxrZFDNP21 zt#Xku%f5W7jq=A?mUfF6C0gaiVhb<~J3HgQ#H7MB6A~j(n;(!pS%Wz|ePXzd*V-M+ z#4@$$DE(bc7H5=89F~SwPMWG~kPH#G2swtyl1qL(PAU>p4F@xg9+UQZICWx;8HsfL z3{}JHPTJv~!xxcCx)>x1j%E{&*9f)3f4Zt~$jE+PVzqj@4d?6iHGg>mqFjfit%fng zDcz2VwK@6kc;TejD^{k#2dJBbdPFFE$SPA#7MC^ZZff*k|ml_8zWhg+M1 z8=J@Mh9(vB7&iHhZ9bXn#JMPLHr|K2S@zg6(24{eDtmrRp0H+h)T~I6rD^$F(PzDB zn<2m~1-Erk>>Udz4mzVLw~0By@y`>&`);*58e>9_?g=75d2&CvDH-D5W7n?|PuA-B zihq*EMYkP-z1YFHWuS^oyp7GzY%3&=~_T1EyJ>-eK{yM>*Ug z!)24Tuz#naxOO_wJ6YC}2S1Iq8mzEhYf05TIfwr-^}c{MpGp_o)gPUHs?=!6caQpx z8|7(PgC^u!#pB3dNRzJeQmMWKWtqNXArCSrrCrXfU;w2*PHIV2tnrU9t%AjvbJy}7 zf?(b{WI-CxGu-pqEvYpB7US~YqDPe{j?HEk{A0a75|>0;btT(t=?-jC|ny6 zIm?lu37;+h-Ot9)4+?LO>&_30V?lrNbyyo;7Lc6#Uh!YL{R~h7AEAG5Jw%&0_)Afu zunAC4bH|rYzWjYHylUw%*`M-9=H391tI1=g;hHc=Z(>J7Vi`T{XNVM1EKg74JG;M{ zwPbRR<8(O!EyEEc!v7gg-u^2F;UYlxsCpfr{QPjhicEGe9MQJZ%!@}JlS$y$b&o<; zA#gM{0+aONUqkcc@N#axx+%t*)shzBuwF;$Qp~lCKa#i@Bg{ExKKf@oK(@~&p6x}l zQE_m7RC1v_c*WvF(ox#h1y-eEMHBxRD}SbbjqEjAASs!_%CEo3%~>=}!Ty@AG1Y0N zx%}vfOoAJlWP{&Qi7eesC^$?WD@X~+(643kvh6SnB{MaQ-hn5f6;>Odz1H3FevHxf z&c4AnK2S=&S$M+A=c97s;eTE*cv(*DH^=dpz$A(shGB;2bw9uby}C=x8SbGMn(Q8U zB__8=yQD{yFZTG~I&KABeBJ8nl;_p%xsWMGcKetj1I*_TFP z|MNPp*LbLR6Z8HK$x9~bFM$LKgSZ^`0nHaubA|#sml@L*C8vNpR{y8x-_5I}>bkQ? zmQtciUnlpy-@Z2VKl0d57O#l4+s%$JpMLL(ux1)^T_1B=lVez1ay6Y$*hSChTzkq9 z?BJ2r{d1b0sh4-*>-ICpzz_k{LK#V1c(ZyDIJmOXXO;A9e2d*Y-vU%wm^DO~Aj^DSEB zeVcbzXW&fVDWnHiFK_;O{ag<;ljCWNAO=B%)a&M?3qfeW3DINlN`d5CfozNOmI z{L9p~UaDo4ZgWAzuYo$x z4ErjOXJCUVFSn$>eQhrSbUNZ4 z4SefR@-<7i$%JIb%DaV-LJVKOfNstYsLcJm$Mes^<9kEJzL-g36OPbTp-~OHshw1zdf~IQ(N~y@Nf5922EPp zvXx|!X0Qg5H6=%?yK@qotQ(x1Sc+W+K1#k<7#~UnkDdq>zuLeU({pVrRpWB)YWTUYp zJnh+=3J3u_f*gkA2=20xk2GPMUiT%#%?T?|EnH2J^e_TlWA-o;FwMq9sR?a-`gwS z%0@0~y{r_4umdC>!7OdjB{u4qQ^!+`rJJ7;|5Roh!$TGZ7w;9-IZvgEwM{~E#!f_y zqdafJHDDq9z8M;$&t+h=BOTsMSh|_J@$%yB|2RJucfe(B+$GMKHs$@JRezd;=9qP4 zyAh(gN596PuYk|*cSO>(YJPIS&C~bB>%lg*vj(fJqhp)v$kX?L8}yX+Nf0Jz_XSdm||*ymU7WlNn5IrDtstfjOQYO1fk*6gFk zK&Wz|0RIdgIoLQa|F?+Q9TvC9=$LUJ$VJSqE_bo@2Cek>Bh8O+Xw}M)TpqcmDl8d# zc=9iZ85QetwD@NS9Gs$0ubm$15S zUP6nMgKKJ*o+oT$-y?vMd*+4k8*Jg*k+=WzH1A$KbiY;X1xxQNqw_oNM*@S}v=Z7D zXdtf%15?~&1`c%x-FMVRcKhE92JMHT2Tso9H*|U5e<|I@NZ(DxFPtb4Gt z{+`?s^zS1Nto`l1csSn;+ClGs<-YHG`)pF+mmkY&QXYV`mxXX>V{1$cb!lsPK4^-? zh!df9YDu`NJVQs!p~z*g;P@jSPprr);r_K?_oI?Tut0$$d>51sK26My6IoOFF3fVF zBOwB*iC4t`Qq`{`<2|H-n2=z5>fhCIG&^z92W5r84Mcz6*l8aG27C) zW+GL5gsT2gVQj3)|N0_&Koni1r{BBC`>!cI6a`_OvDWxoex`9y;Uga%uTO%-3EF+% zD>LcaA}bqXLo**K$B!FWgGZr;YR=kxV~M}4kN-^Gk%L}#FS=T~B!JWmIp#;X4XeZz z!znII$eyBKZai_J>odgBw^x{@ z_vcvyYL!!q%M-3K``w=t7qT$C{PNhm-X~%Fb-&JOm4}y9k%dKip8}0C%1DEiuG=&G zQ#6!K1(BWj{+Irn-TN(5pFR-bmYJpU!umtx@1VfU?v;Mi-7OyL%!B(;*Ft}R&n`Vz zMB89uFM29vE*BmP8;)yZac!!-E}-LR^PD4=6mBl$-sA)L#FpP&5MnEZ9$01fo#U5B zo;7onW8aqNqASPM6RaS?f2jXU{Mv#8pg$?p(WWQi$v^H2OJsg_!$yCc0Y=Y12k_js zy=r>3siP|LZPYdZCWc{{LHdkg%<0}Z9ngdSYO6R{+&h(AERK?Qu?c9N_{U5g*^aKMUM|m@% z1(3G_KDUEZdZ+d~f~K-ReE72Jx$`GU91f7Th+|tgcy7c4Pwe2dMafL&Fg<_lkyK*Z z+VOL1hDF~?SHwMn6W_$l$ti&?EqqfeW2R^7sKxs6kne+|Z9LL+JdGU;XXx5p(To%xgI*&fO1*QYE)iGqn%3xgSKmfV#IK}E8z$RT` zGi5}cRsYxErNCqTFZP4oi4;91#>L;hJs&)uhdv2USUB)E_n6vO&QAMLo}cRb`jl$+ zZR7YY!^pin+#4W3(^z_3Xb8WJzUQ8?z7_B}&Fl3JEt9@7SFWLj4CjP$HF?bRPA~HY z4vjK4xV@Ff!t^F~-Ht|W{8*`UJ-6<8YLW>L>CY$foi(C6`5 zE8_sqHry^$s9?yP-@`3Yu<)I-znRx2R@i{I&te>_(!@8@{cv_J0oA)jxRff&Gz7AN zmEXl9kC=C3E34)ga{hu9ubN^NuuzuReo%#%&PV6GZp|!*L5?>#AUkC&T@hsw0d(wA zs9gR|zkPA2?@|&>Dr5E6;KY^D`~zRc0Vm-{Y7#FDrQk*m@gePwbMN6HeFBGyrD->Q zUs$(i$I$^`zX+;}laUlRk4#|EM+2e(ah?=0cG?Mrlh=Bz@Jzc><0hlM_~=$I zrl4PTcW$~SsChJaa8u+)>z#;4WzIvl@0!;z;E=V;Hetkc+Ijz20{*;Z_^$CqX)aTiYWtj}H; z^Z30Jy1ZbXzWg9aiF3=dna?&U5fo8rVYZ!Zm_KVr+~j%uu99I-ghzMdE~sGdahPG# z4ikSz5-%N{@`Wurx9Wu(jZiM!1wKl5qNB`i;W*FCwo0x!AUFAlt;09s2A{R1l}#q; zyhKy92&NNYv`64Lh?{9}_>sa=Kr$r0j{gyN6rTIJ zmi)v90e$PTzy30!x2rdqSX7E9nrr!cI3}@Ak52ZyyuE`@8u}CkoiIEz^z=le%K91E zk&%}F&e=Sgk*bGEks0=C;p(_a?IQ@YX`)LamZ^uke!_*>QlQp6`{2CHtEYa>RDXkJlE$|r9lvPIG~#<0qk2L#GO>Wf0DJ$BAh@@Eip7hRLS}ttrPUm{D25r-1kD|2kf$) zNt2P*fUKD8(%(5d-T|~D$KRILH(vs98@qjCd*6b^xuW%NxJohTX(LcE>=b{v7R6Qe zg7*-QY-Blz$Up7l-iLf9_kUWdsxb_Fwr)fytwZhWySMIpI;ET`;Fqwjlk?lgiJhDb z=j{U?f(swdV-xjk1v3tXke(nqK|A2vlQ_kJogIN#1%(kY*>^&QKda>yJzuN9(imz4 zgLgLJErbjN6y;@`F$&bDLAL?AsUKBA+{Ww^HzXC|10>l5&diL@O~O_ovNfYJM3T{+ zq)apo<-_VIPGX~!u;+Xek2_EL1IUXJ;tz2rsFYKZc22@bejmvstsmAOxx*)Qu{XzG7nmGc^EuE*y$~ss zy3wHzUT(@~-KMtuyW3J0=T+~1Qed z#Pw#S(B*a4#Qki@Qkl6=%KeDMnx%JWM~Am7;9150`s6)y;=HkaLF~eCcRleP)X?7@ zWp8Y8HZ+;1Z$yJ;FQrtygQ60!rlDS@bW#DEv8v7w?tMSg<7rHqfoHhq_;w6S|GFFt zBw9dgG+R9xBhu3kQ$hNyMC6Qha^$ltD;%E+Qt}EPSCu_Uv?Ku$Hz3NoAEnb4aWi|8 zMLje}j%(eQhU+>@VI|##yHQUVK6b5|m8cSlwaH9A@7u34fu3Q`=YiveZ?3V#`N0;4 z0tRv;3(Fl{9{ad>Yf4XwI1ifb(AfJYA)C;^)`5*7j99OSlQWgQTS^vOy482&# zbQ$wPw}9}aT$$um^BF7ZMBEl6QCMB#(z=R9SleXw1;Sy&#aSlg=};%mYrH0b4d&Mp z^e1e7{dsWX3bCu7{fP8z@*2%|0(uKa?ythGP#hFatQ`GrIW9nuG)cUz#w&>n0wHD_ zo}MnhlDX|q1C4#FW%B65S+|3>56^g!Ud=~2I5;<_2csP52Hm0>`@B1M`91>9A}K(E z9(XHy3*?(#i=T?a$)eZ(n<}QkJ5@!0Zvr1NUbyZw2!6!R1Nh<*&s zrHbB0@Q(bDVTS}kxMV&o#KbpI$M`*^7ri0*bi?Xqizai%&~m@L@ET%*>LM0vztUIE z`^&WE0)vCh@Ed3ne`~v~@CDwu%Xbvtr7?xFJl3y?^&Mz!9A4Yv;po#{DU7Zg7 zrjSaLZC8^2t+p`i!43V<;dCJd%Oc#JdoastXW3P%cRb>ei>dbK24sCw z5lHG)KV9zz`OR}w-#Zn5@|%5}X?{cCgtd3_ zqmFIr6Hxi7p>m&=csO8SOmw>;#@8qcl8G`qJM&8wr1^rPyblX zuhH#f(naNw>58jh(zPMbUW-Y0N`+)4TlEVytUR@rSBGM=df3)YVYcUJjQ;LVRFAKu zlIsd^PHZ?Yb4a9Ub;vBv5};qd`X{vs>*OfC@EbTsYNnMs^FmQ))uJszRI4l()&0*e zAkS{Gz@%u|E~96>e8Lq^gDUQ+J5E{xX;&4PYGoZ_RilIh_XlOvCR5BgN)vf!#x46W z*1CReo#o_|WiH=@EN!U89rd07?Fk1=UW!(C4mk6|rlzgZn<4UmSa)_vL0fl4^fW4$#Gb%S4Y}vr4lWiGA&G0+q;uF{!8Y+ zep528uO4}Tj&Fktk=wfv7Yka@j?{kuha$8_A2o~)y3bs3%ke##1=I#KJ1u02QD3R7 z{jyrPmrku>M!>qOq8S!f=qZQ&7iblU9y4Z2&^(kh9~_CUtwM$%=BAb2zHIc*8Js>| z*DTs}(rN%izol#OaFW<>;|SKGJ|3auBL;tEy1S1!Ud~c$NTnnf3%>5mA^~XpI#!oRXE?W<$iP98wooISmxAr#H0BShBb4PL_ zeE9#wAPt@gwR5NO9Ori&JW&ZDpr&S2V0iULqax2gK+|lse{vbe7VQ@j5K@o504HYd zV>B?CZ3t+|QQtmR_d=u`0!(NAz#%37i_ zjqh9t+b+-3Y{j6`+u4)%NLn(B-3qUkPI;LM$zq-0iF;JGu|^L?yT*kMaSFXSB71+0X$vB+iP*s3Z$rtS7*7*3gt{O?H z_#D|k*ZNz36>X`9Ac^<>*5H}}7RYZC7CJQN=KuVbRQ&JHWRQM)P z^lIMv3hQn@kg29IatJEHaNW3nQ<0{#`Owe6o!u7bt_$Mp|Lvvz9cam&+_0)Rf zjM%F=IuGtU7@B&4f$Q*wPcl(EJV}o5vkCXUK^GK<|-rl^&nP@ts=#sw?T!6lNq;J|_2W<$4V$bB`%)kO8k(aX*Yi#EjH-e2H`C&i zEo=mTH7yIeS}L;Aux?EU;AF?2D3j}224gczOS@R}m9f!V$5~b_QY8%0vq3;oFRvL4 zXEyG<=1KH;%DL1JbcUz1a9O8ag8WI# zTLarFcokPa>v3RHDtNs7T;fkQ&!lI|9>Q2qQ1^l-yxGk20x9_R2Ad%HO65oYVc!*ky z`YlrxDVec(--Q$E+P*#$!=g*u+t|V8^4-xE9-T*ON8Z z^ep?e4UR^DY9;av2)^CSi>IQ%cE{ktWXKZ3?9&zezMCZ1SC-{FD=|p@J=)Og*A(%_ z#^S~atwqGvVdPfk?l%h7Rz7ROa#+FFydgNUVe8E@>__N{Z$>Fdj70yUBH!4Cb#H2O z@5Zlt+k$)dPw?Lj?Y$xiJG_wFQ-X13wG43KMbt2(Cf|NF-pK1pqq6CtzM0_>)vI5$sw-HUlh zqL;xbi==+0u>@9Bof0k_2azNy62bsTQikB-I3!Z6xON^Q0Oq}X%mE15BP_3}O&M== zhj+#^o5fV+@kv(!>BFU+is&&uO4Q>bCusadMlE##Sm;eUL+o9+4*P(vO!5fRE=ELv z-onM_jGcc%w=*Ci*}O2SNkSUfHCQrQ+<}T_RCynQ*@fZwkg%VO3F;Mi z$O>u(0XD}3r?lLR|NZC>5!O~f-){>x2#2iOxn@Q~ZfAbY?*v?%y2jobTZRxm(VxnF zY*rQ7xM~4!|B$&!zMrDmyiPTsy;x~A0u2=d+{kxNFvWvWemcZXPZj&Z$`7e`?AOds zX{uERdSkf@v0l7^){;2Dn~Uo)=vdjK`8G)44H^bL{%8 zniL_)vDO0m3bpoYsedBzJP~@sCxkeeTEIYx^9Kb~#PoF7b`PtC^gspmj6nJqKXtYP zNj${?d2vN-FCi5TpJq+%mG}k<4M2G<^LvFc{gElMkNC?u>smJjbwqqqg=h*E-JoGh zh&s3W^D|T0Y|Y`HC>HZjiNm=ybpp|chl?;C<(=UEW10GcJe_M#q;X$0KVB9b)3KoS zYir!wT?+@5!iGKHf+nXEvf)*DYL=0ABg*Y6M}Z5Sh-b-y_6L{f_E5uhS68HO?2u%9 zdFYizbF?+T?&h0Cp+jSl?I))}F9;Rw0FZsAC|=4SipNPFGZVgkdJ~acs$4f62S&Xp zjZ76l4I?jK_Agy6jVWC%+ui>cz;lVX6+YSI7ZJm%Kva|QHjzDh`zgBpHb-e2^XU}ATw3K2dfKFdLzTQXoHcbDlMorc zPk3LH9~GagX(yHw`K_5Dt4G%ME|9`Wf&ND2FHS-kV=Q($>$(XiY+X$hb>RG#&obk z8HCO5Bn0+!9%3oD8z(_PP#iQh zzA_hCpxRIo49o)KzA7T193kzWr|{_8u^6^bkCh6g%4P z0N?gAK&!$9@dDN*HUF*H>7+Wk^Xl9m^a+1n;iKWj6deAnJU#kR`cKQwqkP(7qe581 zL8v!As5`#UYktJ`X*a8*oOPEjB`?*a)Z+OGU5|~+egpZ&siTosj@1S+`w#%XJ2)KW zH>XY)dIlD(HP;rKV?naM#n;SEaIQ>V#gme~BtWZaOJMEasUv*g0D5_nf4&;ZKgL_O zg=j9gm+25YsnJ`~%S(Q%qr4jp*qGA4LAXxZH4y^S%3R%!z0bb^FcMqEGm4o?-NO~- z)ImH4QVadF{eS|xbbVvHqY@5?x{XAR{emIql$%#1eTQG9U};vLV~JF_#1km?A4|o; zvL>!h){f-X?@%}BOC%ZdLA}Ndc~7|)7_>6#Bn(*v9E2fJECxB%v*xkG+}?It9S$wS zHeVZiT6rAb|=L8JKL%@;4OacNAIh zWHMlZwrv!7WDoo^=x}E_dc&#YQMEgP`3Hlt7+KqB0WQw}hqO`!YEE|xA_DQ*sQ4BZRE-W-OlE*} zahoe~dF15sii}`JC>5AA^oz~wG1*pP{*L^IF{`FeUPxsNh%l+i&)beMc>(U8{NiE& zjxPt03>I^Hlr@4lcAK+a7D%Yrwq!6ix$;B5m`{TO|CfA>=mExcLbD@R{bKAiYBAEf z>qPvHckBHh9FLL2i9shMaIr3UafFQ4A<%~=$d#DNl=BEZw2Jgcy(aSH>EiNQ`ZK~- z=f~`OzOH|s{1S;Ygkh+S?>|i${ySZqO0?QOyZy4W%{&$LBkt!Q&6pQ_fKYibqY;^d zav4%iU9^=@Vs-J235-uY&LvfbUXb&3HhU!173?p;zJg*qis?%5G{_HQz~Q?~F`yU&hHRo=iZBo({k~k7R|>UJ$5{n7A+JZc zY&$V%I%>Nb3~|FXsAaM}!ENIF!}X!1COYJTi*JSlQQ{!cf6^qtj3j43NMc~xosQD0 zOmzJ`cAJ_F{w%oRpxu>dqH@7e%Zx!&TnEs`mzb%YY3chRTZ(xlOWbPBG@A~nAJGyt zJ`6iuBS@+J&y&=VA3FkmsHBXISuKKW9k7Tz6*cdaOp_}CNz2V6?kl#Awox)-vEA-y z_b!q(T`di7J-V{iE*Fd(9=3bjW#^Q9yq-!LM6vya7fMNNg6q~OGp^ie+m>wP& zjWncxcC|}%W_Em({51OYn)Qo+(pWDX{0vdvG@@K~>?iHR1ODCxplz`P$r!fb5HiJV z1VH%jo$kpTB=nNdV^H$UvIMzU|%_Un$Bg;8NR)*<^422tPpRyXNSotp|?ySsIm_^qC zm#H1KPFfNNMuRwQ*XACi7|R@@zD5u;lw<`vtOhm~0Xk>Rlk#&ZNt!lBNOeOo8x$BA zMYi!Vqa%`GjXAUmy$zzjSZ>-iobcc(_rT9==SdsHx5H8}s&>&}@E1=Ye3B!H8gUJy zi|e7#wnI7hbrU(Z&~R@=_PstI^oNU*N*|qCeEaq60a=;f(ADw%96lG$0c^76>t3*M zELZ5*r1~bddS6fl@Ou8#8=P2WG1(RQ%?+c9ALKNA}jZ}<&s-dm<`ere{Msf^v!hz;T^rVY$fH;|sqhs$D z!R@G#^<ogsDH_Ah zoH5N11Tiol%G#rh6Dr5bcSCG6KgkfX1o0g{niz^uHt{NqD^-XgGj6rw*pS}F13PL` z%d)*a?FPJtamN~5=+mw8H}qpuKDCgU>N&&FL{%pW7&8>vZjmjYphSrKT>d;l?i`xB z^0XX8=SMA5t1S;POta96J5S^n=>ia7i}!Joi!_Rom1~&G+B&U;o9omvW<-);{!&q! zgCHy_uVv3?f{yh=agXe8J7~S%f^)z6^O>DVl+F+_x_PF^bK86rJcsgmS``!44k^h( z!P{NiCS@Z9Ct*o84!P*yAookqGxJ*|w`e6fvDS7ZT&ch0caAh9RXeOge2RgeP}Y=g z#2Q&?RC0v4uwhJ=P33wvW34+jc3}Y8jLXsqU<~gpatfDu*4{rE{$8M%^Y^35j8+UA z{Lna2=|2BTO-XSq$qY6I1R4G?$<)6j=ATFbRc1!?vuKTnUpk|_TZ-u~+3ct= zmJp;+z=TM-srdZ1Y8VHwK)dLnI6kE4(rB03xJr#Y!)(U0|Aze1W1r#C+6`Z?eagZ9 z`jmx=CiF0Ep<)Qf7-smIyVN+Pym)~tnrFJT5wvi*sJzyJ!K56L&ZCfLz^!l_o;9a< z=7OUQ1P=hoG+>(PupOD#6xfZWQ!#?R$c=sosi|YE+lreOn}Lk8&$%A0G0^sANs|#9 zC0Wpe@Ca51K&G`Yq%1)U&dsl2wAPj7C4fwI;|mC1WhbE{c;eiq z>3FI1ON{NDJQcr zsXu)j3gNBwVE%xG2o!mMB!8IslHp_FFZ4WsvMqUAWR@Kl%v;7gvPg2<;(5{Jadt(; zM^_bTWtdSgxct-KP16o>X0{Q~S|j@#)b9h8Zap7o!;L0q{WSO3Ss+D@$~pVvwPfo2 zu6!k!QCvH4YB-4@Al$zTL}Bbd{8>nGvlBkq2drhfTgnzIzUH`)DlV{DcVQU%plUF< zo>mkH^=vzi1u5uVLu)#OSQ2H@o(!hm!c=br`=xa<>~3Q(q!P`8S`c*Rk`R$mho96( zeD~eZ!m8NDVg-f@s$iMqdFvnk>u7=zT$NM!KaPf()|4~Hi<;$`rx{4Y;;@WlKz5jF(l9)s z#@tnVA`__)>B@)U3uEa;inE} z#7|Ou6GybL*-dBR<4`ANwajch)Jhz1!{i~q zZu0=eGRlH05m1I!Q!YnJBqMRqs5f=_;pIQ-2r*T_A`D&TA`%DW;G`=H(hjJ@;TeyDyN4C5?R%mrymnVjQq^nu%MC>Gqm6W z$3CBGY4#(^7pw$_Akw9TPuZlHjo9&)tjATJ1QoE^DE%ypY{9p$>+pI;Tz^g53{-Zu zASg>({ScQYVVdR9v|L1!>>0R~d*VkBRmw077Azjn+G8%`ZA-Z$vNr)@KPT7NcNJ7Ey-GJ#9*D2t!_pmqXG$ z#p_o&VK5f8W7TcOwMPeO@JJp2sNK{zHx+_$v67;abSTko-Hj@%9W*!zdk9$|ffD?{tj6v2hT!g2c`f8%? zjXMctZm#Yq+mgnHfDnpsXr)nBZ8^yLOJ0f(Gifzx+CfsCYENK4&2_57Y=%@HYpbS#q z<)L(pixe_jXS08kUOXFblRxr-upGo+2(#0~W#Kn6p<2&Jz8}jo%>})OSv-SG$_Vk86kskCRlQvPX zXQJDZBW&&tr&K5!N-dc4(Gn$e{|u4jGYF|Ti68MTwn)1H!U+F>b(UFHA2{E}nSs|$ zmSL+4!Ll-vYbB|5Rk*GnhtM8xySsuZM+CmD!l< zo%2KwUyA;FKvz3Hw=zd5r80M~oCVKB*YyFD=#f^yu$bx*=5BF6Vtw3Zt2@wCY;(_? zol)UE1kFxUzLfEIvFnPH64%G0s?CNz2yb+#e}eS3);k^L07^9!lU2+*5YVTnE{Smo zgBh}iCiH+;;3)QtAtLeJXmJBTn;p{kdc#Ee!zWX+hrw#^HBGt}32?{nboxH55K4@> z7-El{n8soPXsO@L{9Gdp#f0%Tl5 zOXK`{L_d>`>7OKrb&byDj6@fD^GBC1#ovehYR-~0CU9e9gE3dl zZo||0MrB@h+_Ru~=6ivB78Du4weOV%4VEgSp`3Of8t4{etO>l%tzJJy?uNnL3AeDo zrIDMfFJhzA{(-9R+L}0XEtX!=$I8(?&kas0BNXh#%oRk;{i?)@`+oq&Ksmppy5X(f zAVR4_awm_V=)4eK`O2m*Y7hnm!$gx5Ifb<)N?K0R$!eRg@(9nmOYhg&GZ#DSd9H;- zrYv0l?3J#=I)QQCD`15x-Sj@};^V?T)+xqH%z^a?YUi}i!8Qeeev)x#FXPSt2Bul^HHYQ>C{t9L}QB2ABIeC>#X%T^n+o_*6xVAI|*BTOJZWIY#_skU}lk?0!BT?!gSDvXX28U zG#e2?;9b&cR4~jng#+h2AM}*d@&(Yp^=9sGsw>lkDoeM(=p17-Mr&Vq0*MnbPQ^GC zGA@XMWWNiyiW!$@lqH`43S&JI+E|UTmdJXVP^Bu5#sTs6=R)y_aY`0W69`Q+v~-$^ zPAevk!ZOAy03YkF*38^la%&monqHO>^mAgB(r#pQn*rTcNT(IiZpJj?kT42MoXBKN zsPxM7poEV_0UxyEFviduW!&D&*hmyZ*TM#kDVOu~`n!J@=RsQ;sdg095Yq1X+0opO zySYk)_r)9hPS#jWVKjv{6a-jR?S$UZb~ao={6D(CYZ(70 z3pXGhRC_4`~uTm{f-He^{bC})ruMtO#|6~ezvD=Vun zJQug>AmWnx#W`1^mve9F7Fk(;+hLr<5&3y7!U;b+rD7a+{~=(l5=DK#zV^gE0%s-G zX>4IIHYYNMXv{S&$8DWG+tZm3ZfqzyELel2pb=Qw4MnFJ&}>Ad<$nM6jMEea2o%l* zC69#ns%2V}7)>uVY!3^%y*=hyL*_dX-FA~^BPNO_eX@V7KHWigYznwin%Wq4hZ(o_ za_$Zl32mILF;?yKeC>Bg|V})1bfiOs?Fjf&*ZxJh@kUW3o{i=8$Qej1dOb^L&pBFq~ z$2z1>>*ImiCQpVK+U#k8>nAvua#{#Fji^+@)8~KQ`s7K(T1%4V?DR(5*&1?pr(iD) zFrtZ!;&B{IxXH%mxIs^|p(Cifqy-J>K|_JeP@-T|6cpC^a+y?CA^W-2pSINM2UD{i zz0NFA%(_5puY7e;_;QR+Vr8S8(>_&52zh{Od6FJ99sjP*RDbrL@2iqlc?YF0@o22} zv1Y5W))6^Z;w+vnA&SRVO*T_*Z1ozHC}@Um#^pRwh_mx^RyVkPdq#m!q0e2%QCMF9 zXO!gZB`M8bO1GJ_(9K!s#LTrDw3-o76plIgpOqY`0^SD%v6f!q-OhK0ij)pc#uLl; zVa(t0u{3#}lN*I48pnR0^uUsS3JQ$WSY@%&^RpUl(OP&^s0=4Diu<^U zCQuAdZD%bQ;Siz2NuSc7jX_yO=t4pn5LCPEG3>wAx9|cc0&93eMpSq$Fw_#0= z$rFk^rO+B}6;^~e*#;R-fy^ekxDWD1J?Q!l*q%>$cDy2XPEt6H%{A6$xX2P#l$Iqr zyS`2x-CUS8=e&rhwhdZqZsxKk=6DMiP+H^2)bP znDHQYgO~*CjnQtRMUkVmL0gHT;VD#Q1SMGxVlv07Rbg_(Be;lq8rRhO(ytUkV5}t5g-2mq=PkbA zc+>SJPVUJ$oYg1_8lk1rP_$bi&DevZrm;R1uk_=deC*=!*aIhU_|mN2MO{#&-s7@2 z8nLjKvOE{FINze(Y7&;Ij8AioJUR=w2KBYpGDtG+_Hs6cl96qJY9g!n&La*$>nw%V zWZGdxtPs2X7?mmSJ^1&nc_5 ze_w?Tq_^WdN?D3>uT;J_w{mt8Nw;oN@tn zb{vbJT!!y`DHV-cTv&|?Mp;2&t*2Jx!EwQ(?cUnrE2`St#-|SzIl4$G@*HjO>~zvA zU&dt^YhCLq!i$439Y@&dRqH2hFF{(?3UZUK8xyPV6aOgKY0MmYy` zTG62{=O?|*`!=2U&5w}2APg?TSk2JnB*T<`lCw7)vOFqSp3j-@G)t<>Uy;om121e!hCIi_wK2a}I=5 zsGv-Zk&}~&sdIVaj*gTBN)oD^L8{4&#W+vg;lJm`>%!O*bYy*dz#IZOo~)g7ba#m%e5;$a;Ym{H=&rU(9&a&m9-)XH5b7{oMF6F6gnr@215mCmT(T3=}y8|y7!6X41iw$I-^5Q+9c85_-bH;Z*Z0YVsrbUuw(`AvZq z8VukdBt(fxWtC?Y3oSRw2pM2)KxQ0;(L~PTD@8rr=D_zu&HX~zC&Cwr-^m<7FC$bb zOPwZRpvKmTdx+ydYpM5x)`p$E0pH!}aeK!y&<(6?)bi-d-|&D9hVbAfXD!xfOrio0^8Gu^@n%VR?rh@%QX8;2%1MfX{%DWYg_M#aAi`hq6iyTSEO ze!+*o`6btH-6eHRR$lrGbiBe`HzeOn$#Q7TFVku@QBv%i%SzQ<(t==AFw6^b<8W3@ zHsI_}_j|&g9v{=$0#jrZc|wsFXaicrpqe;&NDW#co`WJVbyu!mTP71i;t0_WC^U$I zh7gbKjy?$GKIL+b+<`9VLtB0<{Z`MCDUGE%DTx#)!V-%Tv6rW$MaD2o8K#ESMa{xo zlVD-!YAb&^P6SvHk`*~kQxJsFBlO2ipVX`XAp}mvq{gu^ z?9&KxA|+`z;%7g89zv>lrnOy&NNf=0iVTqD9wk&wJpUJCi$fNad{pv&s zkTeA*8lr&} z6ISwE3#YxyITm!=A?>nspg5qjp7#6`WA9km?RC)6a&-qA(dh>*3uuQ zT;J$(YsWIw4V;Q=#2Sxr-^$;aY>(c(Z@F{h9;|PZX2ld}XdEZ5qkyd~_ns12qdTNX z8139(^Y*vwrGojF-siQq-s9z0-(qdGLw@gH_~Q5f!maES;pvw-wYo$jXiPP~e$S=w zv3ybHa)&`dKhY&hy38Js`(4ua?THxxUj2o07Nb*&bV!~T6i#7e(-Z!TQ;H7iZgF(0 z@iEC5_aMsjKj$rBQ5b~CiDRaC!S6TsX4P*U--(cJbYQoz-2Dai{q2RZjfg48uZMK!e0qc^io z?N;c5!h#jyB$oWlrBuGoc)*n`u*!~;!u>UP6tXD<)>efTCY4Q&OZ@ggVT?m+1HvJz z9WD;5EZS*uBav2mH+3yF<=IRRi4y^Z5foZutwyQJ3VytJ!x(Dh1VSjRYO+^o;z3Fy z%ve^EFnBiAI3HH5wG4(Sw>Jme-qs8X5B?cfk9yK&z?Cszt;>|P>E5ewCsrqndUx2q zbAz3o9Wo<`+RH4io@aUW6s=G*+WeZ^-+a#9dz%!-yI!Pf5Cj4{y3WndKH$qwK43#H z6EDtF?B3-3rB_&6KTD?-qa^!X-hh&dMl4ZMqQ)+1?TI_A-;`HrCl7z!ZffIdZGkBg z@+_gqHH8hZs_B7NqD1A_8}$N<$6k1ETv58Cr7WnJ${skL%5~ji0u92MlFPYtIX5HX zxa4wnV&6}ncM(D$Wr!1s%m!?Y5;C2W6@5+@mZkX?%|^+xcLZwqkyoHu_p>&Jong+# z&@wQMajtK9HIIbwEq_Ec(yIzV+6tVAyaKDC?Vg2HaRlQmMV05PtoMx9VpGmIY;LhN z(AAWUbrhqq4rp+$28L5(vJWAhf0W=FC&30Dtu3uYI6=Uq8c_w2IsT5E=4f^zKs#Rx z*=UWDAwoV*J8Dj*DN++fA)0Jwnz*0Si~_o4L5pgm=$?^#tS zs}VcbKjGW2K4atFF4;)1ef>ke6aw@v>nk~5eexlY?4)MyX}(=132YG0rvBK9C)*mQBv_F zV8ACzLrM$H)*xd$m1M4gP?dlD$xYO8ja*|fPL3_e^5jocAOIhia-TC5Pb3!#TY}cpKxb8VRHZljdEiX?RrT*xm8uZW`z8xNxPgm!#T0%vqH|2&orpY{mE*{5vtio!~745=W(vtwEJc7Hh66H!zO&x%9 z+zZcCh*#i5h_)$(sR#oeEgk7WC-H!<5N(_6<^`L>f<~m6YpI$m_Sxn~JLedsId`@O z-0L|~6C+FD$B~9F4m&>}D|ft37;atX&Uat3JIraXF0-(Dj>S$wzOX>xzNNQuot>SO zH-2=L^Os-di%)*fJu}DZOF!Y&_x>GM&Nmosb=mm%U-`0i7rk(ii*Nlc@4WLCD{~!` z3cS5_pIp>~J`2NuZiSK_d22W`&X+f=qoik;`zxfgSH9lzwW{q4C)9S*bv(!OBkrM= zzlRySQ%YD!td$g66BLE7GcG1X9b=a>not%V<=hHsHDbc>VDH}HZFt;kc);((H0t|t z`=wICQyz~JDUFITPO+VgaQAYY>mv|!+D(Gs@z$z`Qo#E!r~+PP>=hA4G$wK1hh@=u zz&V_?Xrs|c&o=W6C&L6O4BBb5&{$<~a%Q1+V70|+fps&e=cm;EvHBFEjP)q2wUc;o z=}4&J!A2Q>O*SB^P^C{T%vI;V##c4c0tYJxbQd#ys-Jr?Vs7E`>X+$A$ zcr?sFk0bT5pBDu?dn4{_=M0Mog2ohJ=vZ4g6EU*7AQ|-N?{1L}wn)DDEgz=0_$tsO zySLcd=@W(-d6q&Ddbe_@kTM{Q+cevAG}{qo)bgeAq{0Rfjn*9PPM1a_#*V!mhg(yXAc5mq&!p4(d=`_F+l1P)gykM;d} zU*p3KqT2jAM^ct9ngNgJl)$H#Tevs9-KI_CY!eWo{m zQ{Y+xG%a@0q*TH^&I*|8w2)FhvOD^*T+q4J?2dA_MvmMyKt)GxF<5K;LOdJ)eTHI7 zf?; zT-BG&1dSF^+$0JiI(3!qilV*tI;T%95e1T@PI5rC;JF(4K?P!}&coFA zAG)2jrB+Hs9C?&pB*Mu++PSW;rpKc z!nm*NG`?dL%Y(ueLgHM2HYGM>3{cdB!g!Z+E1=!n?{cnJW11*iH!iL))y%RDqULR@ ze>$cBowaytV{`g`RoCn$DWYPUj~r*Qb&%@g--HQZ)HK;mL2efWYJil2Zo4_&$UW@2 z9!de%0WDRbh3%ncpd)NW2r!L5e1eMvgu^;RVT>=nAf8RSh^wgf$_};8poPXrJJ}GF z3u6`gwWh3cBxe)Q>(9q4lUP^zN=R(U7cHbi3O9CXI$5SBS&ebw#0~9SX$e22#Yt3c zRv72wphpo0-Y3;0k^zNnu$ybX+Qy}`UJY6*Ogzc(ex1VrtuOM@+F{T1sX{j8O zIPmptE!G&26U``;LIx2sUSQ?oPk8GmKjZ9TgFrfr5rpk!mQG)!6C3uN-9P_psZD-S z^&mTIeAO;DqqHBb-zG!~VHkQY=^#gym6cNMyeA5UvxXv1CT{1lh{A-cVCu-n)zeQT zqU=9g7@V!U*1feVq?~MQoW*2&WP>fT+!3^vX>{iaD@Lc8o8-OkzNUDQkFcW+^0bdJ zj^&k8tjsMl-)T+}YfJ@q960O&g|TLgkBVR0_?+_k&GgrxJVP24_jgT+JB!hiapSL5 zA(a{vxKJu^YYXpP0fbbK9iyJDojr#5$pB-U?4$yv#D z?2K}DQh^ptgb1em}gcYWUQR4(y^;6zA? zz@YO1gWXMbw>JshJYf`%l|tE=2hNpA;f)%?-8qz!G#fEWDWV{!H!8eKT1$!|_kb&- zu(DOUoohgw=TzxVr)CwHbdS;Q9kOJEE&EEyh#>9~wHIl$<~*qeIqA-4-1+1m*xYf< zz4UX=y!t+Koz}EcDesU!hXfg*(*b+ef62`+KA=~0dF?O%AFQpd5`{tS-niQG>aclA zo~IN}5j9#K;XV$-*WA$CVl2E4vDbIB>S0NPF~m zHyI8GekE_7y|w^o`|r#-C3He9dWzbry}rN$X4gF`;jpSib^CNg7|d4WFTmffmA7 zO|q`0gvVB}zzK(y9`t6N9b3UqD<615*y3LdE z&}yu(cHs(h-+e&u=0|+>;Sy?aj;6{P4HLqJi(Ghfg+{wdSDCm${W&B;HfS_j#DQjW z>npzg{of<{cer@f4m0-pLq=IfUIb{_n2z~U z*=Wy}isrsoD)5_Vmy+#$$?XsS2iqGr$g}|@GH4QY*66Ii&hq(pSzJ9s99eX_#bD!8 zw(mMvc$HOcYM96U^Lq@ubee4MI-B4Aj?H|P^;iEL+6vEy?0k9C=^q+K+h23%#<%RT z#F>QnA+Y%t0Sglf{BKf|d@Z*%FDw>W$DEbT@JE@SuRhkWvHzu?xO z#rYrq2i|!5T~jwnqu!Rz?_%SY4DvQS?v@sSmx~cg~kW+ZpBb zGD$%LRWb5RvX_r!z&UF%&SIUw$tT#Ij!})JE4hj-l&|Yyd#{Wph~=sF;4oN>@YZiF z$OV8k7Nfxu;Ku1v&w$!--}O%}MU(+wVs2oN#S$*a2 zSh(~)Yb{UWpu(6S^uQ?Z1(K+6j>WZ0oLXCD-@yYU{2Ko|lAQdq3c&_#~1Rhh}W zRH$O??gob~2JBq_kk5Yi8#dGxXs*%ic71*Fk_%!S7IT`Sf1Mkj{xiS(=ie|gA&q8> zFpwZ)S}T9%UN40p=GB)k5=%|8^DWxrhp0FvUXS> z?{cPm?GQpDUF6x9M2#-eRqb&Y3`SU-D1juU5j{>jvSN~URNbKGAeCVpzZc#b!Fg1) zwH8(FZA2Lh76#+YB+l{FjRGI9c1X-PKKS^1)`#mKg%%NenPP9G=`;#j^63_-Fvjye z_eO@ygorSljN#*GhXatPc#gT1^Q@iiG0ZGV35?Et8jmh8))BSWS$*~Ic;6_R3%}v| zjhhV99!1civ%1dmsdLP=Bb0K4&1Du(U1BZhurSvp3;(e7 z!{F{03~v9PJ93Wh=~rma%@MXwGk@;KFb}O$XFUJ8GnisTKH4Ks2V?GaoIwgfv)LqQ z8KhDrE0)C;LxwxI+0c%{NP>nBOSBqIgv&^FzvI@|f8@(ge$P%bWpVv7-G*R!ah}fH z98r~`Bg(;NEvCrOP7yC(=JMq$bb=9^H$Lai%@6rZ%(1+Fk#pxyqhz1WYyZke|N7tf z@WanZ)gnvhUtxKn1L-E)x395v?bm#$1**Bk!u-!T)%7{czM@ruE{k{^-T0a;j1>mK z5;D@3yL&@gjS-D7pxtiG{BFNI&;qW0m}|{mlGD!>T6lEkByReogLi)llF$G33izPO zu@9y`VD|?c2k7rM|8)JTzt--)fQP#qY_}H^P3-M%SuU4zjb~`DX`YJWj5A#1ZNl@b6hb= zgk&yeH%VAXbDGi7qxuh$3bm=VVKB@XW)9;5)PV@mBRnq-WI%J}6<+>}{};`3Kld>; zst}Mwg_3RavI!zvnadh^s&d^s{Yf00DVVa{w zgD`G^RVZzJZE@>-p1i9+#s52 zYE3|C6)^3{??a7m!8rsHBSHpw#NC~oRy-t%LZYai4YOYkRlxO-jC0=l-5F^z=kw+e z6YYQ0Qdz*R;wYZ6BoECI8zb=y7L6sxO8;Zcbv2S8`LaU_}`RHh6yB>MEWddfVYGVCyK+j+V!% zSNd>LAsQ7{HR$Cjdx>VQrMCJm;+~V}QcrKcp%#1B%Yt8@#;wD%OEe0kWd$6Oy2{ z#Og~6EMIuXyD>}NVJT&85qpAuboIX-y2u%gQU<9-i#9Tdi5y62kiwE{hqe}rtbh^+ zAN09X!sSg=4dV70=GR|mF6>d<{xxx^NKAp%24^%y{{}nX{0p~l8S?femM^_RG`hv+ z=l_%MKL0(#VSV_uu8zd<&ayacVK5f9D%EfAeRyZhp?)d)u5p z^(gFWPjC-v1V0{9q|mOx&aj}po6>2g%$Kg{!%|W2_iMV324E8+p%?C{s?3xcloc5+BF& z3ihx?U}~c)B}zsIa+8g7R8@!yLLblFr_m)s&7jO@$rFxf)pX8~rXz-FL2e@mJm^XY z2t$FC7AXx`VJM8Z_-PYf5~@Wd`1?XX64e4SYA1eEe?SKr~nl}mIQ zK?!=Tb!PQ!G)1z@-n|>#DMsA6@g4ot5O$a7%qD-1%9$fj-XK^I9!S*0!Zf8g{j%YS3Ps{#8 z+}8yy@QAl0*Yr}uPzN{_*7BT=!%bGkYpqAb)~Ou#U%x1s7Y)|s7?EI9iVHJbs3Eiv zSf9lt850al6cQx?#NmivhrE3&YD=oK9kKTumyD(IUXlF~F z=)lzyeVj)`SNVL;V)ZFw)z%4bX&nQSd|$O403k7=!JyFel7e=_pdvZ`{&9)5mMqtd zGKX~vA*(5QT-%>Xw!Sw+9+J8Jp}yy)_LkN8#yeYg_(*@^`p=ce&N*~ZFdC(#g&0?m zs-H(GNi0B0fhtVtlJ@g>oHa1HqUTEqS9#(227BNA178(i5ZXS&tsCh04D;vTV`*iL zI0_imEDC|Z=0nomn+$ihA%214cUOq>=JI`oCfd!#j%%6UP zSAY5+xwz!G`(cChm%m_l=O)`bdl>aHD`&58YITLytscRtt6YBfKl1jQud_7Yrqxgg z?7lG}gv6w~Y=8Ajer;~@ahS1p?>Ymq$mv(#i2w8z9{LMX@#c6s$bgpuVP1nz@EgRJraZY1IhE^j?IKo94#1;_>pAB>nihAsF zs$#s=`(oc$Z3We;;>ML*vI1YZ08bP$QNRz?BjYYCQQ3A9?>zJeHHMeRvleQ|JtuKi z;EcmnRUWIym^9&8utHVHf=PAKH2sSKcF9!ow|Qj=vVNoFWqjHt8pRe7^Qprk~GK5a<~ zUAm+V*7*>KnCYYEO`I8!?tIDS@EdHAVAUe?m;RDdFaMN<%5;AD(cAxenpl~#B!j;DdN-u-Tm+i$h+Ql`>Zoe0Q^fZbunZZDzTjA=I2f$Ql$ z1w5??9a>v@sb*04xPxDij`IRtY1JO0IBRWlXtWrig8?Q^5Dkrp7#Eozr`gp1IiTs{ z;AWJhDiv0Gl72La{o;4cVggutvpGIN7Xu1#YeM zgVnl8-2>sU!UNcj7Ulkk{o<;7JlzM6)5{N*vVWzYIQ=YRlod3ikRXuE=EEPQSYycx zjW*SopJX3Ao#lUjF?)BQsp<~=U447(4mWSdao*z=tqVq@gv>~sh@rlrnKCYX`$Hkh zoMA(jdu!MibddWzCC4k`5k3=$piSI8L#Nv$+q+I?yTtQnnOi+iquE4=be!i6fHQsX{Z2cG&)bo zt-%0+EJ3igpeWt!LIwn3_()7tM|SsWeC;@JG9obrTYD)B-IO?v4rJpx&;>n7J#vil zfbTy)x>!>9vHRQ?&0J7ARoOu zq%Gb>ZmlR!IGxl#IpX2xnp}*<8H3dpMCfa=&W>1T5m=-!KGy8ify(!xmkLoO_DfI9 z5qP{gH4E52#=>(HOI2y3Gxmx+RWcKS6%iwo(oYNKT4r2o?6^c5LzWj5#?yf!3(6`Q zhj$P#VUDRwO=*k!FX@#Q_IG3HiVlJ*++fJH#yE*}^5{Gu_l-0?APhD3yiMkV2$txs zzt5S==g4mTiaXbDlK1a2+S^8-Iz?0`kP{B2pwU@mE_{_!r_a#)>LzzS`Y+_S=PCAX zVw-1JKK}+w%gY4HjDdDSDgqUj$b2UE9jGd=?W>ty$IZIho3$@F=aA7H-Sa={fF2o^K1oac+S5j(?-yzXN|QC0b=Z4O-Z4~NWcaqz2IquV)u98rtPIs9VdrYFVp$a{n< zcFwzmWmFP*%#I#|a0SAe@{*ol!5a6G^hFw+p#(jT5$8@+UWliMfUf$_l|VN~9RPKF zDLIvl5&a@0cM-jG#7eGdM$xe%VXd{~dB$k4%dkiYqGr{i+O^l-nR$E02A%mut^fAv zYKJXSvNR#fa&OHD#^fAf+#-%5l&WnAg+MrqHJRsB&OJ#3A|ME(vQR@v9Ghm@$s0?qb3f%}@ht#5I~5>aEGxwG%Ge(7~O?FQEDP*nG}-uY$7 zY|?2np$u!#*^qRw!*H-m&{U|fPN`ZyoI?aLt;LI6cBiajRSYBSF5r+&P zbYG55Dp$0xvpmcsy4V3K86h9Fk@x!rWF755g)RX{W-0`T9DQkYH$_$o026*R2I=A9QN^A zH2FnoDIIJv9gmDd2r|H$h<-j|RA{=^ppKCrT1r76EaW}%fgzLer2J-WNKYx>dV{BX z?tt#zuY3i7)g(LLaqGJ;xN&QfOdCJHRY=@gV1D^5tLx`jSePdcJ=wjpntZfNZ}SHC z?%rm1Zo4dRO_naa!rAlZX+?^M4rD!O z?^Vq;2St%HObZ4@j15B2Oi_PqDuGE|*{Vh;MKFlG~sChA%I^!kM+7G2c1MnU`K+ z{qxWGV&e-w`}O~YO73!IZ2>#H#m&!t#aG|nLATCu>eB11uP)IHWsMTmGGn73tBn0Q z!msCr2^c1ZB+cn|Y-zMjbnJm3Ea#ke=M=hhKL?1Cv8E2wKMtwNP>pktC)=yB@->YC zG;;>a)=q&*&ozIIlDxc-;HFU)yasr*W?EATtThg3|_T!6|QO6V80(4nBg(z~5OeT)mKRbEC&z zswhMgF=l0(^vh&O%mNdq-TPA?HjOV+!=-Rr;Sli&Y}Z?4~@KTI(t34w|_%&))7<+uNuH{N-lweB2B zYc~5gx&Ehrox?^BYh78M&w7;fm>}iRJu(ktF(0getG$-Mxv;D}SH_BvsA!JH-0Lh~e4Cw( z8*FTU!o6>=u)Nx#6-YwmeGNkRd{k*Hrbx-t9%+$)%g6@z81CJozju%H<-g$US{ErQ zr3UgE8nmN@g;U-8QA8|?kt2W)-wFZ^!nGd^#} zxMILyZ=2N3bNaQP^4i-!W^HMnNR52koUMR7PtA{4&k(gjU_{6$D;Om?#@LcwY$69+ zz<5H2B-4xvfky3v@*neMfGuz~8G`|w(`Y$F2R&Snf^-NYz*L}>$%U80;}CFFePRQ~ z;dBn#(=;MN5N-?_5he#xgw<1+B;`O=CEToM!*aGHkwAIHB_!h*u{dbbmgwgq2Ms8L zl?T`cRwHbV6dLJ5L=f&f%=nsWZytJ=T+Qj_3`*n##u5Yq;XqkMOim~=WL*Ity0NLf zQFGHxe=f!$fr%@>e$(p5&;)@aP^v_+AK(7#wkO z`W&4$gwm4i-eI?I$&4cIuF#!}2@X-fZf4wgNQB@)8$fF<7Db>_V&)-eA@+HbYJI@N zjVf&nVQ$E^MO#su*OT#l{8FksnGrTuXf0i!>jdq&6~cIquoJUz?tRv`ZeqT^hO&LI zhPb`V{OUy(tz&L+g(v{q?~`X4PKJbWlR$yXcNz3=(cj(15VJD>9&-!Jtew4tNS2tN zpC=5xA`Y4hESx&esrEbza~(pZP_0$gUivXt-?&5m=@*QO0i%A8EZ5V;7`(}gjJtG~ z&a!s8!Q$c~aTpRbmsmdk4p-m$2|MYB5Q5EDCqKb7sfKm4XIK8fA;?KNwOrp&jlY>M@2-+lHH@aM_~p8Ff-sKX;;`? zUM_9z{=xg@nyqm^aBX)l$t3|08Z*EEnC|K>l_JDZ?!F)VoQM;VktI_^X8B`X)mf35 z5l6r7um9fPixWB+e)k9Q8UwH(s1mhsXg3EcJdRdOAvs0mB_eTyc7R*fdAkU(uEFaL zOdlOOu~S$hNl?y_dV%Z$aIeC}HaK6R+V;X^V!e!`n?bvvkhTsS5{JQy(4;}gq3cD5 zYeTWT^$6#JI=NeOUhapr$hJZYi}5Mar^7wEu=~S*T+~kvzVZMT>1#6AlKW5wBZWf? zkCyH7f(mQuD*?u(?<@MVxcSY&Yg?|L^`#AoRvBrUEk}&2Sbd$h(J~JmMy}0*MbmeP zx}F9Np>aUM@$n&fQ81ZI`Rc`-ms8Keg%Pyq9AvIy&pjZ8z)Kg>hEk%^F^6CNYkv8! z{xwfVQ-1qr|4;tgfBvtWeDzzNfBS+Ofosor{tti3Z~p4fd0nPF`0@Y5@BP6q`TXG? zZuS-b{Fncczx=a5kXJpA$hO8OK=Pk%(7kI*JT zKlmx1|LosW9Q+2GJY;|GfKfK)@$da_n9n?u@BV<)o^kr@H%#ZA{L%08_~8#2j}oX) zc>Pa*!t>|PIQjZ-vG(UY{_f8xl9bnF!PDN`uDWiXPmTS4n_u0=+RR?|JnbA{NOu0{o!Bp?Kj^rUo@Z##z&v?`1?QS^Y8wE z!-EM%O1w{)Jo*V={_6kAWS;T#d*5M{cI`fHWXv=$a}kExO6IktXXTcHBuO?v$U?oc~9abnM<%vZ3N&xmp?poovPV|)_!ekX>3j7S{w;p^Xalpa{=dBVr@!R9%sBelzvU;t{GT`) z8{GVN{PTZlsAvC)($?r~%)>8!$`8K$18OTsiUW#Fg3K8|{y9GzeTT+LOuo-33nI02 z?{hx?y*&=U`wN=JAx%y;nvnNSSlx;8(qQ&~zz_cDh{wNYQR#&7cnpBb4tem!|3LoW zr_@%2_s3iTzIJq3jjSTCttOI^3P!mk+?Z%siv?j?D4uQ;F>P6^^GvNEV2rDe_T`Nc-Tqeeaj+C~w(R(TR&(>7a(ROelN-7^w>DMssSzx<^P4dt~`BwCW_8Q+{VoGfOPw#CU5F$gYw%KW{K>{4X7I@G~ZwW3-~ zSxo0FX6H1mL#0KqHS{&*$?tgi@>^y$=lJQ*_~OUE&v(E3J;sT`w|nUJ72o~d|C?X` z`X8CS{wH3%I$>T;U_;NolGhI{ z20!T!+oR2m>E7QUlLLzM2yh7BBC!}PNKJt;`&|n>mc6va_+i+r`X!5hUQo_nQk4nF z1f7ma5@_cQ*aoRGM*EN0+ke2`(PtdwCfxhZby^rjGCn5T>#QEV?d)Pfn2aJR7#%!a zW*%La5gc}?6DV}H&wg>lfuMUimeR%d{`|RZh><69iNKOo%&Qdm(MI-RiEZ2Xp-yJo)zmy6o z6xKr{O2ok&{n(=RRG1kK9jiWKlY#-f1*1d3w@-0S<7E&V;-dU_|X`70= zZm6xLaSbg3CnU~=TD#D>!ytu1dv#%OCWS$0iO>dNk~i=2=0!Gm+J+0D54;ZHElPmW z3Sl&u&;Voj@y>Z%YjL*4SdEr+>7?sepIu**4ZCwg5{mxw5Fiz8qbZw~s%gnnAA0jH z5AoG*(@@-KjOOrQLYgHMSx8C0n9f;Phm$EDGpNPfdg$KggK^EA)35%VzxgllB01x$ zzx`8A>y-U3e#Dc{zRO6$`Rq0G`HV)7DGop5U@|6E;B2U2ONs~V9Xw#1OU|ovX6G~N zs{M!!d*>Yh9L}~_+hSXXmlZ<3%~|fI*1gp+oak_LH+@bc;j6%ZCB9Y#x1 zr9x{_>#!I4?sZiewn6BetQe#Gb58&1Kk*M{&da35p8baB|M(BM+jQ#u`j;MP(XJ{Hp8#~+-mSO{F%#zc;Us(jOx{# zNADc2X{qX#x@q4YfIU#RmPPHUox;oB<>@A7A;X8SrS)^_>=kZrh8h!s@30X3(xds# zhlkZ->%jI>2&7i%t7y*Njl7&MaJHpsnh=PahN@|3yrcCc!b_yDNkkIF)*F&;EeY$h zN_DB(^%d^zM{)2zU9$?|+c0t#jy~QRtw~Knn&@D1zOlIMtfLj+>wW~cCbPX_23p_S z>O|8iZ=Fo2Y|Wx>*>esh<+YBYH&fE1JVQT_q=}|@HRGF;1*c_8D+-Xo@n@MzTV{pc zwl2Am>PO(}DX)L~fAS}<{}SyR8Y?Is{~O8Ww$kx%ijdkHS zy}#HzU)whD;ybKTrXGPqNsm+ltfFldc1@#m@jV1B!b~`N`ePovI_K4k|Hf}$|0PO- z5*}?PJpBH@;0IrRpG?-AzkbGSxn}zIsh3}U&>Q#V;t{_5UHz6{+^#@*6Yu!5;v;@{ z%TM;LY`k~4dP)J5Rf2QtBP6}~Cf<9hx@FNgTGA^Mf4ti+_@!N9>nkepigt1WlX|tK zes$A-G=uuKRedcX080^M`&VRZrqEy#jA==ehNh{h>z1mjkhX=kp|#*#vRor(Dg0@X z(xSEL8fCxPcY5h$ghO<4eAzi^Y2(q-qm{&HfzcAJ!XJ4_BYJ_rS&I{H;}fqs2;Wk` zU4HyE8Yd=o-a$X<#8F^1Zr) zee{V#DvePBO+{5Vor2uZw@n=!-EF`ArU zJ}p0eE%1U?0wuy9=_S4$WUSU=+WP!mr3V~;?-wYYbAIxix~cIfbTVdi_>_mA|A@z* zJtEUBZ8ck4BVE=(533;thh|%_+>Ec6tm19QzvYx%rUAO_=eC@OWzY9ZPr2Ck;v4p0 zwZpzkedB5pS))a$jeoNdhXC(9Wz$l&9w+obcyf(5({MqGZuZNRI(rFw3q7bdvxh_Wsp*%O-hosq^ZFuhmn@5nbEjKC}R?)t0~rAIOd+APf(9+T|{^8O)2SW9ZGWYIY64H~?! zuki{JlW=&TNK-?ePDx(Oc{!UiZynC1co~|0Z+l$4?d*6wDjhNY-XHVxFCXDw{WX96 zcVBf&CoT+imB!>_vNS`rbIP+9Ocx7k2bq%K8=C5r`S}Tp%A!UESy7-(ZwYu!YJuev z1(+mdytg0BI2J9nH#nJvAdK89#N2W)-gu-2XK4AF>8d4~Huy-lE!e)h$;N!mFK4Eg zl}HZHidY5+=^a8iw1Pwnj9v}G-+q3(zrDj~#bh+%^Y8u$Ege!4{GO%3BsnHcgVy1~ z6*AcGcL~6k9MHFEo|f0EUnRWXd}1z>(qB@1wh=$Q-~7fKH~%f$zIyXe4bvj2$xXx2 zp5Z%>#_UhV7c#1P<6UsxQ?-`L0#Ywki#LgN6^%Z{P0nCs5qe9zz>nD^|CWDW_S#3W zx*5F?2%*rO(pQ^=d^Dl1E2^rZt{sgNv|i$53M%bVj(X6pZWtV0r@n3xSd<6>H>8&Z ziB=dbLke4l*}d3E<-ciwadSR0JBE;0BvtD}`nBs#d}Bfmu^LS)r5F`CT6HSvmouKd zo^x8YSVBsAsjut;w^&VuFaMCPGtT7EPx;X=eo1k5#O(F|oqv4xm;Cnc{)!*`;D_W7 z64L#r>>nJG`ENP<_Sby%)mMD>{85U|YL^ym1a=Ep=n58&B&7U6r*Qd(DbO#S-*!+`Ea_Httb!i_0~MkcEnahBC#owx}t z(rVk%dV!;FA?P}>M-XBNZ$oNIxKOVs!@92STyV4wDLp~Agn(mB(4&Ae`@X`!19FYoiKybZdivhW_MEmC(X zX)AD{+XF*C>E&Txe}nMa;5jILSnEx6pSgO!c9q^aq;D`9@t9)JlW{j!C|DNu@=Nii;mYdF`^ zs&j09jwme4-UM3}av%ATs~x|-J-b9**X!Dr>OUb-IzcIeNi1m_)``oiqN?W9?Si(m zy7WSWkVqNUp|tWOT99bDT+>xs)^z>AlI_+6PIrbdD{yEUr>Gi>bq=N1%TsP-OEyx{ z#uyIvC!~oXOU_AN&UrCiFt+LuJtu$u8~*uE{}0ONf5q|P2v@%3#aDmNt8>Z0 z_x^w%{`?m_J~|{-vsD>OS59&rx7tuaE~O&P3Y5}lHABtj%&IxHwK$)3p7s*F=t>dS zkMW0XKsd%y^y%SHGhEk}#?rOz^WmHfo@GntweIw+gjPYbvAnMn9c)6E*TL#kZhMRc z4TwehmPEA_sbEqViY!5Ez0?eBYdiTqM)rN{&aK&yF)59`6MqrM}!n zautBPi>PTWRqJW{R$KzEw|=U>2?QR8^%ZS0#T5%wMpu)1-DM0nQ$8>h1o%DzZ+>j| z5D_K_OSD#`dQQEV($)){6F6al8dc|{X@)T-h^SO2l-YEwT55D+(0xCGp?d540PNo} z1$IqaGbsB~N>LOUPqm`RQ;HYUkdiJNT3H}evNDP2)0}+(Tm~J5GAT)tbq{7qGJeG4 zFMr7ofB667<$wEIUi|h?`R#9i#p9#j`rRG2I>tFu^FMs=AdH!1fiApCt z_~HM+PyYDd^9R5GGmiEOWIIJDjZQL>G}Jd=rKEFnU^0B&m;^bNXk$pGGtTD=7Pi5< z5nct0&$Sad%)5TTeKH&bC@D!35i}OPjb-1uPY8q;%VnK4iG}y{+Kog*I+*pY@89r) zS$1Vx4zyf5NI5IUc|w{Pq>`)b$Ysxi_t^JOd+fX@=#o|SdkZG_>l=9-Jc*Dpd2I6<5%S>1#wQX=W zD8zaff>}-PP$o%}8Q;9Fc{Q6+wzg9OuM9+n?Hmw_bpLyN=V$*bzbww!|KU$K80Sb` zFnRP7e)2E>Z?yUMIFpYVXBr_i`Oz1A|6lw&RC>&#@BKC3K7Y=lYC+|}67UCq$oGHr zFZlfN15z#VK4b6EkNMHR{P*lF_WADjzeABo#468vZA&6TN|Z3arTkL9n5lm4_hQ*f#jZ3&lmDqXU8kY@!8;kzu{JGu8mHB@hg<8=g)J z(qLpuk-#`N6qyOZcgIF-k*CXr7MHO5&YoQCrUEE$3y$qIERB!P*4pLW+7>o-$0`?^gWSZA4g) zb3)?0ptdg958hCF#5Zmj{cRcUk&u!elO&ofnUcI*@N&9f-a1;>2VuFaJ@q~`3{Q`L z$QQpHbM)heWb&A!gApLnqsM&q^M6b6_zzjMitOMCMXC`hVf^sN{BZn;M_>Gk>GYhs zu^lH55lr87FF43VUToWQ%Hl8Bi~zq!-M} z1+{ZHf;FN*uAXyOK>sdMND>o-k4jo(V`;3zdGKB?9sPpBqpMUfg4F1)WJPxSGJIFD zRzlqS4D=~yqb%bT#%n3(_3es$2!4=p34&MUx|H+S^VxCEqvJ87BHvItzqu}74?n>B zP|wykqQ1!_gU921gVi&9RwI*@D(`xQL!!$EU&Uu^FZ#Ri&|dbcv|tFi>1@iZl9V=~CF{~`2IThhC^ok#j)d13 z&K8`Pw3eso9^UMO&qt54ydhTIRoC+ z{ghIYrM;K5B1`9d^Qz+IY|5g|KxPQF@^u$dqO}>6pSb1PppapGxK|2#=T<3fp^(a~ zI;8bKGx*bw{G~J~orb{C3$bn}i?G(aTnLmlWKyD}LhCttRxz(;G_A#}9Iry!a2c{b zzz1O1Pu<$klx4Xz+hw|hw|MKZULZx+Zr97unY2?+<3TEc)_OgC-2LSt*kSqxBU*|~ zFv$~&%q&&T{jC0`nwi`OF1t28o!F`^NyUPrvE#{M#-pPV`;&si=np!`diV*>dm8Jp ze${~aW)c^Iu$o!o(;5Nj-HCpnnvw#dXkeUW98;oj6 z`}A@q$umu!hV>fVbtGE*to6V8jYezT&3Y0hMZ>(TIiEM2FKVV`LunftTVq`U!r*1! z?daX=yZeqgeee#yCT>-XbfbviY}7h|b$(e3yekN?e$5S4(upyo=A7i^BB-QWhqc)% zCEaCVwyZ3MKzYGfc<@=yUV8r5Q^Ic97+~A?Z|?X6q*NqXNE0e0(bEO9s-&_G=fk?N zmme<`^WIZ8EmdQeBGc~B;d&94cb(xzcYQT|h9wMWj0yLt+_+aSS2?HMvt!+9Vpj)@ zyT)bLbGR28q_43y(Z!VW`yT7H6m%beLrX8oQZs*zJG|0kSga`VX@f8u zL$)i}jLQTJ-uFfCeOZQBmP>3Kt#3QPs~_o7a(#N)HQ*YQZ&0Epl`XmPT|J%->1Cry zj6!u~25+BvZo5pn@{3erjA2w*4y%UQqTzH_bGE3MH=5d|opVys4Z9`76vuF+yz(p+ zt9@cH%h}w5;cVj`5xPEw2W0EQJ+eFDcVkNjhh9p#R7sEWgyO|H&t5G!ty)@N;MJ>8!RIb=~4r;yr$n% zCGDKYTHiHmQ0u=VKqwz>L4q>6-Be_I2Q4JVFQ0#5{O!QoF6G>*oQutr^Lje`zTJ~o z&aw$hKhs^Et`K}{l$$d)6;BQgpFNy#xIbDR^PUem&TA@Y=RA!IDQF>=^TF%g_vQaB zPE>f)BJ>rTZ>$~baCCigE-XZdy517{n&Z4jUoHROE7mo6lgR)_|u$-4+ zv|n07+a`FGB2^cRSqXcrb+m2hhYaAIr)?Ts)8cKI35J?S9sIKI^9?S#0PLZ4>%r%G zho>Kpl#(<}I8+2Dp2 z4SEH%4*^&UNG~VH)z*vhAj?_4c%#$Pu%W{BW zdfBxIS_}e9bFdByo6D5vLjL)kX8ZGsafSQ(1Y2C-@^^Rnq$n+d!NLD@H+ zN?CLxPy)u8r$`0zV>?FeFam)W0_)cMLce*4v^y+Q(tBZ0mY6BYtCAPzCG*D7`V3TO z+_5Ye-}Rxz#~qJh21iOolIERCdWsT*lr$O06u2m6dV3HaR&dDodY*<}-)`WX={s-X zmPA=b7gWyc?;Qj0`_l*CKFbh{HA-?_@nFm{wrF+TW+}H!w?yH3K?`>y@WN{5CZ%Ll8FO=Asj56{87C8_aCR`MjiTZ7|S~TT;>=@ELDo*|^@Xnlbe2y-KuX z+A+=*lRP0!S1IQ=)d@%5fj;=YN#(qea=xnj;(`%}UwX9MtOD8NK(-(+@mc#w*2)v` zG_Iy>rz~8>DW|-0uV|{T82j%r$-hsQPJ(jSHCR_++ZtPk8$tR8BO6j>$rBi*k~~Yv z6SZ8^?VYJaX9+m8yz{*)jO% zhuCsKtj*Ly@YgClh>Zy|@=4J$FPpBOc*?w%%o>TUCDt08$`LZbd57~ZDAi?ePg;jT z)6HEaw?r5;x&Dyev4`C7y6;ocM^cg)O_9xb_NwH?^qkV>_--~V)+T!QPwBpGjXDHj zO(uejdyg|0a6QqB*LCX9;gLJPsGL~p*Z zx+ZS+qEpSOO*`jLJ64_~sGZ~TEb z?&6zo)IP1Eq(p0p^Nzz=%gMY&J56J2%DSYq8Y{-D9?_ow>(wX0o#?JV@-~BTnkF16 zNn%u&lCF3;owBf5r;=VTp?Qxchj)^t|5R5|kY+_tNt*>`wqRPF)3h1BQ%SD^VQ2gL z#-Ql8eR5I!sDQugC#hLHh+2%sH(g6iyjZBmrPdrbL3LYMfn2ZJ~ zXT-JUz4XVw#T6Z>!QVIeiOZ=?cS3|MF_er_7!@g62*$Z*Z=@NIQj9U($?JZ7FvGnt z0}kJK_ea%oJ$K`^k31}wsU(MXec_=aJ9n*wEYk>4&^l1FId-qJLT9C=i`vC|^{xkkZA(La>&{SLpD%lAE4g4= zmBAalOV`th*M`5pmE|L6xMtG_UyJk&O1jX`TWZpzZ*M5q2hw-^c;7U`42Kt?ssmRe zeMw>JrONr?(U?(@A?0AOBd)I+2i5}c(Dq)3jlf&iySr8Zw-v ztjvt=hYQ%IQAIHB$a$<4S=VRyo0F2$%F#HF7pCj9b(gO0k3BxCfTwjrF;-|}(9jn<~g&FXzoNyt)ON5hnCxYGI^?xW>fv-Jo>hP~XoJv%qQMo}X zjgT^wWL=sXzNW3b#;5x>RqNV?>euEr2oa15gp_w{WO0S`Grs#8e1_L?nj{<@^eX8o zU%y=N;=G}BIi5V&ShBHh`jNeKca7DW?jTSKfs%?e%aKZvm^tUOl4%**8`7(!-(c}N zoWTY~yx^-furok?m}2hOz2C7up9VEI1yhoF}8uQ~f$r&TnHa z0E^I=E39#;-E+*@y`vw9fI)VVy-di+2ug z(17s5k-vs(!VlI$mLDKiX?gD)&RK9an8>s8^>yAOeP6+S*XM$lF%Zy&s>~bkg}Z&$ zQ__;uD6%A^q|a-Ib;CZo0Y6Y_-PhyO@1qooG|56bT1m88FfB_eYq35DmEtc`Ne>4s zzD4>LC2Z*bn`pF2R;D)Yz8=ZzXKirUH^;Cw%9o5y&GA_9q+9PT^6OPOzm4?(EId-& zhS7jPc!jVUtPGZGYGWg=&nd@qiXSYMJ`-7hS?qzkD~ zIyh*Z&nrsnXsrwD!rO`&JU~b}2QL9jViZY|p!6#J>F@Qx_IlXr_0KBhY`T>5lLO6X z4_7JY3+6>P%*7e7T-ih36hUpK5KcoYS9PV=F)+%&>q%wj0Tw|=uv_3IkKKIHgLQz*gw!{9V{h_EPW3n_IKZV z=RK_xOl`uc+vnH*`}`xHF{2>!6;IrAz7+q=WATC-h4mjZE*XsEZA(3Ujy?G`V){== zJH^WspFSc>KIh5)1H4Q@2-5u~X`3P_D!`J&##2k#ImI!}FZ_e+u92hla% z*qan+ElG_gd$r*C*@9`^()t{f30~5J4la&gXp)D<(;X6vLEsg{^A5)p(zR zPzdj!6Ivx&hV@=!5K>)mw2Hi)OG_(7&=7gspj=2fKiHEzIm$Uc95K4(l=CJVDQJO~ z0wH?S`D?Z~TGMVQgioPWU|ACbUBwn`^>Gy;tr&u=ArlEoCG2Gxj}8lt9_(R^fi5#3 zuK3|AgaB1AlaKg&_e1{5KIK2lKjPQwOXjj5^%bAlulR-gbN-OOW8b7$bAVJIP(8Mi z@#37NoxQ+6|1Tsj{y${%zr(m0R-{zP=cvP9l0W=6nEmgeOvd?CQ`Me%t&t`n&$G22 z!S|V7fF-3ziIpJlJ}9Bz#(GLxG3rv%S*l6RIp3btoK~LNc71e(ycfatO~(V)OAnXr z4U?2Y>!6Z8pW^I-vaG0Di&qXIJx01^%2{g@gpKrBsoUPWyH77{!_^pHvX^x!=Lh4E z#YxOs=eAwAR@cgqq!1V-koZn;(@$)!FA%Q^-!;moh(_a^4fmZZ`e@O${98R%yIMcd z8EbU^S5l%?XhEp8+Q`O#oY-VFa`Q-~P~)eZO#UUmX@9`~t9{B}Ctq@Ej&UM1g7~`q zoLBW@PWd5!G!ZiqVVzz`^{#GV-rc(&CYn%~zb9oO7yD z$oElcfzcYH6-u%+?f6vi-XrlCN$~d`-oS%ndSBUei%N_M>t{wY%4dB2vgG;Ml*;CK zmEloK)#-3X{IL(aU7A&I@3q_Y(pp`@k7uq6)T^#)X zinn_31QSA{e1@!2Y%PNy>Sme4RUfr}seAK+7sH3@`dsHKH$g~+89(G~@^k)ban4^i zM?5zNIF-;J?@DF-Mi>f?_-rD0GM_ur66 zx7>Z>DqQt9RHqLp_kKj19nyP)_eB}2&;0~C=6Hv|q9t8+=pOIuJJ747_r@~xoHjbB zq|X+dS1ql`K_$x@WnFE*dXEob%}`@QkZ$5F!=lE*~GRunxfiyX@@; z$?EY!GE*a-3&pt+xMYk{W;Kz2+h%QCxYjoK#kVAj-%$A1m_GP+@1-Q<_Kc$X2Nu)c z;r4%skfXKW8xF`VnWx-19*M(9kCp;?RYv@Gj0?scN>LQSvM)pmsz@G=RD z=2v*%^bcF-9L~8N3P^*Qgm?IMfoRXkuWD*{PSrG6TY=1irE5Ph-h8hvptPc; zX*_kUP->U<_aePD?@bL*hizqrtw}`5B(HhAr+E4>v=2;@^>Vpg96YZDVA-t!OW~JA z;MaS~`zL=eIglbjH3e;z;F}7suP0UD2VZOk-kUf@!x}Z`J*D-`9oS(J|8he-7Qq6+ z+8x(8E;sMrgaHYhH9}J%%O^m)>AYTuyBTUMOtqshgHX-EdZ& zGHps4Lb_foy@%HyOj-)?j;3jLHjwlg32SZFNBQc@HHU-Y{<-M}D(8p$k|#$Yc`XO_z55psnXfZ-nBkcSHzt@B;)|%Yq3btl2+grUL zF8SBhvVL&}K~rHA69u~M20c=C!8Tk(2G3YJ#zta3_<_>}d8xrnXw4yQYeH$!Q0(Tx zlF+Dqe0qpf*`Pth&DMwS1U|HZOuAmrk&aFiu&$EsQ__Z1pYzR0&1vbVYz8vL^YJNZ z=NxU@;+(tDl;@@R7JNl2N+xN=(HM>iFsg}#?i(RH47+Gttbe6th<=ojxT5uH!75jO^;{(l;2V)NRMkK~`tzo-g+wtQD z!xVHE+l-PJ1xh#?7aX9jcRyBg9w8)JWT<+Km}mH^32kWmQq$}B_`#y4?<4OudkzjS zQmqt6ndUpW;y33l3#V{Ghj7_hK2Ai-XU6e3Psxpj_Z*N6ZwaLgHh)B2e964}D-t^+ z^=;U956%k~;(&Sj0~mddN(!7?PKNJ&0(zNWSa!*>DCJ3X=!Wyj4M5()n^e+8cHX6= zEeq#@aR+MUg}l6s-%n>Dgg{D(*828lz~1A1i|`GpUT~PVJls#&A7`Y=YP;6NXWhT| z_F4`m6)(;jUd`%o&#NR1jyEO(yS~oh-}BtP&wH+3SQ%>wSCV!q=g*Ea9vx2@75OUV zd16c2Dund5KE}hL+s%n za#H>?%xWg?TQm(G$$|-|*-xns{unj>4$7poRlSjgxc{sgFj%y(WF}daU)`Y_{HEz= zrIT@ZD5i-ad%577lNslAgOx={yKV&TcV*SROWfKTUkC;@+Rqu8lA{qEPckN>^iq4n zzP%yPrD_#PQj;Y$uciym8%N^`gi1G5(z}Zg0_WT{or6I$jNZ$+Q#n7{S3Efyadc30 zDrd=ET@G4zbPxiiC8-e@;aFe?Dd?-T#kd?H1ey`1-p5W)@#6(vUt=elr4UOXZngvC z(qnd<8ooTpnKv!jNy9(YC8ZZ=0S}Gm=M%%v4|5LlbUh9Fz8l+{2Bc#+{0U`q&MTF% zSo}TO&G0D7*aQB+oq)@WtS<~A4`l97*(OC6=#*BZY^G=AT&bsYnekE%)1$A-sxx$_S1I<-8mO( zdqv4!-iDO(@rZ-{5$PRJIlqk!FKAFgl4?n!T10F6#QW+8t{s6A8K&K*J{#lqDtuxQ zS}fP8uj4a^3&VXK-u3UQ=!7JKqdemmj|30W8NWL(nKqV02_6>--`UT3FwQYbEq|vk zV|u@j>wj0Fi(`2FuPC#}%+CH9U!NhQL5x2mIrss|_z_a~^~gg*mivtuXv{qhg(dA$ z(EZ@&?%n1$$7UclMssv9AxjgA{G6{|EO>F=QaXz#l;fnmJCj~6~v)7M$0ig`ZbRa2Q)FZoh4n`5^b_9p2U`H)m{0jz^xONxC)VysVDj zxJS3UXN8n~ni>w3z-UQo8eUFAy1sP{STm`(p<0ZgD3eb3V@pyNOtNN` za?aDWS=h%Ot5r%`VN^-drKBtCa6TQVq{Y%Fe?2R8N3YN2=8I*P0aue!v0nt0^W&ot z2YVyZ^zNseH@V>IFN6rAR3kC6#o~r%`+68yPWE3)p)r0;vUossTH*2&{KO&DDtdlY zLV6HL`nBJizLZyXlKr;$RlM8M_+dGF{fN)o+)LjNX2kj&SFC<%kf86<1@}7mc01=i z(l=zvlP4N=4{r7wXK=kPRnm#!=x{=s8H)6ruU;;AIkS|`;Y7M@Z@3cRti9E>uyrlj z`(S|K7NlavVb=0s-*C{C7)iNN*UZ}k!shq;&s|SRrx>Hi(~|VG;nnP%g|k?4kY@e4 za7SbBdWye0&(7gpi*O<3d_3|zJCj&*w(;(84&xF^Ng{sIG%*s-bs-RQqEoYxS|hGbw+&PUvytBA+?cx+{kb)#%_+PUu4 z*7rY;t8%9MbsKCP>rMa?)_(dr7c2Tw8nm_`7pkkKz9H#2C~>Vzk8Q3Nx)aD;;Iq;#9U_+;MkU z4c%8Mes3!hQLt;IW9QZTlaFlYxTYF%EcM52nQ$Cig7A=SvIt;4NbRijMYIG zLXr4=+@i+5Dskon_ANrMYKDBbF7iR=XCUwvw;WkEKl=J_u!};G+q-H^SR`Y5eOidgaUK)gucWXf0 zZLISSI%ADYEI1ljK6{Y!_;|1Lavn75yN7|d=RyGX2!se_7o${Rq^0uhTKlK#WtdJ0 ze<>1De}tPglrJq_p5gZ`Lb6O-!=}CLJG$!hosfJV^kZ}E3-ltaW%~I}-SY3ei_45> zxBgpbPmw5;lHEGyz1!{Eqo3J$&{{Jd1t0o6HH==)d3G}AylQBdDd{T;!hXX`U4)b~ zLzQzUv>F#l$bM|B_uk~dScs64KH3wcCKz|TIG=J}H#EIUTF6z#Zio6NZ?&cD20J0; zJZEfbo*ZhP9)}J_S(bcK{l>Sh0ec_ZT8c!G>xM;RyLDi-5(?eqi4_7xMrI%2&su8b zapD{?wuDkSv8>g5-|Y%!>UY;k<;J<6@weNz?&0h$nubv#$P&diL0shH87{L(sg`0J zqgN{FlliPmNoPo7h6*NeoI! zbeED&&Z~ifd?O{jL#?{jdN|W%mpWsO{i5aZLBiwXmCAYZc-(ikt^s>O=a<}QMhTp^ zEtRu)xw!^%JqKtrE#o_5a!oL91_r4qNzNKx&DK-WAlKSO-Zsm@ zYj0%YD>7MfJaRle&O-aZQFgC|R&U~B3c43b2ti^rd#U7g*;3jXycwi?Z|)$i+UF_^ zV`!ctP83!94bGk+_F6C2qr#v85nwi}kMuk3v~r~f@zJ9hUh2oKVd-aE8! z86}<~HRx{E^(j3RZi8WooYsoTC=2B{Swit*$~UiPoHs4rO7O1#rS7*c6zpwDfBMee zaHVqAZpO%zJQxd(yOgsrdM(|3Tct1SB~hz6yzElahkJs=C^A#?@;s=d8($#0u2b7Q zsMn9#&9GFm+ZyFNmGi#h>4OP}`y=u+Su@u7)b6v(b2=h4X4uOU@>#`sQ{jCc($I(- zRzwdsOJV#N8sJP)TaB$x@%t5`08}TG=`Y%~;qMlci9sRwPzAmEfe38W0pC+XimX9X zGf5?Ra*N$|K2iExR4PT0OSF>p_Q~wY<@!#E3D% zD#W>_ZZvIuf<{xNYOz5q_Je@~3@KY_d$!3lGk3E9tu*c#m*3sj8Xe znnZ_@`KLB2zdZ)!g-@mz~hV?KighZw=m!&>0gSU48pg&k&df&LXXGXs zK9rSGj7J&9XuNBfoxkSU>ocm>;l*Uo-f)ZmGTorj)o2&m2Of`ErkoR_wkoW5Ehr3) zieNDHk&+}a9PUXHQwi|S(%!yHIZrxcji(Pr9PAZjX>w&2 zp7(+6D(LS2LI{+UOftu@p~8No)3` zz-U2YDqc<-P8R2sw#CaFKTt_u)&6a3LgBjF-c*zvjvS8D$*gSRev z2mwWE*vlGT%xg;5cIykbc_4@BB2dyG#U9${)blZQc|tos!4~JZqQd1ZBpzW1Ys3l= z9^ZnuEWNHpH$uGm)so$3bzJV&!|5%1wcH=?9nx2fQp;XpFgnhDcZ2oJS4g`iJlty- zRgROzg44=U+7{TZq3hI@)l<_~sQBfi$lC_#JR{RARn8BOCZCkC#{2Td0K8m>F^0WNGf7J7 zy2iSs6D-_>Vd6T&ZYkYXc47mG5(%j|MEeo8JfNwjG-qeD`W%;(xU5B_7LhtI9^ZO= zDG)BM0l$gT@0&A2*#P=sAxE=E38XEX>RjD zJghlisUQVXXrxytm!SO!)9hp01-2@2vkIqbysB}k!CQ+c6OnSKA9vyS)gbqMw{0j|)e`n{0p4O3q1nZv$#F94 zv^-+HQsjL;r;}JaXKAY`nJn4g&-uZ_$2@(s$KE(!^Ky=C=dCH|eg=>whQlJ^`E)_u z)Hs(SJ7bR9o+Yfx4^|eIQYoa*QDk_(hxZoS)VQj~(%^iJ_ZES|=sl!Rtc$Sj`YiA2 zxlbwRFr(^4It_-B{fut*tCl}=--aA_ow{u~oz=WJt3fJ^);MoaTB2KvXy$k;k;%`kF%3>=y}nI&3%B{dBw-b^`AlRo!x438sDzB@5cFNsLz>V=SZ0 z(zXtxCE94LlDKXlWv#`?mWh$<9~^SHKi;aI{!pBCET%6wo&E!M`VG?8@3(*6q?8|h{dw<0=3r6r;$*?I*A=IwpcN)`of7XkWAE12mA4eqGd2mif}_2h zB44>$-PaCASIHY^z=EzfZ5N1Bqc|QXoK_80Rf11eB|h7lTHhF*iH5lR+d>Eg8^W4F z9I>vwh3&pOh;AM3uHR0pl-_Fgn~0jj9448;Y?v{`=g02gyvJHk;|x~k$aEiXf)>RC z%6l^B!o)!eq?F6zIRSWUaej^$HA*L>X?k^{Bq6YEOMNy+zy1>@r~j3l(+k!?|cXxM!y9IZ52n2U`hv4oK zU~ou+ySw|~?(QywCU?#`@ApT1_qo6AXV|mZ-L^bgQ%So4a-@C}AIeG?vUp@GEd{L%?=rEIL*^GxtN!aTT&C4S6hFM?zp%P?dM;?FW3GBb zezhoBm3)&zZ&J?K?Ku*$Dp7sh^q;F#t}{w`y6Ilm{vy=`O@<3who4EbYUyImri_C| z%cG5590Ec~k#jhvV!K9Ou7AFowHO?6 zn(l>gzd75?{rC*BD|>GQXGvJ?_(+NB%{QryK{fj-Xmz(%FRw5;UO~H{>TNEsfY1yr zsJ%lYKy>&oVy6mU^OLdhW+gJ`|dNj)z=d0 z=)Mj#4Lcdg>&{*_YzvEFXWhomJl5Y6DNDAU`cCVfydf?vPN9Fdd(?9(_N&VaSlz>1 zPB5xtutD3YN-`5p&NH6y*E1r1@yj!TFX0>u-j-yUE0rYaH(l2E_pZBa~xGn zz}!BCE=%jcW!;37>#NUS+0S2IX6pM{J1RDCz)~OUPVFF;H5NW%p9E=!)m}0LI#27d;Kx#wl z<7={mPez~o7Uz~Qe@`&|E!VBv`c}joy|RgUJiEN|H!kYA5C_JjW`9e~?xW9om6jg| zmAA9%&5ku%6lgN?4#>GP#TAMjqGs#OC-;LJ#iI#)?BlLW@CRrpz~4W@^&mC4^J8Py zW5o3e~28^BZF93X0?~>*)8Z`UC~_(*1>L4BUBMTMyS^Xd$cm+l_D6B~Wx@ zu%3dXtL1@i8aMoTQ;2JGF9fuskBLkC6D5%rLCPw<<$l=;I*QaoN5VU>1vvj4rmYl0 z5Q!WiAEFwccI5sSmyg03s;Q-crAf57ne_PUBv~LdndYJJS5}4iqdq>1*|ddum~@Q^ zP4{o@?!Q{ia{N*&l@ASDcbAAh?~u%~tSH!$#)J8!QcSCtI$!^G*=>X~Ik#Z+4T#yQ z(n9ZC6*$WKEC~vE3ZPfqtaLQ2wHEs~qQs@8$bqqv;+)oO-P5~zqzt>=(bU8ntDB*{ zku0-aOH=8!!zf*TV1gmAUm`t3H)1%rJUdX9^jdCiyUxP?zO^>J^@O?swdDsZ z#I=6HuJ6Jp|Jy*}vHhrI(fgNJnV?t7-s|-tvQ=-#RZf?>FkHu28q7%Bp?8XU$=En+ zr(0*cUFeVsR?kz9;6RRhtY9PS`t|om#Iq)=piT>s>_mAnwB=BZV*b36w5gD%%7aPy zFnZ&v8pe(I2HZ%m^%I3;Pi_0%mX(Osj6qgDMO#uO%%_ZUtJx1c754+e(-Gu#E*!z( zoa=LiZak;_#J$kmZr0yLr3JFciBH5$Ro2sZ%$1O{M0sA9(M$ehUN;vkzj_5*9alq9~&hbLdd;1lCR?nboL zBhl*a?kP({_tm}^NcB;HdaH0FMA-1o92xD2qh8n59prQU^c~8p+hEQ7xX?8sT;;ca z$Rj2VbD1^0xfJCZ_wIbWQ5GdKW~Ce7I_yV6!>)lkWx5!n+Ze}GwFvYTDj9KbB%K)_ zk+!-kAF!5GU+31J;x~qD6XT06(BJDotd-%s^@C+i$vnSyGwaQ9W^*)h*YF37Uk}o}@YD&qvG3m$i$^As(8`J?L+_d+V4K#G}Z`7lpA< z?i}K$9v!<)EiwM+I?MJcT3t12F#~Eb9;C-aM%pH{8;K>a!)XK#dU+)JHnnU`(iF$?^W4ovo4y}YqAl3#4o z#?<8MkV%(W>K}q1jlonWF4p%?K_)??`BC||cK0x>Ll{l6vSmIyMt2e*(0lR+Qs7n4 zK%KWz?BGkG$O~ahW5ub%IfLU9GW%abt$cDr1n*8oXG=5#;o(Hl$h}YMboQ1wo=5D? zaBXuCor*G0WoqCvs!D?$WjiCSnKj6?PZVCjYFDpjb6#)*+3*#L**ROWDZkk%f98H| zY*%1$=i#lom##dpTOA=8H6}hIz48B5`x7I|*)z@i%1}C4CySl10etberhvvny;PoDa zoglHh9PfhBNr@#$yvz6nN03S>HNKU8Ng{A0=?Mr#C>d&^Y0eYduRBDzT)jrWubH`I zN~=KUBaYF=T{)m|d?%6ogPvt8dDO1=pk;(k$^b1O+AI2QmNy~s=c)2YGz0CK^PYxv zQi3pRuS2f`Ni&-}f*o!YWp zfKYb)hZM9`m{qu*ZEI;0R-#3~L4c8;qBD>V4CY0sLgGfFB}l*o8*bTzmRYkMbMiry zEbue~RjS3AYAsj&uqrPOm-U&&5}>xsST1HtBw>M3TVq)dW2cTClwn7>GK8V&P&rH{SU3z)2m8jM?v0%7~)j1Z!!58|Bvy&32T^($P&})a+ zfR0_Zn+!pZ@r#=dg$uWni~KrIZ-j27x5$wjs@o#N)+*MWCj9Ho?^>Z%1wLYnI;AyX zNpKDi8;)v-9?E9g`1bf<@oy;m-!G=)f9RG2r~SliWB}aTJL$XLU@DYcwIhEi78e2q zn~ici=*KC8f7P^4DNWT3{4R#w>DppX)m$gGpT7fitbe}y-yd8`9VrFp=;gll2D{~0 z?jOXLc<^nQ>`!(V-3opPLc_}|M>JFK248*Qv#6!F6OYHCZY=3a{y>gn4eSfg6iMc4 zbuzOwu>>=Fr%=KUowZo4%?8ymg?CUvV~hFbriyxnm#OyykbZSTEv`ajyO4YlyS@lzw1>eWABFkd5mzfWkl z;VaVz8YEtJ_q-q#>99r3Y!yg3ZwVM>a(mdZ3PIG|5k3L;vk|vHCv_YC9Ate9EB!NI z5XR!tJp2~NWKDB3=63lBUt3HUUTg`*ghp&E0`H#R4SYn0WXKP@$Gol7#B;>4TpvG> z?Ck%wW}ZkWB76MY0Lm!;(h+F<#z7V9%pC#qCO1^92H(9;DK(aadyi}z;Pkd5RUK9c z*9Dr&iPfkEdb|xhrq<~@0v5dzpF+B zda&`09(UL`BuKAW_$FS-yMm6z z-<_+>--4?^nyn`9V$`3tHlbf=;_vO`s@IZsZX$8HQ3XOYNai?ZbMS7r;>$BX zh}L@*WVIR>)U+aPlkewmc8GVfl+~eht~kF-34}BX#>PE8diEtG&ToyV7J)2D!l#(2 zw8-jkIJIidAx$Tx`7$9g?=C#BaVUd?Aa0m4vUcRX__ye5w;XaG*>rqqK}QhFH%|G6 zZ}5$2=0Eq=ZN|>)3mXYeh|5xL8VpCWbDpdc(5^lDS@4$X!)|nf?pZeDT6dC;a={8A zhevT*L!lT#0BT}v0=;^}DZ=UPfS)-OzHqHmPKI#~B#?;@q`Mv1*Eq|y5y7A38ckV~ zFK`H=GpO6C`JKECfmsF$=NzgC>c2~$k71iD-+~sFzZKw_c$8ug z6!~0-uvS&b>eXg07~UwYGP%05HWN1D#BF%>Bdg29F!mvZl6eZPbV9>ws9w`3mJ`Y!Y)DH^#^P^h2I;O4%3kb#ETb8Vov(_cV3I65g9;T-A<@2G1BDHAS6CvOR2Mq z)I56%>{piA*niI?Frp7jftM(3CDl zMqR*{&L2)2*(6~ZHy+)~^=0m-^cXHI7|j)BOqc!1TFfM{x%Mt0`IexOr;LbMrG4#% zVyg+>4T7r>y8DnN|lvN?^HI>l-_Tl?M`W%BqK!umE^tu z2Do zu4lP=hE-V=4zxu(U*{-(FMgUjdHXS@kv0aaL{x3sQBY&2u72n3GmFWq$hvwK`3rdm9 zA0~a8PbEnF@dV>Y@LP&9X)<*Lr2?C@nFD&?kl)QmTRQfGRR5{AJUfu4Ozj|=k{74u zh}+6T!NyLXXo)3kp@QIc%@AZ_wwfNe4~z8mn*2jO*xL%GWc#(|k;@HT7e>o6)Ey`K zHvtU_T-Nb#wu?mdJ8Fc-u37;9Akm+BsT^##8yLqYnMu5$*y_KYUD!6{8;@V4l=a@x z6aFN*-{F@I6knHD7qhXSD8xeV3muM(_yIIop1>^wIBUEY8Ev7J;Y&az{ce@SqAbvV zk7j@53;j?n8EZ->L)mT^<(S<6by1dY_INFu*F&D4pWLpI<`ldGV>rPcd%!t4kWbZp z4`-g(6FW9M@mZtmu{N>%1Z@voAXCd_voDBWwMxctH&~mg)0wB$nLW*4!)Tjh7C^WZ zSaM-0;X2j2AMkn2!H#q<X8F}Z}~kb=7Iy9g%~?w=>A6nv`4Dz^X-$D^)|W7MDKZL5eEPKgF^OxP`u9rQi?wr zgZT--gJfCx*zd->W+(9$A0d*}^4(I~ZBAYvns7S#>D{^V$4xWQ1~ORN)O7Og=|a9k;@FrYD62 zy0y%;Y$gWCB{#6N5}q<)G9qreCMlrag~>V6tB20ES6pt^v#_58-cg^zll~#KxbJae z#XIXAWWtt@kZ78s)V^cyVq`+h-odsxB7 zC-4?;t0Jj{;dFoL7Ubl(J&bIE|KroszpLa55x$ zu-7=0jZk}wU4ck|y zOmlFe;#S2(ZB3bLxK%z}1a?^w5qZ{BQ#B zMQT*JcX9Cup?4xyJmh)u+lUvh`7it1E(CGbBK<&z%%HFqdP2to6fz$yad|}%>8v>WYlP-1aYQYfG-b7)b~Ehs0i8>i7^bn!k_E7+ zes-pp@qs!;Imn1(57qR)7!K%+>R3WUGr)kI=FqkKEoZg$+TeQ zePeTJWzNm7x`iq*e|rtl7Vj0K1r>nDhejH?#O*^l!)WA;hi=kE&l|I3U@Iy$##XMw z;k`nO83$P`F6?EAt!Fx#;vA@HmFHp-UC2okCB}PK=zX0T{k86^vMZkHm6D;E0Z9fc z!Rq#~N03WaVjsumQ+%S=@nXaLr&g~y?jMqRJFmr#}-Ry^)+$sE8kAn zFI!t+w$I5vfnG5$KK?OPzdqsUcU1}ay80;dCjVL+P68;01L>98sn}&h^X1ltPu{7E zT}{b9pUy?8!^Ouv#2T;FwSoKquTgE|{S}{FAhjZsQU>)zN{K@Qa?j5g%NOzu9;5_G zkqll#nOnlx3l0!11YGlD9dV#vF~i_YzbnIY`(;3@U?nYHh2nsPN8ha&ezYXz`9=do zCvd50|L_}84J#4yV0ba_z328azCBg4?7`*N8qg}LYuhO66GULplHtJKY^)?@ve&WE zbTF&gy6^2yM@yHFR|MZ|Josa@D0*Cx4$yq}Hrh(W52i{U&7~AMpaO;ea}7pL8;mJL0vJ=W2e?uH zJRf{%K>p7wfiE4u|KEF48)YZR``XEmlhO-1V4VmZJaACF2|6y>9axnPYfoZ}p?=~& zcO#P{a}GzJs2TVxd@W3YyIX_rd@cI#+>YX4sL#x-Wi;Ek0K2XQAs`uO^z9n8+0nPn z{+rZ9bH4i~psOob9MfKTF(UZ2nqt9g?08ZZER=})G;Cev=m(-)mi*U;UBg$F?q1!2 z*G7|ix}oSPUWN!y-p(2QjSz~`KFGZwu%-VA9`fkHBLX9x^#Gl0`74S8K@m%-7RL)h zdke{9N*z%zw|bcz|TS5f7CYBi!;KSoR~v+#n_kz3QU-*SKwn{m`v%`sC*5 z8VC&A{1ATEeG1T+6k7q8e6GCz0!^|yJAU-#v;2_*ENS;&Ch0vtIrsh;Fg)z`sSN9i z%9grlf&aqEYWae!MM-zNF+LNR6t3&nV7NA|oBw`0NbnDc=P%S%LqKTV*X?K>&G{Y8 z!d}UIP+-7$#dPm}ce{cD{H!KB76!t|RPIRgH~#DdY)zWIjLcq|)q*>Wzq+A^-bViC z4V(`))8*w5Grk~F6AL}H&$tWeMa|seZ6N1tKe zIkNsM!!9t&T@mCNQm9SpbXB!*l;$^{XdQOS$|KsVt=GabV0X7%Jn5mm6I)Qmc^0%3 z+UReA^%We}*TPEkNk18e@k`<=vaconnbQNHaoq07NmM8%C{3oBom~Hd| zgLoDw=Tl3{f<>HkUe||kJ(o!)6x){+YHTSCs2e}bfBl)$8cb6kG1qHuI8TVJH+?Us zWD+Y!E}IlY!|nvNYNxyaZguN>g1+RHwsa{tNil>k3H*q>SSzrO{GaECp&RCibTxch z@e@m-#=pE-BQFW;fruOMR!Xt1=c1WV_w;~8cDV$Z{p-ma2s~sD9nU+UEhmpY-+Tz> zjEoG^n~rNoD}pw`!LR_ZhkyMvfzU>Vjp8%ip}g=$5b(|Ypol!^y?(R z2kkX--@%$93V=gGHF2WV>EJD}VdqNcyP?rz{8ya+xx(J9_p!^8N!`R!$n`_V)_&!X zPvm)3qbYBRpTd8DqzNml0LQ2=u@Or^3~T7ZvJepIIkAHsM(`3M&0>9K?MSGdYQ2~o zmc|MQ@E_U{)#N$Guo9$c?kzuwd)_Y2bFnnkm*g#zL(3(r9xzBpl3(5ykVsMDAHvdD z06w<<=XXq!OjF=o=bWTQ3o5$z^$%7?qg-vxQVAQVtzzq}l9>I_G|Fjoamc&{dfHUt zDjasm?Qj2iER!U|*dOyj@3h^@VjF{>_bKHzb=2^wDdk*~o>Jjhq*XX?`)O(zmILYx zY3hbw5;k=H_q_r~lX7Y2g##ssvFR4pOEiVbs)8IK4 z;&=3M=A)gSTiMvj!}@m#;14~pbKdpqQXM$`ZR+{;Fa%FS?36>5U;Wfk%B%E;G>tA3 ze-+GQMND~?+G0jzBt`rtFF$-fRT0}O>A!vj#=t*%-n$q6z~$-bmsgJg8yjnj?=!Fz zi)WEiIz9()kOl>P8tp@95n?Yj^DeGPcW{vYkAj8^R8Cy#op{@&zY zZFAumY)W9&+!;>!?|qf^4Dv+WFXA~d%}l3mNT~k{{Jp+t-;Osp4&NVReb|<)kpS`- z{p2Oat<`U8lx2=n`2Kid9gim1;y)Xq_Y+>%V^;TY@`Z_SZ=A*ReubvFw{W}gbk6@b z7O15m^}OaLK<>29AK!lj$5^dG!j;4hqe|{3>`te(aZ;zzA@g6%<$N%07?EuBYo`-Z1@w z#y$nPQ{-vj5py}e#p2z=ce9`rR;{_@O#&K+`3)23oq20t6;b|7_$eB)z(IulM`4XLAq`mK5|T@MJ^+#SWq{>+U}zV zkwGFR%ope8GQc$O*#FITn778x2x$Jx6$sW8Q03nq-fYqTmeHrst|73+e6Z)hi z@vB65_{DHNP_yH%UQwT(GxXYakPTh7K*DI5N-kv&1V4KiCWYSZJ#QscLEo9db3%Kl(c_ z=8LK}C~HJOrtP=)t?#hTw3SePf8|u@xE^8u2X5=w!#&TSbIDe06$AzK?=)?CBORK> z`;?cxM-Cdp1Dh5{5{#h^FcWBQ3~AHDfTM`yQ6I5Qd;J=^vih6HP>WRL~t zp}oPxsyle-mSC00qs6c4?bxi5!-NLy=$n=h>#~_FoD-DzUVL;}&6s}L>P?(S78Aw$ z7=HVv#a$-IgDrB%y@KL)ELQU{tjbKd@$ki~pLVrf@K5=+6xj4rfAV4ngL`2KV6IvK z+%!D~PG0~{M!@G+t*J3_GmF_a{jt)>dxvS?77kenFV?UEMrd)HPG;T4m{;%`DlF|` z-)yAM$l3`X?xsZr{EtE!5>7jh4i(uPFab#vBq(3-FA=Xt&BJF|;QUykmko&;NPNh@ zV&yQrymj{^pPR58+LOfA?;Vb%Z3hVM5AjbrB+L_t+kUZsblw`Wb6U4w0{InaRGw?h zfmKHw!EzDFRs`}xPZ2F+%CI|&QTGGseb>`ZX-@fPx67>*0cKZAV*6ZHD2h4!I5~=$KEaHt3ej`uI(4X zVK-5wwI+|TkbfhlAFm(g4$Fh;j*8~D+0v`{U98f64xY1RJXVdQg%nM>eC`vB^g>-G zYlt^)B_NIzR%G?ttmz{3O_gVG26_ZBFEuqfbS%w3^jG)XO1|*^9hDy80k=ND`f(^z zoj2u2UIMUF28xfrte$72a+Cn1ez{%+IW1dtt*|jK_mP0yp9~()5ti5+rTO z2px;!x>TPd^~d(w!WYB~j)c~7hW}jghg@QdiW(#=#A-Jt*D99Nj1q*t;1POm*<-@F zVL@T_G7Tc2uGDrkuKP_7{=IT6TZ@p+Xf66q{T@~$?66-}Wo|?WJVs1JVw61|VC$=a zMH=|7NM`=QHiP=jN?3S#x;mZ*-{A(>@Iz-sD&7@mX8pPfP37JBSHEBt+1svH^T$TU zfNf)uELJ_WPa+jjH1Z9&eu{GI8O|DR(f`c8W4N!kk8;)Nx>R(&^~7POpzIgRw#1>xFS>@2XZ*z_N4wpTe0+(WScX%Ez*GQ58kg zzcjdCtHaXa+-s{T9WOXYEeep5>7jxU>$y%f=M2VcD0PN`u*T=a4duJ5dINf?VIzG zR;N&HMgd(NE-$W;;4A_I6Nv9DWK06&=F*cAwo!}r{*@B7%j|x?V!w`ZOk9NBI1k8R zSI3_zD3PXSFgjOolDo~&*=qbL`Xe$7!rrcTuTGNE$*f}h&NY7mes7KcRHG_ z-wSj*OjWGHveg@AAtzuSumBGHF!w_Y9-B8i&f>`+hST3E-Jd;qASB>1GuTYiZ6`fT z?;_M*)_NjR-m65rpNb&B$1RL9dL6c(mzEaj#}&U^p`W3A@_rLt{>%Pc@6@TZpWO6i z1$(#QW=-8^<8OhCKr|OQR=A=hKJr%+BW;L&Ur8oeMP+HRpBm5wCE`p?^#>gx)9n|6 zTYi8Rc-DSznW%KiTt3>w%7!dg5y5;G4ImxTW*MNR1aNxY_ZA>kRfWXqO^aBkC^ucT zlhY7Wp4;$SA6z4`niDspZ>Xxb4chn6xUh%7g)BjrElV0NaBsfFT5ciyJ@ccFvR*T0 zjXUjUKVjrW;+mF22n2<&dL`^9XBR^Q?Y^jjhHx8BM($l^ixMaAC`)hY@UBj=x~F zH%B!5d0hTpDDT@|;Al7g8b=p@Xv{*h*)tR{O}v5JK@^O!8M;g*UN@C7ms!yTp-m-O zR~`hDmPTKLZiKw<@NIV_GOQL%(qWFFg)ThIi%2Omu>s<2!)^J6;n5eW${RXQ z>Qz%24xqH^>)$Xh=9qdcP$lCGq%~M{|MSy7uETmHBno1-k4ObmA1n$s^>iw7%$=F> ziBpYe9CG(O+6vyA&DC=6^fvEyoF$X3)Mx^Rz;k!i5g+Bzbrmlh5$WNsE}NDlOIPcd zg<^y>N2ZMTKy!kvzHMyaHx}AQ6+U1dD2(#CH6`nmW9R^8E*yfn<9qLv--zd@)`>7l zN{RO`a;wc54d9Avuu$o%*3fx)kuSK{H*ib45>1v`oKPqwd|pV9i6MvKzW!(|6yeQK zzWp0*%q5CwLM}?LcyOAjg;$KsGm~$+$j!g3?f>;9Ck{D-sMM<8RnmiO%LZWOy2`6(~3H2t^3zT1^)bqG&coQM#FLd z)pegbP1UV>y*160z1Sv?<`jWSc}D0*2s6r*F|1M+C0Xwk?9OKx66ldY?#s?_LB`2o zoVjo=a5A`9r|%3BmmW&qt?;v#hoY`Ux8Hf|x$J;olE}q<{IYB{o?Juwe7;O2#IpIHgB~NBez>Q7!%SbEOTx2>c~V z=oh5DfEN?6DWqqF{b4W@?@=intvN*b3!iAy8mdOujSaaXCgHFU7y(b z6g;yHEE`dUcT&tQHecTCu*my18d-?%1^lt8uXb$~JA8LtD+Az^PB&Ko%w_`UmJ&3a zbQh2oN`_+rZR!32+Uh)RM-~IA`(s~dx()*mfL)}A` ze8M`jkuJMxmO+gj{@*&`ENIs3(llGpZ`zq7;dWW1L)2RM(4#ge&ofH9#@kf*gZ$=P zwQUG7Isv7+iE5v4(IBrr(E|Lpu^X!uy!Dd;~3N`4d#W>a{6ia;$wk~f?P{p)$bjOd525( zl~isW#`y85^2@>2Tqa7BL%wU?t;a9dycPn-DcymZ$Ff8*&;SNhK1gdZR5B`V$;OBk zH_by%FJFToRt24ZU!Wj9Th0#?3Z2cGFmsCTp5uf;cYO1GuisMf(g1#K;T^ta@Uv z%_cKqAFI!eGY^L2FaIb!yLAgU0n!mr2!6ZURANNo4QsjS)!Z7IdDR>=JGiPxapyJe*f;sUPSlL1;kCh%{ygV~e z1g<`rT7&}QDC4P2=ZoJBOWF6q(UUL_r`vPAAK<;*nfo{|E1tifC4YsYEdNBK9b~k(1 z-pwO{sEq7|QtBnuW?a?}UzJ%wcS9fT^#mO1DhetwukyKxrQm8CGwpyZ7uoG3TiHf^ zrk8eG7;kgD6D^>HEPoH+{(X}3x)TsS7g`$G_lVCLmMiuJzLv7ogQKM6n|9h%qk09F0gNnb?%XKXb%! z5eY{*S%o?L+~zY^{Regne4mNG|X z?)>`GyT#3e+<4Nlgiz=r%Sh*Kn9;~eIDVA&Ys5oV&Pr_PPZ_I z6Z3hW<4doU2=R1#-LUj{o06bed?9S%eD}zF_r3cg!-Pi>662{3jTWcD&k`A_=8MXQ zvikc^y5ysnN+$Z|t2+QFp9Z_{5@sL~?&{K?H~bz3W<;?XxP=0sALn;Ae^lRZCLRf* zDrszT)JFZ=p8Rq@0=Ju^NfY~17*uSr)ysgHo-aBFsP!4Hj`~0=w>)9deQ*+tuC$OA zPFRs2jr1g0u$SDSd8ic%BTg_cH-ouc?Egm;2wLdv+pS?C9CpO9L({H~>$x47|F+2nDg%PfArYY>xw z3rjqrNJd`WzbS zvc}xN%tQ>V3~NsU1m#Q!Jt3l8s3or-bdQ}P{#AWcilNu z?B-a~<#oO4X}wfR^oKUZj|#`1jDk_ONh(gybQdnJ)F&0Ky@>Bfdq5T9bqdKv?G4yT zQcocmqQ~iUB-G#Dn}rX$XObNFOyQj*tDeQ-Nvb1gIcZY|FAPAy>MR-{+8F%4{>PQf zpGis$FODF!{2_z&sV}>+!CD{jl0iK*qh3KGkdSvjz?kpe16cRZ{uq_<81Fd2CUq`l zw?k|@{QEcLPEf2!#gZL_Kw&u^qKfn-bbpui4hA)i>wO%i+Qu zf>d!sw$T*8I9XZheyPmI!K^VWo}9c}Ik1=qgT#zmqjN+&fpi-2xyT5)`Rn~-;bK8K zj!!PZTT13P~xwdF2RpBb=W?hjHkm4%8X)*+6$L}dQTxf<{xo)GkS6n0yl4?j&M7> z5sX9hiwEWASA+mByYAYSzMH+b znz3>~9g!Mx==HGv#1xzrVp-cf!o!o zo2b2AOt>Z_t+FV^=6yEvs$R`$z40|oj-K$RGVX{{g^14NwtX9{)>ufizl$>))O8n; zze%~DvZS>f>$?!MCQSNOKw)eZF=Gi#d)o;VErhlvKkIBRBN`4l6;rrB7;7${dbY_ihhd9lR zhc61i?R>LBTP}`-t5V^ghgYl`6YSHfc8%lORef2{F>n;11efH1JrzdJv^~qmTxXS; z-F7|Wel_Iy-z3HD`=rJia*gXjeN*YR<9x^8ow%=`KKx6V60wUwDcS(-ly;V~Z19_U z^O*{b&kBT9z7?fxokDCh08He^jFQ>1mjSHfKpZ<><=~TFZsG-{Md&bd^6tf19sEsp z&cojkS&+9|H_0&Yy6$%%n9$?N%TT0pI5DX>a*M(U;guqXNdNwQ`g5U>K&5aG9YhJH z!o^+-;zClxV?t*dGZSDC=y7lAY5h`B@|rH=RJj)lniT@R0p<-nWVE!Ur7fYeFwB#n z`(WO>pS#zqJ0?T3+j0bngG0qIoqN08Ha0gD7d16CB}I~%K!Ko_#bR{?4umm$cD;$g zh4|y{9?{<2f_B573l{m>z${L2I}>aT2bD|g3jUAF;S$EnVm*XKUPnd|MIpoRypf)*gV=kd!(g3{N+ zJy{|RPA}Mj<3e|j8I*@|B^nGuQXiS5CX|v5gX+GSCsPxvo))^wx=nMJt|@)tLV~qH zjz!lkJJ*O!Iv>fr-vZtS$L#5@Cz54Q6TspXt&a0>Kih)Rtn_9fw4kA1tE=Xw^l!hN zWqjxEyTh-;sKk1O;VIZqLOZLHZKR&nf>Wee~Jtjwsi-r2>$(PJ9 zmA?N(*Vj33{uaE$(B&Rki*p*odq%4j$gEg`B!2(T< zm<7$pQD98OcT|A|<(=l7G}d-}*x9*Tw-u&rU+jAd^>$$J)^PO2smTA~rq#1-q0q)R zSOQ$(RKcYe$`x%YCyX7oE@HA#v@e64&QX@xSXdmkku^ouRJR{ax09FYSQBoip3;DV zsI1tvi$SC9h4bz|0S_0c`S29Zy=zK?t8G6UE#O_KQX)m|8#gD{RT?fo5T1YFQS%yz z`CaylG$za33>Arj-b(-Qk|Oe|fl{|IZSR%44n%Ye8~kV%0{?pN%(rLS7x`kkN;N0n zN_#HdwVPGj^RRv?{KWCBK-Nno;@7Ebl8~(fiNX|fve~PjavK#}k0g#(!2#%k+E|5I z^kut4v=dD{Ml*eASkg=}?+KX?vz3cfylV)b5^Pj!C3w~-;;Er6Jg0hYgm{lYelowD zergsMQgBDpQfF`=q$IH+-V_HXC8)>Tx5Shan)ul9xE_7E^WL88j zG?RrgJhSHDo&pI0zLf}-ckD_b;=t*0l$@Nb9Jyr-5bt0-)cEi7U1bA^m@Aufo#^!o zrKs(bq(OzWiGgr_a>rTPv58YsFJ=~|pVBB11fO$pyUEZixJLCp!$3f2v&cz`X*R0D zquBhw&xp0qv(6TJ*o^Mko5#c3c4`CZDoLKvkkE!GO^K$XQk5{ zbpF3c+s<~!pB4m86YV#G>d3m@qPn*e8O968A6iedS1>MY=tQR4N1mQ|%odT0C1?;| zgoZH;P{>FiEqOI%urQao=-UXJ$f*_mHF>E|rK;ZokLe7pPt*rAVdYn;87b5d)KZeD z+3U_QomtB-JT3Qt^cbtWt!L zD`V7DuYB)i-hkdQEjMC+qv-&QdG8KyqS|lWC&@moYkq(Jm_6Z% z$;)9jt#tn7))D<+`4-?7Zdp>8gWK+C%qJy}^^5<{kqW=hnMcF~404;_H$mD98P4VF ztpSm{ZHB}(uVYj#SddN@GKggTPNW`Vd!zBx6n!1I5 zU1fc2FXW@D)n>YDJXSma@XB#f@tIZGIc`S<*$?9qEKUT00y4kW!AFNBDWa!T!KBD7M@Z zL{$Lkee0R%QV7~RTV%55UKKW6jHXApOoe5%^xajlwz-`1eAX33=U_C$5Jk%|h>7!_qZT&1&abpaMSiXQ<73ZNTrrSf%hl{Jid3HH~$M;;1NeYQ`P;;d^3zTI+ZKsga&&Qf}yO+fe4!A8g zY{-#?B;Hl? zLgONd?RnBj)M$Vb0?#!+-= zwrv|5YlDqXY-?kl*mknviEU?Nd!vnQ-}Bvj|IE|#XS#c)s;26#u6qBSkVor@XN%gO z;FbbP(Y`st-Jq4ztY3W7)>b)~zx?O?U=UAFS-N_IBq8EF@HR_fOoqDI>oIEpgJY1? zemHCG3YYRrO^@W67({cESh!53iT)G)dn0;OO|Ut>k1)yKq2MpBN+u`h2E&qa(6au# zA3_VXpRR-p?<9{+CYO9d=53pSe4=nB0a^C~dCu35ZWozMOPBr%LXPExB12F6l+FKC z^}C0=2-0*m??#CO;xHV*ysMfQ!=Hbe*1x6q_+>2eY*}=&i`nl(k8A_WUC}uW=5)J; zQyvg_{#}CV9n)gHv#KfsaeJDi8YmnOw@fac0HrF~rWW={BXjL0q}I#pZ{aoTIA6N# zr0LKV4F=Al3wh}=o{MC94bH~#%5IWV6eTASSAf3g1s;na;IM?J*X(6_5(+BVc%2BR zR4EU0zdZPVt=z^z6Y?|MoN;1AR*!~D!X?9+iT3dm%gf^3_X&Ud0T(eqeIM%3w$vIN zBlIR3k0&s4%|YULwt@+?t#9|wyT1p!mvB61Gd0ykk@&fn7@ls;%Q!1rH9IouD;B9g zD&$E|p#8%o=|ZT<0DEpHjr?SwO{_Idehc^HRhQ4yN0F!aifgnVG|}c(ujtDnAO8mD z?Qh_Wmw%UvQsJ`qaU&y{8V7y!In6Bf6xi$&CH*GY;SyW}0nw*~;X8{s7`)r+1Vp2G zd(nu-mBsl^O)kv1@dUGQa4Pn)e;&1_hM9{_M1QB3yX8#+H<$Sgeu@t4vn-sk;R%_HMH=# zMyxSvAAP&AmY=Cc=GNvu3PJNqiKjfP8VpUKQsG2VCKG25Z|&g5PNzo*PqeLADgd6&ycNq2#vH=voJfZzJ){tQ1Sk z@5;AIa@WODD;bRj*qW4!cN9m?e7Q(dJl;}K@7XQQ(aO0p^qDHig9;Dc=QY}$ajc6C zU)hz4oo6yO^osu|Q_z1U;1%-wr5;zH5zc=}A&o`K*uhXl?r=@**?FP)#4$4JP&tZW z6J(Ph%ah|>$@F9(dkJNN~#wbFvH%kw*q23%d z4HU`so+Ub5{yb7kbWsfBATz|QC-Nq3%(0b&68w569Y!fe34u3RmY9I~gFq=s#=hsD z=Ab<{#jn%!eZrh3X~}$(AVwt!c9j)tU| zRoK!!6I|F*{e1pcW=oN`z*%_|w6BAaq7BYwn;}EB_D+}M+KknBivFHq^CWM{CQpG3 zO{a|2OFo8?UD05|@#;#S?IzJq>-~;b#z0DG8p^{X-sXEkj!r)h!e^>cWq#2FeltxM zZV6`IIVh|Ln6L^Wm{J@vikS&uZmXCzIEg70{=1UaQqgu^R!ZW95m%;quap0XiV&5z zSxVT{B1lSEm%6nVw0sne z&L~!m@IoxL$xO3$jztI{vEn>w9%xj+ZY?&DmRpR+5SgO!ghmv#T><1BwW{vm^7_y2 zLg+11KTVlK_>JJV1WiV+K;Q03TGi$ohEWu3^Z=FF3bh7GN!vHx_ZQyMq!A}3D_fBB zsSEQ2*WIweKW&>QG&7&HEnUGgrLYIam@4g*n zQjc{|4O0|Ez?h~P$lTr=JbsZ%T9?+HRYntg`cM2YxnGH@(jImK2 z%d%~fDC8BTGhLSV9!x3{A%?8bPq)uiWF7?BaaNcf(8mx8OkXFZt-d`F>nnKZ=n71a zUh4vLUa^|AdSlnW>Ga(4>3>5GnE@#@q?;b1>H+6gBIe`=|asno~$) zrDFCoa;9cJuXn=Gka=bfDgcX4rMqtf#1~aU?|z|V_<6M*d`qQJxgJksYUP~jOl@Iv zO(yq%TT&Hmj#IAKH3hOD_B-PZt;&K8>`G4Y|FTe1q$}Uc)($r+5GCK&*Zk+up#KsH z5`&f*o5PgIf}AkfDl7i>kGg5nZS5RI-UHMCLx>UNo}|NTHbJrCE$icw;awD5 z7mZmtVD^LHTyypDoW{81dxFfC6`KGhAGape-rC(mUmU(6KjekSS>#^Gq(ykRJ_Lgu zl#v-)W*dhNVLHBDpk==p$cr9@6~J}|t%!|GuS*~?Pt2x7{NFY^<)M*K!_aK9-0Y`# zbbia3FfE2*i{Hukk$5RIu3erj@K~I|e>-{2Z;^>v3HW?R(D= z)wj-Yqp@iFtNVpvNu;hOp-DORUTFFr23{CF43fb%S`42xZ)Vu7-_hfjvj8|XgebhM z*-qsl6xO5sL`Ru=rF$RFPWFdv5?BCMX~n$jVu3Mq%)V%}cDq9G6%$bab#Ii+3rZzw)wa<}HVj}u_iT^1w4849m zO+%YX;hgVq^yl=6^X0e;4MMPt_V~LpK;`LVOH5@$1gDGrj>yC8^mb;&a`r5CUM`SH zAO_RzcZo*qSvewOmj-kvi4c1;QjYTyc?6c~WEr+-)sJF}GvVHk{6~mfc)(%AtY&!U z4qIlDh6{Fcg$TP&=Uec|#5b`Mw44e4m}H5}9BLb-JknXzxfQaVB7WM7J`|igXSd7h zNSW=^*E?1_gYuD@s>iT5V!x!1hWs)LQEg=%Dzs9#a4N+PC%q`(4#e$%qQl!%>Ek1{`3eM^4)yZ16J75H`p0|YDrMLevva+ z;;e2t&RZpWGSHxDtq}?}NKd(i!-b#0Sen~}{vPHr-ia`O$Bcwm&`xo@B4@AZ_}TU} z!%g}&BVUD1&d*9Hj8_=uW?hgH#Q$dUn@W8p6O`(%_Einw=F9r1A9b%$#ZiP1)7P&_m#7^?I9GL|wj*LJc_smjWx?KhE}N z{SerQ39+sgl6ZM1|0tVLv4Fv?3bz>}sIF!V3XHfQELK%+T;@afexP9(z84*QYj_S8 zD}V-^$p9%yCyVb;;1f4}M`OS#!0Yfq?)5K=gm8Z}^}er2L=^&*SsWrE9Wt^5n`nmq zeqrG$UKni$V-~m~wy5?`oD!lg`3$&D8hhOy)jhxSqF-lniXUmhRV$d3BHpe2I($Ye zinNSGPP}EyJ|DaF?moRLKxN9WzRnux6zF+Xc&u7Wc-0o8txF1AZ2=YVtt3M4-^rly z*3C5FD2c3G%fq8+-9ok3EhQK8l*Pn!IhA)U#>h;qcaw7Rv{8;x8) zgmx23o;piG;ASRams?RUW$msEmYA9g4z1)EO^*GB3qz(~Y5B~C-jG`yi04!MTy$eP zuoN)yg#&6`lc^+V2@sRwaCRf`qIc`LhA`l6684KlrFA*!)s&(tYnaG!pw8j%Q~gbo zLJZq!&iS~;BCl3cw98MV?xkDD(6&z2~0FqsGVt0EpD05yjQ5mvB+c z6`a0Sx5OhM7+|Af{|$0FRQd5Vk*9MLmCgIPef`kpQGsn+{GDYx_X^?#Lf}I{UhHhg zir7>tc5H`ns5838Mqqm1yiY-;_p4Y1GyFQNS|GJhjQ42i_Fx=3`lPfh5r}4U892PF zu=oX~JSV3cwC!510SStuT1&2qpZ$^PH{PUF%)7#P=e)f5<-JZZM}Ag{PAZX8x5mv% zMpT8)=Iht+yUuu2NLjc{o2sqXF#TsxJ2yQNihE@S#_-b zM)#A-kGH$HjV=+Di8%KQgN^P#Z}022_I558G(8sf&bTz!00-f}JCleTcF0LNywk_| zS!jT`h!KUr_kKA8gFsh(hF`70l|C_8Fi8^3L!H5Z&TNf)=dDEu)Yk`(6i7PC8ap;} zs<0)o%1~QxJAM$k9OKL#QPSPkRbX0Hh)1yOAdsg_o?LSGCt_SZR_QJ^AHh0j!fW=H z_q|Z>MBFJDmx{;*sMv(QUQiUVO5P@>i%ea5VOJ`e5gLRAZ2g@HH6PK-b=St)@=>?I?v^X4;=Um*_7xmt^(@zP5 z^f9W8`*n;`=wCw&3ARsBrO_h2VaPdN*5c&V!Wm>9ahVWG!#Fe)@Dw>pD*T5)X)v`& z5CcWxMDfBZ6EhY2gh)yzi+)=A$eF6LRoNQtYQiUqg}JyUj+h0+UL|vrxEko>N!&GO ztjTf%F;O*<=D0rlUyX9xasTShZnr4T8<%HG$VW00Fxqk-OR~v(v^({`zD%Qm6c}JK z!MM^pvBUcfkK)1lpMYnhr`wSLV?Wc}*hS5?YHo|0z;GUB$HNB$U(;la%MYKaoPfCN z9YIyo+-HZ+NEYGe^r6s+jqV$)k+{5K!MSMq3kB{c6YUWrqr6R{DAynsjJTK`HbeQz zsSVJyIbyF`Imd@&kLY@kO=Lm~SNmfvS_7OuLuby|^ z$x);absFJ8cxscf^t#d6raw>CfXh#+-IoyE0dQn>Ou^Q4(bOsbzf4bQSxBQO`s*ZYd~#EseQaAW+cJ3g+&-2& zKOV9)5V++mGYbzuZLZ<>4#f4s0a=SpTo&@WA!lhCzGA0;J2ciYxa+Xs<@Sph;)00S zw779AEx=wvdg-jt=K(Uj-vONGWv&H@7u>IcQ%ny0Qf83rw+6iWMLDYxw#a(4IZ4^) zT@=*mh|RZZpY2mGKAZ7@1Y#);6W&VIDZbX!$mFkwcYeAj=7E=CTh`4uBY+G#c!GAx z99yD6Z-NJTN1rm56r-q^*!2=9ZI|M0hbYnrjQaWEBR0Lyh)$yaf_WA( zo7ngC9q2nXG}B>mNHNFV&0xp}c`=e3zD~p&g4(K;$-C7A$?lf7Xc4W4i%P>R-w#3-1IXW^oP(?`8__6(+$XQyHi=x7U=%yyLz((M;>-F@~{E`-m5Z&)( zOrFlwpe%km{}jJ@)%a)ifOmrwDnoL8ySytQX~->PGaXsPo1K~YSX^V?fzTw5R1M2XkI)5Iv9F6RJB4rz71P6{pIp!rnh{lWXW=zbN^3y zLNlWEJ#}=CI`i;?Y~#)oc8&2}e(v@yWEj7(lReVTIp~czp?U%}6Fk_=WZcZbCP?l~ zw5%Nd^HI`%$jJ+9xl4|cKEy_qPHjcA<+A-i%pkc|iB3GBVk?O;F1l9g8uPP1vafE| zh9!8fU$%+bOA{t|t<+31_%Ecx7xl8*4>8~g06MbI<-P~e&0X{Q5G^hLbP<+RRl{Z5 zG?YI=mZ#s7_ZpH`hPmaX08#Q}v)=mJ;^$j&3-Nj$>{)}9axvbK-t%JJ>|M+Bk*c6L zpiCbSj~O>c>qnFq&WL}TBsWSh1jOUawsCu@uEG3h2X`|dp3QHgbVQ3LA8+tUPPUj4 z9pJ76SSzT{f4f@2O5fCv@l)2KPw`Th3(+7eg{$Mr%SKaohAxSbS^}}T`KegqS9yQs zWqetW{V#|##m5Jhm}V$xHA7nGV2k0RPh5m(Sx5NklfT|?<`H1RP4U%X9 zSKCF7A0L?&j{=$$Cy%FNJ2QtB%=h3t{ElIQg1p$xe`7lSDDUOujMDwO%uVY7*{mn5 ze~xnBI}**bJ1BjA_D#Y2SvbJAysP$+N!^3$S`FDU0op1uoXPJ-T6`=enVnmAeUQ_& zI5}d(ElRsM80l)+v?tuPE&b&A9|sfma#odH3qS@1%q|302ZeKH~*A5eAZ7BMXJ zMmX_~cdz~VbfdFsTWaSbc?9<20_Jf8+>s=UrP$ODqIgU>mMkG+I#e37hbeZqFy=wNGdLgN;j!aS|DX>fP4*3!K59yLT^9kwK9AQ)l;TnlnKz^OXerrd<;f-BmJ{ZLiu1BG&$U;r?4*B3 z^0$4Lwx5v=WR_hC=SeNY_igje-DNis0a9#+gs8)?a#a}cLW}(=njb&h8^(-Erl zuvlc8A<8ji;)E3Brj+}3Vq0uuE7`cF=xgjqfjo^h#5!q0z63ij{R;N&O!r{dVOG{Ll6Sl?2GEw5iJH!_9Cte%B# zO6aJ8ITlY#L>Em{0IOGv0+#Bd*DupVuLEDWloBUbbW+9GPV00X zK;4TLMOy_suOXn6%+eBAai0)7sQ`omlDbI*Xz@|kEO=2Jv&xZl{(I)6Wd}#MYwE`u z1G9@2`1<)G#oUkhW$b^e_I`aY)d2_nYh3j;kvFHD%{=gCk=IF=w`&nT6)k>mqok=?+8N5*ZdPl{T(3v(y&ieh#vLrh zKTRzs_M`r`E3aK{A}nUOWc$9OpH@#a@(|ZGw)XxR)Rmt<^t!c#=$~>qU2|LFX|b{$ z0-H!8ui&=5OXrsyBj=ZWHfJDwwodJqb;O>lL|V7caSx9mvjHi%U>+R*4?&P$ zgUm$9D9ZPd$Gx(Am#Fu2I{sN0ZiK$y_67!i`xUI!+hwu9v1knrT&aP8_$G{gTJi{! zPSV1IGP8|HlQ&v2Me%+~R0gp`ILt(7xk5=E2CFpJp$kDBS2-RbJu=Blw;*bCB&DCS zp%ql=_&V*qT%14DN7&sI4Mt|T8tq$0a`9}pg@iAsgnKK4-wWvVmKp7Isqsb%)0=1) z1#Fl6n%B{oM24gN3Uz2OsnzgDx1vr0>(cJUUma^jXE5o7jiNnBE~=7YV1)&}-i(QV-55Lx+-yD>9Mq(MREdp@32i zM{pz@_EXTro>=kBgKVjGOIr@Nun>CRc=}TJYTvPFfib&0gi=+~NgP$mQ%sR2=BhuP zGzz{bg!UILnq(xYmQd!VQUs}_e6JcUXM!+w^{ZYQf;tYbbbC8jtU>)^iKI2;D#cBi z;BueKr@d?<^q0!?f$D#3KfaQEb$hPzq_ohW>!7egEGBJa!28t8nNv}#5+W{Tkl0Bq;E|(2QOHlO60q7(- zU-YBLbfd(j%YHc0V%75kBWIC~2-CCjVjv&_zIld3yb?*fcUEA@6-Fe5@TyXF_0W?= zVH8TDQAP0{BMYDLnne&FkGqyI!N`(<8J_{@2IF@ebO~ytQZF8sDs8Ve0VDF!{GLkz zOt8>z62@tqNt9%85`rxGt0?7MLd}^FO%{^#+Rz|iBDQz~Y98Z{`nAO^R}XUW z(30$>`)@*|2T$@&Z+DZ8>t^ilm4H|woWSlP52kLBY~mX+#K;_w_&qtTxJKgewVs~j z6gDP1{&S2RnQ4-flI_GG=Y37NywN7ol@5~Pj0oz3J6~WjYe{nYagTs(Vu&OiddYV= zhre=>9XK;vSxOHUf4V+H<6qm)AP$aBpM!b-l^zmw zpZcL1f|`}+T-lc*8geA2;bu%cg?Kl`8@wdn;eIzp!u1W!x0A}!zKxG#T-2+_K&X4= zc@^Tw+s*UDcXl6ir9jwxApyCIf@dfW?sGn4RvMzeaK@KWwTCu8abeqQlAP@5hZu87 zW^7+=m)C81{4#SQlyT81@l^|xDDgq8sN(BRo|uKFzE6X=Z(UuEpFuS=qx27NB|W{H zM`0uPEy8nKhrX*q@3V2li1$YmP%0ZO_?PgvlyWVCa%rxnlw@HMtg)Ql- z&BDE7rxh5RGz?e1%fhd`U5|H6i;p~18)hTHe4-TnbLfN$@gPcF8H&@jeRgc1IoPKT zE4U0nKi}RX`n$?T`j-T+4u1P_h27p`L0F zrUbmHzs!ZKO^E-#_5}v!VRYN53$K98X4qgo-L6`z7WlM>d2u}$<2JXq9s!m7gymeBaoVjre3K;f2?}HtUay$eW{&T zzL{OPnOtZ+lAT@r@&yo2p*Cp6-WLxamTG*AuskxbucB>fh_7U zs&p~vr7Oi*TU16Kne3n@0(cb7yF%BCq2Sx=qoNCcIZN`rgJGj)K!d(Nsm2ioKJ$wN zl!UepzYc$T=^)7U$NEK$tsCP&FDs|3fyA)q|_0KoXR3$tiW~=RJVXcXml8O$avbtr1VXy1A996 z@%oO;3;(-_vc@$wmq#;mb>zTel^7Z^gkRKR^K;N@VbBIp2I@y@g*NE(3w215c2?=q z?MC+Q0Z0-u%bdm3c#7|NB(Ttey5o0(Lh4+bzNx%mN>ki!h34B68`wjLD8-#?Z6wRDsL$RhNtD zV)sDEMPS{uDB)h{!2c~)n^?t-^90dPK5qNn=#=hA@VHSzlHmROKCjMmcPm)lz;m(o znt}+R-VA7MNh1Y&lSn^bYQfutfc@{1ixr1@Mf%U?|8M<0k9w6)I%Le0{(BQNMKtd` zbzCT5L79dlN^rJMEM~K`?(Ky_a`bei`sI^b_20w4gooC2aEmX?_In!pgGKxDOQ^gl zn;l}oL?5{)__@58t{v%8L-2BmNW-yU!%%|?ImsUXLR$CV8h8mM38k$qKRv+Xr>P@4 zJTh1>wg2WWlfcO&tn&SD2wv%5nK0q>ivJ|`zZv7=exZ*1@8}CSTaN_y-z@*T8L0n% zyZ9nm@^*lhr5q+3Q`d#K-Ep}_?xzriZm#^l>EdC8@kt1Ny0sQvL zs9^-0A0g`S`SX*S(l2u2D^%a@z|?-?z@tn;E{;d@aaHxay(Df4y?sdpfIviTK}vk# z_x!3(*?bgRG}eYQ+WcHz(f26wYdzPcG{7>XS*fIj8QjEDKplMh2{i=v`{!c-v5aJ_ zlAcXgNaQa~ykRDb8RT=_+QzJZPd-EpD9fKd(YZz38lCabNP$t2{7jv(OhL^ug&Hc2 z_+Ngm;Xb1)lSThLsmQjNDRnwTM@%C|?MPDeO&4on`eSLQFpWSaN=0cHyVf+V+sdhu zEHt%W8!#1bXWHG`^oHsBb$f=wcIn8LCo-sReHL1rXJ(Vpf;s$P?O0M~XfTIctP8k( zf;vLVgu{_HGTv_0E(v27^nz_zEwxbFTZ$=vf|~*9UJ?R$cZ8b%5Sh2Xltkjfik=LlOw33%CmULc@*y}&g8(lwIa-QyCf8ku97u6K;;r;m` zOvKlb9jdH;4xqll$Tv1m@QGBzG5?L*gE}^ zQW+8&vU_?+3nm}@uNCy&yd{cHOa}csBiCf{68SGwKlWe)w2#eOirppbb#39pi_7jl zk{8s+Nf*o!hIEzoX_-HwkY;)D1G$f@IU#~-OErK>58>hi`G*Q04=Hl*$D2E|&8Tuv z|Gyy-r_()$^6I6zq+(zyU3`Y{*wv~_Ob(w|sc~?N()$!vYWG`WlYX=-zcGrEI-gH& zq)V+j;TYO7w(jeh=|zBZ#)~(`VziWYpk}G-gOJ7Nd2Klm=NY)y`(+y^B#@E}-~f7F zlo5QoL*&NJqEz)L$)YrFUBR$S|CnS+7Yu;ZPeTotym!NYtZ8_&i&b+98$ZSX%%5kJ z=B;fFZH;A|(7bn1pVv&bt#8f42e-V$r}X#^PS&2r3`-;{XOkK^p*YR^RknSw83$x| zAIK-lXr;Qou539`lUpxY3AgsG9dO0e$=+|a$Q`m#X_F3c&xM-15?iqBlK?|((=;@(h zmZ*izUC|(~om$c+x&F&yRR7_aBncmnKR33gOkCvsKxpcaOKCJl9t2zc%A?!v_-m2C z1JHtTHwS&dafs{licFt`Wo6fJJ#^`HwW@7f)2O=1UQP*HCi$@6LHKz;jmyUeyzZ}G z-Mj9`)}<2u_y?6L{2bw9v_aqrHNLj zH#N8AtX_xTjnD6n)wW3-WRr8rnHr|APQemvhJQ)#-O&fuP! zIM+LhT{l6+xU6`2FjhhVaF}ns$9nvH&Rfwzu0xPQ(l68qdb+V$x@5!{{LEc#pcwqt zvwI=(rhW1DY;&?s>{N`ZmAXjqc)gA`^7)U^eWOp`$n11jQ_Df5*QN=cA~Jzt2W66M zg5$|h);Xe`;O~fN&`lPnz0vLt$!EgQD};A*;JF?gdPu?$a+Jm?(=H{sb;~hh_1upY zN=ga$KUDhTMFe^tS4i*#>#oh#gOR^!NUVoK`VWD_(OQ~ zKXLWiofRO1q={;0#S4-`(8T9w3BTg`g!qTBa;yvyX5L)!w7@~m+?r>L!$!vSG=(l- zq1vOBrOV(5=RE6I`M=26f9{am`DKb)sE&k>p{(JuOSS(3` zU7ZoeOuyLa!g2Fm{<9%xs?{+YOFoVYw{E?ON@;tUV_8!-8U0B(yO^9 z^#_;AYd~B>9mp_OsdZh00!>7x;Zj`xS6t=cAWE^?81-!8K4;h6VfuA#7WU6cCcR31 z=y5WeOj)Iee+tJZcWY^kMn+OG*=Bd|x5*5NQ|Askt)4kr$z|RjG%@Ym_*CfyO zsy8g3qiX@4%+JfyZ2s=B1Ns*C(JVSXE21Pq!!wa&K|b26o^`R@53uSa-BM1)fmd0a zmxd-g(GyR@v?*Zv8l9+>r!yDs3+7#z%~dz_Br81Uktx=Xk5?q6wE18Abl*WLD=Ya; z?b5xtC3_BPL~K`S4h=Vxn*Ul%Vs>=pL#e@sj#C6a?_^R_$vk$ULv0mbKZuq-Z&+wM zb#|<(8l+b!7jSuq6<%n!g*750dk-gA$=l z5P5)=;GodagUpN-a!>SaiYSHbjQz^!_W)+tBc8K=G1i=MwLA3s&8s*$fYbfcM+C7O@FUI`F~cqt#_P zq=VT#?wk^RMql@Y>LH~2W+|=q3NgaOVv@t{z;gn?w8Dz36G-%{lWSO`=ik< z1RmSD_JGh5hL$Vwtijv(%t+PW#u2dRvDo#TlUMp>W1~zuR|v&JPc;V^tgn9&)e^zc5~84M&Ro=YAs(wX6>xbYfwj1+IZ-8xdso# z;p3Wem_@U#4fHrZdh}TC)Wiu1$vBPn=k*LMg~rJ>5@w9yWjB~-kL@AlZ1#VniUy}} zZ1_WG_cXQz748?1>}NOGDoD6Gd!doy1HV-BDj&S}?8SbL)Y74tqtc7vVoTQq9_UFb z=yQ_%u)MxiS#9;oOFdNc<|%MIu^Y$0s)MG#Ibz(}@4FN!&G92LS{t3mJ|%hQsd@j< z4?0=b8JT7$d~U>Hx&2G>$&SEQGl8m%i;6wbNgMuVp}A*l6&>z0mJ^I_LJrt z^JEe)RsAB;&bY){?3dTNeZ)X=d;7ZQ`@B}4Y9)VAusuWeTFb`sd4R=FRke5Xu(t@g zy|u%Q=Xa|9G0mzm_zihtT_f|o%DA?Vo_EBZqiNJ44Bq<5r4$w~&mGuUEzvpqxz!DF zRC8(n)xHd2DL`igKxk~Z!w2d(H~N==PwMgKRi5X0EMfI2NrCmLU~8B1`4&{OgU3p{ zYsTaO%EPsh^G(3Vx0qNN+qdh_eef6~OS{MXez+qh_X$4bHbBAs1p4_mscqFR(*f>B zR=~({&nJ~B2h5oXtRE`nqY2sZL-4h_;^+J4U}RpW@{+u$Z1J%P*3D6&QdxKZzm!_9 zr=ewA76%Nxy{9(1@!Fhxoyk(_qbct7G?Bdr9EEfb0a^X{XnW^Nl8ZQ6-+WiVy3a@`e_-T^EkFF%zHGN)VqWSw}*78c5o*P z4!&dO+aI~-?!DFB9uk6_% z=d(jLSt@q%hb1D0(Slo{+xSc)#Z!%l9){eGMm^wD#QE)w^W-A04cpG;JzDbw@z!>0 z35|ytz z?#Ipa#_N*{_c~SBwpaLgM6uf`LgjLgSvR|~eKGU{eI<_Y&z(vF&lhM4hi(bBc{96r zY!J@fhV(a&toC3oPUYtn%G>QX1_B)6-BhiC3ZtJpFXY!Vd{HBD2}I23Y3vckNo%nT zRoh0UcTc=8OX%x7HTBPqh#vI8nv8qNh)6k~c=g`dOT!53+RRtRAH>~%ekBe4;ghYl z(c+inoA+(mv@9TyRg#FGdI()WB_~_aDFD?;%Z3%GJFvNb5Mpo`b6(b89ZOkHOmJdr zUHj2${Z0o`^00;EG|k%KSb8SD-65?O)acp=;vk9_n^gj=+J9LbSxI|L&vGVYh9z@r z0iqHf{Sn4#K8`R_iEHMz?9oHnXj3vREB7$VMnEiEX6F-ety!1G9v7gSdjrT*Hv|s# zeR=ASIdg2`7t4zPudiV$?AI57GtZ=^Xt2+GP?kQBv>#oz11^#JyuoAe-~lOj4vek> z24CFtT6`1Br@!rf$F7Jl_DT6Sjt`+5WeH5+BCs|1NR3s%(Mk6UKDm~w=U{#~VjK3q zQP@~zT4Q+B?RlcR@3^HLX7r{GHwVa0_R=e{-YvaCSEJs~;v=>!0cw`v28s!U@zA-N ztypSjQ+1=Ha;#iJ*Ca?9Jdq6CuCy=euN^af%#}8D%e(NA>z|^Iu+d+`H!lO2nT+ig z;0K_Kbf^M$DTgX*S9Q*Mui&MH-?%6m_L7xz$-ypv$#U0WVEHQLmGM#RvLi?0(F8bl z?H(Op%wzSo2KowrYGAldgCt$6QPh?b+9@x>jOZ%J+#*hGT(NPEpkaqFEb)Xsg)wMc zov+y^@_gmbr|9pWVfcxp0FCm@mNQNHP z#11UrXlrC+M%%FgoAwtUr_}R?PfTv+FjK6mS4W_bMrd`oEwi_zb8Vm8L^2(i&BUdr zTc+}Xh0);2l7E>g2q?X?p_1{wIE{zTLaN15?|)37WzU!oBq_UFBZ!$kwtb$cGHmTX zY1vj2azScn>>G|H;LO5r%lnK;`n4WKzxiJQ@9`Bn9VX`K_LjwS|FUhhHBw%N#Zz^Q zQ!q@>x>23~Gya5;k;RE(!u|yl*i%H)E&B5@x$W|T)9{q$cCuVkw~2QAaaJ(n*1}GB zc7j`iVZPCZvA<7$y^E}M$=KlcNj9mMq*J@AnD?`pUuI>)cyB@km3gBVFLpHdGceLa zS)Dfhs6C9EbK)pmq7^3G?3P-;GCD#yC#VrazBps3`VX66+0n)$fCYEoLfLc_KTARB z%{$3TmbCcS!f}|6yPS&kF{Nx0DYP^oc2>ILZJw^iLv(kKY5V83(FeoMIr;Ss z(^8u6S;aCgzi_Bc^}H3AtSfRABKYo`}g+&l`7y3sazB1#oxIoWBp&4=-dScT-N(JyL;O*7zgVl~$oY+S98@2aV|SquU-lO8sy z#Cw8`)(gAp#c0+_8+2vB1a0c`$%a`aGm&&kD0|qc@V^E;jp50@)4OSmEB5pae!Z-7 zJH=l9ykklL<73Y)8J?GR@{34e#7TI0MVvgLe&;s$9uDe0yisk9vUY8u!oE>+XOwM) zu@+XOFmg70n*<%+d8`~G3)2CUXFisrICjOT;dwj=q$tOXhCjQzN29PY!Vz3<7&$~f zu^LFS#kce&hv2R7GXbuTf8B~L3Xk#0;pKMIdF9++kqJjzf>*Y2P{l>(`t;j|-b5Tg z*BQa2(ydqsmtHFaNg9^nGsuV(m323|Nb90ees+p*A@)fbLX$_Ym-R2_%NuX|?B|~8 zvpw%v7$n^ifX%x6eU2iKNn`%G*GO35BO+p7YKYzJ))e%QtSS%f^+T!Iq^8&E1!LngWFa-3*_b z0&{1Le|@>O&fNpi^o6N3;GIV?J7=JIzK*7#3?)vBEF6$kaT)5}=pPFBi9K#64{vKR z(}3*`3Id(yS1(5HMNrN@<}L9gT-Y^2^^5AEv;)Z8h|E`TpUJff0*{M}i6rDL#7Af` z>9TpH?pN5@W3ekG%#rM3S8OmsA>hN4l`$cc<9Zs$`!9KP7qLKj#xdE(yJ=wM?c!KD z4fsg~{4P$)gaJ5$!y{9Inp{Amq*Rqfr#jxRu&na=a{#0_w1QM#7KhBJh!?n~#;~+- zVyBjuI|J<@O%{utb)X6`KgWraa$;2e7@|9fCkM0ctVl06u-TAFWvx6$z6DZ7MXV#Q zF+|I5Q^PB=kEkT!Wl$t}eBYr5k&v4~47jnfbMvEwo#tnN8<i4U--> z!h^{$u|UxgOxN|vVd4M4ETN8tBq6Lnph?O}WaAn$cSh^IM@I1265mE2iczbYOW3d9Fw2Q5 zCy5Tbxo8&?C6UM~Eyoo5HQD(x!dH~<1x5>aa`sRX3|SnhN?hoDK}4S*~0rQ6qFV?=!RFal4o|S?V+36PkIAgrX2e{=r+Z9o9sl;y5)8@4podza`3>!k^ z0#XRI`@~B+;^q83uNv5HwH`h&Dk)8L>)W=EN(z;?W}zxya4gm zF#fq7fjK5ygD%p9WYkteLI=j*Cgk}JX8&Vlg6#)`SMc+*!75yr~YeZ zmR$;*_^4L7P#v}3psy#WrSI^=!i0Bn$y%C2ueK9Gj*X_AeL%P$!g|Xf5BphaW_BVw zF+`kO{5E{b+bpYXo*?1=fZJx0+Pg5%jS)Ji8X*sv*h_Kp1i;<;xhf$!n1jsS{T%Z( zpuIOW)8Zm0Nh4g3a-v12V|(L1{D@!0-sm~|mBerS#ajGgV>xE>&v;zcCsIEh?cv&NuLkfiP84gfAc<4?n_l>Y{3UmIP`n}{F^D{-x{aS zskZBnQnmzRh!#oE1RUDE&i6zdKEDlSESf*v6IChL;@eVBD@iG3ZBpu8W<@C=W308-#v91^de;X*-mpd2)fJ<>$^k%j(+PXxBB0MIF%Nk>H<5P7%An@R1^(ddpah zlDZJzL|3hd>1`HQ@=r2)hwuE{vHGLr8WAlY@dqNGh#sGJSjav7gR3qTUZ^BJXcUvK z#_O|%elG)G$Yz|eZpyYy=EmmR-Es(wwoUJMrMRb*Ig&}pdALLT5AgpBlbnya?#|Pj z6tq|MY>HBp6Nw0h>$!H7bF6$~rz~Rg=M9b*xUFhyclhuWdEC$m;x7@QOTHj)GRG6; zmALM5IcqtXFKcSci<7gEKJa_qYn{O1g%27c_NWfntl_4HekY#nNYi=Qg|tzC2N$6x z-DieOET@0&yT~V*pY5S&IYn$ZqH#EmDiWIB1?Q=b4mKWM&Q=9;u?ML)Ix0{x>KART zdJ7AK6HjcF zNxnOGdpPN$VJA-ri?j!zHeW5Jme(?3aAR9BJD@(XDd&x3#Iti<{!TAn>|NI>YXFeB zP-g-z2L;P^iG8R@A79J}n_)Y)LnYg#>PM7042h`qyhYFBJajs)U~C*F+4O!V5|NB> zU(B_;WGpI_jY^_4Ij}S|=}7Rp?9&W8ao~^J%mAw9*H)-)zfu-RudEbyHA{~`3nbaM zA^%SRs}EH0l(2pPq-{`wW}sOnx@GR_nkErRdz;$bn{3>>PkrGbOW)j~xxPi{-(m63 z|BTvxz|iRv6h<8U+ZTB7$ro%lOe!D#hMCu0);{_jOZUF!{>Rglr;3aYO;A34it<S-Q2Xj*Zn|x9<_0Twmg)EjS>nTY zdKKWU++*pJk65_23`1{l>~G$~t$)M4kG^K(%RlkpwPR$8?@+2{Fw0{Uj-Mc({Syx_ z-6PD;G5qEX#i?QJtO5QS?R%Ga@UNe+eElI&$)vq-orQmCvEf*l>L^8`6bgZsEt8ve7<>CP3s?V{t#2-{@a@|aelkq?%o$3T?y+*?FWmh9HCQRc zwC-G|={pqPKg-a`N%Ulcjf-Eh_}N7k|MF+n?yuw3o2-2NTa4Bsjq`6Y@zZxHR`cjb zG`!&DL^>cVBtq|!i`f%vQb35ew!qf4>ufEoQNMM8mD`JWwL06^FLM9)8o}a8ipP(W zwd$;V{wWVH-X<=+#@zqy=eV7_EPVVa8=rs7!{gKBD(7*><`{nSHP+T{QonYQ`+tO- z6VSYOA73A3$R49IS0z6?M{%OW+L!m3|Ko2_o0F*JD{QXRiIVD0`JWDn&@2qcLDwSM ztM^#B_!(ZV&Gu$HbuL=D!}{&}RI3Te^$R@spj4*Y@E?!R8OC# zSc+)ezRA|Td1~{wSh}%5dwY}c-en&C%Rds{8KW|4(QZXMB-sDcQ@%=60z$*cmBoiMW-DLIZ1s1YFoH>RqI@G?pz|zHw%ztr_l{+hRwsh7%`#o2!HtT2JW#)so zsZ0-{zwm7KC|=+qYiMQ$yD~&3DPw1?w4=kWwV@XuawFuY3OMCFrl~z)4=jZc zqyp_L^K9K+CU(XcdiQ4>``Zt3>Se->4esCCVEfiB8gHKxGe-om# z9EI5v6skE)9aN`7*x15rga|u>smFK^uClOp8;OoHdxj)8LUC&3sRS&#d*?>3L}qw| zq?ARsEX;C=>O1c;`t@ysyA7K2i+DddLFwF2nOj~b{?~75KDb6lDbQVtZ=YrEmme~6 zVgxN(q;cmm3xD~X)tk#i%E59Z!TeQLLLX^nD7^V5xylROKwm9L4Lw`H9U6gL0mIaG z9h-JqP()B)W#iimEL^@vePw}WGr`Q5#2XJ-`=T4_B5V?OovnL|_!OwV{|jb*@k?^i z6r!;~=ZhukckWVOJZ2}Jr*F{g7G_GC5yQ_ijQg|akFOTBrKHJy7;o)bOsBLV}+=|d_ z6I!cmT=;}=ahmx2X_U-jmn%@nU}Ph#VhPuCFkJ^dSHvlKP{?5qO;CCBZHDhGvAnRs z*3FwF)huRpl0w`hQ^=z05)f#1fx^i*7rf&9nf%2O_!#VjbQ*WOnUlJG}{t_=R*;o^3Dn?-0p?%)F`4X2P#6Il3d{CbFrlE*Y%0h&gP zgFF2uQ$IIRTy!T-{?xn7{F`m`%S&h#2VuBmUpvp7VPcKUuzGVL)q)KfQeD#dr2B}PWns7u?$9K5d7MhDK-ZC)foYjYkq}1;b7+>)56>|4vkw`Xoy5T*`c<9C z`j#+fqrwoGpP>4|Z@|it`}!)Ii<>0U!Wlcx@VR#xJ3B`v?;_mSnE2Iyhq4MR-`zwM zCn*eNptVVTGp2NG7*kIZ1q5BKXR-e_NeM(qm(trm<9MKBe|3jI7Ad}dmSR;STDr!{ zy=~0VB4#FwF@Bt}pZpEZu_j6y-923enwutyv2}zNAx#tAcCpI_X1+oxR$T!T2h+9D zoE%o}1f$A9RA(quV{Atfj~SGv#?YKMAwL1LQ)I`c&@;axj&J6I9-Qmx*`Zq&%8Ov;vGmmF(m!RaxDm(M_g8cBF*i z7)N%j5K0{)R^~@WY6=MnTQypXtN4+QRT?5&E%c?yN=+n5(860^OZ8a&m_35!0Gf$a zDr30@Npl;&14gk*X1I)@L(<-+wYrAaj?jz4WU56>OQzbh8*8+;n}nf4voqM`DsCx{ z(bb<-frq!bMr)%+6l>_&EV@jcvXVri=`pS41)6KAOdTB1=`{T@&rGq;>jGeKOag*&!f8z50_y5C{PuI!*|NH;N?CDW#GgbWx z>YFr{*9qGG?%qztF+d)3H>N_(CIyQ zX<`;DWXDFZ+*FQo5R&x#(Axu4p^vw@n(CEnS=`YP?5sl?a@zIsB_?cc(_UVoQ)>}- zHOPB`38@cP7OPxF7d~OVNhBwR)gjzc4vmnown=+q6R*)G zK}XLOaVr&^d=^blvynyZ8l82+g&K8(TCV&eLxBBnJ_&0?jRw zot~pOUPMN1IvX|OM8_;vuv{J0*``zPfK$ZDIf$@9XJaeP{6kufRQpH6$W?HMtJqeI zx3)@aqt;g^y`%~PVcIz55}F2arvp|V%j#-dhY~BFhaiogCsQF)%AqA4yp1(lTdj1$ zozEgwK-B7hTgI)Fak3^o=Tm=clh*nse#=J*4Lw)FtyXbz7OJ(CrX!4Xtm+7E!9~Vx zI_v9n>MgKL;>I?u(D7rIQ36-r6FqjZK11+&5MvAdS=~WpoHT=XU+uj@DT5 z;u7%gf1vsU3H!a?_MG&DyA*rA?lB1{4{*2LH`{yM!`-ygB&HT7F(aRRr-x*_6)S!0zHtW}J z(`tAmJBbtePZemH61lmPR8LHiDY&WX-u~IQmYnBqwjccRrU(H#2>wopyz=X(gc0f@z++Vc#P>PNkW3nW$N=wv}yr*X_(x^ zIN4%0RiHX_#+UD|wmeX#8sMnmuj-HD&`AtmK0JYJ4+rtGE?mDi3<&rrV23oj8Aall zBeSaefwW^{L9a%BK#Zf|X6EpuaXWo2%2Xm51y S1HV210000 -
input pointcloud (PointXYZIRADT)
input pointcloud (Poi...
split  points by return_type
split  points by r...
ring outlier filter
ring outlier filter
weak points
weak points
normal points
normal points
ring outlier filter
ring outlier filter
combine
combine
good points
good points
create histogram
create histogram
noise points
noise points
diagnostics
diagnostics
create binary histogram
create binary hist...
estimate visibility
estimate visibility
output pointcloud (PointXYZIRADT)
output pointcloud...Viewer does not support full SVG 1.1 \ No newline at end of file +
input pointcloud (PointXYZIRADT)
input pointcloud (Poi...
split  points by return_type
split  points by r...
ring outlier filter
ring outlier filter
weak points
weak points
normal points
normal points
ring outlier filter
ring outlier filter
combine
combine
good points
good points
create histogram
create histogram
noise points
noise points
diagnostics
diagnostics
create binary histogram
create binary hist...
estimate visibility
estimate visibility
output pointcloud (PointXYZIRADT)
output pointcloud...
set ROI
set ROIText is not SVG - cannot display \ No newline at end of file diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp index 6e46123cfba4e..1aa4acc63b93a 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp @@ -30,6 +30,8 @@ #include #include +#include +#include #include namespace pointcloud_preprocessor @@ -48,6 +50,12 @@ enum ReturnType : uint8_t { DUAL_ONLY, }; +std::unordered_map roi_mode_map_ = { + {"No_ROI", 0}, + {"Fixed_xyz_ROI", 1}, + {"Fixed_azimuth_ROI", 2}, +}; + class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter { protected: @@ -73,6 +81,17 @@ class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter double visibility_threshold_; int vertical_bins_; float max_azimuth_diff_; + std::string roi_mode_; + float x_max_; + float x_min_; + float y_max_; + float y_min_; + float z_max_; + float z_min_; + + float min_azimuth_deg_; + float max_azimuth_deg_; + float max_distance_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp index 7858322713063..e6e435c7e7ca8 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp @@ -34,6 +34,15 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( { // set initial parameters { + x_max_ = static_cast(declare_parameter("x_max", 18.0)); + x_min_ = static_cast(declare_parameter("x_min", -12.0)); + y_max_ = static_cast(declare_parameter("y_max", 2.0)); + y_min_ = static_cast(declare_parameter("y_min", -2.0)); + z_max_ = static_cast(declare_parameter("z_max", 10.0)); + z_min_ = static_cast(declare_parameter("z_min", 0.0)); + min_azimuth_deg_ = static_cast(declare_parameter("min_azimuth_deg", 135.0)); + max_azimuth_deg_ = static_cast(declare_parameter("max_azimuth_deg", 225.0)); + max_distance_ = static_cast(declare_parameter("max_distance", 12.0)); vertical_bins_ = static_cast(declare_parameter("vertical_bins", 128)); max_azimuth_diff_ = static_cast(declare_parameter("max_azimuth_diff", 50.0)); weak_first_distance_ratio_ = @@ -42,8 +51,9 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( static_cast(declare_parameter("general_distance_ratio", 1.03)); weak_first_local_noise_threshold_ = - static_cast(declare_parameter("weak_first_local_noise_threshold", 10)); + static_cast(declare_parameter("weak_first_local_noise_threshold", 2)); visibility_threshold_ = static_cast(declare_parameter("visibility_threshold", 0.5)); + roi_mode_ = static_cast(declare_parameter("roi_mode", "Fixed_xyz_ROI")); } updater_.setHardwareID("dual_return_outlier_filter"); updater_.add( @@ -93,6 +103,23 @@ void DualReturnOutlierFilterComponent::filter( uint32_t vertical_bins = vertical_bins_; uint32_t horizontal_bins = 36; + float max_azimuth = 36000.0f; + float min_azimuth = 0.0f; + switch (roi_mode_map_[roi_mode_]) { + case 2: { + max_azimuth = max_azimuth_deg_ * 100.0; + min_azimuth = min_azimuth_deg_ * 100.0; + break; + } + + default: { + max_azimuth = 36000.0f; + min_azimuth = 0.0f; + break; + } + } + + uint32_t horizontal_res = static_cast((max_azimuth - min_azimuth) / horizontal_bins); pcl::PointCloud::Ptr pcl_output( new pcl::PointCloud); @@ -124,7 +151,7 @@ void DualReturnOutlierFilterComponent::filter( if (weak_first_single_ring.points.size() < 2) { continue; } - std::vector deleted_azimuths; + std::vector deleted_azimuths; std::vector deleted_distances; pcl::PointCloud temp_segment; @@ -146,9 +173,35 @@ void DualReturnOutlierFilterComponent::filter( // Analyse segment points here } else { // Log the deleted azimuth and its distance for analysis - deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth); - deleted_distances.push_back(iter->distance); - noise_output->points.push_back(*iter); + switch (roi_mode_map_[roi_mode_]) { + case 1: // base_link xyz-ROI + { + if ( + iter->x > x_min_ && iter->x < x_max_ && iter->y > y_min_ && iter->y < y_max_ && + iter->z > z_min_ && iter->z < z_max_) { + deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth); + deleted_distances.push_back(iter->distance); + noise_output->points.push_back(*iter); + } + break; + } + case 2: { + if ( + iter->azimuth > min_azimuth && iter->azimuth < max_azimuth && + iter->distance < max_distance_) { + deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth); + noise_output->points.push_back(*iter); + deleted_distances.push_back(iter->distance); + } + break; + } + default: { + deleted_azimuths.push_back(iter->azimuth < 0.f ? 0.f : iter->azimuth); + deleted_distances.push_back(iter->distance); + noise_output->points.push_back(*iter); + break; + } + } } } // Analyse last segment points here @@ -160,27 +213,54 @@ void DualReturnOutlierFilterComponent::filter( continue; } while ((uint)deleted_azimuths[current_deleted_index] < - ((i + 1) * (36000 / horizontal_bins)) && + ((i + static_cast(min_azimuth / horizontal_res) + 1) * horizontal_res) && current_deleted_index < (deleted_azimuths.size() - 1)) { noise_frequency[i] = noise_frequency[i] + 1; current_deleted_index++; } - if (temp_segment.points.size() == 0) { - continue; - } - while ((temp_segment.points[current_temp_segment_index].azimuth < 0.f - ? 0.f - : temp_segment.points[current_temp_segment_index].azimuth) < - ((i + 1) * (36000 / horizontal_bins)) && - current_temp_segment_index < (temp_segment.points.size() - 1)) { - if (noise_frequency[i] < weak_first_local_noise_threshold_) { - pcl_output->points.push_back(temp_segment.points[current_temp_segment_index]); - } else { - noise_frequency[i] = noise_frequency[i] + 1; - noise_output->points.push_back(temp_segment.points[current_temp_segment_index]); + if (temp_segment.points.size() > 0) { + while ((temp_segment.points[current_temp_segment_index].azimuth < 0.f + ? 0.f + : temp_segment.points[current_temp_segment_index].azimuth) < + ((i + 1 + static_cast(min_azimuth / horizontal_res)) * horizontal_res) && + current_temp_segment_index < (temp_segment.points.size() - 1)) { + if (noise_frequency[i] < weak_first_local_noise_threshold_) { + pcl_output->points.push_back(temp_segment.points[current_temp_segment_index]); + } else { + switch (roi_mode_map_[roi_mode_]) { + case 1: { + if ( + temp_segment.points[current_temp_segment_index].x < x_max_ && + temp_segment.points[current_temp_segment_index].x > x_min_ && + temp_segment.points[current_temp_segment_index].y > y_max_ && + temp_segment.points[current_temp_segment_index].y < y_min_ && + temp_segment.points[current_temp_segment_index].z < z_max_ && + temp_segment.points[current_temp_segment_index].z > z_min_) { + noise_frequency[i] = noise_frequency[i] + 1; + noise_output->points.push_back(temp_segment.points[current_temp_segment_index]); + } + break; + } + case 2: { + if ( + temp_segment.points[current_temp_segment_index].azimuth < max_azimuth && + temp_segment.points[current_temp_segment_index].azimuth > min_azimuth && + temp_segment.points[current_temp_segment_index].distance < max_distance_) { + noise_frequency[i] = noise_frequency[i] + 1; + noise_output->points.push_back(temp_segment.points[current_temp_segment_index]); + } + break; + } + default: { + noise_frequency[i] = noise_frequency[i] + 1; + noise_output->points.push_back(temp_segment.points[current_temp_segment_index]); + break; + } + } + } + current_temp_segment_index++; + frequency_image.at(ring_id, i) = noise_frequency[i]; } - current_temp_segment_index++; - frequency_image.at(ring_id, i) = noise_frequency[i]; } } } @@ -217,6 +297,7 @@ void DualReturnOutlierFilterComponent::filter( pcl_output->points.push_back(tmp_p); } } + // Threshold for diagnostics (tunable) cv::Mat binary_image; cv::inRange(frequency_image, weak_first_local_noise_threshold_, 255, binary_image); @@ -230,7 +311,7 @@ void DualReturnOutlierFilterComponent::filter( cv::applyColorMap(frequency_image * 4, frequency_image_colorized, cv::COLORMAP_JET); sensor_msgs::msg::Image::SharedPtr frequency_image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frequency_image_colorized).toImageMsg(); - + frequency_image_msg->header = input->header; // Publish histogram image image_pub_.publish(frequency_image_msg); tier4_debug_msgs::msg::Float32Stamped visibility_msg; From 8142ef113592f6dcea717753bd6f74e0ef8a87de Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Thu, 3 Mar 2022 04:06:09 +0000 Subject: [PATCH 21/25] chore: sync files (#470) * chore: sync files Signed-off-by: GitHub * update build_depends.repos Signed-off-by: Kenji Miyake * sync sync-files.yaml Signed-off-by: Kenji Miyake * update files Signed-off-by: Kenji Miyake Co-authored-by: kenji-miyake Co-authored-by: Kenji Miyake --- .github/sync-files.yaml | 1 + .github/workflows/build-and-test-differential.yaml | 2 +- .github/workflows/build-and-test-self-hosted.yaml | 2 +- .github/workflows/build-and-test.yaml | 2 +- .github/workflows/delete-closed-pr-docs.yaml | 4 ++-- .github/workflows/deploy-docs.yaml | 5 ++--- .github/workflows/sync-files.yaml | 4 ++-- build_depends.repos | 4 ++-- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index f38ec151216a4..614496c01d6cc 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -27,6 +27,7 @@ - source: .github/workflows/build-and-test-self-hosted.yaml - source: .github/workflows/check-build-depends.yaml - source: .github/workflows/clang-tidy-pr-comments.yaml + - source: .github/workflows/sync-files.yaml - repository: autowarefoundation/autoware-documentation files: diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 4be5ce9620a6b..7e3d186886b6b 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -71,5 +71,5 @@ jobs: with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/tier4/proposal/.clang-tidy + clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy build-depends-repos: build_depends.repos diff --git a/.github/workflows/build-and-test-self-hosted.yaml b/.github/workflows/build-and-test-self-hosted.yaml index 5ae9846739dcb..9f31a07199783 100644 --- a/.github/workflows/build-and-test-self-hosted.yaml +++ b/.github/workflows/build-and-test-self-hosted.yaml @@ -2,7 +2,7 @@ name: build-and-test-self-hosted on: schedule: - - cron: 0 19 * * * # run at 4 AM JST + - cron: 0 0 * * * workflow_dispatch: jobs: diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 61ec72be49316..125d209ac1603 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -3,7 +3,7 @@ name: build-and-test on: push: schedule: - - cron: 0 19 * * * # run at 4 AM JST + - cron: 0 0 * * * workflow_dispatch: jobs: diff --git a/.github/workflows/delete-closed-pr-docs.yaml b/.github/workflows/delete-closed-pr-docs.yaml index abc983e3e368d..b7b009fb00263 100644 --- a/.github/workflows/delete-closed-pr-docs.yaml +++ b/.github/workflows/delete-closed-pr-docs.yaml @@ -2,7 +2,7 @@ name: delete-closed-pr-docs on: schedule: - - cron: 0 19 * * * # run at 4 AM JST + - cron: 0 0 * * * workflow_dispatch: jobs: @@ -15,6 +15,6 @@ jobs: fetch-depth: 0 - name: Delete closed PR docs - uses: autowarefoundation/autoware-github-actions/delete-closed-pr-docs@tier4/proposal + uses: autowarefoundation/autoware-github-actions/delete-closed-pr-docs@v1 with: token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index 59b116e7731f8..7f0f72d94a799 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -4,7 +4,6 @@ on: push: branches: - main - - tier4/proposal paths: - mkdocs.yaml - "**/*.md" @@ -20,7 +19,7 @@ on: jobs: prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@tier4/proposal + uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 with: label: documentation @@ -36,6 +35,6 @@ jobs: ref: ${{ github.event.pull_request.head.sha }} - name: Deploy docs - uses: autowarefoundation/autoware-github-actions/deploy-docs@tier4/proposal + uses: autowarefoundation/autoware-github-actions/deploy-docs@v1 with: token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/sync-files.yaml b/.github/workflows/sync-files.yaml index 0c5b4b04fa084..32f4613f6de7a 100644 --- a/.github/workflows/sync-files.yaml +++ b/.github/workflows/sync-files.yaml @@ -2,7 +2,7 @@ name: sync-files on: schedule: - - cron: 0 19 * * * # run at 4 AM JST + - cron: 0 0 * * * workflow_dispatch: jobs: @@ -17,6 +17,6 @@ jobs: private_key: ${{ secrets.PRIVATE_KEY }} - name: Run sync-files - uses: autowarefoundation/autoware-github-actions/sync-files@tier4/proposal + uses: autowarefoundation/autoware-github-actions/sync-files@v1 with: token: ${{ steps.generate-token.outputs.token }} diff --git a/build_depends.repos b/build_depends.repos index f7785c24cfeae..fa60d2107015e 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -7,11 +7,11 @@ repositories: core/common: type: git url: https://github.com/autowarefoundation/autoware_common.git - version: tier4/proposal + version: main core/autoware: type: git url: https://github.com/autowarefoundation/autoware.core.git - version: tier4/proposal + version: main # universe universe/tier4_autoware_msgs: type: git From a3c200e40edfe5b963fb972ada7298117ac94817 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 3 Mar 2022 19:08:46 +0900 Subject: [PATCH 22/25] fix(behavior_path_planner): fix reservation of predicted path (#474) Signed-off-by: Fumiya Watanabe --- planning/behavior_path_planner/src/utilities.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index bdaa5adae8bb0..10ce43e18e6d5 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -352,7 +352,7 @@ PredictedPath convertToPredictedPath( { PredictedPath predicted_path{}; predicted_path.time_step = rclcpp::Duration::from_seconds(resolution); - predicted_path.path.reserve(path.points.size()); + predicted_path.path.reserve(std::min(path.points.size(), static_cast(100))); if (path.points.empty()) { return predicted_path; } From 14afa557ef56484050b2bb0568a0b4219c9f5692 Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Thu, 3 Mar 2022 20:02:00 +0900 Subject: [PATCH 23/25] fix(dual_return_outlier_filter): change visibility diag initial value (#471) * Change namespace of debug topics Signed-off-by: Shunsuke Miura * Update initial value of lidar visibility, separate err/warn threshold Signed-off-by: Shunsuke Miura * Fix compile error Signed-off-by: Shunsuke Miura --- .../docs/dual-return-outlier-filter.md | 3 +- .../dual_return_outlier_filter_nodelet.hpp | 5 ++-- .../dual_return_outlier_filter_nodelet.cpp | 29 ++++++++++++++----- 3 files changed, 27 insertions(+), 10 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md index dca02739797bd..9cf07746b075d 100644 --- a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -57,7 +57,8 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `weak_first_distance_ratio` | double | Threshold for ring_outlier_filter | | `general_distance_ratio` | double | Threshold for ring_outlier_filter | | `weak_first_local_noise_threshold` | int | The parameter for determining whether it is noise | -| `visibility_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN | +| `visibility_error_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR | +| `visibility_warn_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN | | `roi_mode` | string | The name of ROI mode for switching | | `min_azimuth_deg` | float | The left limit of azimuth for `Fixed_azimuth_ROI` mode | | `max_azimuth_deg` | float | The right limit of azimuth for `Fixed_azimuth_ROI` mode | diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp index 1aa4acc63b93a..41c2130058f39 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp @@ -74,11 +74,12 @@ class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter private: void onVisibilityChecker(DiagnosticStatusWrapper & stat); Updater updater_{this}; - double visibility_ = 1.f; + double visibility_ = -1.0f; double weak_first_distance_ratio_; double general_distance_ratio_; int weak_first_local_noise_threshold_; - double visibility_threshold_; + double visibility_error_threshold_; + double visibility_warn_threshold_; int vertical_bins_; float max_azimuth_diff_; std::string roi_mode_; diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp index e6e435c7e7ca8..56d26e420464d 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp @@ -52,8 +52,11 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( weak_first_local_noise_threshold_ = static_cast(declare_parameter("weak_first_local_noise_threshold", 2)); - visibility_threshold_ = static_cast(declare_parameter("visibility_threshold", 0.5)); roi_mode_ = static_cast(declare_parameter("roi_mode", "Fixed_xyz_ROI")); + visibility_error_threshold_ = + static_cast(declare_parameter("visibility_error_threshold", 0.5)); + visibility_warn_threshold_ = + static_cast(declare_parameter("visibility_warn_threshold", 0.7)); } updater_.setHardwareID("dual_return_outlier_filter"); updater_.add( @@ -62,11 +65,11 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( updater_.setPeriod(0.1); image_pub_ = - image_transport::create_publisher(this, "/dual_return_outlier_filter/frequency_image"); + image_transport::create_publisher(this, "dual_return_outlier_filter/debug/frequency_image"); visibility_pub_ = create_publisher( - "/dual_return_outlier_filter/visibility", rclcpp::SensorDataQoS()); + "dual_return_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); noise_cloud_pub_ = create_publisher( - "/dual_return_outlier_filter/pointcloud_noise", rclcpp::SensorDataQoS()); + "dual_return_outlier_filter/debug/pointcloud_noise", rclcpp::SensorDataQoS()); using std::placeholders::_1; set_param_res_ = this->add_on_set_parameters_callback( @@ -79,15 +82,27 @@ void DualReturnOutlierFilterComponent::onVisibilityChecker(DiagnosticStatusWrapp stat.add("value", std::to_string(visibility_)); // Judge level - const auto level = - visibility_ > visibility_threshold_ ? DiagnosticStatus::OK : DiagnosticStatus::WARN; + auto level = DiagnosticStatus::OK; + if (visibility_ < 0) { + level = DiagnosticStatus::STALE; + } else if (visibility_ < visibility_error_threshold_) { + level = DiagnosticStatus::ERROR; + } else if (visibility_ < visibility_warn_threshold_) { + level = DiagnosticStatus::WARN; + } else { + level = DiagnosticStatus::OK; + } // Set message std::string msg; if (level == DiagnosticStatus::OK) { msg = "OK"; } else if (level == DiagnosticStatus::WARN) { - msg = "low visibility in dual outlier filter"; + msg = "WARNING: low visibility in dual outlier filter"; + } else if (level == DiagnosticStatus::ERROR) { + msg = "ERROR: low visibility in dual outlier filter"; + } else if (level == DiagnosticStatus::STALE) { + msg = "STALE"; } stat.summary(level, msg); } From 6f778c91452856bbb213c0f651c87aa567336ec6 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Fri, 4 Mar 2022 01:06:54 +0900 Subject: [PATCH 24/25] revert(dummy_perception): change leaf size and disable ray trace (#468) * Revert "chore(dummy_perception_publisher): change raytrace param (#414)" This reverts commit d29e0e1d6630ef53edea1dd66bebf1a657aa6e8b. Signed-off-by: taikitanaka * chore(dummy_perception): revert change leaf size and disable raytrace Signed-off-by: taikitanaka --- .../launch/dummy_perception_publisher.launch.xml | 2 +- simulator/dummy_perception_publisher/src/node.cpp | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml index 73385ef459233..4818bb554c5a1 100644 --- a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml +++ b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml @@ -17,7 +17,7 @@ - + diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index a14d083a4e2e4..da5939bccea7d 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -180,9 +180,7 @@ void DummyPerceptionPublisherNode::timerCallback() new pcl::PointCloud); pcl::VoxelGridOcclusionEstimation ray_tracing_filter; ray_tracing_filter.setInputCloud(merged_pointcloud_ptr); - // above this value raytrace won't be as expected - const double leaf_size = 0.02; - ray_tracing_filter.setLeafSize(leaf_size, leaf_size, leaf_size); + ray_tracing_filter.setLeafSize(0.25, 0.25, 0.25); ray_tracing_filter.initializeVoxelGrid(); for (size_t i = 0; i < v_pointcloud.size(); ++i) { pcl::PointCloud::Ptr ray_traced_pointcloud_ptr( From eb8015b72b443c26944d7428211601f295817cc4 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Fri, 4 Mar 2022 12:26:30 +0900 Subject: [PATCH 25/25] feat(behavior_velocity,behavior_path,route_handler): use route handler in behavior velocity (#472) * feat(route_handler): add getter for routing graphs Signed-off-by: tanaka3 * chore(behavior_velocity): add pkg and remove unused Signed-off-by: tanaka3 * feat(behavior_velocity): add route handler and repalce lanelet map Signed-off-by: tanaka3 * feat(behavior_velocity): replace routing graph Signed-off-by: tanaka3 * feat(behavior_velocity): replace overall graph to route handlers Signed-off-by: tanaka3 * chore(behavior_velocity): remove remainings Signed-off-by: tanaka3 * chore(behavior_velocity): refactor for readability Signed-off-by: tanaka3 * chore(behavavior_path): refactor use safe return Signed-off-by: tanaka3 --- .../lane_change/lane_change_module.cpp | 5 +-- .../scene_module/pull_out/pull_out_module.cpp | 6 ++-- .../pull_over/pull_over_module.cpp | 5 +-- .../planner_data.hpp | 9 +++--- .../behavior_velocity_planner/package.xml | 1 + .../behavior_velocity_planner/src/node.cpp | 32 +++---------------- .../src/scene_module/blind_spot/manager.cpp | 3 +- .../src/scene_module/blind_spot/scene.cpp | 10 +++--- .../src/scene_module/crosswalk/manager.cpp | 6 ++-- .../src/scene_module/crosswalk/util.cpp | 3 +- .../scene_module/detection_area/manager.cpp | 5 +-- .../src/scene_module/intersection/manager.cpp | 4 +-- .../intersection/scene_intersection.cpp | 9 +++--- .../scene_merge_from_private_road.cpp | 4 +-- .../src/scene_module/intersection/util.cpp | 3 +- .../scene_module/no_stopping_area/manager.cpp | 4 +-- .../occlusion_spot/scene_occlusion_spot.cpp | 4 +-- .../scene_occlusion_spot_in_public_road.cpp | 5 +-- .../src/scene_module/stop_line/manager.cpp | 6 ++-- .../scene_module/traffic_light/manager.cpp | 5 +-- .../virtual_traffic_light/manager.cpp | 7 ++-- .../include/route_handler/route_handler.hpp | 8 ++++- planning/route_handler/src/route_handler.cpp | 19 +++++++++-- 23 files changed, 86 insertions(+), 77 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 472d59a4d9a0d..67592fdf8a33e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -338,8 +338,9 @@ std::pair LaneChangeModule::getSafePath( // select valid path valid_paths = lane_change_utils::selectValidPaths( - lane_change_paths, current_lanes, check_lanes, route_handler->getOverallGraph(), current_pose, - route_handler->isInGoalRouteSection(current_lanes.back()), route_handler->getGoalPose()); + lane_change_paths, current_lanes, check_lanes, *route_handler->getOverallGraphPtr(), + current_pose, route_handler->isInGoalRouteSection(current_lanes.back()), + route_handler->getGoalPose()); if (valid_paths.empty()) { return std::make_pair(false, false); diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index a634d095f63a5..1a8701ff1672d 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -422,7 +422,7 @@ std::pair PullOutModule::getSafePath( // select valid path valid_paths = pull_out_utils::selectValidPaths( - pull_out_paths, road_lanes, check_lanes, route_handler->getOverallGraph(), current_pose, + pull_out_paths, road_lanes, check_lanes, *route_handler->getOverallGraphPtr(), current_pose, route_handler->isInGoalRouteSection(road_lanes.back()), route_handler->getGoalPose()); if (valid_paths.empty()) { @@ -493,7 +493,7 @@ std::pair PullOutModule::getSafeRetreatPath( // select valid path valid_paths = pull_out_utils::selectValidPaths( - pull_out_paths, road_lanes, check_lanes, route_handler->getOverallGraph(), current_pose, + pull_out_paths, road_lanes, check_lanes, *route_handler->getOverallGraphPtr(), current_pose, route_handler->isInGoalRouteSection(road_lanes.back()), route_handler->getGoalPose()); if (valid_paths.empty()) { @@ -574,7 +574,7 @@ bool PullOutModule::getBackDistance( // select valid path valid_paths = pull_out_utils::selectValidPaths( - pull_out_paths, road_lanes, check_lanes, route_handler->getOverallGraph(), current_pose, + pull_out_paths, road_lanes, check_lanes, *route_handler->getOverallGraphPtr(), current_pose, route_handler->isInGoalRouteSection(road_lanes.back()), route_handler->getGoalPose()); if (valid_paths.empty()) { diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 195b482708a21..06242c9e92cdf 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -353,8 +353,9 @@ std::pair PullOverModule::getSafePath( // select valid path valid_paths = pull_over_utils::selectValidPaths( - pull_over_paths, current_lanes, check_lanes, route_handler->getOverallGraph(), current_pose, - route_handler->isInGoalRouteSection(current_lanes.back()), route_handler->getGoalPose()); + pull_over_paths, current_lanes, check_lanes, *route_handler->getOverallGraphPtr(), + current_pose, route_handler->isInGoalRouteSection(current_lanes.back()), + route_handler->getGoalPose()); if (valid_paths.empty()) { return std::make_pair(false, false); diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp index da9896b898ca0..aabddf6ba6836 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp @@ -15,6 +15,8 @@ #ifndef BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_ #define BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_ +#include "route_handler/route_handler.hpp" + #include #include @@ -69,16 +71,11 @@ struct PlannerData std::deque velocity_buffer; autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; pcl::PointCloud::ConstPtr no_ground_pointcloud; - lanelet::LaneletMapPtr lanelet_map; // occupancy grid nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; // other internal data std::map traffic_light_id_map; - lanelet::traffic_rules::TrafficRulesPtr traffic_rules; - lanelet::routing::RoutingGraphPtr routing_graph; - std::shared_ptr overall_graphs; - // external data std::map external_traffic_light_id_map; @@ -86,6 +83,8 @@ struct PlannerData boost::optional external_intersection_status_input; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; + // route handler + std::shared_ptr route_handler_; // parameters vehicle_info_util::VehicleInfo vehicle_info_; diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 2c835429311e8..a69c2b04733a9 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -27,6 +27,7 @@ pcl_conversions rclcpp rclcpp_components + route_handler sensor_msgs tf2 tf2_eigen diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 06060d51d8c84..9bc3c94624a96 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -180,7 +180,10 @@ bool BehaviorVelocityPlannerNode::isDataReady() if (!d.no_ground_pointcloud) { return false; } - if (!d.lanelet_map) { + if (!d.route_handler_) { + return false; + } + if (!d.route_handler_->isMapMsgReady()) { return false; } @@ -252,32 +255,7 @@ void BehaviorVelocityPlannerNode::onLaneletMap( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) { // Load map - planner_data_.lanelet_map = std::make_shared(); - lanelet::utils::conversion::fromBinMsg( - *msg, planner_data_.lanelet_map, &planner_data_.traffic_rules, &planner_data_.routing_graph); - - // Build graph - { - using lanelet::Locations; - using lanelet::Participants; - using lanelet::routing::RoutingGraph; - using lanelet::routing::RoutingGraphConstPtr; - using lanelet::routing::RoutingGraphContainer; - using lanelet::traffic_rules::TrafficRulesFactory; - - const auto traffic_rules = - TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle); - const auto pedestrian_rules = - TrafficRulesFactory::create(Locations::Germany, Participants::Pedestrian); - - RoutingGraphConstPtr vehicle_graph = - RoutingGraph::build(*planner_data_.lanelet_map, *traffic_rules); - RoutingGraphConstPtr pedestrian_graph = - RoutingGraph::build(*planner_data_.lanelet_map, *pedestrian_rules); - RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); - - planner_data_.overall_graphs = std::make_shared(overall_graphs); - } + planner_data_.route_handler_ = std::make_shared(*msg); } void BehaviorVelocityPlannerNode::onTrafficSignals( diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp index f4edbc1b737a2..6189c003eb926 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp @@ -70,7 +70,8 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) void BlindSpotModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - for (const auto & ll : getLaneletsOnPath(path, planner_data_->lanelet_map)) { + for (const auto & ll : + getLaneletsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { const auto lane_id = ll.id(); const auto module_id = lane_id; diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index 3070e4b4ed82f..b0e8d4c8b74b8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -45,7 +45,8 @@ BlindSpotModule::BlindSpotModule( turn_direction_(TurnDirection::INVALID) { planner_param_ = planner_param; - const auto & assigned_lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id); + const auto & assigned_lanelet = + planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); if (!turn_direction.compare("left")) { turn_direction_ = TurnDirection::LEFT; @@ -74,8 +75,8 @@ bool BlindSpotModule::modifyPathVelocity( geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; /* get lanelet map */ - const auto lanelet_map_ptr = planner_data_->lanelet_map; - const auto routing_graph_ptr = planner_data_->routing_graph; + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* set stop-line and stop-judgement-line for base_link */ int stop_line_idx = -1; @@ -518,7 +519,8 @@ bool BlindSpotModule::isTargetObjectType( boost::optional BlindSpotModule::getStartPointFromLaneLet( const int lane_id) const { - lanelet::ConstLanelet lanelet = planner_data_->lanelet_map->laneletLayer.get(lane_id); + lanelet::ConstLanelet lanelet = + planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); if (lanelet.centerline().empty()) { return boost::none; } diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp index d4425eb8ff917..52d2da760d575 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp @@ -90,8 +90,9 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) void CrosswalkModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { + const auto rh = planner_data_->route_handler_; for (const auto & crosswalk : - getCrosswalksOnPath(path, planner_data_->lanelet_map, planner_data_->overall_graphs)) { + getCrosswalksOnPath(path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr())) { const auto module_id = crosswalk.id(); if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( @@ -112,8 +113,9 @@ std::function &)> CrosswalkModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { + const auto rh = planner_data_->route_handler_; const auto crosswalk_id_set = - getCrosswalkIdSetOnPath(path, planner_data_->lanelet_map, planner_data_->overall_graphs); + getCrosswalkIdSetOnPath(path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); return [crosswalk_id_set](const std::shared_ptr & scene_module) { return crosswalk_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp index 6fc8498a79d4a..1f6b8d9a9eb46 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp @@ -175,7 +175,8 @@ lanelet::Optional getStopLineFromMap( const int lane_id, const std::shared_ptr & planner_data, const std::string & attribute_name) { - lanelet::ConstLanelet lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id); + lanelet::ConstLanelet lanelet = + planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); const auto road_markings = lanelet.regulatoryElementsAs(); lanelet::ConstLineStrings3d stop_line; for (const auto & road_marking : road_markings) { diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp index f07106d6ea1af..8dc92269a4e1d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp @@ -73,7 +73,7 @@ void DetectionAreaModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { for (const auto & detection_area : - getDetectionAreaRegElemsOnPath(path, planner_data_->lanelet_map)) { + getDetectionAreaRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { // Use lanelet_id to unregister module when the route is changed const auto module_id = detection_area->id(); if (!isModuleRegistered(module_id)) { @@ -88,7 +88,8 @@ std::function &)> DetectionAreaModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto detection_area_id_set = getDetectionAreaIdSetOnPath(path, planner_data_->lanelet_map); + const auto detection_area_id_set = + getDetectionAreaIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); return [detection_area_id_set](const std::shared_ptr & scene_module) { return detection_area_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index b9e88e71a1287..b46f18a139a70 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -100,7 +100,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node void IntersectionModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto lanelets = getLaneletsOnPath(path, planner_data_->lanelet_map); + const auto lanelets = getLaneletsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); @@ -126,7 +126,7 @@ void IntersectionModuleManager::launchNewModules( void MergeFromPrivateModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto lanelets = getLaneletsOnPath(path, planner_data_->lanelet_map); + const auto lanelets = getLaneletsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 786d44190af87..9e2e86ccfe421 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -56,7 +56,8 @@ IntersectionModule::IntersectionModule( : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id) { planner_param_ = planner_param; - const auto & assigned_lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id); + const auto & assigned_lanelet = + planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); turn_direction_ = assigned_lanelet.attributeOr("turn_direction", "else"); has_traffic_light_ = !(assigned_lanelet.regulatoryElementsAs().empty()); @@ -85,8 +86,8 @@ bool IntersectionModule::modifyPathVelocity( geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; /* get lanelet map */ - const auto lanelet_map_ptr = planner_data_->lanelet_map; - const auto routing_graph_ptr = planner_data_->routing_graph; + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* get detection area and conflicting area */ std::vector detection_area_lanelets; @@ -603,7 +604,7 @@ bool IntersectionModule::checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids) { for (const int lanelet_id : target_lanelet_ids) { - const auto ll = planner_data_->lanelet_map->laneletLayer.get(lanelet_id); + const auto ll = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lanelet_id); if (!lanelet::utils::isInLanelet(pose, ll, planner_param_.detection_area_margin)) { continue; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index 36dd26cef593c..92f42ed7a2905 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -61,8 +61,8 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; /* get lanelet map */ - const auto lanelet_map_ptr = planner_data_->lanelet_map; - const auto routing_graph_ptr = planner_data_->routing_graph; + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* get detection area */ std::vector detection_area_lanelets; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 6dbe52073ae53..b0a91cba575f3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -265,7 +265,8 @@ bool getStopPoseIndexFromMap( const std::shared_ptr & planner_data, int & stop_idx_ip, int dist_thr, const rclcpp::Logger logger) { - lanelet::ConstLanelet lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id); + lanelet::ConstLanelet lanelet = + planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); const auto road_markings = lanelet.regulatoryElementsAs(); lanelet::ConstLineStrings3d stop_line; for (const auto & road_marking : road_markings) { diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp index df372e73e1406..82e4211f52c6f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp @@ -80,7 +80,7 @@ void NoStoppingAreaModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { for (const auto & no_stopping_area : - getNoStoppingAreaRegElemsOnPath(path, planner_data_->lanelet_map)) { + getNoStoppingAreaRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { // Use lanelet_id to unregister module when the route is changed const auto module_id = no_stopping_area->id(); if (!isModuleRegistered(module_id)) { @@ -97,7 +97,7 @@ NoStoppingAreaModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { const auto no_stopping_area_id_set = - getNoStoppingAreaIdSetOnPath(path, planner_data_->lanelet_map); + getNoStoppingAreaIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); return [no_stopping_area_id_set](const std::shared_ptr & scene_module) { return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index ad7d7da0f428a..426a2efbfbf6e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -60,10 +60,8 @@ bool OcclusionSpotModule::modifyPathVelocity( 0.0); } const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; - const auto & lanelet_map_ptr = planner_data_->lanelet_map; - const auto & traffic_rules_ptr = planner_data_->traffic_rules; const auto & occ_grid_ptr = planner_data_->occupancy_grid; - if (!lanelet_map_ptr || !traffic_rules_ptr || !occ_grid_ptr) { + if (!occ_grid_ptr) { return true; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp index 60e2d211e9ca6..a351bce4a62c3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp @@ -60,12 +60,9 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( 0.0); } const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; - const auto & lanelet_map_ptr = planner_data_->lanelet_map; - const auto & routing_graph_ptr = planner_data_->routing_graph; - const auto & traffic_rules_ptr = planner_data_->traffic_rules; const auto & dynamic_obj_arr_ptr = planner_data_->predicted_objects; - if (!lanelet_map_ptr || !traffic_rules_ptr || !dynamic_obj_arr_ptr || !routing_graph_ptr) { + if (!dynamic_obj_arr_ptr) { return true; } PathWithLaneId clipped_path; diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp index ebb038c191f85..7c78395470e73 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp @@ -94,7 +94,8 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) void StopLineModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - for (const auto & stop_line : getStopLinesOnPath(path, planner_data_->lanelet_map)) { + for (const auto & stop_line : + getStopLinesOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { const auto module_id = stop_line.id(); if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( @@ -107,7 +108,8 @@ std::function &)> StopLineModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto stop_line_id_set = getStopLineIdSetOnPath(path, planner_data_->lanelet_map); + const auto stop_line_id_set = + getStopLineIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); return [stop_line_id_set](const std::shared_ptr & scene_module) { return stop_line_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp index bb82168f170dd..2b2d8dff8b36a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp @@ -123,7 +123,7 @@ void TrafficLightModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { for (const auto & traffic_light_reg_elem : - getTrafficLightRegElemsOnPath(path, planner_data_->lanelet_map)) { + getTrafficLightRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { const auto stop_line = traffic_light_reg_elem.first->stopLine(); if (!stop_line) { @@ -147,7 +147,8 @@ std::function &)> TrafficLightModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto lanelet_id_set = getLaneletIdSetOnPath(path, planner_data_->lanelet_map); + const auto lanelet_id_set = + getLaneletIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); return [lanelet_id_set](const std::shared_ptr & scene_module) { return lanelet_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp index c637ffbd73ef0..7ac6918f13ed2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp @@ -81,8 +81,8 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node void VirtualTrafficLightModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - for (const auto & m : - getRegElemMapOnPath(path, planner_data_->lanelet_map)) { + for (const auto & m : getRegElemMapOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr())) { // Use lanelet_id to unregister module when the route is changed const auto module_id = m.second.id(); if (!isModuleRegistered(module_id)) { @@ -97,7 +97,8 @@ std::function &)> VirtualTrafficLightModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto id_set = getLaneletIdSetOnPath(path, planner_data_->lanelet_map); + const auto id_set = + getLaneletIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); return [id_set](const std::shared_ptr & scene_module) { return id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 711709dfda9a7..7cf38a40d0ee2 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -73,7 +73,13 @@ class RouteHandler bool isHandlerReady() const; lanelet::ConstPolygon3d getExtraDrivableAreaById(const lanelet::Id id) const; Header getRouteHeader() const; - lanelet::routing::RoutingGraphContainer getOverallGraph() const; + + // for routing graph + bool isMapMsgReady() const; + lanelet::routing::RoutingGraphPtr getRoutingGraphPtr() const; + lanelet::traffic_rules::TrafficRulesPtr getTrafficRulesPtr() const; + std::shared_ptr getOverallGraphPtr() const; + lanelet::LaneletMapPtr getLaneletMapPtr() const; // for routing bool planPathLaneletsBetweenCheckpoints( diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 9619794208fc0..ac48a20ea0209 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1295,11 +1295,26 @@ lanelet::ConstLanelets RouteHandler::getCheckTargetLanesFromPath( return check_lanelets; } -lanelet::routing::RoutingGraphContainer RouteHandler::getOverallGraph() const +bool RouteHandler::isMapMsgReady() const { return is_map_msg_ready_; } + +lanelet::routing::RoutingGraphPtr RouteHandler::getRoutingGraphPtr() const +{ + return routing_graph_ptr_; +} + +lanelet::traffic_rules::TrafficRulesPtr RouteHandler::getTrafficRulesPtr() const { - return *overall_graphs_ptr_; + return traffic_rules_ptr_; } +std::shared_ptr RouteHandler::getOverallGraphPtr() + const +{ + return overall_graphs_ptr_; +} + +lanelet::LaneletMapPtr RouteHandler::getLaneletMapPtr() const { return lanelet_map_ptr_; } + lanelet::routing::RelationType RouteHandler::getRelation( const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const {